본문으로 바로가기
예전에


를 소개한 적이 있는데요. 그 후


을 다시 소개했었습니다.  그런데 방문하신 몇몇 분들께서 C-code로 다시 표현해달라는 이야기를 하셨는데요. 사실 포스팅을 해야겠다고 생각만 하고 미뤄두고 있었네요. 근데 결과를 보시면 정말 간단하다는 사실에 아마 경악을 금치 못하실 겁니다.^^. 일단 다시 그 보정 필터의 개념도를 보도록 하지요.


위에 있네요. 저 그림에 주석을 좀 달고 다시 시작해보겠습니다.




먼저 

          Temp1
          intTemp1
          Temp2
          FilterAngle

순서로 구하게 됩니다. 그림상에 마지막부분의 FilterAngle은 단지 라디안(radian)을 degree로 표현을 다시 하는 것 뿐입니다. 결국 FilterAngle이라는 변수가 최종값으로 필터의 결과입니다. 

위에서 보상신호는 가속도센서에서 출력된 값을 이용해서 구한 각도입니다. 가속도센서의 출력이 아니라 거기서 구한 각도라는 것을 확인해주세요. 가속도센서에서 각도를 구하는 방법은 


을 확인해주시구요. 그리고 자이로신호라고 되어있는 부분은 자이로신호를 그대로 넣어둔것이 아니라 자이로 신호에 스케일팩터를 반영


해서 각속도(rad/sec)의 결과입니다. 즉, 보상신호는 가속도센서에서 출력된 각도이고, 자이로신호는 자이로에서 출력된 각속도입니다. 

이제 그림의 Temp1을 계산하는 루틴입니다.

Temp1 = (-Pre_AccAngle) + Pre_FilterAngle;

위 코드에서 Pre_AccAngle은 이전의 가속도센서에서 구한 각도입니다. Pre_FilterAngle은 이전의 보정필터의 결과값입니다. 위 그림과 함께 보시면 됩니다.

다음은 intTemp1인데요. 이건 단순히 Temp1을 적분하면 되므로

IntTemp1 = IntTemp1 + (Temp1 * Ts);

위와같이 구현하면 끝입니다. 여기서 Ts는 sampling time으로 설정하고 계신 값을 그대로 사용해주면 됩니다. 혹시나 하고 이야기하는 거지만, 프로세서에서 구현하시는 분들은 타이머인터럽트를 사용하셔서 일정시간마다 계산하도록 하셔야합니다.

이제 Temp2인데요. 위 그림에서 보시다시피 Temp2는 Temp1에 Kp를 곱하고 intTemp1에 Ki를 곱하고, 그러고 난 다음 자이로의 출력각속도와 더해주어야합니다.

Temp2 = (-Kp * Temp1) + (-Ki * IntTemp1) + GyroAngularVel;

이렇게 되지요. GyroAngularVel이 자이로의 출력값입니다.

마지막으로 Temp2를 적분해서 FilterAngle을 구해야겠네요.

FilterAngle  = FilterAngle + (Temp2 * Ts);

위에 제시된 딱 4줄이 보정필터 전부입니다.^^. 이렇게 간단해서 제가 포스팅을 잘 못하고 있었습니다. 쓸말이 없어서 말이지요^^. 하지만, 제어블럭도를 잘 모르시는 경우 위 블럭만으로는 코드를 작성하는데 어려움이 있을거라는 생각이 들어서 이렇게 포스팅을 합니다.



댓글을 달아 주세요

  1. 이전 댓글 더보기
  2. mijing 2011.06.17 16:25

    시뮬레이션환결설정은어디서하나요????

  3. mijong 2011.06.17 17:14

    음 타입을 fixed step 으로 하고 하고서 parameter estimation에 들어가서 할려니까 too many input argument 라고 뜨는데요 이거 데이터량도 9초 정도 만 넣는데 왜 많다고 나오는지요 ?????????? ㅎ

    • BlogIcon PinkWink 2011.06.17 17:18 신고

      그 경우는 이미 에스티메이션 툴박스에서 mijong님이 시간축을 입력하신 경우이기 때문에 fixed step을 사용하지 않으셔도 됩니다.

  4. mijong 2011.06.17 17:45

    음 잠시 정신이 없엇네요 안되서 일단요 엔코더의 실제 값과 자이로 속도 값과 가속도 센서의 값을 잘뽑앗습니다.
    자 이제 메트랩에서 data라는 변수에 각값을 합쳐서 시간도 0.01 간격으로 잘맟추어서 t라는 변수에 넣고요 이제 윙크님이 쓰신 파라미터 추정 을 하는 방법에 대한 포스팅을 잘따라갓지요 그리고 showplot 를 햇더니요 회색의 엔코더 값이보이고 파라미터추정값과 자이로 가속도의 값을 연산해 나온 파란색 선이 보이더군요 그런데.. 분명히 실제 값을 따 라가면서 나와야될 파란색 선계산값과 회색선 엔코더값이 불일치 하더라고요 따라서 당연히 게인 값이 잘못 계산된거일거구요 분명히 엔코더 자이로 가속도 센서값은 제대로 나오고요 즉 진동일때 자이로 속도가 엔코더를 따라가고 (고주파) 손으로 움직이는 부분 (저주파)일경우에는 가속도 센서가 잘따라가고요 자 그럼 이제 게인값이 잘못 나왓고 입력된 각 data값이 잘못돼지 않앗다는걸 알앗는데요 그럼 제가 이실험에서 무엇을 수정해야 제대로된 게인값과 파란색선이 회색의 엔코더 값을 잘따라가게 할수 있을 까요 ?? ㅠㅠ 너무 힘들어요 ㅠㅠ
    진짜 이렇게 매일 질문을다는데요 답변해주시는 윙크님 정말 감사합니다. ㅠㅠ

    • BlogIcon PinkWink 2011.06.17 17:49 신고

      어감만 봐도... 분노가 느껴집니다만...
      저로서도 어떻게 해드리기가 힘드네요....
      말씀하신 내용들은 모두 맞습니다.

  5. mijong 2011.06.17 17:55

    ㅠㅠ 흑 분노는 아니고요 ;; 화난거 처럼보엿다면 죄송요;;; 음 그러니까 위의 글은 좀 차분해진다음 쓴글이라서요 ^^ 음 지금 제실력으로 이실험에서 뭐가 잘못됫는지도 모르겟고 여기서 그만두면 시간도 아깝고 해서 요 안되니까 자기 자신한테 화가 나더라고요 ^^ 혹시 윙크님 의견을 듣고 싶어서 그럼니다. 님의 생각에 과연 제가 어떤 실수 를 햇을지 윙크님 생각을 듣고싶습니다. 정말 죄송한데 혹시 메일은 안받으신다고 하셧지만요 정말죄송한데 제 가 뽑은 데이터를 보내드릴순없을까요 ??ㅠㅠ
    그리고 제가 줄을 많이 연결해서 이게 마찰 이되서 게인값을 추출하는데 영향이 있을수도 있나요??

    • BlogIcon PinkWink 2011.06.17 18:46 신고

      ㅎㅎ... MATLAB에서 실험한 데이터를 이용해서 필터 계수를
      a1(Kp)에 1.414 a2(Ki)에 1을 두고,
      그리고 그 결과를 보시고 잘 되는지 아닌지 알려주세요.
      (아 실험은 약간 느리게 움직여보세요. 대략 초당 1번에서 2번정도의 느낌)
      만약 잘 된다면 에스티메이션에서의 문제일거고
      만약 저 게인이 정말 이상하게 안 맞다면, 실험데이터나 다른 곳의 문제이겠죠...^^

      그리고, 제 메일은 pinkwink.korea@gmail.com입니다. 그러나 데이터를 보내신다고 해도 제가 확인할만한 시간이 없을 수도 있습니다. 전 현제 휠체어생활을 아직도 하고 있어서 집에서는 꼼짝없이 침대에 누워있거든요.

  6. mijong 2011.06.18 11:47

    얏홍 !!!!!!!!!! 윙크님이 말씀하신대로 그 게인 값을 넣으니까 약간오차가 있지만 많아봣자 약 10도정도?? 게인값이 완전히 맞지 않아서 그럴수있다 생각하면 꽤나 잘나와요 그렇다면 실험데이터와 소스상의 문제는 아니고 메트랩에서 게인 값을 구하는 과정에 문제가 있나보네요 그렇다면 제가 정확한 게인값을 얻기위해서는 메트랩을 다시 돌리면서 구해봐야하는데요 음...에스티메이션에서 의 문제라는게 혹시 제가 시뮬링크에서 잘못 필터를 설계하셧다는 말씀이신가요??
    분명히 윙크님 포스팅 잘따라가서 햇는데요 ㅠㅠ
    그리고 잘정리해서 메일로보냇는데 한번만 봐주시면 감사하겟습니다. ㅠㅠ

    • BlogIcon PinkWink 2011.06.19 08:39 신고

      이제보면.. 게인을 설정하는 부분의 문제라고 결론을 내 볼수도 있겠네요. 그리고 메일을 보내셨다고 제가 꼭 확인한다고 장담드릴수는 없습니다. 다시 말씀드리지만, 다리를 다친 이후로 오랫동안 의자에 있으면 다리가 부어서 통증이 심하거든요. 그래서 정시 퇴근을 하다보니 다른 데이터를 확인할 시간이 그리 많지 않습니다.

  7. 2011.07.02 22:57

    비밀댓글입니다

    • BlogIcon PinkWink 2011.07.03 12:59 신고

      이 포스팅에서 적분은 두 군데서 일어나며 그 변수를 저는
      IntTemp1과 FilterAngle로 잡았습니다.

      그리고 위 포스팅에서 밝혔듯이 그 때의 적분은

      IntTemp1 = IntTemp1 + (Temp1 * Ts);
      FilterAngle = FilterAngle + (Temp2 * Ts);

      와 같이 처리하고 있습니다.

  8. 어려운쿼드 2012.08.13 18:07

    블로그 내용 잘보고있습니다...
    하나 질문이 있는데요...

    센서에서 읽은 값은 센서 좌표(기체좌표)이기 때문에 오일러각을 이용해서 지구좌표로 전환해야 하는걸로 알고있는데..

    이러한 추가 작업 없이도 위와 같은 코딩을 짜면 지구좌표 기준의 롤과 피치각이 나오는 건가요??

  9. 타키 2012.10.09 15:23

    작성하신 글을 보고 아래 댓글을 쭈욱 읽어보니 완벽하진 않지만 이해가 되네요.
    필요한 지식이였는데 너무 감사합니다.

  10. 윤원혁 2012.12.02 23:39

    안녕하세요

    숙제로 자이로와 가속도센서를 이용해 자세 추정을 하는 프로젝트 도중 많은 도움이 되었습니다
    그런데 아직은 결과가 잘 안나오네요 ㅠㅠ 그래서 한가지 궁금한 점이 있어 질문드립니다

    우선 프로젝트 내용은 x, y방향 가속도센서와 z방향 자이로를 이용해 보드의 자세를 추정하는 거구요(z축이 중력가속도 방향과 수직일때) 저는 가속도 센서에서 나오는 a_x와 a_y 값을 이용해 theta_acc = arctan(ax/ay)를 구했는데요
    여기서 얻은 thetaa_acc값을 어떻게 해야 될지 잘 모르겠습니다.. 우선 각도를 나타내는 방법이 여러가지가 있는데
    0~2pi 범위로 나타내면 되는지 궁금합니다. 그리고 +z방향으로 회전하는 경우 즉, 각속도가 양수인 경우에는 theta_acc가 양수인게 맞지만 -z방향으로 회전하는 경우는 음수인게 맞는것 같은데 이를 어떻게 처리해야 할지 잘 모르겠습니다..

    답변 부탁드릴게요.. 감사합니다

    • BlogIcon PinkWink 2012.12.05 09:02 신고

      초기부호를 잘 설정하셨으면, actan도 부호가 잘 나옵니다. 꼭 부호에 대해 고민하실 것은 없습니다.
      단, actan를 사용하는 경우 90도에 대해서는 대응할 수 없습니다.
      일단 알고리즘 학습단계라면, 그냥 -90~90 범위에서 먼저 보시는게 나을듯합니다.

  11. 윤원혁 2012.12.02 23:42

    위에 댓글들을 보니 역시 가속도센서에서 나온 값과 각속도의 부호가 같아야 한다고 되어있네요
    그럼 가속도센서에서 얻은 값을 -2pi~2pi 범위의 숫자로 바꿔서 계산해주면 되는건가요??
    저는 ATmega128을 이용하고 있는데 내장된 atan 합수를 쓰니 -pi/2~pi/2로 출력해주는 것 같더라구요

  12. balancing 2014.05.14 04:16

    안녕하세요.
    그동안 블로그를 참조하면서 프로젝트를 하는데에 지대한 도움을 받아서 감사의 말씀 먼저 드립니다.
    현재 balancing 로봇을 만들고 있는데 외바퀴로 Unicycle 형태로 만들고 있습니다.
    일단 보조바퀴를 달아서 앞뒤제어를 먼저 하려고 하는데에 있어 pinkwink님의 동역학설계방법 및 lqr제어기 설계를 참고하여 게인값을 구하는데 까지는 따라하였는데 실제로 적용시켜보았더니 제대로 작동하지가 않아서 원인을 살펴보다가 현재 센서값을 IMU를 이용하여서 sci통신을 이용하여 1ms단위로 받고 모터인터럽트도 1ms로 하여 연산을 하고 있습니다. IMU에서 받은 각도값을 이용하여 각속도를 구하여 적용시켰는데 각도 및 각속도값에서 문제가 있는 듯하여 아날로그센서를 이용하려고 하고 있습니다. 서론이 길어서 죄송합니다.... 서론에 대한 내용에서도 도움을 구할 수 있으면 좋을 듯 하여 같이 언급하였습니다.... 결국에 질문은 앞의 글에서 처럼 폴에 센서와 엔코더를 달아서 실험을 하여 Kp와 Ki값을 구하여 필터를 구현한다고 한다면 실험한 환경은 폴에서이고 실제 적용할 곳은 제 로봇인데 그렇게 실험한 값을 사용할 수 있는 것인지 알고 싶습니다.
    말이 길어져서 죄송합니다..... 도움을 주신다면 정말 감사하겠습니다..!

    • BlogIcon PinkWink 2014.05.14 12:33 신고

      물론 실험환경이 실제 시스템을 100%담지는 못할겁니다만, 그래도 실제환경과 흡사하다고 해도 될 겁니다. 그냥 실험을 따라하는 것이 아니라, 내가 구동시키는 온도, 진동, 충격 등을 실험할때 잘 반영하면 됩니다.
      그리고 제어기가 시뮬레이션과 실제 구동에서 차이가 나는 것은 많은 이유가 있기 때문에 다각도에서 잘 확인해보셔야할듯 합니다.

  13. 공대생 2014.05.18 12:04

    IMU 관련 질문입니다.. pitch, roll 보정은 가속도센서와 자이로센서를 상보필터 2차식으로 융합해서 보정했는데, yaw 값은 지자기 센서와 가속도센서로 얻은 값 이 후에 어떻게 처리해야할지 모르겠습니다 ㅠㅠ compass_yaw 값이 적분과정이 없어서 오차누적은 없지만 값 자체 오차가 있더라구요...(Y축으로 수직이동한 것 처럼 이상적인 각도에서 붕 떠 있어요ㅠㅠ) 상수로 처리하는 문제인지 아니면 자이로로 얻은 yaw와 어떤 식을 통해 융합되는건지 궁금합니다!! (그리구 제가 compass yaw 값을 잘못 구했을 수도 있는데.. compass_yaw식 아신다면 알려주실래요ㅠㅠ 논문참고했는데 안되네요ㅠㅠ)아참! 좋은글들 덕분에 그동안 프로그래밍을 많이 성공시켰는데, 너무 감사합니다~!

    • BlogIcon PinkWink 2014.05.18 23:28 신고

      그건 지자기 센서의 결과를 봐야합니다.
      상보필터의 주 컨셉은 주파수특성이 다른 두 센서를 융합한다는 것으로... 지자기센서와 자이로의 융합도 결과를 보고 융합할 것이지 아닌지를 정해야합니다.
      아니면 아예 칼만필터를 써야할지도 모릅니다^^

  14. 이경헌 2015.01.25 02:33

    안녕하세요 윙크님^^
    현재 PI보상필터 공부하고.. 소스로 구현중입니다.
    ( -각도값 ) + 보상필터거친 이전 각도값 = 보상필터거친 각도값 인데요..
    제가 TxPrintf 를 뿌려서 확인해보니 각도값이 만약 -10도이면.. 보상필터각도값도 -10이 되어야하는데..
    매우 느리게 변합니다... (-10도로 변하기는 해요.. 느려서그렇지..)
    문제가 뭔지.. 잘모르겟습니다.. 정말... 기다리면 각도를 맞추긴하는데.. 너무 느리네요.. 5초에 1도씩정도 움직여요.
    이유가 뭘까요..ㅠㅠ

    • BlogIcon PinkWink 2015.01.25 09:59 신고

      말씀하신 증상은 통상 시간을 잘못처리할때 나타납니다. 물론 아닐수도 있습니다만...
      적분과 미분을 수행할때 시간을 잘 처리하셨는지 확인해보시는것도 필요할 듯 합니다.

  15. 안녕하세요 2015.04.08 18:48

    안녕하세요. 핑크윙크님.
    현재 세그웨이로봇을 만드는 중입니다.
    파라미터 값 추출하려고 엔코더각도값과 센서각도값을 비교하려고 하는데요.
    지금 상황이 엔코더값 / 보상 각도값을 뿌려보면 각도값이 현저히 느리게 나옵니다.. ( 드리프트현상인거 같아요. )

    FilterTemp_1 = FilterAngle - acc_angle_x; // 이전각도값 -각도값
    FilterIntTemp += _IQmpy( FilterTemp_1, _IQ(0.001) ); //적분
    FilterTemp_2 = angle_gyro_x - (_IQmpy( Kp, FilterTemp_1 ) + _IQmpy( Ki, FilterIntTemp ) );
    FilterAngle += _IQmpy( FilterTemp_2, _IQ(0.001) );

    소스는 핑크윙크님꺼 보고 했는데요..
    0.001은 현재 제가 타이머인터럽트 주기를 1m/s 로 돌리고있어서 0.001을 주었습니다.
    그리고 kp ,ki 값은 7.5와 0.001을 주었구요... ( ki = 샘플링 주기라 해서.. 0.001을 했습니다. )
    그래프 사진을 보여드리고 싶은데.. 보여줄수가 없네요..

    정리해서.. 말씀드리자면
    가속도센서만 이용한 각도값과 엔코더각도값은 병진운동말고는 거의 비슷합니다.
    그래서! pi보상만 해버리면 아주 잘나오겠구나! 생각했는데..........
    가속도/자이로 보상필터를 걸친 각도값은 엔코더각도값에 비해 반응속도(?)가 느립니다.
    엔코더각도값을 보상 각도값이 따라간다는 느낌.. 이라 할까요...
    그래서.. 이 현상이 드리프트 현상인거 같은데.. 어디가 문제인지.. 도통 감이 안오네요..
    도움 부탁드립니다..ㅠㅠ

    • BlogIcon PinkWink 2015.04.09 10:15 신고

      코드를 제가 볼 수는 없습니다. 시간도... 능력도.. 허락하지 않아서^^
      일단 현상만 보면....
      혹시 이론과 코드가 잘 구현되었다고 가정하고...
      일단 필터 계수를 바꿔서 경향을 보세요...
      필터 계수를 바꿔주면 응답 속도나 성능이 바뀐답니다.
      (둘다 잡을 순 없어요^^)
      그리고... 코드상에서 다시한번
      하드웨어적인 부분도 확인하셔서
      시간을 잘 잡았는지도 확인하세요...
      샘플타임과 제어주기가 일치하고..
      그걸 이용해서 적분하고 미분하고 있는지 말이죠^^

  16. BlogIcon 류현석 2015.05.11 20:09

    여기서 1/s가 라플라스 변환에서 인터그럴(e^(-st))*f(t)인데 이거 그냥적분해서 풀면되는 건가요?

  17. EBJ 2016.02.15 20:08

    안녕하세요. 글 잘 읽었어요. ^^ 코드 구현 중에 의문점이 있어 질문드려요~
    위에 코드를 그대로 적용해 아두이노를 이용해서 각도를 구해보니 Pitch 각이랑 Roll 각은 잘 나오는데 Yaw 완전 이상하게 나오네요...
    pitch 각과 roll 각에 영향을 받는거 같기도 하고... 그냥 값이 완전히 틀리게 나와요. 뭐가 문제일까요??
    그리고 마지막에 FilterAngle = FilterAngle +( Temp2 * Ts) 라고 적어 놓으셨는데 왜 필터앵글을 더해주나요??

    • BlogIcon PinkWink 2016.02.16 10:20 신고

      흠... 이 방법으로 자이로와 가속도센서를 융합하면... Yaw는 안됩니다.ㅠㅠ. 안타깝지만... 말이죠... Pitch와 Roll만 검출 가능합니다. Yaw는 별도로 지자기 같은 센서를 하나 더 쓰셔야합니다.

  18. 김연주 2016.11.29 02:11

    Temp2의 값의 단위는 rad/s 인가요?
    그리고 Inttemp1*ki과 temp1*kp의 단위가 다를꺼같은데 어떻게 더해지는건지 자세한 설명좀 부탁드릴께요!

    • BlogIcon PinkWink 2016.11.29 13:06 신고

      temp/2는 개념상 각속도 성분이며... 모든 단위계는 rad을 사용하고 있습니다. plot을 위해 도로 변환하는 것만 빼구요...

  19. 김연주 2016.11.29 21:34

    Temp1 의단위가 rad인데 이를 적분하면 rad*s 가되는건가요?
    그리고 temp2를 구할때 자이로의 각속도rad/s 에서 가속도쪽에서 나온 값인 rad을 뺄수있나요? 단위가 다른데

    • BlogIcon PinkWink 2016.11.30 08:30 신고

      rad안계(degree가 아니라)를 사용했다는 말씀입니다.^^
      그리고 만들어진 블럭선도만 보고 단위계를 일일이 따라가려하지 마시고... 블럭이 만들어진 원 수식을 보셔야합니다.
      http://www.pinkwink.kr/78
      에서 원 수식이 나타나 잇습니다. 그 수식을 따라 블럭을 만든 것입니다.

  20. ㅌㅌㅊㄴㅇ 2017.02.22 09:30

    정말 답답했는데.. 사랑합니다

  21. 우성이 2017.07.10 21:24

    IntTemp1을 적분할때
    이전각도인 FilterAngle에 IntTemp1*Ts를 더하는게 아닌
    이전값인 IntTemp1[n-1]에 현재 값 IntTemp1[n]*Ts를 더하는 건가요?