[Arduino] 2. MPU6050 가속도 / 자이로 센서 (2)

Arduino

[Arduino] 2. MPU6050 가속도 / 자이로 센서 (2) - 필터

Easyho.log 2024. 7. 26. 11:05

1. 왜 필터를 써야 하는가?

전 게시물에 의하면 MPU6050에서 자이로스코프 센서는 오차가 난다고 했다. 이유는 바이어스 드리프트 (Bias Drift)에 있다. 자이로스코프의 출력 값에는 항상 작은 상수 오차(바이어스)가 존재할 수 있다. 시간이 지남에 따라 이 바이어스는 변하며, 이는 측정된 각속도 값에 지속적인 편향을 추가하여 누적 오차를 발생시킨다. 이 영향으로 자이로스코프가 정지 상태임에도 불구하고 각속도를 측정하게 되어 장기간의 통합 과정에서 큰 각도 오차가 발생할 수 있다.

또한, 시간에 따른 오차 누적 (Drift Over Time)도 원인 중 하나이다. 자이로스코프는 각속도를 측정하여 이를 시간에 대해 적분함으로써 각도를 계산하기에 작은 오차가 계속해서 누적되면 시간이 지남에 따라 큰 각도 오차로 이어질 수 있다.

튀는 값들

이 오차를 줄이기 위해서 우리는 필터라는 걸 씌울 건데 이 필터에도 종류가 여러 가지가 있다.

2. 칼만 필터

칼만 필터는 노이즈가 포함된 측정 데이터를 기반으로 상태를 추정하여 보완해주는 필터이다. 기본 원리는 다음과 같다.

예측 단계 (Prediction Step)
- 시스템의 현재 상태와 모델을 사용하여 다음 시간 스텝의 상태를 예측한다.
- 상태 변수를 예측하고, 오차 공분산을 예측한다.
업데이트 단계 (Update Step)
- 새로운 측정 값을 사용하여 예측된 상태를 업데이트한다.
- 칼만 이득(Kalman Gain)을 계산하여 측정 값과 예측 값의 가중 평균을 구한다.
- 상태 변수를 업데이트하고, 오차 공분산을 업데이트한다.

칼만 필터에 쓰이는 공식들은 여기에 나와 있다.

[칼만 필터_ Wikipedia] : https://ko.wikipedia.org/wiki/%EC%B9%BC%EB%A7%8C_%ED%95%84%ED%84%B0

칼만 필터 - 위키백과, 우리 모두의 백과사전

위키백과, 우리 모두의 백과사전. 칼만 필터(Kalman filter)는 잡음이 포함되어 있는 측정치를 바탕으로 선형 역학계의 상태를 추정하는 재귀 필터로, 루돌프 칼만이 개발하였다. 칼만 필터는 컴퓨

ko.wikipedia.org

칼만 필터를 아두이노에서 구현하는 방법에는 2가지 방법이 있다. 라이브러리를 사용하여 구현하는 방법도 있고 라이브러리를 사용하지 않고 구현하는 방법이 있다.

1. 라이브러리를 사용하지 않고 구현

코드는 다음과 같다.

#include <Wire.h>

const int MPU_addr = 0x68;  
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;

// 가속도계와 자이로스코프의 감도 계수
const float ACCEL_SCALE = 16384.0;  // 가속도계: ±2g, 16384 LSB/g
const float GYRO_SCALE = 131.0;     // 자이로스코프: ±250 degrees/sec, 131 LSB/(degrees/sec)

void setup() {
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  
  Wire.write(0);     
  Wire.endTransmission(true);

  // 가속도계 및 자이로스코프 설정
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1C);  
  Wire.write(0x00);  // set to ±2g (least sensitivity)
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1B);  
  Wire.write(0x00);  // set to ±250 degrees/sec (least sensitivity)
  Wire.endTransmission(true);

  Serial.begin(115200);
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);  // request a total of 14 registers
  
  AcX = Wire.read() << 8 | Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = 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)
  GyX = Wire.read() << 8 | Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  // 원시 데이터를 물리 단위로 변환
  float ax = AcX / ACCEL_SCALE;  // 가속도: g 단위
  float ay = AcY / ACCEL_SCALE;  // 가속도: g 단위
  float az = AcZ / ACCEL_SCALE;  // 가속도: g 단위

  float gx = GyX / GYRO_SCALE;  // 자이로: degrees/sec 단위
  float gy = GyY / GYRO_SCALE;  // 자이로: degrees/sec 단위
  float gz = GyZ / GYRO_SCALE;  // 자이로: degrees/sec 단위

  // 결과 출력
  Serial.print("AcX = "); Serial.print(ax);
  Serial.print(" g | AcY = "); Serial.print(ay);
  Serial.print(" g | AcZ = "); Serial.print(az);
  Serial.print(" g | Tmp = "); Serial.print(Tmp / 340.00 + 36.53);  // equation for temperature in degrees C from datasheet
  Serial.print(" | GyX = "); Serial.print(gx);
  Serial.print(" deg/s | GyY = "); Serial.print(gy);
  Serial.print(" deg/s | GyZ = "); Serial.println(gz);
  Serial.println(" deg/s");

  delay(1000);
}

2. 라이브러리를 사용하여 구현

#include <Wire.h>
#include <MPU6050.h>
#include <Kalman.h> // Kalman Filter Library

MPU6050 mpu;
Kalman kalmanX; // X axis Kalman filter
Kalman kalmanY; // Y axis Kalman filter

int16_t AcX, AcY, AcZ, GyX, GyY, GyZ;
double accAngleX, accAngleY;
double gyroX, gyroY;
double angleX, angleY;
unsigned long prevTime, currTime;
double dt;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }

  prevTime = millis();
}

void loop() {
  // 현재 시간 계산
  currTime = millis();
  dt = (currTime - prevTime) / 1000.0;
  prevTime = currTime;

  // MPU6050에서 데이터 읽기
  mpu.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);
  
  // 가속도계 데이터 변환
  double accX = AcX / 16384.0;  // ±2g, 16384 LSB/g
  double accY = AcY / 16384.0;
  double accZ = AcZ / 16384.0;

  // 자이로스코프 데이터 변환
  double gyroX = GyX / 131.0;  // ±250 degrees/sec, 131 LSB/(degrees/sec)
  double gyroY = GyY / 131.0;

  // 가속도계를 통해 각도 계산
  accAngleX = atan2(accY, accZ) * 180 / PI;
  accAngleY = atan2(-accX, sqrt(accY * accY + accZ * accZ)) * 180 / PI;

  // 칼만 필터 적용
  angleX = kalmanX.getAngle(accAngleX, gyroX, dt); // Kalman filter on X axis
  angleY = kalmanY.getAngle(accAngleY, gyroY, dt); // Kalman filter on Y axis

  // 결과 출력
  Serial.print("Kalman Filtered AngleX: "); Serial.print(angleX);
  Serial.print(" | Kalman Filtered AngleY: "); Serial.println(angleY);

  delay(100);
}

3. 매드윅(Madgwick) 필터

매드윅 필터는 조금 다른 개념을 이용한다. 이 또한, 자이로스코프계와 가속계를 보완하여 만든 필터지만 이 필터는 가속도계와 자이로스코프 데이터를 융합하여 쿼터니언(또는 오일러 각) 형태로 자세를 추정한다.

쿼터니온?
쿼터니언(quaternion)은 3차원 공간에서 회전과 같은 회전 변환을 효율적으로 표현하기 위해 사용되는 수학적 개념이다.
쿼터니언은 다음과 같이 4개의 실수 w,x,y,z로 표현된다.
q = w + xi + yj + zk

작동 원리는 다음과 같다.

1. 데이터 수집

2. 쿼터니언 초기화

3. 쿼터니언 미분 방정식 : q˙= 0.5q ⊗ ω

4. 오차 함수 계산: 가속도계 데이터를 이용해 중력 벡터를 추정하고, 이를 통해 오차 함수를 계산한다.

5. 그래디언트 하강법: 오차 함수를 최소화하기 위해 그래디언트 하강법을 사용한다. q_new = q_old − β∇f

6. 쿼터니언 업데이트: 계산된 오차를 바탕으로 쿼터니언을 업데이트하여 새로운 자세를 추정한다.

코드는 다음과 같다.

#include <Wire.h>

#define MPU6050_ADDR 0x68
#define ACCEL_XOUT_H 0x3B
#define GYRO_XOUT_H 0x43
#define PWR_MGMT_1 0x6B

void setup() {
  Serial.begin(115200);
  Wire.begin();
  
  // MPU6050 초기화
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0);
  Wire.endTransmission(true);
}

void readMPU6050(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);

  *ax = (Wire.read() << 8 | Wire.read());
  *ay = (Wire.read() << 8 | Wire.read());
  *az = (Wire.read() << 8 | Wire.read());
  Wire.read(); Wire.read(); // 온도 데이터 건너뛰기
  *gx = (Wire.read() << 8 | Wire.read());
  *gy = (Wire.read() << 8 | Wire.read());
  *gz = (Wire.read() << 8 | Wire.read());
}
struct Quaternion {
  float w, x, y, z;
};

class MadgwickFilter {
public:
  Quaternion q;
  float beta;
  float sampleFreq;

  MadgwickFilter(float beta, float sampleFreq) : beta(beta), sampleFreq(sampleFreq) {
    q = {1.0f, 0.0f, 0.0f, 0.0f};
  }

  void update(float gx, float gy, float gz, float ax, float ay, float az) {
    float q1 = q.w, q2 = q.x, q3 = q.y, q4 = q.z;
    float norm;
    float f1, f2, f3;
    float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
    float step[4];

    norm = sqrt(ax * ax + ay * ay + az * az);
    if (norm == 0.0f) return;
    ax /= norm;
    ay /= norm;
    az /= norm;

    f1 = 2.0f * (q2 * q4 - q1 * q3) - ax;
    f2 = 2.0f * (q1 * q2 + q3 * q4) - ay;
    f3 = 1.0f - 2.0f * (q2 * q2 + q3 * q3) - az;
    J_11or24 = 2.0f * q3;
    J_12or23 = 2.0f * q4;
    J_13or22 = 2.0f * q1;
    J_14or21 = 2.0f * q2;
    J_32 = 2.0f * J_14or21;
    J_33 = 2.0f * J_11or24;

    step[0] = J_14or21 * f2 - J_12or23 * f1;
    step[1] = J_11or24 * f1 + J_13or22 * f2 - J_32 * f3;
    step[2] = J_11or24 * f2 - J_33 * f3;
    step[3] = J_12or23 * f1 + J_13or22 * f3;

    norm = sqrt(step[0] * step[0] + step[1] * step[1] + step[2] * step[2] + step[3] * step[3]);
    step[0] /= norm;
    step[1] /= norm;
    step[2] /= norm;
    step[3] /= norm;

    float qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * step[0];
    float qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * step[1];
    float qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * step[2];
    float qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * step[3];

    q1 += qDot1 * (1.0f / sampleFreq);
    q2 += qDot2 * (1.0f / sampleFreq);
    q3 += qDot3 * (1.0f / sampleFreq);
    q4 += qDot4 * (1.0f / sampleFreq);

    norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
    q.w = q1 / norm;
    q.x = q2 / norm;
    q.y = q3 / norm;
    q.z = q4 / norm;
  }
};
MadgwickFilter filter(0.1f, 512.0f);

void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  readMPU6050(&ax, &ay, &az, &gx, &gy, &gz);

  float axf = (float)ax / 16384.0f;
  float ayf = (float)ay / 16384.0f;
  float azf = (float)az / 16384.0f;
  float gxf = (float)gx / 131.0f;
  float gyf = (float)gy / 131.0f;
  float gzf = (float)gz / 131.0f;

  filter.update(gxf, gyf, gzf, axf, ayf, azf);

  Serial.print("Quaternion: ");
  Serial.print(filter.q.w);
  Serial.print(" ");
  Serial.print(filter.q.x);
  Serial.print(" ");
  Serial.print(filter.q.y);
  Serial.print(" ");
  Serial.println(filter.q.z);

  delay(1000); 
}

4. 상보 필터

싱보 필터는 각 센서의 장점을 결합하고 단점을 상쇄하는 방식으로 동작한다. 가속도계는 장기적으로 안정적이지만, 단기적으로는 노이즈에 민감하다. 반면, 자이로스코프는 단기적으로는 안정적이지만, 장기적으로 드리프트(누적 오차)가 발생한다. 상보 필터는 이 두 센서 데이터를 결합하여 정확하고 안정적인 각도 추정을 수행한다.

수식은 다음과 같다.

θ = α(θ_prev + ωΔt) + (1 − α)θ_acc

여기서
θ: 추정된 각도
θ_prev : 이전에 추정된 각도
ω : 자이로스코프 각속도
Δt : 샘플링 시간 간격
θ_acc : 가속도계를 이용해 계산된 각도
α : 상보 필터의 보정 계수 (0과 1 사이의 값)

아두이노 코드에서는 상보 필터의 보정 계수를 0.98로 뒀다. 이는 자이로스코프계에 가중치를 더 많이 줬다는 뜻이다.

구현이 간단하지만, 자력계(magnetometer) 데이터를 포함하지 않기 때문에 Yaw 축 회전에 대한 자세 추정에는 한계가 있다.

코드는 다음과 같다.

#include <Wire.h>

#define MPU6050_ADDR 0x68
#define ACCEL_XOUT_H 0x3B
#define GYRO_XOUT_H 0x43
#define PWR_MGMT_1 0x6B

float alpha = 0.98;
float dt = 0.01; // 샘플링 시간 간격 (초 단위)

float angleX = 0.0;
float angleY = 0.0;

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // MPU6050 초기화
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(PWR_MGMT_1);
  Wire.write(0);
  Wire.endTransmission(true);
}

void readMPU6050(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
  Wire.beginTransmission(MPU6050_ADDR);
  Wire.write(ACCEL_XOUT_H);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU6050_ADDR, 14, true);

  *ax = (Wire.read() << 8 | Wire.read());
  *ay = (Wire.read() << 8 | Wire.read());
  *az = (Wire.read() << 8 | Wire.read());
  Wire.read(); Wire.read(); // 온도 데이터 건너뛰기
  *gx = (Wire.read() << 8 | Wire.read());
  *gy = (Wire.read() << 8 | Wire.read());
  *gz = (Wire.read() << 8 | Wire.read());
}

void loop() {
  int16_t ax, ay, az, gx, gy, gz;
  readMPU6050(&ax, &ay, &az, &gx, &gy, &gz);

  float axf = (float)ax / 16384.0f;
  float ayf = (float)ay / 16384.0f;
  float azf = (float)az / 16384.0f;
  float gxf = (float)gx / 131.0f;
  float gyf = (float)gy / 131.0f;
  float gzf = (float)gz / 131.0f;

  // 가속도계를 이용한 각도 계산
  float angleAccX = atan2(ayf, azf) * 180 / PI;
  float angleAccY = atan2(axf, azf) * 180 / PI;

  // 자이로스코프를 이용한 각도 추정
  angleX = alpha * (angleX + gxf * dt) + (1 - alpha) * angleAccX;
  angleY = alpha * (angleY + gyf * dt) + (1 - alpha) * angleAccY;

  // 각도 출력
  Serial.print("Angle X: ");
  Serial.print(angleX);
  Serial.print(" | Angle Y: ");
  Serial.println(angleY);

  delay(1000);
}

'Arduino' 카테고리의 다른 글

[Arduino] 4. Arduino Nano 사용해보기 (Mac) (0)	2024.08.01
[Arduino] 3. HC-SR04 (초음파 센서) (0)	2024.07.26
[Arduino] 1. MPU6050 가속도 / 자이로 센서 (1) - 구현 (2)	2024.07.23

현재글[Arduino] 2. MPU6050 가속도 / 자이로 센서 (2) - 필터

컴공생이 끄적이는 오늘의 학습

백준, 프로세스, 스터디, 데이터베이스시스템, 모델링, 컴퓨터아키텍쳐, 관계데이터, BOJ, 데이터모델, cs, SQLD, db, OS, 컴퓨터, 아두이노, 데이터베이스, 티린이, 컴공, 운영체제, 2024,

Today :
Yesterday :

컴공생이 끄적이는 오늘의 학습