mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 2

(내용을 보완한 글을 새로 올렸습니다~! 하단부 알림글을 먼저 읽어보시기 바랍니다~!)

오늘은 상보필터(complementary filter)또는 보상필터라고 하는 센서 융합방법에 대해 포스팅 하겠습니다~

이전 포스팅에서는 칼만필터로 융합했었는데 수박 겉햝기식이라 내부 구조를 변경하고 이해하기엔 좀 힘들었는데 요 방법은 좀 더 직관적이고 이해하기 쉬워서 제가 맘대로 조절하기도 편하더라고요

왜 다시 융합으로 돌아왔느냐~ 하면요 ㅠ roll, pitch, yaw를 센서가 알아서 계산해서 제공해주는 값을 사용했는데 그게 update rate(샘플링타임)가 100Hz였습니다. 근데 쿼드콥터 네이버 카페에 쌍둥아빠이기성이라는 분이 댓글을 달아주셨는데 최소 200Hz는 되어야 뜬다고 하더라구요.. 전 그것도 모르고 왜안되나~ 삽질만하고.. 오히려 50Hz로 낮춰보기까지.. ㅠ 제 센서가 100Hz인데 더 높일수가 없어서 다시 mpu6050으로 돌아왔습니다. 

mpu6050의 update rate는 가속도, 자이로 각각 4~1000Hz, 4~8000Hz인데 왜 범위냐면 내부의 LPF(Low Pass Filter)를 사용할 여지가 있기 때문인거 같습니다.

아무튼, 더 빠른 센서가 필요해서 MPU6050으로 돌아왔는데, 예전 칼만필터 소스를 아두이노에서 돌리려니까 한 루프에 10ms가 넘더라구요..;;

그래서 다른 필터를 찾다가 상보 필터가 있길래 공부해서 적용시켜봤습니다.

상보 - [명사] 서로 모자란 부분을 보충함. - 네이버 사전

두 센서의 서로 모자란 부분을 보충해서 더 좋은 결과를 만들어내는게 상보 필터입니다.

가속도 센서와 자이로 센서에 대해서는 이전 포스팅을 참고해주세요. -  mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 1 

이 블록선도를 구현할 건데요. 

먼저, 가속도 센서값을 roll, pitch로 바꿔야 합니다.

바꾸는 수식은 되게 많은데.. 제가 사용한 방법은 아래과 같습니다.

xAngle이 roll이 됩니다. 마찬가지로 pitch도 구할 수 있는데 yValue대신 xValue를 사용하면 되겠죠?

스케일링을 안하는 이유는 어차피 약분될거기 때문입니다. 

다음으로는 자이로 센서를 roll, pitch로 바꿔보겠습니다.

mpu6050은 가속도 자이로 모두 -90도~90도의 값을 -16383~16383로 주기 때문에 스케일링을 해야합니다.  -> (오류 : 하단참고)

 -> (오류 : 하단참고)

참 쉽죠? ㅋ

이걸 dt(시간변화량)으로 곱해서 누적시키면 각도가 나오게 됩니다. 적분하는거죠.

그럼 이제 가속도와 자이로센서 각각의 각도를 구했습니다. mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 1 제 포스팅에 있는 그래프처럼 서로의 장단점을 보완해 봅시다.

자이로 값이 기준이 됩니다. 가속도 값이 보정값이 되는거구요. 

간단하게 말하면 자이로 값과 가속도센서의 오차를 누적시키고 누적된 오차를 자이로센서값에 더해서 가속도센서값에 근접시키는 겁니다.

다만 얼마나 빠르게 근접시키느냐에 PI제어를 추가한 거라고 보시면 되겠습니다. P게인 I게인 값을 조절하는데, 가속도 값에 빠르게 근접시키면 노이즈까지 따라갈테고 느리게 근접시키면 노이즈는 무시되겠지만 반응이 느려지는 관계를 잘 절충해서 조절해야합니다.

PID제어에 관한 개념도 제 블로그에 있으니 참고바랍니다. - 쿼드콥터 PID제어

식으로 표현하면 

입니다. 이해하기 쉽게 써볼라고 길게 늘려봤는데 이게 수학적 문법에 맞는 식인지 모르겠네요..ㄷㄷ 

식 그대로네요.. 여기에 글로 해설해 봤자 식을 읽는거에 불과하니 괄호 하나하나 이해하면서 해석해보시기 바랍니다.

High Pass Filter와 Low Pass Filter식을 유도하는거는 pinkwink님의 블로그를 참고하시기 바랍니다. - http://pinkwink.kr/78

아두이노에 사용한 소스 코드도 올리니까 한줄 한줄 이해해 보시기 바랍니다.

double x_angle = -atan((double)ay/(double)az)*180.0 / 3.1416; // 가속도 센서 각도 구하기 integral_gx += (-gx/16383.0*90.0) * dt; // 자이로 값 적분해서 자이로센서 각도 저장 cf_err_x = cf_result_x - x_angle; // 전 값과 에러 도출 // P 제어에 사용 cf_err_sum_x += cf_err_x * dt; // 에러를 적분해서 저장 // I 제어에 사용 cf_kp_val_x = cf_err_x * CF_KP;            // P value cf_ki_val_x = cf_err_sum_x * CF_KI;        // I value cf_pid_out_x = (-gx/16383.0*90.0) - (cf_kp_val_x + cf_ki_val_x + cf_kd_val_x); // 자이로 각도값을 PID제어를 통해 얼마나 빠르게 가속도 각도값에 맞출 것인지 결정 cf_result_x += cf_pid_out_x * dt; // 상보필터 결과를 저장 // 자이로값이므로 적분으로저장

코드 -> (오류 : 하단참고)

결과는,

이렇습니다. 빨간색이 가속도, 초록색이 자이로, 파란색이 상보필터네요. 여기서는 P게인이 낮아서 부드럽지만 다소 지연이 발생하는걸 보실 수 있습니다. 자기가 사용하는 상황에 맞게 튜닝 해야겠죠? ㅠㅠ 이부분이 제일 어려운거 같습니다 ㅜㅜ

팁으로 아두이노에서 dt(한 루프 걸리는 시간)를 구할 때는 millis()나 micros()를 사용하시면 됩니다.

아, delayMicroseconds(int n)함수는 n에 숫자 제한이 있더라구요.. 그것도모르고 사용하다가 고생했네요 ㅠ

------------------------------------------------------------------ 10월 11일 수정 사항 -------------------

코드에서 에러 적분하는 부분에서 +를 하나 빼먹었네요.. 어쩐지 I가 잘 안되더라 ㅠ 수정했슴니당

------------------------------------------------------------------ 14년 2월 2일 오류 정정 -------------------

 "-> (오류 : 하단참고)" 라고 쓴 부분 모두 한가지 오류를 범했습니다.

MPU6050은 내부 레지스터를 이용해서 출력 값의 범위를 조정할 수 있습니다. (데이터시트 참고)

"-90도~90도의 값을 -16383~16383로 주기 때문에" 이 부분이 바뀔수도 있다는 겁니다.

레지스터 조작에 따라 가속도센서같은 경우는 -16383~16393범위에서 -2g~2g의 값을 출력시킬수도 있고  -8g~8g의 값을 출력시킬수도 있습니다.

자이로센서는 -32768~32768로 -250~250[degree/sec]을 출력하던가 -500~500의 값을 출력할수도 있습니다.

물론 범위를 넓히면 정확도는 그만큼 줄겠죠. 

데이터시트부터 차근차근 볼걸 너무 대충봤다는 생각이 듭니다..

논문 쓰면서 다시 돌아보다가 틀린부분이 있어서 바로잡습니다.

------------------------------------------------------------------ 14년 7월 16일 추가사항 -------------------

예전부터 이상하다고 여긴것에 대한 수정사항입니다.

이 글에서 두 센서의 측정치를 각각 각도으로 변환하는데, 오일러각이 아니고 각 축에 대한 각을 따로따로 구한겁니다.

이 부분 혼동하지 마시길 바랍니다. 작년에는 이런거 모르고 그냥 했는데 쿼드가 떴네요... 아마 0도에서 거의 움직이지 않기때문에 두 좌표계 변환을 고려하지 않아도 된 듯 싶습니다.

책에서는 두 센서 모두 오일러각으로 바꿔주던데.. 변환 식을 얼핏 보니 탄젠트가 들어갔던걸로 기억합니다. 0도에서는 탄젠트로 어떻게 계산하는지 이해가 안가서 덮어뒀던 기억이 나네요.

------------------------------------------------------------------ 15년 4월 25일 추가사항 --------------------

글에 잘못된 점이 많아서 새로 포스팅하고 있습니다. 이 글을 지울까 했지만, 이것도 하나의 시행착오고.. 혹시라도 다른 분들이 겪지 않았음 하는 마음에 그대로 두기로 했습니다. 새로운 포스팅은 

가속도, 자이로 센서값을 오일러각으로 변환 - 1

을 참고해주세요~

--------------------------------------------------------------------------------------