자이로센서와 가속도센서를 이용한 각도추정 1차필터

사실.. 자이로센서와 가속도센서의 병합(fusion)에 대한 이야기는 더이상 할 필요가 없을 줄 알았습니다. 그런데, 제 블로그에 자주 오시는 "재오리"님께서 알려주신 링크를 따라 가보니 재미난 이야기가 있더군요.[방문해보기] 언제 한번 포스팅을 해야겠다고 생각했지만, 차일피일 미루다 이제 하게 되네요^^ 먼저, 하도 오래전 이야기라 역사를 좀 이야기를 해야겠네요^^. 

각도를 측정하는 센서중 가장 정확하다 싶은 것은 아마 다들 엔코더라고 생각하실것 같습니다. 그런데 항공기나 배, 하여간 공중에 떠있거나 어떤 기준점을 잡을 수 없는 경우 이 엔코더는 무용지물이 됩니다. 왜냐면 엔코더는 자신을 지지하는 곳에서 바라본 상대각도를 잡기 때문이지요. 결국, 로봇(혹은 어떻 항공기든)이 자신의 기울어진 각도를 알기 위해 관성센서(자이로나 가속도)를 많이 사용합니다. 

여기서 자이로센서는 각속도를 검출하는 센서로서 사실, 한번이라도 테스트를 해보신분들은 그 어마어마한 적분오차(드리프트에러)에 깜짝 놀라게 됩니다.[관련글] 또하나의 방법이 가속도센서를 가지고 각도를 검출하는 방법이 있습니다. 요거 한번 사용해보면 정말 기분이 좋습니다. 원체 잘 잡아주거든요. 그러나 얘도 문제가 있습니다. 바로 센서에 병진운동성분이 인가되면 각도 추정에 문제가 생기게 됩니다. [관련글] 당연히 위 두 센서의 문제점을 극복하고 각도측정을 올바로 하기 위해서 두 센서를 병합(fusion)하게 되는게 일반적인 공부의 순서(^^)인데요. 요게 만만치가 않지요. 사실 인터넷을 조금 서치해보면 칼만필터라는 아름다운 필터를 만날 수 있습니다. 

단점요?? 어렵죠... ㅎㅎㅎ 그래서 저는 재작년(2008년)말에 세종대학교 홍성경 교수님의 논문을 보게 됩니다. (언제한번 만나뵙고 싶군요. 제 블로그에 하도 언급을 많이 해서...^^) 그리고 완성된 수식을 역으로 이해하는 것은 마법의 힘을 빌려야할지도 모른다는 우스개소리를 듣는 칼만필터를 미련없이(^^) 버리고 자이로와 가속도센서를 병합하는 상대적으로 쉬운 필터기법을 공부하게 됩니다. [관련글] 방금 이야기한 그 필터는 2차 저역/고역 통과필터를 사용하는 것인데요. 처음이야기한데로, "재오리"님께서 알려주신 자료를 보고, 1차 필터를 테스트 해보게 될 생각을 한 것이죠^^ 이 후 전개하는 내용에서 실험환경은 이전[관려글]의 2차필터를 테스트할때와 동일합니다.^^

위의 펜들럼에서 진자와 암의 경계선상에 자이로와 가속도센서를 장착하고, 암을 흔듭니다. 일단, 필터부터 소개를 해보죠^^

dot_theta_g앞에 1/s가 있으니 자이로 센서를 적분한 결과에는 고역통과필터를 장착하고, 가속도센서에서 얻은 각도인 theta_c에는 저역통과필터를 장착합니다. 위 수식을 좀 다음어서

와 같이 표현합니다. 이를 블럭선도로 표현하면

와 같습니다. 간단하죠?? 우리가 알아낼 것이라고는 a만 알아내면 됩니다. 이제 실험해야죠^^

암을 아주 흔들어서 진자의 흔들린 각도를 엔코더로 받은 것입니다. 일단 저게 각도에서는 참값이라고 해두죠. (더 정확한 센서가 없으니 말이죠..ㅠㅠ) 그럼, 가속도센서에 얼마나 병진운동성분이 인가 되었는지 확인해보죠

위와 같습니다. 위 빨간선은 가속도센서에서 구한 각도입니다.[관련글] 이제 저는 예전에 언급한적이 있는 MATLAB/Simulink에서 제공하는 Parameter Estimation 툴박스를 사용할려고 합니다.[관련글] 당연히 필터의 결과가 엔코더와 같기를 바라는 것이죠.

시뮬링크 블럭은 위와 같이 꾸밉니다

파라미터추정 툴박스를 사용한 첫 화면입니다.

MATLAB이 아주 열심히 필터의 계수 a를 변화시켜가면서 참값(엔코더)을 따라가게 할려고 애쓰고 있는 중입니다.

그렇게 해서 구한 a값이 2.8489네요. 딱 좋군요^^ 이걸 다시 필터에 적용시켜 보겠습니다. C로 구현하길 바라는 분들을 위해 m-file로 작성했습니다.

> 실제 메인코드는 위에서 45,46번행입니다. 뭐 사실 한줄이지요^^. 아주 간단히 구현할 수 있겠네요. 다시한번 말씀드리지만, AccAng는 가속도센서에거 구한 각도입니다. Gyro는 당연히 자이로센서의 결과인 각속도구요. 모든 수치는 단위가 라디안입니다. 그림그릴때만 degree로 변환합니다.^^

음.. 괜찮네요^^. 위 필터의 성능을 확인하기 위해 한번더 실험했습니다.

이번에는 암을 위와같이 흔들었구요

가속도센서에서 잡은 각도가 위와 같습니다. 역시 필터가 동원되어야겠군요. (아참.. 자이로는 당연히 드리프트오차가 있어서 그래프를 제시도 안했습니다.^^)

괜찮네요. 물론 아주 고주파의 환경에서는 테스트하지 않았지만, 뭐 쓸만하다는 생각이 드네요. 언제한번 시간이 허락하면 실제시스템에 적용한 이야기를 해봐야겠네요^^.