[mpu-6050] 가속도, 자이로 센서에 칼만필터 적용하여 자세제어하기 예제

1. MPU-6050: 가속도, 자이로 센서?

쿼드콥터를 만드시거나 혹은 자세제어가 필요할 때 가속도, 자이로 센서를 사용합니다. 휴대폰에서 이미 많이 쓰이고 있기 때문에 익숙하실텐데 실제 아두이노에서 MPU-6050 센서를 이용해서 작업을 해보려고 하면 안드로이드처럼 이쁘게 값이 나오질 않습니다. 게다가 바로 사용가능한 소스코드를 검색으로 구하려고해도 잘 보이지 않을 뿐더러, 구현을 위한 이론은 복잡하기만 합니다. 모든걸 한번에 이해하려 하지 마시고, 일단 여기 자료대로 한번 따라가면서 구현을 해보고 재차 자료를 찾아 공부해 보시길 바랍니다.

먼저 가속도와 자이로 센서를 왜 같이 사용하는지, 왜 여기서 출력되는 값을 바로 사용할 수 없는지를 알아야 합니다. [여기]를 클릭하시면 가속도, 자이로 센서에 대한 기본 지식을 익히실 수 있습니다. (설명 정말 잘 되어있네요)

요약하자면… 가속도 센서는 가만히 있을 때 센서에 작용하는 중력가속도를 X, Y, Z 축으로 벡터 3개로 나누어 크기를 측정해 줍니다. 진동과 외력(이동)에 측정값이 왜곡될 수 있지만 시간이 지나도 오차에 강한 특징을 가집니다. 이리저리 자세가 변하더라도 원래 자세로 돌아오면 이전에 출력한 값과 똑같은 값이 나옵니다.

자이로 센서는 센서의 회전이 발생하면 XYZ 축의 각속도 변화량을 측정해줍니다. 그런데 각속도이기 때문에 각도(위치)를 구하려면 적분을 해줘야 합니다. 적분을 하는 과정에서 센서의 노이즈도 같이 적분되기 때문에 누적오차가 커지게 됩니다. 결론적으로 자이로 센서는 실제 움직임과 비슷한 값(정확한 값)을 출력하지만 누적오차가 발생합니다.

그래서 이 두 센서의 장점만 모아 값을 보정해줘야 합니다.(가속도 센서의 오차, 자이로 센서의 정확도) 이 역할을 하는 것이 칼만필터입니다. 칼만필터에 대한 일반적인 설명은 매 아래 첨부된 파일을 참고하세요. 이 글에 첨부된 소스는 칼만필터를 이용해서 센서의 각 축에 대한 기울기 값을 구하는 아두이노 코드입니다. MPU-6050은 각 축에 대해 -16383 ~ +16383 까지의 값을 출력해줍니다.

그리고 잘 생각해보시면… 가속도 센서는 중력가속도를 이용해서 기울기를 검출하는데 중력가속도 방향과 일치하는 Z축에 대한 회전은 감지할 수가 없습니다.(Yawing) 머리 꼭대기에 센서를 수평하게 놓고 고개를 좌우로 회전하면 중력가속도 방향의 변화가 없기 때문에 Z축 변화량이 없다는 겁니다. 그래서 Z축 기울어짐은 자이로 센서에 의존해야 하고, 자이로 센서의 값은 앞서 말씀 드린대로 누적오차가 발생합니다. 그래서 정확하게 내가 쳐다보는 방향(Z축 회전각)을 알기 위해서는 지자기 센서가 추가로 필요합니다. 

2. 연결방법

mpu-6050

3. 동작방법

소스 코드에서 Wire.h 는 I2C를 동작하게 해주는 라이브러리입니다. 자세한 설명은 [여기]를 참고하세요.

연결시 Analog pin 번호 잘 확인하세요.

 

4. 코드 (스케치)

소스가 길고 알고리즘은 저도 잘 몰라서 전체를 설명드리기 힘듭니다. 중요 부분만 언급합니다.

연산을 위해 math.h 와 I2C 통신을 위해 Wire.h 헤더 파일을 include 합니다. 소스의 수정 포인트는 모두 loop() 함수 안에 있습니다.

line 170 에 있는 아래 코드에서 센서로부터 gyro, accel 값을 가져옵니다.

error = MPU6050_read (MPU6050_ACCEL_XOUT_H, (uint8_t *) &accel_t_gyro, sizeof(accel_t_gyro));

line 189, line 210 : 여기에 있는 Serial.print() 코드의 주석을 해제하면 센서로 부터 가져온 raw data 값을 Serial 출력할 수 있습니다.

line 221 : 센서에서 읽어온 값에 칼만필터 적용하는 부분입니다. 아래 3개의 함수를 거쳐 적용됩니다.

gx2 = angleInDegrees(lowX, highX, accel_t_gyro.value.x_gyro);

predict(&angX, gx2, loopTime);

gx1 = update(&angX, accel_t_gyro.value.x_accel) / 10;

line 242 : 최초 실행될 때 측정되는 n개의 값을 평균해서 저장 => 이후 측정되는 값을 보정하는데 사용합니다. (미세하게 기울어진 값이 수평으로 인식되는 상황에서 0으로 맞춰주는 역할)

line 264 : 측정된 X, Y, Z 값(-16383 ~ +16383)에 따라 필요한 처리를 여기서 하면 됩니다.

 

예제소스 다운로드 :

칼만필터 설명 자료 : 출처 RealSYS 

 

Post Author: TORTUGA

TORTUGA
궁금하신 점은 새로 개편한 홈페이지의 QnA 게시판을 이용해주세요!!!!!!! http://www.hardcopyworld.com/gnuboard5/bbs/board.php?bo_table=qna

62 thoughts on “[mpu-6050] 가속도, 자이로 센서에 칼만필터 적용하여 자세제어하기 예제

    김연우

    (2014년 8월 6일 - 2:37 오후)

    안녕하세요 . 소스가 도움이 많이 되서 이것저것에 사용을 많이 하려합니다.
    근데 궁금한 점이 있습니다.
    측정된 X, Y, Z 값(-16383 ~ +16383)이 나오는데 이는 우리가 흔히
    알고 있는 각도가 아닙니다. 이 숫자의 의미가 먼지 궁금합니다. 감사합니다.

      TORTUGA

      TORTUGA

      (2014년 8월 7일 - 11:35 오전)

      답변을 단다는게 잘못달았네요.. 아래 댓글 참고하세요.

    TORTUGA

    TORTUGA

    (2014년 8월 7일 - 11:33 오전)

    가속도 센서에는 중력이 지구중심 방향으로 작용하겠죠. 가속도 센서는 지구 중심방향으로 향하는 중력가속도 벡터(중력 가속도가 작용하는 방향과 크기)를 X, Y, Z 축으로 성분으로 분해해서(X, Y, Z 축 방향의 크기로 분해, 이 값들을 다시 합치면 원래의 벡터가 되겠죠.) 그 값을 전달해줍니다.따라서 중력과 수평하게 가속도 센서를 맞춘다면 X, Y 축 성분의 값은 0이 될겁니다.
    -16383 ~ +16383 값의 크기는 부호있는 2byte 크기에 맞춘것일테고 이 값은 변경이 가능할겁니다.

    […] 핀에 연결해주면 됩니다. 가속도 센서 모듈(MPU-6050)에 대한 자세한 설명은 [여기를] […]

    냐냐냥

    (2014년 9월 8일 - 7:46 오후)

    ㅠㅠ
    소스를 돌려봤는데요. 모두 0 0 0 이 뜨네요. 머가 잘못됬을까요?

      TORTUGA

      TORTUGA

      (2014년 9월 10일 - 12:56 오후)

      일단은 I2C 연결이 제대로 되었는지 확인이 필요할 것 같습니다. 동작할 때 센서 모듈에 LED가 들어오는지도 확인하구요. 문제가 없다면 아래 아두이노 공식 페이지에서 MPU-6050 예제 파일을 받아서 한번 돌려보세요.
      http://playground.arduino.cc/Main/MPU-6050

    도너

    (2014년 9월 15일 - 9:49 오전)

    소스 코드 중에 angleInDegrees 함수가 자이로의 각속도값 즉, dps를 각도로 변환한 함수인가요?

    서무석

    (2014년 10월 2일 - 5:57 오후)

    Angle x,y,z : 34, -74, 1641
    값이 이렇게 나오내요!!
    일단 가만히 값을 받았을경우 맞는건가요!!??

      TORTUGA

      TORTUGA

      (2014년 10월 8일 - 9:51 오후)

      x, y 값은 0 근처에 나오는게 맞습니다만.. z값이 좀 작아보이네요;; 16600 근처에 나와야하지 않나 싶어요

    프로그래머

    (2014년 11월 18일 - 6:37 오후)

    저 예제소스 만으로 자이로센서 역활 할수 있는 건가요?

      TORTUGA

      TORTUGA

      (2014년 11월 19일 - 4:03 오후)

      예제소스가 가속도, 자이로 센서의 값을 칼만필터를 이용해서 보정합니다. 보정된 값을 사용하시면 되는데.. 실제로 해보면 가끔 오류(?)처럼 잘못된 값이 나오는 경우가 있었습니다. 신뢰도가 중요한 프로젝트를 하신다면 보다 정교한 소스를 찾거나 위 소스를 수정해서 사용하셔야 할 것 같습니다.

    한지호

    (2014년 11월 20일 - 11:46 오전)

    -16383 ~ 16383 을 우리가. 알고있는 0도 에서 180도로 바꿀수있는 방법이 혹시 따로있나요??

      TORTUGA

      TORTUGA

      (2014년 11월 21일 - 10:51 오후)

      3축의 값을 삼각함수를 이용해서 계산해야 합니다…

    심재신

    (2014년 12월 2일 - 5:17 오후)

    내용 잘 보았습니다. 좋은 참고가 되었네요… 적 타은 경우엔 각도값과 추가로 자이로 가중치 값이 필요한데 GY2변수로 추축은 되나 이게 맞는지 틀린지… ㅎㅎㅎ
    참! 위에 -16383 ~ +16383 값은 아두이노에서 map 함수 사용하면
    쉽게 우리가 알고 있는 각도로 표시 됩니다.
    gy1_raw = map(gy1,-16383,16383,-90,90); 이렇게요…
    감사합니다.

    이종현

    (2014년 12월 2일 - 11:36 오후)

    X축이랑 Y축은 잘 작동되는데 Z축은 값이 변경되는게 이상하게 변경되던데 원래이런건가요?
    좌우랑 상하를 움직일때는 x랑 y값이 각각 잘변하는데 회전을 할경우 z축의 변화가 없습니다.. 말로 설명하기가 좀 그런데 이해가 안되면 답글 부탁드려요,,ㅠ

      TORTUGA

      TORTUGA

      (2015년 1월 6일 - 11:07 오후)

      z축을 중심으로 회전할 경우 가속도 센서를 사용할 수 없습니다. 가속도 센서는 중력 가속도를 측정하는데 Z축으로 회전하면 중력가속도의 변화가 없거든요. 따라서 자이로 센서의 값만 적용됩니다. 그래서 값이 이상하게 나오지 싶습니다.

    쿼드콥터

    (2014년 12월 29일 - 10:26 오후)

    1. 30도 기울어진 상태로 가만히 놔두면, 30도로 값이 가는게 아니고 0도로 다시 돌아가더라고요. 이걸 수정하려면 어떻게 해야할까요?

    2. 중력 방향을 기준으로 z축이 아닌 x,y축의 회전을 가속도계에서 측정할 텐데, 만약 이 센서가 장착된 물체가 가속운동을 하면 각도가 변화하는 것이 우려되고, 실제로 책상위에 두고 밀었을 때, x축 각도가 변하지 않아야 하는데 미는 순간 급격히 변화하는 것을 확인했습니다. 이는 별도의 코딩을 통해 개선해야할까요?

      TORTUGA

      TORTUGA

      (2015년 1월 6일 - 11:11 오후)

      1. 이 부분은 제가 확인했던 동작과 틀린것 같아 뭐라 말씀드리기가 힘드네요.
      2. 자이로센서 값으로 어느정도 보정이 되지 않을까 싶은데.. 값의 변화는 여전히 있을것 같습니다. 별도로 처리해 주셔야 할 것 같습니다.
      저도 칼만필터에 대한 이해가 깊은것이 아니라서 많은 도움 못드리는 점 죄송할 따름입니다.

    물음표

    (2015년 1월 27일 - 3:41 오전)

    가속도 값이 원래 이렇게 크게 나오나요?
    정확한 데이터를 구하기가 힘드네요,,

      TORTUGA

      TORTUGA

      (2015년 1월 28일 - 1:49 오후)

      기본 셋팅은 아두이노의 2byte integer 크기에 맞게 나오는 걸로 알고 있습니다. 설정을 통해 값의 범위는 조절 가능할겁니다.

    질문이요

    (2015년 3월 11일 - 9:56 오후)

    에러2가 떠서 정확한 값을 얻기 힘듭니다. 혹시 원인을 알 수 있을가요?

      지나가다

      (2015년 3월 18일 - 3:45 오후)

      센서에 연결된 점퍼선을 (GND나 VCC부분, 어쩌면 둘 다) 제거했다가 다시 꽂아보세요. 안되면 2~3회 반복해보세요

      w

      (2015년 3월 18일 - 4:18 오후)

      에러2 는 I2C연결이 잘못된거일듯

        최순협

        (2015년 5월 22일 - 1:04 오후)

        혹시 자이로와 점퍼선을 납땜해야만 구동되나요 LED가안들어와서
        카톡으로 도움주시면 정말감사하겠습니다 shyeop

          최순협

          (2015년 5월 22일 - 1:05 오후)

          카톡아이디 shyeop 쿼드콥터만들고있는 대학생인데 도와주세요

    최순협

    (2015년 5월 22일 - 1:04 오후)

    혹시 자이로와 점퍼선을 납땜해야만 구동되나요 LED가안들어와서
    카톡으로 도움주시면 정말감사하겠습니다 shyeop

      TORTUGA

      TORTUGA

      (2015년 5월 22일 - 4:03 오후)

      납땜을 하든 브레드 보드용 점퍼선을 이용하든 일단 연결만 제대로 되면 됩니다.

    Hyeop

    (2015년 6월 1일 - 10:31 오후)

    지금 현상황이 아두이노로 쿼드콥터 제작중인데 mpu6050 자이로센서값출력은 했는데 그 이후에 작업이 필터적용하고 필터에서 나온 각도를 pid제어기에 넣어 모터의 속도조절해 자세제어하는건대 이과정에 대해서 어떻게해야되는건지 구체적으로 알려줄실수있는지요
    mpu6050은 자동으로 칼만필터적용이되어 저작업 생략하고 바로 pid제어를하면되는건가요

      TORTUGA

      TORTUGA

      (2015년 6월 3일 - 10:52 오후)

      가속도 자이로 센서의 값을 읽어서 서로 상호 보정하도록 소스를 짜서 사용하는 걸로 압니다. 인터넷에서 칼만 필터 혹은 상보 필터로 검색해서 소스를 구해서 사용하시는 것이 좋을 것 같네요. 저도 이론적인 부분은 잘 모릅니다. 직접 짜기는 힘드실겁니다.

    seung

    (2015년 6월 8일 - 5:40 오후)

    값이 Angle x,y,z : -3555, -12, 1717
    값이 이러케 나옵니다. x,y의 값이 0에 가까워 해야하고 z축은 16000이상은 해야 한다고 하셨는데.. 저는 무슨 문제일까요..

      TORTUGA

      TORTUGA

      (2015년 6월 25일 - 2:02 오후)

      센서 연결이 잘 되었는지(특히 I2C 데이터 라인) 확인해주세요. 그리고 센서의 위치가 적절히 배치되어 있는지도 확인해주세요.

    […] MPU6050 관련 […]

    Parkhun

    (2015년 6월 12일 - 9:04 오후)

    예를 들어 자이로 센서를 눕혀서 설치를 한다고 했을때 센서값이 Angle x,y,z : -289, 1120, 1702로 떴을때 이 값을 기준값으로 해서 모터의 제어를 이용하면 쿼드콥터의 균형을 잡을수 있나요?

      TORTUGA

      TORTUGA

      (2015년 6월 25일 - 2:01 오후)

      음.. 자세제어에 대해서는 저도 전문지식이 모자랍니다;;;

    Seung

    (2015년 6월 16일 - 2:35 오전)

    angle x,y,z 의 값이 진동으로 인해 값이 너무 왔다갔다가 합니다. 이걸 보정할수 있는 방법이 있을까요?

      TORTUGA

      TORTUGA

      (2015년 6월 25일 - 2:00 오후)

      노이즈 제거하는 필터로 데이터를 한번 걸러야 하지 않을까 싶은데.. high path filter 이런걸로 한번 검색해보세요.

    HJ

    (2015년 6월 25일 - 5:38 오후)

    angle x,y,z 값이 -16383 ~ 16383 사이 값이 아니고 그 이하(또는 그 이상)의 값으로 출력된다면
    어떤 것을 의심해 봐야 할까요?

    Angle x,y,z : 1301467 -899534 15009
    Angle x,y,z : 40763748 -27779926 478387
    .
    .
    .
    .
    .

      TORTUGA

      TORTUGA

      (2015년 6월 28일 - 5:23 오후)

      MPU-6050 출력값의 범위를 바꿀 수 있는걸로 알고 있습니다. 설정을 변경하면 될 것 같습니다. 자세한 내용은 구글링을 좀 해봐야 할듯;;;

    장다훈

    (2015년 7월 4일 - 4:39 오후)

    안녕하세요 블로그 게시된 글을 보고 많은 도움이 됬습니다.
    근데 제가 사용하는것은 아두이노가 아니라 AVR라 잘 모릅니다…
    혹시 헤더파일 (Wire.h)이 어떤 작용을 하는지… AVR로는 어떤 헤더파일과 관련이 있는지 알 수 있을까요??

      TORTUGA

      TORTUGA

      (2015년 7월 5일 - 1:49 오전)

      Wire.h 는 I2C 통신용 라이브러리입니다. 불행히도 전 AVR을 사용해본 적이 없어서;;;

    PSG

    (2015년 7월 20일 - 7:47 오후)

    z값은 센서를 돌리는거에따라 값이 바뀌나요 아니면 축이 회전할때의 순간 속도만 출력을 해주나요?

      TORTUGA

      TORTUGA

      (2015년 7월 22일 - 2:59 오후)

      가속도 센서는 축이 기울어질 때 값이 바뀌고, 자이로는 축이 회전할 때 바뀝니다.

    SU

    (2015년 7월 20일 - 7:50 오후)

    같은 센서를 사용해서 그대로 실행시켜봤는데 센서를 고정시켰을 때
    Angle x,y,z : -267, 57, 1768
    Angle x,y,z : -268, 57, 1781
    Angle x,y,z : -274, 41, 1746
    Angle x,y,z : -261, 53, 1764
    .
    .
    .
    이런식으로 센서값이 나옵니다 x,y는 잘 나오는데 z값은 움직이는 순간을 제외하고는 변화가 없네요
    어떤 문제가 있는걸까요?

      TORTUGA

      TORTUGA

      (2015년 7월 22일 - 3:00 오후)

      글쎄요.. 원래 그런거 같은데.. 참고로 가속도 센서는 Z축을 중심으로 한 회전은 감지를 못합니다.

    김진우

    (2015년 8월 5일 - 4:26 오후)

    안녕하세요 mpu6050를 가지고 놀고있는 공돌이 입니다.
    코드를 이용해 얻은 센서값을 10진수로 변환하여 lcd에 표현하고 싶습니다.
    간단히 설명해주실수 있나요!
    이제 막 시작하는 단계라 막막하네요 ㅜㅜ

      TORTUGA

      TORTUGA

      (2015년 8월 26일 - 6:43 오후)

      센서로 얻은 값 자체가 int 형 이므로 10진수입니다. 그냥 그값을 print() 함수로 출력하시면 됩니다.

    김연준

    (2015년 8월 23일 - 10:07 오후)

    i2c 통신을 시작할때 ACCEL_XOUT_H 이 값을 wirte한담에 mpu6050에 데이터 요청을 하는데 이 뜻이 ACCEL_XOUT_H 레지스터 주소를 기점으로 계속 8byte씩 꾸준히 데이터를 읽으니 즉 자동으로 Y,Z,temp 그리고 gyro x,y,z 값까지 전송이 되는건가요?

      TORTUGA

      TORTUGA

      (2015년 8월 26일 - 6:44 오후)

      솔직히 거기까지는 잘 모르겠습니다.;;;

    Daniel Heo

    (2015년 10월 2일 - 3:32 오전)

    코드중에.. gx += Cal; 이 부분을 안정화하려고 쓰신다고 하셨는데 Cal은 처음에 샘플링 데이터가 5개 이하일 때 이미 계산이 된건데.. 그 이후부터는 그냥 계속 이값을 더하면 당연히 발산할텐데 이부분을 왜 쓰신건지 궁금하네요.

    HEIXZ

    (2015년 10월 10일 - 9:07 오전)

    설명이 되게 잘 되어있어 너무 좋아요 ㅎㅎ 그런데 궁금한게 있어서 그러는데
    원래 온도도 나와야하지 않나요? temperature 변수가 있길래 온도를 아두이노에서 받아서 해봤는데 온도가 잘 나오질 않아서요 칼만필터 적용안했을때는 값이 잘나오는데 적용하니 값이 잘 안나오네요

      HEIXZ

      (2015년 10월 12일 - 5:11 오전)

      아 그리고 추가적인 질문 있는데 아두이노 프로세싱을 통해 값을 확인하려 하는데 회색화면만 뜨고 동작은 안하네요 오류도 없는데 왜 안되는지 모르겠어요

    Lhin

    (2015년 10월 13일 - 6:33 오전)

    궁금한게 있는데 제가 가속도 자이로 센서를 이용해서 이동거리와 걸음수를 구하려 하는데요 값을 어떤식으로 써야할지 모르겠네요 혹시 아시나요?

    박민지

    (2015년 11월 12일 - 8:15 오후)

    동작하는데 에러는 발생하지않지만, 90도가까이 기울려도 출력값이 1만6천까지 가지않고 2천 정도까지만 갑니다. 왜 그럴까요?ㅜㅜ

      TORTUGA

      TORTUGA

      (2015년 11월 12일 - 11:28 오후)

      소스에 문제가 있을 수도 있을것 같네요. 오래된 내용이라 업데이트가 필요할 것 같습니다. 최신 개발환경과 라이브러리에서 다시 테스트되면 본문 수정하겠습니다.

    김연각

    (2015년 11월 27일 - 7:04 오후)

    예제소스를 다운받으려고 하는데 안보이는데

    혹시 메일로 보내주실수 있나요??ㅠㅠ

    부탁드립니다!!

      TORTUGA

      TORTUGA

      (2015년 12월 2일 - 10:14 오전)

      본문 링크 다시 살렸어요

    한별희

    (2016년 2월 2일 - 5:14 오후)

    안녕하세요 이번에 알씨카를 제작하려는데요 아두이노를 이용해서 벽을 자유롭게 이동하는 로봇을만들려하는데….
    mpu-6050 을 사용해야 될듯해서 처음사용해보는데..
    너무 어려워요 ㅠㅠ자문좀 받고싶은데 …
    제가 만들고자하는것은 https://www.youtube.com/watch?v=rUKZVLGzLZw 에 나와있는 디즈니사에서 버티고라는 로봇입니다 이로봇처럼 벽을 자유롭게 프로펠러로 가는 알씨카를 제작하려는데
    지상에 있을때말고 벽에서 자유롭게 이동할수있게 프로펠러를 각도를 돌려 벽에 붙어있게 제작하려는데
    mpu-6050를 잘활용해야하는데.. 너무 어려워요
    (벽에있을때의 기본설정값?) 설정하는법과 벽에서 움직일때의 변화값을 인지해서 프로펠러를 각도를 트는법좀 알려주세요.

    신용환

    (2016년 3월 31일 - 1:04 오후)

    흠, 재밌네요.
    이걸로 특정시간 이후부터 센서의 현재 위치를 구할 수도 있는건가요?
    이건 각속도만 구하는데, 이걸 계속 trace해나가면 현재 위치를 구할 수 있게 되는건가요?

    송태규

    (2016년 8월 13일 - 8:06 오후)

    올려놓으신 코드는 칼만필터가 들어있는것이 맞나요?
    아니면 다른 필터이거나 필터가 없나요?

    김찬

    (2016년 12월 1일 - 3:32 오후)

    아두이노말고 랩뷰로 제어할 수 있는 방법은없나요 > ㅠㅠ

댓글 남기기

이메일은 공개되지 않습니다.