[mpu-6050] 가속도, 자이로 센서에 칼만필터 적용하여 자세제어하기 예제
1. MPU-6050: 가속도, 자이로 센서?
쿼드콥터를 만드시거나 혹은 자세제어가 필요할 때 가속도, 자이로 센서를 사용합니다. 휴대폰에서 이미 많이 쓰이고 있기 때문에 익숙하실텐데 실제 아두이노에서 MPU-6050 센서를 이용해서 작업을 해보려고 하면 안드로이드처럼 이쁘게 값이 나오질 않습니다. 게다가 바로 사용가능한 소스코드를 검색으로 구하려고해도 잘 보이지 않을 뿐더러, 구현을 위한 이론은 복잡하기만 합니다. 모든걸 한번에 이해하려 하지 마시고, 일단 여기 자료대로 한번 따라가면서 구현을 해보고 재차 자료를 찾아 공부해 보시길 바랍니다.
먼저 가속도와 자이로 센서를 왜 같이 사용하는지, 왜 여기서 출력되는 값을 바로 사용할 수 없는지를 알아야 합니다. [여기]를 클릭하시면 가속도, 자이로 센서에 대한 기본 지식을 익히실 수 있습니다. (설명 정말 잘 되어있네요)
요약하자면… 가속도 센서는 가만히 있을 때 센서에 작용하는 중력가속도를 X, Y, Z 축으로 벡터 3개로 나누어 크기를 측정해 줍니다. 진동과 외력(이동)에 측정값이 왜곡될 수 있지만 시간이 지나도 오차에 강한 특징을 가집니다. 이리저리 자세가 변하더라도 원래 자세로 돌아오면 이전에 출력한 값과 똑같은 값이 나옵니다.
자이로 센서는 센서의 회전이 발생하면 XYZ 축의 각속도 변화량을 측정해줍니다. 그런데 각속도이기 때문에 각도(위치)를 구하려면 적분을 해줘야 합니다. 적분을 하는 과정에서 센서의 노이즈도 같이 적분되기 때문에 누적오차가 커지게 됩니다. 결론적으로 자이로 센서는 실제 움직임과 비슷한 값(정확한 값)을 출력하지만 누적오차가 발생합니다.
그래서 이 두 센서의 장점만 모아 값을 보정해줘야 합니다.(가속도 센서의 오차, 자이로 센서의 정확도) 이 역할을 하는 것이 칼만필터입니다. 칼만필터에 대한 일반적인 설명은 매 아래 첨부된 파일을 참고하세요. 이 글에 첨부된 소스는 칼만필터를 이용해서 센서의 각 축에 대한 기울기 값을 구하는 아두이노 코드입니다. MPU-6050은 각 축에 대해 -16383 ~ +16383 까지의 값을 출력해줍니다.
그리고 잘 생각해보시면… 가속도 센서는 중력가속도를 이용해서 기울기를 검출하는데 중력가속도 방향과 일치하는 Z축에 대한 회전은 감지할 수가 없습니다.(Yawing) 머리 꼭대기에 센서를 수평하게 놓고 고개를 좌우로 회전하면 중력가속도 방향의 변화가 없기 때문에 Z축 변화량이 없다는 겁니다. 그래서 Z축 기울어짐은 자이로 센서에 의존해야 하고, 자이로 센서의 값은 앞서 말씀 드린대로 누적오차가 발생합니다. 그래서 정확하게 내가 쳐다보는 방향(Z축 회전각)을 알기 위해서는 지자기 센서가 추가로 필요합니다.
2. 연결방법
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)에 따라 필요한 처리를 여기서 하면 됩니다.
예제소스 다운로드 : [wpdm_package id=’2383′]
칼만필터 설명 자료 : 출처 RealSYS [wpdm_file id=11]
