[제어] OpenCM 9.04에서 MPU-6050 연결을 위한 I2C 통신

[한창호]

OpenCM 9.04 (ARM Cortex-M3) 에서 I2C 통신을 이용하여 자이로센서(MPU-6050) 데이터를 읽어보자.

I2C 통신을 위해서는 SCL (클럭신호) 단자와 SDA(데이터 신호) 단자를 연결한다. (다음 그림은 아두이노 연결도)

가속도 센서 (Accelerator)

중력가속도를 이용하여 기울어져 있는 정도를 나타낸다. X, Y, Z 축 방향과 일치한다.

출처: MPU6050센서를 통한 각도계산 : 네이버 블로그 (naver.com)

자이로스코프 센서 (Zyro)

각속도를 나타내며,  자이로 값을 적분하여 각도를 얻는다. Roll, Pitch, Yaw에 해당한다.

센서 실험

OpenCM 9.04보드에서 MPU-6050 센서를 테스트 해보자. (아두이노 소스와는 약간 다른 점들이 있으나 거의 호환 가능하다.)

가속도 센서 값과 자이로스코프 센서 값 방향

* MPU 6050 설정 config

- 자이로 설정:  Full Scale Range는 2000도/Sec로 설정하면 된다. (GYRO_CONFIG)

- LPF를 설정하여 노이즈를 줄일 수 있다.  (DLPF_CFG)

예제 소스:  (전체 소스는 맨 아래 첨부 함)

// MPU-6050 test for OpenCM9.04,  2018.06.08 liberman
// VCC - 5V or 3.3V
// 24(SCL) –> MPU SCL
// 25(SDA) –> MPU SDA

#include<Wire.h>

const int MPU_ADDR=0x68;       // I2C address of the MPU-6050
int AX,AY,AZ,Tmp,GX,GY,GZ;
 
void setup()
{
  Wire.begin(25,24);    // i2c bus init SDA->25, SCL->24
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);     // PWR_MGMT_1 register 
  Wire.write(0);           // set to zero (wakes up the MPU-6050)
  Wire.endTransmission();
  SerialUSB.begin();
}

void loop()
{
   Wire.beginTransmission(MPU_ADDR);
   Wire.write(0x3B);                      // starting with register 0x3B (ACCEL_XOUT_H)
   Wire.endTransmission();

   Wire.requestFrom(MPU_ADDR,14);           // request a total of 14 registers

   // 가속기 센서
   AX=Wire.read()<<8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
   AY=Wire.read()<<8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
   AZ=Wire.read()<<8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

   // 온도 센서
   Tmp=Wire.read()<<8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

   // 자이로 센서
   GX=Wire.read()<<8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
   GY=Wire.read()<<8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
   GZ=Wire.read()<<8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
  
   SerialUSB.print("AX: "); SerialUSB.print(AX);
   SerialUSB.print(", AY: "); SerialUSB.print(AY);
   SerialUSB.print(", AZ: "); SerialUSB.print(AZ);
   SerialUSB.print("\tGX: "); SerialUSB.print(GX);
   SerialUSB.print(", GY: "); SerialUSB.print(GY);
   SerialUSB.print(", GZ: "); SerialUSB.print(GZ);
   SerialUSB.println();
   delay(500);
}

실험결과

상보필터 (complementary filter) - 가속도 필터와 자이로스코프 필터를 이용하여 정확한 각도를 계산한다.

Accelerometer 신호는 low-pass filter를 거쳐 출력되고, Gyroscope 신호는 high-pass filter 신호를 거쳐 출력된다. 이 두 신호를 합쳐 Roll과 Pitch의 각도를 계산하게 된다. (참고: http://hs36.tistory.com/19)

α = τ/(τ + Δt),
자이로스코프 = ω×Δt
Δt = sampling rate (예 0.04),
τ = 일반적으로 가속도 노이즈의 소요시간 보다 긴 시간을 의미하는 시간 상수 (예, 1초).

deg_ac_y = atan2(ac_x, ac_z) * 180 / PI ;                                            // 가속도 각도
deg_gy_x = gy_y / 131. ;                                                                         // 자이로스코프 각도
angle = (0.95 * (angle + (deg_gy_x * 0.001))) + (0.05 * deg_ac_y) ;    // complementary filter

자이로센서 값의 출력 범위는 ±250°/sec 인데 출력 값은 –32,768~32,767 이다. 즉 32,767/131=250 이 된다.

칼만필터 (Kalman filter)    참고: http://www.matlabinuse.com/Mastering_MATLAB/43103

다음은 칼만 필터의 기본 h는 filter state로 psi 행렬로 나타낸다.

다음 수식으로 다음 상태를 어떻게 예측하는지 수식을 유도해 보자.

처음 상태 hk와 covariance P가 update되고 prediction과 accelerometer 측정으로 계산된 각도 사이의 차이인 h_inn이 계산된다. 그 후, covariance S와 Kalman gain K가 구해지고, 수정된 prediction h_k+1이 계산되고, prediction error의 covariance matrix P가 계산된다. 은 측정 noise covariance이고, accelerometer의 expected noise Q이다.

다음 소스코드는 상보필터와 칼만필터를 사용한 예제 코드이다. (참고: http://sensibilityit.tistory.com/447)

  // -------------------------------------------
  // dt
  DeltaT = (micros() - timer) / 1000000.0;
  timer = micros();   // next time

  // -------------------------------------------
  // Convert gyro values(–32,768~32,767) to degrees/sec (±250°/sec)
  double FS_SEL = 131;
  dGyX = (double)(GyX - baseGyX) / FS_SEL * DeltaT;
  dGyY = (double)(GyY - baseGyY) / FS_SEL * DeltaT;
  dGyZ = (double)(GyZ - baseGyZ) / FS_SEL * DeltaT; 
  // Get angle values from gyro    
  DegGyX += dGyX;
  DegGyY += dGyY;
  DegGyZ += dGyZ;
  
  // -------------------------------------------
  // Get angle values from accelerometer
  float RAD2DEG = 180 / 3.14159;
  DegAcX = atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2)))*RAD2DEG;
  DegAcY = atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2)))*RAD2DEG;
  DegAcZ = 0;   //Accelerometer doesn't give z-angle  

  // -------------------------------------------
  // Compute Complementary filter (상보필터)
  float alpha = 0.96;
  compDegX = alpha * (DegX + (double)(GyX - baseGyX) / GYRO2DEG * DeltaT) + (1.0 - alpha) * DegAcX;   // roll
  compDegY = alpha * (DegY + (double)(GyY - baseGyY) / GYRO2DEG * DeltaT) + (1.0 - alpha) * DegAcY;   // pitch
  compDegZ = (DegZ + (double)(GyZ - baseGyZ) / GYRO2DEG * DeltaT);                                    // yaw

  // -------------------------------------------
  // Compute Kalman filter (칼만필터)
  kalmanX.setAngle(DegAcX); // Set starting angle
  kalmanY.setAngle(DegAcY);

  kalDegX = kalmanX.getAngle(DegAcX, (double)(GyX- baseGyX) / GYRO2DEG, DeltaT );
  kalDegY = kalmanY.getAngle(DegAcY, -((double)(GyY- baseGyY) / GYRO2DEG), DeltaT );

실험결과

* 자이로 센서의 각도(DegGyY)는 적분상수에 의해 누적 오차가 발생하여 끝부분에 이르면 제일 높은 값으로 나타난다. 즉, 다시 0점 위치로 돌아가지 못하고 만다. drift 현상이 발생한 것이다.

* 가속도 센서의 각도(DegAcY)는 – 보통 진동 (외란)이 많이 발생하는데 그래프에는 속도를 늦게 측정해서 그런지 많이 나타나지 않았다.

* 상보필터 각도(CompPitch)는 처음 부분에서는 가속도센서의 각도에 맞도록 천천히 증가하여 같은 위치로 올라가는 모습이 보이고, 마지막 부분에서 자이로 센서의 누적 각도보다는 정상적인 위치로 돌아오는 것을 볼 수 있다.

* 칼만 필터(KalPitch)는 처음부분에서 상보필터보다 빠르게 가속도 센서의 각도(DegAcY)에 접근하고, 또한 마지막 부분에서도 상당히 빠른 속도로 원래의 가속도 센서의 각도로 회복되는 것을 볼 수 있다.

소스파일 첨부

Kalman.h

4.16KB

mpu6050_test.ino

6.37KB

wire.cpp

6.18KB

wire.h

3.35KB