쿼드콥터 제작기 06 - mpu9250(6050) 가속도와 자이로센서

쿼드콥터는 모터만 돌린다고 알아서 날지 못한다. 이유는 여러가지가 있는데 바람이 변수가 될 수도 있고 캘리브레이션을 해도 모터각각의 출력이 조금씩 차이가 날수도 있다. 또 쿼드콥터의 무게중심이 정확하게 맞는 것도 아니기 때문에 기체가 한쪽으로 기울어진 정도에 따라 기울어진쪽 모터를 더 돌려야 기체가 안정되게 뜰 수 있고 이러한 신호를 주는 주기는 앞글에서 말했듯이 2~3ms 내외여야 빠르게 외란에 대응해 쿼드콥터가 자세를 잡을 수 있게 된다.

기울어진정도에 따라 모터의 출력을 제어하는 알고리즘을 PID 자세제어라고 한다. PID에 대한 자세한 설명은 다음글에서 하고, 먼저 기체의 기울기를 알려면 가속도와 자이로센서가 필요하다. 가속도+자이로 6축센서인 MPU6050이 저렴하면서도 다른센서에 비해 안정적이라 쿼드콥터에서 많이 쓰이고 있다. 난 지자계센서가 추가된 mpu9250을 샀지만 지자계는 나중에 쓰니까 지자계를 제외하면 mpu6050과 똑같다.

센서를 이용해 각도를 구하는 방법은 처음에 생각했던 것보다 조금(아니 많이...) 복잡했다.. 각도를 구하려면 가속도와 자이로센서를 써야한다. 둘은 서로 장단점이 있는데 가속도센서는 값은 오차없이 잘나온다. 하지만 진동에 취약해 값이 요동치게 나온다.

반면에 자이로센서는 요동치는일 없이 깔끔하게 잘나오지만 자이로센서의 출력값에는 바이어스(bais=치우침)값이 포함되어있고 자이로센서 값을 구할때 적분을 하면서 이값이 계속 누적되면 점점 오차가 커지게 된다.

손으로 들고 x축으로 왔다갔다 한 결과다. 빨간색 그래프가 자이로센서값이고 파란색 그래프가 가속도센서값이다. 자이로는 깔끔하고 정확하게 출력되지만 점점 오차가 쌓이는 것을 볼 수 있는데 반면에 가속도센서 그래프는 손노이즈에 값이 심하게 요동쳐서 부정확하지만 시간이 지나도 오차값이 누적되지 않는다.

이러한 두 센서의 장점만을 골라 우리가 원하는 각도를 구할 수 있다. 이 때 사용되는 필터가 칼만필터와 상보(또는 보상)필터가 있는데, 칼만필터는 아두이노에서 처리하기엔 무겁다는 말이 많아 상보필터를 쓸것이다.

참고로 필터를 써도 시간이 지남에 따라 자이로센서에서 생기는 오차값은 생기기 마련이다.  루프를 돌기 전 여러번의 센서값을 구해 생기는 오차의 평균값을 주기마다 자이로값에 빼주면 오차가 줄어드는 것을 볼 수 있지만 없어지지는 않는다. 이를 보완해주는게 HDR(Heuristic Drift Reduction) 알고리즘이다

아두이노 소스

#include "Wire.h"
#include "MPU6050.h"
#include "I2Cdev.h"
 
#define pi 3.141592
#define RADIANS_TO_DEGREES 180/3.14159
#define fs 131.0;
MPU6050 mpu;
 
int16_t ax,ay,az;
int16_t gx,gy,gz;
int16_t mx,my,mz;
 
//자이로(각속도)
float gyro_x, gyro_y;
 
//최종 가속도,자이로 각도
float accel_x, accel_y;
float gyro_angle_x=0, gyro_angle_y=0;
 
//상보필터 거친 각도
float angle_x=0,angle_y=0,angle_z=0;
 
//자이로센서 바이어스값
float base_gx=0, base_gy=0, base_gz=0;
 
unsigned long pre_msec=0;
 
void calibrate(){  
   
  int loop =10;
  for (int i=0;i<loop;i++)
  {
    mpu.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
    base_gx += gx;
    base_gy += gy;
    base_gz += gz;
    delay(80);
  }
  
  base_gx /=loop;
  base_gy /=loop;
  base_gz /=loop;
 
}
 
 
void setup() {
  Wire.begin();
 
  Serial.begin(19200);
  mpu.initialize();
  calibrate(); 
}
 
void loop() {
  //단위시간 변화량
  float dt = (millis()-pre_msec)/1000.0;
  pre_msec = millis();
  
  mpu.getMotion9(&ax,&ay,&az,&gx,&gy,&gz,&mx,&my,&mz);
 
  //가속도값 아크탄젠트->각도변환
  accel_x = atan(ay/sqrt(pow(ax,2) + pow(az,2)))*RADIANS_TO_DEGREES;
  accel_y = atan(-1*ax/sqrt(pow(ay,2) + pow(az,2)))*RADIANS_TO_DEGREES;
  
 
  
  //자이로 -32766~+32766을 실제 250degree/s로 변환
  //앞에서 계산한 오차의 평균값을 빼줌 
  gyro_x = (gx-base_gx)/fs;  
  gyro_y = (gy-base_gy)/fs;
 
  //변화량을 적분 
  gyro_angle_x = angle_x + dt*gyro_x;
  gyro_angle_y = angle_y + dt*gyro_y;
 
  //상보필터
  angle_x = 0.95*gyro_angle_x + 0.05*accel_x;
  angle_y = 0.95*gyro_angle_y + 0.05*accel_y;
 
  Serial.print(gyro_angle_x);
  Serial.print("   ");
  Serial.print(accel_x);
  Serial.print("   ");
  Serial.println(angle_x);
  
}

이소스는 HDR없이 상보필터만 적용했는데 오차범위가 40분이 넘어가도록 0.1을 넘지않는다.. 게인값을 잘준건가 아니면 구한 바이어스값이 딱맞아떨어지는건가 ㅋㅋㅋㅋㅋ

--------------2015.10.31수정---------------

I2Cdev와 MPU6050 헤더 물어보시는 분들이 많아서 추가합니다

https://github.com/jrowberg/i2cdevlib

-참고

http://mechasolutionwiki.com/index.php?title=%EC%83%81%EB%B3%B4%ED%95%84%ED%84%B0 -상보필터

http://hs36.tistory.com/32   - HDR 알고리즘

http://hs36.tistory.com/18   - 자이로, 가속도센서 설명