GPS + IMU 센서퓨전 2 (robot_localization)

저번에 일일히 구현한 DR 코드는 폐기하기로 했다. (필터랑 GPS 복귀시 블렌딩 같은거까지 구현할 시간이 없음)

대신 이번엔 ROS 내부 패키지인 robot_localization을 사용하기로 했다. 이미 모든 계산 모듈이 들어있기에, 내가 해줘야할건 토픽과 파라미터를 맞추는것이다.

 

우선 사용 센서는 저번과 동일하게 GPS, IMU 두가지이고, 이번엔 차량에서 나오는 피드백 데이터인 /erp42_serial/feedback 토픽을 활용하기로 했다. 속도와 엔코더, 스티어 값 등등이 나오는 피드백이다. 하지만 스티어 값은 데이터 밀림이 심하여 사용하지 못했고, 엔코더는 1회전 당 정확한 펄스수를 몰라서 사용하지 못했다. 그나마 속도값이 정확한듯 싶어서 feedback 토픽의 속도값을 활용하기로 했다. (kph 단위)

 

품질 판정 로직은 저번과 동일하다. navstat에서 fixstat == 3 && flags2 >=72 일때만 정상으로 판단하여 gps_quality_monitor 노드에서 /gps_ok를 True로 발행, 그 외에는 비정상으로 판단하여 /gps_ok를 False를 반환하고 DR 추정 모드로 전환한다. 

 

DR 추정 방식은 robot_localization EKF를 사용했다. /imu/data를 입력받고. yaw, yaw_rate를 사용하여 /odom_dr로 발행한다.

 

핵심 노드는 pose_arbiter인데,
1. /gps_ok == True면 GPS Pose를 사용.
2. /gps_ok == False면 DR Pose를 사용.

3. GPS -> DR 전환 시 : DR에 GPS 마지막 위치에 맞춰 offset(적용)

4. DR -> GPS 전환 시 : 0.5초간 블렌딩

 

위와 같이 동작한다. /pose_for_pp와 GPS모드인지 DR모드인지를 /pose_source로 발행한다. 따라서 기존에 사용하던 pp.launch로 rddf 기반 주행을 하되, gps 드랍 상황에서도 DR모드로 자동전환되어 끊김없이 주행 가능하다.

 

 

위 사진은 rosbag으로 pp.launch와 내 코드를 켰을때 나온 rqt_graph를 띄운것이다. 설명한대로 node들이 잘 연결된 것을 볼 수 있다. 자세한 테스트는 토요일에 k-city가서 해볼 예정이다.