최소자승법을 이용한 각도추정 보정필터의 설계

이번에 태국학회에 다녀 왔습니다. 이번에 다녀온 학회에서 발표한 내용을 잠시 소개할까합니다. 주제는 가속도센서와 자이로센서를 이용한 각도추정인데요. 이미 이전에 [[프로젝트/HardwarePart] - 자이로센서를 이용한 각도 측정시 가속도센서로 보정하기]에서 말씀드린적이 있습니다. 그 내용에서 보정용 필터의 계수들을 최소자승법(least-square)[[공학기초/Theory] - [공업수학] 최소자승법 Least Square]로 도출하는 방법에 대해 이야기하는 것입니다. 

가속도센서와 자이로센서를 이용한 기존의 각도 추정에 대한 정리 !

위 그림은 자이로센서의 적분을 통해 각도를 추정할때 보정용으로 가속도센서에서 측정된 각도를 사용할때 많이 사용하는 보정방법중 하나입니다. 위 블럭을 식으로 꾸며보면,

인데요. 그 의미를 좀 명확히 알기 위해 식을 풀어보면

 
이 됩니다. 즉, 말로 이야기하면, 자이로를 적분한 각도에는 하이패스필터를 가속도센서에서 도출된 각도에는 로우패스필터가 장착된것입니다. 이 보정법이 정확하고도 아름답게 작동되는 환경은 좌우로 마구 흔들리는 환경이라도 그 평균은 거의 가속도센서의 값이 참값일때 가장 잘 작동됩니다.^^ (보통의 경우 대부분 그렇다고 할 수 있나요?^^)

 
그런데 위 필터의 계수인 Kp, Ki를 선정하는 것이 그리 쉽지는 않습니다. 무작정선정하면 또 안될거구요. 그래서 생각한것이 최소자승법입니다.

필터계수를 구하는 체계적인 방법에 대한 고찰 !

먼저 위 실험기구를 구성하구요. 자이로와 가속도센서를 장착하고, 센서의 성능을 확인하기 위해 엔코더를 설치했습니다. 자이로와 가속도센서만으로 각도를 검출하는데 문제가 있도록 하기위해([프로젝트/HardwarePart] - 가속도센서를 이용한 각도 측정과 그 한계, [프로젝트/HardwarePart] - 자이로센서를 이용한 각도검출과 그 한계 참조) Arm부분과 Pole부분은 모두 회전을 허용하고 있습니다.

처음 필터 수식에서

 
엔코더에서 도출된 각도를 theta_e라고 두고 자이로 출력을 theta_g, 가속도센서에서의 출력을 theta_c라고 합니다.

 
그러면 위와 같이 미지수행렬(Kp, Ki)을 구성하고 나머지로 묶어낼수가 있게 됩니다.

그러면 위에서처럼 최소자승법을 사용할 수 있게 되는 것이지요이 때, A, B는

입니다. 
실험을 수행하고 얻은 계수는

입니다. 이를 다시 대입하여 성능을 확인해보니

참값과의 비교결과가 정말 잘 나오는 군요. 대략 한시간 이상을 실험해도 드리프트는 없습니다. 응답속도는 엔코더의 값을 해석하는 속도보다 대략 10ms정도 느립니다. 당연히 저희가 잡은 한 스테이트, 즉 샘플링타임이 10ms라 아마 한 스테이트정도의 응답이 늦게 나오는것 같네요^^ 

이 논문덕분에 즐거운 태국학회를 하고 왔네요^^

Design of angular estimator of inertial sensor using the le.pdf