본문으로 바로가기

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

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



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


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

 
이 됩니다. 즉, 말로 이야기하면, 자이로를 적분한 각도에는 하이패스필터를 가속도센서에서 도출된 각도에는 로우패스필터가 장착된것입니다. 이 보정법이 정확하고도 아름답게 작동되는 환경은 좌우로 마구 흔들리는 환경이라도 그 평균은 거의 가속도센서의 값이 참값일때 가장 잘 작동됩니다.^^ (보통의 경우 대부분 그렇다고 할 수 있나요?^^)
 
그런데 위 필터의 계수인 Kp, Ki를 선정하는 것이 그리 쉽지는 않습니다. 무작정선정하면 또 안될거구요. 그래서 생각한것이 최소자승법입니다.

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


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

처음 필터 수식에서

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

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


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


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


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


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

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



댓글을 달아 주세요

  1. 이전 댓글 더보기
  2. 죄임쓰 2010.11.11 10:42

    안녕하세요~ 열심히 관련글을 정독하고선 약간의 희망을 갖고 질문하나 올립니다.(아! 그 전에 멋진 글 감사합니다~^^*) 최소자승법으로 필터계수를 추정하기 전글(자이로센서를 이용한 각도 측정시 가속도센서로 보정하기)에서는 테스트할 때 진자를 손으로 잡은 부분과 다시 진자를 민 부분이 있던데 혹시 이 테스트에서도 그렇게 해 보셨나요? 손으로 잡고 있을때 결과가 전글보다 좋은지 궁금합니다. 제가 적용하고자 생각하는 곳은 차량 주행중 피치와 롤각을 얻어보고자 하거든요... 위와 같이 구현하면 가능하겠죠?ㅋ

    • BlogIcon PinkWink 2010.11.11 15:37 신고

      네.. 여기서는 보이지 않았지만.. 그렇게 해보았습니다.
      좋은 성능을 가집니다.
      그러나 진동이 심한 경우 오차가 아주 심해질 수도 있습니다.
      일반적으로 논문을 검색해보면
      진동이 심해지는지 아닌지에 따라 필터계수를 능동적으로 변경하는 연구들이 많이 있습니다.^^

  3. 죄임쓰 2010.11.11 17:37

    네.. 진동이 있을경우 오차가 심하다고 하니 조금 불안해지네요.. 진동이 심하다고 함은 병진운동 성분이 아니라 상하(연직방향)운동 성분을 의미하겠죠? 진동에 대해 필터계수를 능동적으로 바꾸는 논문을 검색해 봐야겠네요.. 혹시 아시는 논문 있으면 추천도 좀 부탁할게요.. 이쪽 프로젝트는 처음이라 맨땅에 해딩하고 있습니다!^^;;

    • BlogIcon PinkWink 2010.11.11 18:03 신고

      글쎄요.. 예전에 대충보고 지나간거라 기억이 잘 나지 않습니다.
      위 글과 비슷한 필터를 찾으신다면
      complementary filter로 검색하셔서 최근것은 대다수 제가 말씀드린 기법을 사용한 것일 겁니다.

  4. KCC 2010.11.15 01:32

    이곳에서 많은정보 얻어갑니다.. 감사합니다

    정말 궁굼한게 핑크윙크님 학부생 이신가요..?
    전공은 무엇이신지,,

    • BlogIcon PinkWink 2010.11.15 02:37 신고

      네 감사합니다.
      작은 지방대학에서 이번에 박사학위를 취득할 예정입니다.
      많이 창피하지만, 이제야 졸업을 하게 되었네요..
      지금 졸업 논문을 준비중이라 아주 바쁘게 지내고 있어서 블로그 업데이트가 상당히 늦답니다..ㅠㅠ
      조금 더 기다려 주시면 좀 더 많은 내용으로 업데이트를 할 예정입니다.^^

  5. mijong 2011.03.10 11:25

    도트 theta _e는 뭔가요??

    • BlogIcon PinkWink 2011.03.10 11:32 신고

      본문에 분명 "엔코더에서 도출된 각도를 theta_e"이라고 했으니 dot이 붙었다면 그것을 미분(실제로는 차분)한 것이겠죠

  6. mijong 2011.03.10 12:33

    이제 저두 자이로의 각속도 출력과 가속도 센서로 각도 출력 해보고 ..

    자이로 각속도를 타이머를 이용해서 각속도를 누적해서 각도를 출력하고

    엔코더는 아직 못구햇어요 ㅠㅠ;;;;

    아무튼 ki와 kp의 값을 구하는게 문제인데..

    일단 간단하게 출력된 값들중에 최소 10가지 데이터를 뽑아서

    각각 의 theta값에 대입한뒤 평균을 내서 윙크님이

  7. mijong 2011.03.10 12:33

    쓰신 값이랑 비교해 보려고 하거든요..
    이렇게 해도 되나요???

    그리고 만약에 이렇게 하면 1/s 는 어떻게 처리해 줘야하는지??

    • BlogIcon PinkWink 2011.03.10 14:08 신고

      10개정도가 아니라.. 저는 전체 데이터를 가지고 작업했기때문에 대용량 데이터를 행렬로 저장해서 행렬연산을 한 것입니다. 그리고 1/s/는 적분을 의미합니다.

  8. mijong 2011.03.10 12:39

    그리고 도트theta_e의 경우 차분이면 뭔가요?? 그러니까 원래식의 1/s가 없어지면서 미분하는건 알겟는데 실제 적으로 무슨 뜻인지 모르겟네요.. dot_theta
    g는 각속도라는 값이 있지만 도트 e는 뭔지?? 모르겟네요 ㅠㅠ

  9. mijong 2011.05.05 20:08

    ㅎㅎ 질문이있는데요
    혹시 자이로의 누적 되어 생기는 오차가 점점많아져 나중에 엔코더의 값이 0이나왓을때 자이로가 누적되서 약20도정도를 가르킨다면 이거는 값을 구하기 어려운가요?? 이 필터가 자이로의 누적도 보상해준거 아닌가요??

    • BlogIcon PinkWink 2011.05.06 08:22 신고

      필터의 결과는 드리프트가 없어집니다. (잘 설계된경우^^)
      자이로만 적분했을때 발생하는 에러를
      가속도 센서를 이용해서 없애주는 거지요...
      그렇게 하라고 엔코더신호를 가지고
      파라미터 추정을 한 것이구요^^

  10. BK 2011.07.31 14:14

    포스팅 해주신 것들이 많은 도움이 됐습니다 감사합니다!
    맡고 있는 프로젝트 보고서 쓸 시에 참고문헌에 넣고 싶은데
    혹시 위에 언급하신 직접 작성하신 논문 제목 좀 알 수 있을까요??

    • BlogIcon PinkWink 2011.07.31 14:44 신고

      Proceedings of the IASTED International Conference
      November 24 - 26, 2010 Phuket, Thailand
      Modelling, Identification, and Control (AsiaMIC 2010)

      DESIGN OF FIRST ORDER COMPLEMENTARY FILTER
      USING THE LEAST SQUARE METHOD

      과 같은 학회의

      WHEELED MOBILE ROBOT CONTROL USING INERTIAL SENSOR

      입니다. 이건 뭐 내용은 그리 훌륭하게 잘 편집된 것이 아니라... 몹시 창피합니다만.... 실제 최소자승법을 설계한 것을 투고한 논문은

      Journal of Institute of Control, Robotics and Systems (2011) 17(2):1-6
      DOI:10.5302/J.ICROS.2011.17.2.1 ISSN:1976-5622 eISSN:2233-4335

      최소자승법을 이용한 상보필터의 설계
      Design of Complementary Filter using Least Square Method

      입니다.

    • BK 2011.08.01 12:30

      훌륭한 논문, 감사합니다!

    • BlogIcon PinkWink 2011.08.01 13:07 신고

      반어법이시죠?.. 논문자체의 퀄리티가 그리 훌륭하지 않습니다.ㅠㅠ
      너~무~ 태국을 가고싶었다는... ㅋㅋㅋㅋ
      좋은 연구 되시길 바랍니다.^^

  11. MinHo 2011.09.07 17:16

    궁금한게 있습니다. 여기서 보면 dot theta g는 gyro의 각속도값을 나타내나요? 각도로 만들어서 표현하나요? 논문자료에서도 보면 dot theta e가 있는데 이것은 encoder의 값을 미분해서 각속도 성분으로 만들어서 계산을 하는건가요? dot의 개념을 잘 모르겠네요 dot는 미분아닌가요? 각속도값 그대로 사용하라는 얘기인게 맞는가요?

    • BlogIcon PinkWink 2011.09.08 17:17 신고

      dot의 의미는 단순히 시간에 대해 미분했다는 의미입니다.
      자이로센서는 원래 그 출력이 각속도니까.. 출력자체가 각도의 미분으로 표현을 한 것입니다. dot_theta_e는 엔코더에서 구한 각도를 미분했다는 의미입니다.

  12. MinHo 2011.09.09 10:26

    그렇군요~저는 또 각속도를 미분했다는말인줄알고ㅎㅎ저는 어쩔수 없는 상황때문에 기울기센서로 각속도를 보정하는데 생각처럼 잘 안돼네요..기울기센서의 응답성이 너~~무 느려서... bandwidth가 1Hz....아주 죽을맛입니다..
    Rate의 응답성도 그리 빠르지가 못해서.. 뭔가 헤쳐나갈 방법이 없을까요?

    • BlogIcon PinkWink 2011.09.09 10:53 신고

      칼만필터와 기울기센서로 검색을 하시면, 꽤 좋은 성능을 얻었다고 주장하는 논문을 볼 수 있더군요. (예전이라 제목은 기억이 가물거리네요...ㅠㅠ)
      그래도.. 시스템의 진동이 크지 않다면, 가속도센서가 나을텐데요.

  13. MinHo 2011.09.09 12:59

    하드웨어는 변경이 불가능해서요..어쩔수없이 울며겨자먹기로 해야죠... 칼만필터 말만들어도 덜덜 되는데요ㅎㅎ

  14. MinHo 2011.09.19 18:02

    지금 열심히 이것저것 하면서 테스트를 진행중입니다. 칼만필터는 제가 너무 몰라서 배열에 데이터넣고 반복적인 실험으로 최상의 값을 찾아서 넣을려고하고있습니다.. ^^;; 여기서 질문하나 보상필터가 온도에 따른 bias error도 보상가능합니까? 야외에서 실험하는도중 Kp,Ki값이 틀어진거같아서요 기본적인 rate의 에러만 보상하는 회로인가요? 온도보상은 안되는거같은데..그게 궁금합니다.

    • BlogIcon PinkWink 2011.09.19 18:24 신고

      이 글에서는 온도보정을 하지 않습니다.
      당시 학교 연구실에서는 온도챔버를 가지고 있지 않았습니다.
      그래서 온도에 대한 보정 실험을 수행할 수가 없었습니다.ㅠㅠ

  15. MinHo 2011.09.19 19:49

    아하 그렇군요...ㅠㅠ.. 요새같이 갑자기 날씨가 아침저녁으로 온도차가 크니까 값이 이상하게 나오더라고요 전 제가 코딩상 뭐가 문제가 있었나했죠...24시간을 기준으로 한번 데이터를 뽑아봐야겠네요 어차피 온도는 롱텀이니까 쭈~욱 받아봐야겠네요

  16. 샤인^hermit 2012.02.06 11:19 신고

    안녕하세요 핑크윙크님...
    위에 있는 ki kp를 구하는 식을 매트랩으로 코딩하는 데 있어서
    제가 매트랩을 잘 못다루는 관계로 위의 식을 매트랩소스로 코딩을 하는데 한계에 부딪치고 있습니다.
    가능하다면 매트랩으로 코딩한 소스를 좀 얻을 수 있으면 감사드리겠습니다.
    레퍼런스로 사용하고 싶어서 그렇담니다.
    항상 좋은 자료 올려주셔서 감사드립니다.. _(_ _)_ 꾸벅

    • BlogIcon PinkWink 2012.02.07 08:29 신고

      사실 알려드리는건 문제가 없는데 행렬식을 그대로 수식으로 꾸민거라 뭐 간단하답니다.
      function y = complementary2ndLS(theta_e, theta_c, dot_theta_e, gyro, ts)

      temp = (theta_e-theta_c);
      int_temp = cumsum(temp)*ts;

      A = [temp, int_temp];
      B = gyro - dot_theta_e;

      y = inv(A'*A)*A'*B;

      입니다
      여기서 theta_e는 엔코더에서 얻은 각도이고, theta_c는 가속도센서에서 얻은 각도입니다. ts는 샘플링타임이구요

      한번실험하고 전체를 행렬로 사용하고 있습니다.
      MATLAB 유저들이 전형적으로 사용하는 방법이지요^^

  17. ljsSSS 2012.07.25 17:29

    안녕하세요.. 자이로 가속도 공부하고있는 대학생입니다.
    저위의 식대로 kp ki를 추정해보려고합니다.
    일단 자이로센서와 가속도 센서 모두 값은 제대로 출력이 되는것을 확인하였습니다
    저같은경우 NTC사의 자이로, 가속도를 사용하였구요. 가속도는 수평일때 z-y 각도를 atan 해서 45도가 정확히 나오고 기울이는대로 그 각도값이 매우 잘 나오고있습니다.(진동에 매우 민감하더군요 ㅠ)

    자이로 같은경우 수평일때0 이고 기울이면 -1 -2 -3 이런식으로 떨어지고 반대방향으로 기울이면 1 2 3 이런식으로 증가하지만 30초? 40초정도 작동하고있으면 오차값이 쌓이게 되어 값이 몇백으로 마구마구 증가하게 되어 기울여지는 정도를 보기 어려워집니다.


    이러한 상태.. 즉 가속도 , 자이로를 작동하는데까지 성공한상황에서 보상필터를 만들어보려고하는데
    kp ki를 추정하는 과정에서 굉장히 애를 먹고있습니다.
    저같은경우 엔코더 없이 자이로 따로 가속도 따로 켜서 값을 추정해내었는데
    위와 같은 방식으로 구하려고보니 theta_e 즉 엔코더가 없는 상황인데 그렇다면 어떤식으로
    kp ki를 추정해야 할지 막막합니다. 사실 엔코더가 뭔지도잘모르겠구요

    일단 수평값이 가속도는 45도 자이로는0 으로 시작하는데 kp ki를 추정하기 위해선
    두값이 비슷하게 나오도록 맞춰줘야 하는건가요?
    그리고 이런상황에서 보상필터를 짜기 위해 무엇을 참고하여 따라가면 좋을까요? ㅠ
    MCU는 아트메가128 , 프로그램은 코드비전을 사용하고있습니다.
    좋은 정보 감사합니다. ^^

    • BlogIcon PinkWink 2012.07.26 08:03 신고

      상보필터에 대해서는 http://pinkwink.kr/254 에서 C로 구현하는 것에 대해 언급했습니다.
      각 센서의 사용은 아날로그타입을 사용하신다면 ADC정도면 될 것이고, 참값과 비교하는 실험을 수행하셔서 필터설계를 완료하셔야할 듯합니다. 엔코더나 여타 센서의 사용법은 인터넷에서 쉽게 얻을 수 있더군요.
      좋은 결과가 있으시길 바랍니다.

  18. BlogIcon PEACE- 2017.02.01 14:35 신고

    우선 글 정말 잘 봤습니다^^
    안드로이드를 통해 실험중입니다.
    엔코더가 없다면 e에 대한 값들이 측정이 안되는데 그럼 kp와 ki의 미지수 값을 구할 수 없는건가요?ㅜㅠ

    • BlogIcon PinkWink 2017.02.01 17:58 신고

      아닙니다... 튜닝으로 어느정도 맞출 수 있습니다.^^

    • BlogIcon PEACE- 2017.02.01 18:20 신고

      직접 값을 찾아야 한다는 말씀이신가요? 그렇다면 어느기점으로 시작해야 금방 찾을 수 있을까요? ..막막하네요 핑크윙큼님의 Ki와 Kp를 기준 입혀보고 변화를 주어야할지.. 핑크윙크님의 값대로 입혀본 결과 각도는 잘나오는데 오차 누적 현상이 남아있는어요!! 가속도 센서의 값이 튀는 현상은 없어졌네요.

    • BlogIcon PinkWink 2017.02.02 07:56 신고

      만약 개념이 잘 구현되었고... 문제가 없다면... 사실 제가 제시한 값에서 하나씩 조금씩 움직여보면 그래도 쓸만한 계수를 찾을 수 있습니다. 뭐.. 그렇더라구요^^. 예전에 한 연구원은 익숙해지니까 튜닝속도가 오히려 엄청 빠르더라구요^^

    • BlogIcon PEACE- 2017.02.02 10:14 신고

      와 정말요~? 그런 분도 계셨군요 ^^
      게인값이 커질수록 드리프트가 줄어드나 외압에 의한 값의 큰 변화가 여전히 문제되고 있네요. 이쯤에서는 제가 해결해야할 문제니 저만의 과제가 생긴 기분이네요. 감사해요. 윙크님.

    • BlogIcon PEACE- 2017.02.02 13:02 신고

      자꾸 질문 드려서 죄송해요!
      물어볼 곳이 윙크님 뿐이어서 ㅠ....
      궁금한 점이 하나 더 생겼어요.
      상보필터를 적용 하고
      센서의 자유로운 움직임 후 책상위에 올려둔 상태(롤=0, 피치=0)로 돌아왔을때 롤, 피치 값이 0으로 돼있어야하지 않나요?..
      두 센서가 상보하여 가속도 센서의 특성을 살렸다면 원래 값으로 돌아와야하는데.. 돌아오지 않는 자이로 특성이 보이네요.

      자꾸 질문만 하면 윙크님 답답하실 것 같아서 제가 사용한 소스의 일부를 보이겠습니다.

      *상보 필터를 적용하여 구한 각*
      xAngle = (s*s)/(s*s + kp*s + ki)*(mGyroDegree[0]) + (kp*s + ki)/(s*s + kp*s + ki)*(mAccDegree[1]);

      *사용 변수*
      # 's=1/dt;' 이며, dt는 센서의 측정 간격입니다.
      # mAccValues와 mGyroVaules는 가속센서와 자이로 센서 측정 value.
      # mGyroDegree[0] = mGyroDegree[0] + (mGyroVaules[0] * dt) * 180 / Math.PI;
      # mAccDegree[1] = Math.atan2(mAccValues[1], mAccValues[2]) * 180/Math.PI;

    • BlogIcon PinkWink 2017.02.03 10:09 신고

      네... 시간이 지나면서 조금씩 알아가게 되더라구요^^

    • BlogIcon PinkWink 2017.02.03 10:12 신고

      http://pinkwink.kr/254
      위 링크에 C 코드를 구현해 두었습니다. 물론 이해하기 어려울 수 있지만... 보여주신 코드는 제가 볼땐 틀린듯 합니다. 특히 적분과 차분의 처리에서.ㅠㅠ.
      s는 뭘 대입하라는 문자가 아닙니다.
      http://pinkwink.kr/254
      를 확인하시면 좋을듯 합니다.

  19. FIsh 2017.04.19 17:01

    안녕하세요, 위에서 소개해주신 방법으로 게인을 튜닝하는데 있어서 궁금한 점이 있습니다.
    엔코더로 얻은 각도 / 가속도센서로 얻은 각도 / 자이로센서로 얻은 각속도 이 세 데이터를 각 샘플링 타임마다
    받아와서 배열로 하여 데이터로 사용중에 있습니다.

    for n = 1:size
    temp = Encorder.signals.values(n) - Acc.signals.values(n)
    int_temp = cumsum(temp)*ts

    dot_Encorder_theta = (Encorder.signals.values(n) - pre_Encorder_theta)/ts;
    A = [temp,int_temp];
    B = Gyro.signals.values(n) - dot_Encorder_theta;

    y = (A'*A)\A'*B;
    pre_Encorder_theta = Encorder.signals.values(n);

    end

    위와 같이 코드를 작성했는데 매번 루프를 돌때 마다 Y값(Kp,Ki) 이 제각각 다른값으로 나올텐데 위에서 말씀하신것
    처럼 실험을 통해 도출된 gain이라고 말할 수 있는 Y값이 최종적으로 루프를 다 돌았을 때인지 궁금합니다.
    혹시 위 코드가 잘못된 건가요..?

    • BlogIcon PinkWink 2017.04.20 09:54 신고

      이 글은 필터계수를 찾겠다는 것으로 각 변수가 모두 벡터(혹은 행렬)로 되어 있으면 바로 적용될 수 있을 겁니다. (코드를 볼 시간적 여유는 없네요)

    • FIsh 2017.04.21 16:21

      Matlab에서 블록도로 자이로를 적분해서 얻은 각도와 실제로 MCU에서 자이로 합을 샘플링 타임으로 곱해서 구하는 각도과 데이터가 다르게 나오는데 혹시 이런 부분에 대한 문제는 없으셨나요??

    • BlogIcon PinkWink 2017.04.21 17:39 신고

      처음에는 차이가 크게 나서 다시 여기저기 점검하고 찾은 에러도 고치고 해서.. 같은 실험데이터라면 거의 비슷하게 나옵니다. MCU에서는 실시간으로 각도를 계산하고, 그 때의 RAW 데이터도 같이 저장해서 MATLAB에서 다시 각도를 계산해보면, 둘이 비슷해야죠.

  20. 하이람 2017.12.14 22:35

    센서로 각도를 구하는데 있어서 PinkWink님의 글이 많은 도움이 되었습니다. 감사합니다.
    혹시 이곳에 연재하신 가속도,자이로 센서 관련 글들의 그림들과 논문의 최소자승법식을 졸업논문에 인용하여도 괜찮을까요?

  21. 누미던 2020.06.30 20:01

    2차식 필터인가요?? 대게 1차식 형태만 봤는데 처음 알았습니다..