이전 글에서 로봇의 자세를 측정하기 위한 ARS(Attitude Reference System)를 EKF로 설계하는 방법에 대하여 설명하였습니다. 여기서는 1차 상보필터(complementary filter)와 회전행렬(Rotation Matrix)로 ARS 만들어 보겠습니다.
이전 글에서와 마찬가지로 각속도, 가속도, 지자기 값을 읽어들일 센서로 Analog Device사의 ADIS16365와 ADISUSBZ 라는 제품을 사용하였습니다. 프로그램 구조 또한 동일하며 소스코드에서 바뀌는 부분은 ARSbyEKF.cpp을 대신하여 ARSbyRotationMatrix.cpp를 추가하였습니다.
상세한 설명은 다음 pdf 파일을 참고하시기 바랍니다.
소스코드를 첨부합니다.
아래 동영상을 보시면, yaw 각의 오차는 있지만 pitch 각이 +-90도 근처에서도 안정적으로 동작함을 볼 수 있습니다. 하지만 회전행렬을 이용할 때는 주의할 점이 있습니다. 3차원 공간상에서 물체의 자세는 roll, pitch, yaw 3개의 각으로 표시할 수 있습니다. 그러나 회전행렬에서는 물체의 자세를 9개의 원소로 표시합니다. 즉, 6개의 원소는 종속되어 있다는 뜻입니다.
회전행렬은 3개의 x, y, z 좌표축 방향의 단위벡터로 표시됩니다. 3개의 벡터의 크기가 1이어야 하고 서로 직교해야하는 성질이 있습니다. 그런데 계속해서 적분을 하다보면 이러한 성질이 만족되지 않을 수 있습니다. 그래서 일정한 주기마다 이러한 성질을 만족하도록 회전행렬을 보정해 주어야 합니다.