한동안 자이로 센서와 가속도 센서로 AHRS 만들기에 도전했었는데요. 이번에는 자이로 센서와 가속도 센서, GPS 센서, 바퀴의 오도메트리 정보(차량의 바퀴에서 오도메트리 정보를 읽을 수 없는 상황이라 GPS의 속도 출력 정보를 오도메트리 정보로 대신함)를 결합한 INS(Inertial Navigation System)를 설계하고 실제로 차에 장착하여 실험해 보았습니다. 아래 지도에서 빨간색 선이 차로 이동한 경로입니다.
먼저 차가 경로를 따라가면서 GPS, 자이로, 가속도 센서의 데이터를 취득하였습니다. 소스코드에 첨부된 imu_data.txt, gps_data.txt 파일을 참조하시기 바랍니다. 그리고 이 데이터를 실험실로 가져와 재생하면서 Kalman filter 알고리즘을 수정해 나갔습니다. 두 개의 kalman filter가 사용되었는데, 자이로 값을 적분하여 차량의 방위를 계산하기 위해 하나를 사용하고 가속도 값을 적분하여 차량의 속도와 위치를 계산하기 위해 또 하나를 사용합니다. 다음 문서를 참조 바랍니다.
상기 문서에서 Kalman Filter의 동작을 간략히 정리하면 다음과 같습니다:
1. Attitude를 계산할 때
(1) Gyro 값을 적분하여 자세를 추정합니다.
(2) Accel 값으로 roll, pitch 각을 보정합니다.
(3) GPS의 Heading 각으로 yaw 각을 보정합니다.
2. Velocity를 계산할 때
(1) Accel 값을 적분하여 속도와 위치를 추정합니다.
(2) 차량의 뒷바퀴에 장착된 엔코더로 속도를 측정하여 속도를 보정합니다.
(차량 좌표계에서 차량의 y,z축 속도는 0입니다.)
(3) GPS의 위치로 위치를 보정합니다.
그리고 구현된 코드는 다음 파일입니다.
아래 그림에서 붉은 색 선은 GPS의 이동 경로이고 파란색 선은 GPS와 IMU를 결합한 이동 경로입니다. 빨간색 동그라미에서 GSP의 위치 수신을 끊었는데도 IMU만 가지고 위치를 잘 추종함을 알 수 있습니다. 실험 당시에는 바퀴의 오도메트리 정보를 사용할 수 없어, 오도메트리 대신 GPS의 속도 정보는 사용하고 있었습니다.
