'complementary filter'에 해당되는 글 1건

  1. 2013.09.29 mpu6050 가속도, 자이로 센서를 융합하기[상보필터] - 2 (45)
Quadrotor20132013. 9. 29. 19:31

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


오늘은 상보필터(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

을 참고해주세요~



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






Posted by 너를위한노래

댓글을 달아 주세요

  1. 이전 댓글 더보기
  2. 입문자

    쿼드입문자인데요 요즘 여러자료를 수집하는중인데 질문하나 드려도 될까요?ㅠㅜ
    혹시 저 파형은 오실로스코프로 측정하신건가요??

    2014.03.29 17:30 [ ADDR : EDIT/ DEL : REPLY ]
    • 아뇨 시리얼로 출력시킨 숫자들을 모아놓은다음에 txt파일로 만들어서 매틀랩으로 그린겁니다~

      2014.04.01 23:06 신고 [ ADDR : EDIT/ DEL ]
  3. 쿼드

    이해가 안되서 질문드립니다. 위의 코딩을 보니 상보필터 결과값 cf_result_x를 구하는 과정에서 PI제어가 들어가는데 그렇다면 결과값이 PI에 따라 달라지게 되는것같은데 어떻게 정확한 각도가 출력이 되는거죠...? 상보필터결과에 PID제어가 들어가는게 이해가 안갑니다 ㅠㅠ 제 콥터는 중심을 잡지못하고 자꾸 한쪽으로 휙휙휙 하기만 하네요 ㅠㅠ

    2014.04.10 00:14 [ ADDR : EDIT/ DEL : REPLY ]
  4. whitesheep

    자이로 센서 출력 범위를 -2g~2g로 설정했을때 (-gx/16383.0*90.0) * dt와 같이 코딩하면 되나요??

    2014.05.01 12:27 [ ADDR : EDIT/ DEL : REPLY ]
  5. 박준석

    질문이있습니다.
    너무 막연해서그런데요...; 코드 내에서 dt 값은 임의 설정인지 궁금합니다.
    그리고 게인값을 범위가 어떻게되나요 ㅠㅠ 1보다 작은지....100보다작은지.....ㅠㅠ 수식적으로 접근하는방법도 모르겠구요 ㅠㅠ
    mpu6050을 사용하고있습니다. ㅠㅠ

    2014.05.17 17:51 [ ADDR : EDIT/ DEL : REPLY ]
    • dt는 (루프 시작할 때 시간)-(이전 루프 끝날때 시간) 이런식으로 계산하시면 될 거 같고.. 게인값은.. PID제어를 더 공부하셔야할 것 같네요.

      2014.06.27 10:39 신고 [ ADDR : EDIT/ DEL ]
  6. 임동준

    p value와 I value 값이 무엇을 말하는 건지 잘 모르겠습니다 ....

    2014.06.02 19:05 [ ADDR : EDIT/ DEL : REPLY ]
  7. 주현승

    안녕하세요 전자공학도 2학년 입니다 이번 방학때 쿼드콥터에 도전해보게되었습니다..
    다름이 아니라 atan쓸때 gx에 -는 왜 붙여주는지요?
    가속도하고 자이로 roll pitch값을 일단 뽑아내 봤는데 오히려 가속도 값이 더 정확한거 같은데 자이로는 거의 변화가 없네요 90도로 꺽었는데도 말이죠,,, 제가 한게 맞나요..?

    2014.06.24 20:19 [ ADDR : EDIT/ DEL : REPLY ]
    • (-)부호는 센서 방향에 따라 다르게 코딩하셔야합니다. 제 시스템인 경우에 -를 붙인거고 센서를 거꾸로 달았으면 -붙이지 않아도 되겠죠~
      자이로가 변화가 없다는것은.. 일단 roll을 출력하면서 pitch축을 돌리지 않았나 점검해보시고, 아니면 출력값에 어떤 수를 곱해서 출력해보세요. 곱했는데 출력에 변화가 생긴다면, 자이로 센서 값 스케일링을 검토해보셔야 할 것 같습니다.

      2014.06.27 10:37 신고 [ ADDR : EDIT/ DEL ]
  8. 주현승

    result=(gyro+(accel-preresult)*cf_p_gain+(accel-preresult)*0.5*cf_i_gain)*0.5
    혹시 식이랑 코드랑 햇갈려서 그러는데요 코드에는 전결과값에서 가속도각을 뺐는데 이론에는 가속도값에서 결과값을 뺐네요? 부호차이가 있지 않나요?? 어느쪽이 맞는 건가요?

    2014.06.30 22:51 [ ADDR : EDIT/ DEL : REPLY ]
    • 코드가 맞네요.. 이론은 에러를 표현하기위해 그렇게 표시한거구요~ 테스트하실 때 두 경우에 대해 출력해보면 어느게 맞는지 알 수 있을듯 합니다.

      2014.10.01 20:55 신고 [ ADDR : EDIT/ DEL ]
  9. 채성혁

    안녕하세요 쿼드콥터를 직접 만들어보려고 공부중인 학생입니다. 나름의 공부로 칼만필터나 상보필터를 적용해서 MPU6050으로 오일러각을 보정해서 받는것까지는 성공했는데요, 그 다음에 무얼해야 할 지 잘 모르겠습니다.
    각도에따라 PWM값을 PID로 안정적으로 출력해야한다는것은 개념적으로 생각하고 있는데 그러면 PID에서 목표하는 각도와 현재각도의 차이를 e(error)로 잡아야 하는건가요??

    2014.09.05 05:19 [ ADDR : EDIT/ DEL : REPLY ]
    • 네~ 목표각도와 현재각도의 차이를 에러로 삼아서 자세제어를 해야 될 듯 합니다. 여기에 쓰이는 현재 각도를 알기 위해서 상보필터를 통한거니까요~

      2014.10.01 20:57 신고 [ ADDR : EDIT/ DEL ]
    • 채성혁

      답글감사합니다 석연찮던 부분이 해결되었네요!
      그런데 한가지 더 궁금한 점이 생겼는데요 그럼 상보필터에서 P게인이나 I게인으로 값을 보완해주는 것도 일종의 PID제어인가요?

      2014.10.08 01:05 [ ADDR : EDIT/ DEL ]
    • 네 저는 그렇게 해석하는게 더 편한것 같아서..ㅋ 제가 처음 봤던 자료처럼 LPF(가속도_와 HPF(자이로)의 결합으로 이해하셔도 무방할 것 같습니다!

      2014.10.08 11:50 신고 [ ADDR : EDIT/ DEL ]
    • 채성혁

      아 그렇군요 답글 감사합니다~~!

      2014.10.08 19:12 [ ADDR : EDIT/ DEL ]
  10. 마상용

    안녕하세요~ MPU6050을 막 사용하기 시작한 학생입니다.
    쭉보고 있는데 이해가 안되는 부분이 생겨서 질문드립니다. 자이로와 가속도를 사용해서 roll과 pitch를 구하는 것까지는 알겠는데.. 왜 yaw는 구하지 않는 것인지 궁금합니다. x,y,z의 3가지 데이터를 가지고 왜 2개의 값을 구하는지 모르겠습니다.ㅠ

    2014.09.26 17:24 [ ADDR : EDIT/ DEL : REPLY ]
    • 지구 중심으로 향하는 축방향을 -z로 봤을 때, z축을 중심으로 기울어진 각도(기울어졌다고 표현하기 뭣하지만)는 가속도센서로 구할 수 없기 때문입니다. 따라서 roll과 pitch는 가속도센서로 드리프트를 줄일 수 있지만 yaw는 자이로센서만 가지고 구해야 하기 때문에 이 글에는 언급하지 않았습니다. 하지만 수식은 나와있네요

      2014.10.01 20:52 신고 [ ADDR : EDIT/ DEL ]
  11. 중얼거북이

    와 정말 유용하네요 ... 잘보고 갑니다....

    2015.01.12 17:39 [ ADDR : EDIT/ DEL : REPLY ]
  12. 황유경

    안녕하세요 선생님. 유용한 정보 잘 봤습니다. 궁금한것이 있어서 그러는데요
    dt에 millis()나 micros()를 사용한다고 하셨는데 안드로이드에도 dt에 쓸 함수가 있을까요?
    아직 dt에 대한 개념이 안 잡혀서 헷갈립니다

    2015.02.24 20:28 [ ADDR : EDIT/ DEL : REPLY ]
  13. 레지스터 조작에 따라 가속도센서같은 경우는 -16383~16393범위에서 -2g~2g의 값을 출력시킬수도 있고
    -> -32768 ~ 32767 범위입니다. 센서 테스트할때는 어디 올려놓고 살살 돌려보는 정도만 테스트하니까 1g정도밖에 안나와서 기본 2g scale에서는 범위를 1g만큼인 -16384 ~ 16383이라 착각 할 수도 있는데 (저도 처음엔 착각했습니다 ㅋㅋㅋ), 세게 흔들거나 하면 2G 이상이 나와서 -32768 ~ 32767값 나옵니다

    2015.02.26 21:45 신고 [ ADDR : EDIT/ DEL : REPLY ]
  14. 살려줘요

    후... 고등학생인데 저 코드를 보면 답답하기만 하네요
    dt는 어떻게 구하는지 조금 많이 자세히 설명해주시면 안될까요 ㅠㅠ
    그리고 CF_KP, CF_KI는 제가 정하고 싶은 값인가요??

    2015.04.04 20:18 [ ADDR : EDIT/ DEL : REPLY ]
  15. 김종용

    안녕하세요! 저 결과를 확인하는건 프로그램을 사용하는건가요??

    프로그램을 사용한다면 어떤 프로그램인지 알고싶습니다!!

    저도 mpu6050을 제어해야하는데 너무 막막하네요..

    2015.04.24 01:49 [ ADDR : EDIT/ DEL : REPLY ]
    • 그래프를 그리는건 matlab을 사용했는데요, 시리얼모니터(=하이퍼터미널)를 통해서 출력된 데이터들을 .txt파일로 저장한다음 matlab을 이용해서 점찍어준 것 뿐입니다~ 많은 분들이 이걸 물어보시는데.. 다음에 기회되면 자세히 포스팅 할게요~

      2015.04.25 16:00 신고 [ ADDR : EDIT/ DEL ]
  16. 저 공식을 이용하여 코드를 짜봣는데
    쿼드콥터에 안달고 손으로
    일정각도로 기울여봣습니다
    그랬더니 값이 변화하긴햇지만
    다시 0으로 되돌아가더라구요
    되돌아가는 과정에
    0을 기준으로 위아래로 왔다갔다하다가
    0으로 돌아오는데
    이거 제가 잘못한것인가요?

    2015.08.28 21:41 [ ADDR : EDIT/ DEL : REPLY ]
    • 가속도 센서 각도가 0으로 되어서 그런게 아닌가 싶은데.. 가속도 센서 각도를 별도로 확인해보시기 바랍니다.
      그리고 혹시 i게인을 0이 아닌 다른 수로 설정하셨나요? 맞다면 0으로 해서 테스트 해보시면 왔다갔다 하는 현상이 없어질 수 있습니다.

      2015.08.30 00:49 신고 [ ADDR : EDIT/ DEL ]
  17. 포스팅하신 것 잘 봤습니다 ㅎㅎ. 저도 현재 보상필터로 각도를 추출해서 PID 제어기에 넣어주고 있는데.. 찾아봤더니 DMP가 더 빠르고 정확하다는 글도 있더라구요.. 저도 실제로 DMP도 써봤는데 오버슈트가 좀 많더라구요. 어떻게 생각하시는지 여쭙고 싶습니다.

    2015.09.01 15:10 [ ADDR : EDIT/ DEL : REPLY ]
  18. 박민지

    아두이노와 매트랩을 연동하여 상보필터를 사용하였을 때의 그래프와 사용하지 않았을 때의 누적오차가 발생하여 부정확한 그래프 두가지를 보고 싶은데 ㅠㅠ 매트랩 코드를 어떻게 해야할지 모르겠습니다 ㅠㅠ 혹시 사용하신 코드 알려주실 수 있나요?ㅠㅠ 부탁드립니다 ㅠㅠ 졸업논문에 교수님께서 넣으라고 하셧는데 아무리 도전해도 안되요 ㅠㅠ

    2015.11.29 16:27 [ ADDR : EDIT/ DEL : REPLY ]
  19. 비밀댓글입니다

    2015.12.11 17:07 [ ADDR : EDIT/ DEL : REPLY ]
  20. 뮤즈

    맨위에 xAngle 있는 수식 이부분 다시 한번 설명해주시면 안될까요? 계산방법을 잘 모르겠습니다;;

    2015.12.23 19:52 [ ADDR : EDIT/ DEL : REPLY ]
  21. 자율자동차인표

    안녕하세요 읽고 많은 도움이 되었습니다. 한가지 궁금점이 생깁니다. 우선 저는 IMU센서를 사용하고 있는데 이 센서로 보정을 하면 어떤 형식으로 데이터가 나오는지 알고싶습니다!

    2020.02.23 15:55 [ ADDR : EDIT/ DEL : REPLY ]