ADIS 16365와 EKF(Extended Kalman Filter)로 ARS(Attitude Reference System) 만들기
로봇의 자세를 측정하기 위해 ARS(Attitude Reference System)를 설계하는 방법에 대하여 설명합니다. ARS를 만들기 위해 먼저 디바이스로부터 각속도를 읽어와 적분하여 자세(roll, pitch, yaw)를 구합니다. 하지만 적분하는 과정에서 노이즈와 계산 오차가 조금씩 누적되어 결국은 각도가 틀어져 버립니다. 이를 막기위해 가속도 센서에서 구한 중력의 방향으로 roll과 pitch 각을 보정해야 합니다. 가속도 센서는 중력 가속도 외에도 센서에 작용하는 모든 가속도를 측정하기 때문에, 가속도 센서에서 측정한 가속도 성분들 중에서 중력 가속도만을 분리해 내는 것은 쉽지 않습니다. 여기서는 측정되는 가속도의 크기가 9.8 근처의 값을 가질 때 roll과 pitch 각을 보정하도록 하였습니다. 만일 지자기 센서가 있다면 지자기의 방향을 측정하여 yaw 각도 보정하도록 하였습니다.
상기 과정은 EKF(Extended Kalman Filter)를 사용하여 예측과 보정으로 나눌 수 있습니다. EKF는 자이로의 각속도를 적분하여 오일러각을 예측(predict)하고 중력 가속도의 방향과 지자기의 방향으로부터 구한 각도로 오일러각을 보정(update)하는 과정을 수행하게 됩니다.
먼저 각속도, 가속도, 지자기 값을 읽어들일 센서가 필요합니다. 여기서는 Analog Device사의 ADIS16365와 ADISUSBZ 라는 제품을 사용하였습니다. ADIS 16365는 각속도와 가속도를 측정하는 센서이고 ADISUSBZ는 PC의 USB 포트로 센서의 값을 전송하는 보드입니다. 만일 지자기 까지 읽기를 원한다면 ADIS 16405 모델을 주문하시면 됩니다.
ADIS 16365와 ADISUSBZ 보드의 드라이브 설치는 다음 블로그를 참고하세요: http://abipictures.tistory.com/518
* Analog Device사의 다운로드 사이트에서는 드라이버가 Windows XP까지만 검증되었다고 나와있습니다. 실제로 Windows 7에서는 드라이버가 설치는 되지만 측정값을 읽을 수 없었습니다.
ARS 구현을 위해 EKF를 적용하는 방법은 다음 문서를 참고하십시오.
여기서 오일러 각을 칼만필터의 상태 벡터로 그대로 이용하는 것은 문제가 좀 있습니다. pdf 문서의 오일러각 예측 부분을 보면 C의 역행렬이 있는데, 여기서 theta, 즉, pitch 각이 +-90도 근처에서는 무한대가 됩니다. 실제로 센서를 수직으로 세우면 자세가 튀는 현상이 발생하고 roll과 yaw 각도 부정확해지는 현상을 볼 수 있습니다.
구현된 소스코드는 다음 파일을 참고하시기바랍니다.
소스코드에서 EKF는 ARSbyEKF.cpp 파일에 구현되어 있습니다. ARS, EKF를 공부하시는 분들께 도움이 되기를 바랍니다.
아래 동영상을 보시면 yaw각에 오차가 누적되는 것을 볼 수 있습니다. 실험에 사용한 ADIS 16365는 지자기 센서가 없어 yaw각을 보정하지 못하기 때문에 발생하는 현상입니다.
다음에는 회전행렬(Rotation Matrix)을 이용하여 ARS를 구현하는 방법과, 쿼터니언(Quaternion)을 이용하여 ARS를 구현하는 방법에 대하여 설명드리겠습니다.
* 2011.9.5 업데이트
- 시스템 모델의 입력에 대한 공분산 계산식 오류를 수정함