MPU6050을 이용해서 가속도계, 자이로스코프 데이터를 입력 받아서 시각화하는 내용까지 했습니다. 1편과 2편 글을 참고해주시고요.
이번 시간에는 데이터 보정과 각도/기울기를 구하는 방법에 대해 알아볼 거예요. 그 중에서 핵심은 바로 Complementary Filter(상보 필터)입니다.
물리량 변환
먼저, MPU6050 코드에서 ACC / GYRO 값을 실제 물리량(g, °/s 단위)로 변환하도록 코드를 수정해봅시다.
원시 데이터를 민감도 계수로 나누어 g와 deg/s 단위의 물리량으로 변환할 수 있습니다. 기본값인 ±2g와 ±250°/s 민감도일 때, 아래와 같이 변환할 수 있습니다.
이를 코드로 작성하면 다음과 같습니다. (핵심 부분만 설명)
#define ACCEL_SCALE_FACTOR 16384.0f // ±2g
#define GYRO_SCALE_FACTOR 131.0f // ±250°/s
... 생략 ...
uint8_t data[14];
int16_t acc_x_raw, acc_y_raw, acc_z_raw;
int16_t gyro_x_raw, gyro_y_raw, gyro_z_raw;
while (1) {
esp_err_t ret = mpu6050_read_bytes(MPU6050_DATA_START_ADDR, data, 14);
if (ret == ESP_OK) {
// 데이터 파싱 (Big-Endian 형식)
acc_x_raw = (int16_t)(data[0] << 8 | data[1]);
acc_y_raw = (int16_t)(data[2] << 8 | data[3]);
acc_z_raw = (int16_t)(data[4] << 8 | data[5]);
gyro_x_raw = (int16_t)(data[8] << 8 | data[9]);
gyro_y_raw = (int16_t)(data[10] << 8 | data[11]);
gyro_z_raw = (int16_t)(data[12] << 8 | data[13]);
// --- 물리량 변환 ---
float acc_x = acc_x_raw / ACCEL_SCALE_FACTOR;
float acc_y = acc_y_raw / ACCEL_SCALE_FACTOR;
float acc_z = acc_z_raw / ACCEL_SCALE_FACTOR;
float gyro_x = gyro_x_raw / GYRO_SCALE_FACTOR;
float gyro_y = gyro_y_raw / GYRO_SCALE_FACTOR;
float gyro_z = gyro_z_raw / GYRO_SCALE_FACTOR;
// 센서값 출력
printf("ACC(g): X=%.3f, Y=%.3f, Z=%.3f | ", acc_x, acc_y, acc_z);
printf("GYRO(deg/s): X=%.2f, Y=%.2f, Z=%.2f\n", gyro_x, gyro_y, gyro_z);
printf("\n");
} else {
ESP_LOGI(TAG, "I2C Read Failed: %d", ret);
}
vTaskDelay(pdMS_TO_TICKS(200)); // 200ms
}그리고 나서 실행해보면 아래와 같은 결과가 나옵니다. 아무것도 안 움직였고 MPU6050이 책상에 수평으로 놓여져 있습니다.
ACC(g): X=0.038, Y=-0.040, Z=1.036 | GYRO(deg/s): X=-2.51, Y=-1.09, Z=-1.01
ACC(g): X=0.040, Y=-0.039, Z=1.042 | GYRO(deg/s): X=-2.53, Y=-0.85, Z=-0.79
ACC(g): X=0.044, Y=-0.044, Z=1.041 | GYRO(deg/s): X=-2.58, Y=-1.06, Z=-0.41
ACC(g): X=0.038, Y=-0.039, Z=1.039 | GYRO(deg/s): X=-2.65, Y=-0.82, Z=-1.04
ACC(g): X=0.038, Y=-0.041, Z=1.038 | GYRO(deg/s): X=-2.57, Y=-0.97, Z=-0.93
ACC(g): X=0.039, Y=-0.036, Z=1.045 | GYRO(deg/s): X=-2.53, Y=-0.93, Z=-0.66
ACC(g): X=0.029, Y=-0.043, Z=1.039 | GYRO(deg/s): X=-2.72, Y=-0.87, Z=-0.92
ACC(g): X=0.033, Y=-0.047, Z=1.039 | GYRO(deg/s): X=-2.50, Y=-0.99, Z=-0.85
ACC(g): X=0.041, Y=-0.043, Z=1.044 | GYRO(deg/s): X=-2.58, Y=-0.85, Z=-0.71우리는 Ax = 0, Ay = 0, Az = 1g 를 기대합니다. 그리고 Gx, Gy, Gz는 0을 기대합니다. 실제 결과는 아주 약간의 오차가 있습니다. 센서는 정상 범위 안에서 동작하는데 약간의 오프셋(초기 편차, Bias)이 존재하는 상황입니다.
가속도계 데이터는 약 ±0.05g 오차, 자이로스코프 데이터는 약 -3 ~ +3 °/s 정도의 오차 정도는 기본적으로 나올 수 있다고 합니다. 공장 출하 편차 영향입니다. 따라서 센서가 정상적으로 동작하는 매우 정상적인 범위라고 볼 수 있는거죠.
다만 정밀한 자세 계산(roll, pitch, yaw)을 하려면 반드시 보정해야 합니다.
데이터 보정
데이터 보정 (Calibration)는 수집된 센서 데이터의 정확도를 높이는 가장 기본적인 단계입니다.
- 가속도계 보정 (Accelerometer Calibration): 센서가 움직이지 않는 상태(중력만 작용하는 상태)에서 각 축의 오프셋(offset) 값을 찾고, 이를 사용하여 읽은 데이터에서 오차를 제거합니다. MPU6050을 수평으로 놓여있을 때 Z축은 1g, X, Y축은 0g가 되도록 만듭니다.
- 자이로스코프 보정 (Gyroscope Calibration): 센서가 움직이지 않을 때 X, Y, Z 축의 드리프트(Drift) 또는 바이어스(Bias) 값을 측정합니다. 이 값들은 센서가 움직이지 않아도 출력되는 회전 속도 오차이므로, 이후 측정값에서 빼주어 0 dps에 가깝게 만듭니다.
데이터 보정 방법은 간단합니다. 전원을 켤 때 센서를 가만히 둔 상태에서 약 200회 정도 읽어서 평균값을 오프셋으로 저장하고요. 계산 시에는 원시 데이터에다가 오프셋 값을 빼서 보정된 값을 사용하면 되겠습니다.
코드 작성
MPU6050 자동 캘리브레이션 기능을 추가한 코드를 작성해보겠습니다. 하는 김에 함수를 기능적으로 분리시켰습니다.
- 원시 데이터를 가져오는 함수
- 오프셋 값을 계산하는 함수
- 원시 데이터에 오프셋 값을 적용해서 보정하는 함수
먼저 필요한 정의, 전역 변수를 추가해줍니다.
#define CALIBRATION_SAMPLES 200
#define SAMPLE_DELAY_MS 200 // 200ms 주기 (1초에 5번)
#define ACCEL_1G_LSB 16384 // 1g의 기준값
int16_t gyro_offset[3] = {0, 0, 0}; // 전역 오프셋 변수
int16_t accel_offset[3] = {0, 0, 0}; // 전역 오프셋 변수#1. 센서의 가속도, 자이로스코프 원시 데이터를 가져오는 함수
/**
* @brief 센서의 가속도, 자이로스코프 원시 데이터를 가져옵니다.
*/
esp_err_t mpu6050_read_accel_gyro(int16_t *raw_accel, int16_t *raw_gyro)
{
uint8_t data[14];
esp_err_t ret = mpu6050_read_bytes(MPU6050_DATA_START_ADDR, data, 14);
if (ret == ESP_OK) {
// 데이터 파싱 (Big-Endian 형식)
raw_accel[0] = (int16_t)(data[0] << 8 | data[1]);
raw_accel[1] = (int16_t)(data[2] << 8 | data[3]);
raw_accel[2] = (int16_t)(data[4] << 8 | data[5]);
raw_gyro[0] = (int16_t)(data[8] << 8 | data[9]);
raw_gyro[1] = (int16_t)(data[10] << 8 | data[11]);
raw_gyro[2] = (int16_t)(data[12] << 8 | data[13]);
} else {
ESP_LOGI(TAG, "I2C Read Failed: %d", ret);
}
return ret;
}#2. 오프셋 값을 계산하는 함수
/**
* @brief 센서의 오프셋을 계산합니다. 센서를 움직이지 않고 평평한 곳에 놓아야 합니다
*/
void mpu6050_calibrate()
{
printf("\n=== MPU6050 Calibration Start ===\n");
printf("센서를 완전히 고정한 상태로 두세요...\n");
int32_t acc_sum_x = 0, acc_sum_y = 0, acc_sum_z = 0;
int32_t gyro_sum_x = 0, gyro_sum_y = 0, gyro_sum_z = 0;
int16_t raw_accel[3];
int16_t raw_gyro[3];
for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
if (mpu6050_read_accel_gyro(raw_accel, raw_gyro) != ESP_OK) {
printf("MPU6050 읽기 오류\n");
continue;
}
acc_sum_x += raw_accel[0];
acc_sum_y += raw_accel[1];
acc_sum_z += raw_accel[2];
gyro_sum_x += raw_gyro[0];
gyro_sum_y += raw_gyro[1];
gyro_sum_z += raw_gyro[2];
vTaskDelay(pdMS_TO_TICKS(10)); // 보정은 좀 빠르게 10ms. 200개 샘플이면 2초면 끝
}
// 평균값 계산
int16_t avg_ax = (int16_t)(acc_sum_x / CALIBRATION_SAMPLES);
int16_t avg_ay = (int16_t)(acc_sum_y / CALIBRATION_SAMPLES);
int16_t avg_az = (int16_t)(acc_sum_z / CALIBRATION_SAMPLES);
int16_t avg_gx = (int16_t)(gyro_sum_x / CALIBRATION_SAMPLES);
int16_t avg_gy = (int16_t)(gyro_sum_y / CALIBRATION_SAMPLES);
int16_t avg_gz = (int16_t)(gyro_sum_z / CALIBRATION_SAMPLES);
// 오프셋 계산
accel_offset[0] = avg_ax;
accel_offset[1] = avg_ay;
accel_offset[2] = avg_az - ACCEL_1G_LSB; // Az의 목표값은 ACCEL_1G_LSB (16384)
gyro_offset[0] = avg_gx;
gyro_offset[1] = avg_gy;
gyro_offset[2] = avg_gz;
printf("오프셋 계산 완료.\n");
printf("오프셋 Ax: %d, Ay: %d, Az: %d | ", accel_offset[0], accel_offset[1], accel_offset[2]);
printf("오프셋 Gx: %d, Gy: %d, Gz: %d | ", gyro_offset[0], gyro_offset[1], gyro_offset[2]);
printf("\n");
}#3. 원시 데이터를 읽고 오프셋을 적용하여 보정된 값을 반환하는 함수
/**
* @brief 원시 데이터를 읽고 오프셋을 적용하여 보정된 값을 반환합니다
*/
void mpu6050_get_corrected_data(int16_t *corrected_accel, int16_t *corrected_gyro)
{
int16_t raw_accel[3];
int16_t raw_gyro[3];
if (mpu6050_read_accel_gyro(raw_accel, raw_gyro) != ESP_OK) {
printf("MPU6050 읽기 오류\n");
return;
}
// 오프셋 적용 (보정)
corrected_accel[0] = raw_accel[0] - accel_offset[0];
corrected_accel[1] = raw_accel[1] - accel_offset[1];
corrected_accel[2] = raw_accel[2] - accel_offset[2];
corrected_gyro[0] = raw_gyro[0] - gyro_offset[0];
corrected_gyro[1] = raw_gyro[1] - gyro_offset[1];
corrected_gyro[2] = raw_gyro[2] - gyro_offset[2];
}마지막으로 메인 함수 app_main 내부에서 캘리브레이션 적용하기
void app_main(void)
{
i2c_master_init();
if (mpu6050_init() != ESP_OK) {
ESP_LOGI(TAG, "MPU6050 Initialization Failed.");
return;
}
// 보정 실행 - 오프셋 계산
mpu6050_calibrate();
// 보정된 데이터 사용
int16_t corrected_gyro[3];
int16_t corrected_accel[3];
while (1) {
mpu6050_get_corrected_data(corrected_accel, corrected_gyro);
// --- 물리량 변환 ---
float acc_x = corrected_accel[0] / ACCEL_SCALE_FACTOR;
float acc_y = corrected_accel[1] / ACCEL_SCALE_FACTOR;
float acc_z = corrected_accel[2] / ACCEL_SCALE_FACTOR;
float gyro_x = corrected_gyro[0] / GYRO_SCALE_FACTOR;
float gyro_y = corrected_gyro[1] / GYRO_SCALE_FACTOR;
float gyro_z = corrected_gyro[2] / GYRO_SCALE_FACTOR;
// 센서값 출력
printf("ACC(g): X=%.3f, Y=%.3f, Z=%.3f | ", acc_x, acc_y, acc_z);
printf("GYRO(deg/s): X=%.2f, Y=%.2f, Z=%.2f\n", gyro_x, gyro_y, gyro_z);
printf("\n");
vTaskDelay(pdMS_TO_TICKS(SAMPLE_DELAY_MS)); // 200ms 주기
}
}실행 결과
실행 결과를 보면 알 수 있듯이 정말 많이 좋아졌음을 알 수 있습니다. 센서가 가만히 있을 때 거의 0g, 0°/s가 되도록 보정되었습니다.
=== MPU6050 Calibration Start ===
센서를 완전히 고정한 상태로 두세요...
오프셋 계산 완료.
오프셋 Ax: 602, Ay: -648, Az: 659 | 오프셋 Gx: -337, Gy: -120, Gz: -107 |
ACC(g): X=0.002, Y=0.001, Z=1.003 | GYRO(deg/s): X=0.15, Y=-0.11, Z=0.08
ACC(g): X=-0.004, Y=0.004, Z=0.993 | GYRO(deg/s): X=0.10, Y=-0.02, Z=-0.03
ACC(g): X=0.004, Y=-0.001, Z=1.003 | GYRO(deg/s): X=-0.15, Y=-0.15, Z=0.08
ACC(g): X=0.000, Y=0.005, Z=0.995 | GYRO(deg/s): X=-0.09, Y=-0.31, Z=0.11
ACC(g): X=0.001, Y=-0.001, Z=1.006 | GYRO(deg/s): X=0.04, Y=-0.03, Z=-0.04
ACC(g): X=0.004, Y=0.001, Z=0.994 | GYRO(deg/s): X=-0.14, Y=0.08, Z=-0.05
ACC(g): X=-0.002, Y=0.003, Z=1.003 | GYRO(deg/s): X=-0.44, Y=0.13, Z=0.07
ACC(g): X=-0.003, Y=-0.007, Z=1.000 | GYRO(deg/s): X=-0.01, Y=-0.06, Z=-0.06
ACC(g): X=-0.000, Y=-0.003, Z=1.001 | GYRO(deg/s): X=-0.21, Y=0.04, Z=0.03이처럼 데이터 보정은 이후 단계로 넘어가기 위한 필요한 가장 기본적인 단계라고 보면 됩니다.
가속도계 기반 각도 계산
우리는 가속도와 자이로스코프(각속도)를 알게 되었지만 각도/기울기가 얼마인지는 잘 모릅니다. 각도를 계산하는 방법에는 가속도계 기반 각도 계산, 자이로스코프 기반 각도 계산, 마지막으로 가속도와 자이로스코프 데이터를 융합한 계산법이 있습니다.
먼저, 우리는 가속도계 데이터만 있다고 가정하고 각도를 계산하는 방법에 대해 알아보겠습니다.
가속도계는 정적인 기울기를 측정하는 데 유용합니다. 다음과 같은 공식으로 롤(ϕ) 및 피치(θ) 각도를 계산할 수 있습니다.
- Roll은 X축 회전입니다. X축이 좌/우 수평상태라고 했을 때, 앞/뒤로 숙이면 Roll 값이 바뀝니다.
- Pitch는 Y축 회전입니다. Y축이 앞/뒤 수직상태라고 했을 때, 좌/우로 숙이면 Pitch 값이 바뀝니다.
코드로 구현하자면 Roll(롤, X축 회전), Pitch(피치, Y축 회전)는 다음과 같이 계산할 수 있습니다.
roll = atan2(Ay, Az)
pitch = atan2(-Ax, sqrt(Ay^2 + Az^2))단위는 라디안이므로, degree로 변환하려면 × 180/π 를 해주면 됩니다. 아주 쉽게 구현할 수 있죠?
#include <math.h>
// --- 가속도계를 이용해 각도 계산 ---
void mpu6050_compute_roll_pitch(int16_t *corrected_accel, float *roll, float *pitch)
{
// LSB를 g 단위로 변환
float ax = (float)corrected_accel[0] / ACCEL_SCALE_FACTOR;
float ay = (float)corrected_accel[1] / ACCEL_SCALE_FACTOR;
float az = (float)corrected_accel[2] / ACCEL_SCALE_FACTOR;
// atan2 결과는 라디안 → degree 변환
*roll = atan2(ay, az) * 180.0f / M_PI;
*pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / M_PI;
}
...
// 보정된 데이터 사용
int16_t corrected_gyro[3];
int16_t corrected_accel[3];
float roll, pitch;
while (1) {
mpu6050_get_corrected_data(corrected_accel, corrected_gyro);
// 가속도 값 계산 후 추가
mpu6050_compute_roll_pitch(corrected_accel, &roll, &pitch);
printf("ROLL=%.2f deg, PITCH=%.2f deg\n", roll, pitch);
printf("\n");
vTaskDelay(pdMS_TO_TICKS(SAMPLE_DELAY_MS)); // 200ms 주기
}수학적 원리
구현은 쉬운데... 그렇다면 왜 저런 공식이 나오는지 생각해봅시다. 이건 심각함수랑 기하학적 원리를 알면 됩니다. 따라서 tan와 atan에 대해 잠시 설명하겠습니다.
tan(각도) = y / x, 각도 = atan(y / x) 입니다. 여기서 x는 밑변, y는 높이를 의미합니다.
즉, tan은 각도(x)를 입력받아 그 각도의 탄젠트 값(y)을 출력하는 삼각함수이고, atan(아크탄젠트) 또는 tan⁻¹은 탄젠트 값(y)을 입력받아 원래의 각도(x)를 찾아주는 역함수입니다.
- tan (탄젠트): 직각삼각형에서 특정 각도의 '높이(opposite) ÷ 밑변(adjacent)' 비율을 계산해 줍니다.
- atan (아크탄젠트) 또는 tan⁻¹ (역탄젠트): 탄젠트 값(비율)을 입력받아 원래의 각도를 찾아줍니다.
그러면 atan랑 atan2 는 무슨 차이일까요? atan2는 atan를 더 잘 사용하기 위한 프로그래밍 함수라고 보면 될 거 같습니다. atan를 사용하면 두 가지 문제가 있습니다.
- x가 0이면 y / x 계산이 불가능 (0으로 나누기) 합니다. 예: 각도가 정확히 90°일 때.
- 사분면을 구분하지 못합니다. atan(1/1) = 45°, atan(-1/-1) = 45° 이 똑같이 나옵니다. 실제론 첫 번째는 45°, 두 번째는 225°여야 합니다.
따라서 atan2(y, x)는 원점 (0, 0)에서 점 (x, y)까지의 각도를 (정확히!) 구해줍니다.
y
↑
| ● (x, y)
| /
| / 각도?
|/________→ x
(0,0)atan2(y, x)는 x와 y의 부호를 따로 확인해서 정확한 각도를 계산합니다. atan2는 360도 전체 범위에서 정확한 각도를 알려줍니다.
💻 참고: 14 | Measure angles with the MPU6050 accelerometer - Carbon Aeronautics
Roll을 구하는 방법은 아래 그림과 같은데요. 삼각함수 원리를 이용한 걸 알 수 있네요.
실제 공식은 아래와 같지만 Acc_x는 변하지 않습니다. 즉, X축 성분(Ax)은 roll 계산에 영향을 주지 않기 때문에 atan2(Ay, Az) 공식으로 구할 수 있습니다.
Roll은 X축을 중심으로 한 회전입니다. Y-Z 평면에서 본 각도를 계산합니다. 센서가 X축을 중심으로 회전하면 Ay, Az 이 두 성분의 비율로 결정된다는 거죠.
Pitch를 구하는 방법은 아래와 같습니다.
음... 근데 이번에는 atan2(-Ax, sqrt(Ay^2 + Az^2)) 공식을 사용했습니다. Roll 구할 때와 마찬가지로 Acc_y는 제거할 수 있지 않나요?
순수한 Pitch 회전에서는 Ay는 변하지 않습니다. 하지만 roll 회전이 있으면 Ay가 변합니다. 따라서 √(Ay² + Az²)를 해줘야 roll이 얼마나 있든 상관없이 "수평면에 대한 기준선"을 정확히 계산할 수 있습니다. (그렇게 따지면 Roll 은 왜 그렇게 구하는데...)
아무튼 뭐. 결론은 삼각함수 tan, atan를 이용해서 roll과 pitch를 구할 수 있다는 겁니다.
실습 결과
실습 결과입니다. 아무것도 하지 않은 초기 상태입니다. 거의 0에 가깝게 나온 것을 알 수 있죠.
=== MPU6050 Calibration Start ===
센서를 완전히 고정한 상태로 두세요...
오프셋 계산 완료.
오프셋 Ax: 603, Ay: -687, Az: 646 | 오프셋 Gx: -338, Gy: -119, Gz: -100 |
ROLL=0.32 deg, PITCH=-0.10 deg
ROLL=0.59 deg, PITCH=-0.02 deg
ROLL=0.14 deg, PITCH=-0.24 deg
ROLL=-0.13 deg, PITCH=0.08 deg
ROLL=0.15 deg, PITCH=0.46 deg
ROLL=0.07 deg, PITCH=-0.00 deg앞으로 들어올리면 어떻게 될까요? Roll이 변하겠죠. 오. 잘 변했습니다. 각도계가 없어서 실제로 저 각도인지는 잘 모르겠지만 얼추 비슷한 거 같습니다.
ROLL=29.77 deg, PITCH=-0.07 deg
ROLL=30.11 deg, PITCH=-0.09 deg
ROLL=29.74 deg, PITCH=-0.10 deg
ROLL=29.51 deg, PITCH=-0.03 deg
ROLL=29.76 deg, PITCH=-0.00 deg90도 세웠을 때는 다음과 같고요
ROLL=91.37 deg, PITCH=0.24 deg
ROLL=91.57 deg, PITCH=0.04 deg
ROLL=91.74 deg, PITCH=-0.00 deg
ROLL=91.74 deg, PITCH=-0.02 deg옆(좌/우)로 숙이면 Pitch가 변합니다.
ROLL=-0.25 deg, PITCH=65.42 deg
ROLL=0.13 deg, PITCH=65.21 deg
ROLL=-0.95 deg, PITCH=64.99 deg
ROLL=0.80 deg, PITCH=63.71 deg
ROLL=-0.07 deg, PITCH=63.45 deg다만 이 방식에는 약간의 문제가 있습니다. 가속도계는 저주파 영역(느린 변화)에서는 신뢰도가 높습니다. 반대로 고주파 영역에서는 노이즈 때문에 신뢰도가 낮습니다.
이게 무슨 말이냐면 센서를 진동, 떨림을 주듯이 막 흔들거려보세요. 값이 들쑥날쑥하게 변합니다. 실제로는 그정도는 아닌데 말이죠.
ROLL=17.79 deg, PITCH=23.04 deg
ROLL=-1.09 deg, PITCH=13.08 deg
ROLL=12.79 deg, PITCH=53.70 deg
ROLL=-159.98 deg, PITCH=42.02 deg
ROLL=14.55 deg, PITCH=2.07 deg
ROLL=9.67 deg, PITCH=-6.37 deg
ROLL=12.06 deg, PITCH=14.04 deg가속도 기반 Roll/Pitch는 안정적이지만 빠른 움직임에서 정확도는 떨어진다고 볼 수 있습니다. 그렇기 때문에 Complementary Filter 나 Kalman Filter를 적용해서 가속도 + 자이로 융합을 통해 정확도를 높이는 것이지요.
자이로스코프 기반 각도 계산
가속도계만을 이용해 각도를 계산하는 방법 말고도, 자이로스코프(각속도)만을 이용해 각도를 계산할 수도 있습니다. 바로 적분을 이용하는 건데요.
자이로스코프는 Δt 시간 동안의 회전 속도 Gyro_rate를 측정합니다. 이 속도를 적분하여 현재 각도를 추정합니다.
쉽게 말해서, 속도에서 거리를 구하듯이, 각속도를 적분하면 각도를 얻습니다. 속도에서 거리 구하는 것과, 각속도에서 각도를 구하는 것은 원리가 동일합니다.
속도계가 60 km/h를 보여줌
1시간 동안 달리면 → 60 km 이동
0.5시간 동안 달리면 → 30 km 이동
거리 = 속도 × 시간
==============
자이로가 30 deg/s를 측정
1초 동안 → 30° 회전
0.5초 동안 → 15° 회전
각도 = 각속도 × 시간 (각도 = ∫ 각속도 dt)코드로 구현하면 다음과 같습니다. 간단합니다.
// ---각속도를 이용한 각도 계산 ---
void mpu6050_compute_angle_from_gyro(float gx, float gy, float gz, float *angle)
{
float dt = SAMPLE_DELAY_MS / 1000.0f;
angle[0] += gx * dt;
angle[1] += gy * dt;
angle[2] += gz * dt;
}
...
float angle[3];
while (1) {
mpu6050_get_corrected_data(corrected_accel, corrected_gyro);
float gyro_x = corrected_gyro[0] / GYRO_SCALE_FACTOR;
float gyro_y = corrected_gyro[1] / GYRO_SCALE_FACTOR;
float gyro_z = corrected_gyro[2] / GYRO_SCALE_FACTOR;
// --- Gyro 기반 각도 계산 ---
mpu6050_compute_angle_from_gyro(gyro_x, gyro_y, gyro_z, angle);
printf("GYRO Angle: X=%.2f°, Y=%.2f°, Z=%.2f°\n", angle[0], angle[1], angle[2]);
printf("\n");
vTaskDelay(pdMS_TO_TICKS(SAMPLE_DELAY_MS)); // 200ms 주기
}실습 결과
아무것도 하지 않았을 때 다음과 같습니다.
=== MPU6050 Calibration Start ===
센서를 완전히 고정한 상태로 두세요...
오프셋 계산 완료.
오프셋 Ax: 357, Ay: -659, Az: 635 | 오프셋 Gx: -336, Gy: -118, Gz: -105 |
GYRO Angle: X=-0.02°, Y=0.74°, Z=1.98°
GYRO Angle: X=-0.03°, Y=0.72°, Z=1.98°
GYRO Angle: X=-0.02°, Y=0.68°, Z=1.96°
GYRO Angle: X=-0.04°, Y=0.67°, Z=2.00°
GYRO Angle: X=-0.04°, Y=0.68°, Z=2.02°
GYRO Angle: X=-0.07°, Y=0.69°, Z=1.99°들어올렸을 때는 다음과 같습니다. 나쁘지 않은 거 같습니다.
GYRO Angle: X=28.45°, Y=-0.64°, Z=1.56°
GYRO Angle: X=28.27°, Y=-0.64°, Z=1.57°
GYRO Angle: X=28.42°, Y=-0.62°, Z=1.60°
GYRO Angle: X=28.36°, Y=-0.63°, Z=1.63°90도 까지 갔다가 다시 바닥에 내려놓으면 음... 각도가 많이 틀어졌네요.
GYRO Angle: X=2.04°, Y=-0.79°, Z=7.92°
GYRO Angle: X=2.00°, Y=-0.83°, Z=7.94°
GYRO Angle: X=1.98°, Y=-0.86°, Z=7.95°
GYRO Angle: X=1.99°, Y=-0.87°, Z=7.92°
GYRO Angle: X=2.01°, Y=-0.86°, Z=7.94°
GYRO Angle: X=2.02°, Y=-0.83°, Z=7.90°이 방식의 문제는 계속 적분하기 때문에 드리프트(오차)가 누적된다는 겁니다. 따라서 얼마 못가서 각도가 점점 틀어지게 되는 것이죠.
다시 말해, 자이로스코프는 고주파 영역(빠른 변화)에서는 신뢰도가 높습니다. 대신, 저주파 영역에서는 드리프트 때문에 신뢰도 낮습니다.
그래서 보통 가속도 기반 + 자이로 기반 융합(complementary/Kalman)을 사용합니다.
Complementary Filter (상보 필터)
Complementary Filter (상보 필터)의 핵심은 가중치(α)를 사용하여 두 각도를 합치는 것입니다.
- α (큰 가중치, ~0.98): 자이로스코프(Angle_gyro)에 높은 가중치를 부여합니다. 빠른 움직임(고주파)에 즉각적으로 반응하여 움직임의 정확성을 높입니다.
- 1 - α (작은 가중치, ~0.02): 가속도계(Angle_accel)의 낮은 가중치를 부여합니다. 느린 속도로 자이로스코프의 누적된 오차(드리프트)를 가속도계의 안정적인 장기 기울기 정보로 '교정'하는 역할을 합니다.
주파수 영역으로 이를 해석해보면 다음과 같습니다.
- 자이로스코프는 고주파 영역(빠른 변화)에서는 신뢰도가 높습니다. (저주파 영역에서는 드리프트 때문에 신뢰도 낮음)
- 가속도계는 저주파 영역(느린 변화)에서는 신뢰도가 높습니다. (고주파 영역에서는 노이즈 때문에 신뢰도 낮음)
따라서 상보 필터는 자이로스코프 데이터에 고역 통과 필터(High-Pass Filter)를 적용하고, 가속도계 데이터에 저역 통과 필터(Low-Pass Filter)를 적용한 후, 그 결과를 합치는 것과 수학적으로 동일합니다. 두 필터의 전달 함수를 합치면 1이 되어, 전체 시스템이 모든 주파수 대역의 정보를 빠짐없이 추적하게 됩니다.
코드 구현
핵심 코드는 다음과 같습니다. 가속도계를 이용한 각도 계산 공식과 자이로스코프를 이용한 각도 계산 공식을 합쳐서 가중치를 적용해서 구한다는 사실을 알 수 있습니다.
...생략...
#define ALPHA 0.98f // 자이로스코프에 부여할 가중치
// ** 각도 저장 변수 **
float roll_angle = 0.0f;
float pitch_angle = 0.0f;
// ** 샘플링 주기**
float dt = SAMPLE_DELAY_MS / 1000.0f;
// --- 가속도계를 이용해 각도 계산 ---
void mpu6050_compute_roll_pitch(int16_t *corrected_accel, float *roll, float *pitch)
{
// LSB를 g 단위로 변환
float ax = (float)corrected_accel[0] / ACCEL_SCALE_FACTOR;
float ay = (float)corrected_accel[1] / ACCEL_SCALE_FACTOR;
float az = (float)corrected_accel[2] / ACCEL_SCALE_FACTOR;
// atan2 결과는 라디안 → degree 변환
*roll = atan2(ay, az) * 180.0f / M_PI;
*pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0f / M_PI;
}
// --- 상보 필터를 적용하여 각도를 계산하는 함수 ---
void complementary_filter(int16_t *corrected_accel, int16_t *corrected_gyro)
{
float accel_roll, accel_pitch;
// 가속도계 각도 계산
mpu6050_compute_roll_pitch(corrected_accel, &accel_roll, &accel_pitch);
// 자이로스코프 기반 각도 변화 계산
float gyro_x = corrected_gyro[0] / GYRO_SCALE_FACTOR;
float gyro_y = corrected_gyro[1] / GYRO_SCALE_FACTOR;
// 2) 자이로 적분 각도
// Roll_gyro = Roll_prev + Gyro_Roll_Rate * dt
// Pitch_gyro = Pitch_prev + Gyro_Pitch_Rate * dt
float gyro_roll = roll_angle + gyro_x * dt;
float gyro_pitch = pitch_angle + gyro_y * dt;
// 상보 필터 적용
roll_angle = (ALPHA * gyro_roll) + ((1.0f - ALPHA) * accel_roll);
pitch_angle = (ALPHA * gyro_pitch) + ((1.0f - ALPHA) * accel_pitch);
}
void app_main(void)
{
i2c_master_init();
if (mpu6050_init() != ESP_OK) {
ESP_LOGI(TAG, "MPU6050 Initialization Failed.");
return;
}
// 보정 실행 - 오프셋 계산
mpu6050_calibrate();
// 보정된 데이터 사용
int16_t corrected_gyro[3];
int16_t corrected_accel[3];
while (1) {
// 센서 데이터 읽기 - 보정된 값
mpu6050_get_corrected_data(corrected_accel, corrected_gyro);
// 상보 필터로 각도 계산
complementary_filter(corrected_accel, corrected_gyro);
// 각도 출력
printf("ROLL=%.2f deg, PITCH=%.2f deg\n", roll_angle, pitch_angle);
printf("\n");
vTaskDelay(pdMS_TO_TICKS(SAMPLE_DELAY_MS)); // 200ms 주기
}
}실습 결과
아무 것도 하지 않았을 때.
=== MPU6050 Calibration Start ===
센서를 완전히 고정한 상태로 두세요...
오프셋 계산 완료.
오프셋 Ax: 341, Ay: -681, Az: 650 | 오프셋 Gx: -337, Gy: -121, Gz: -107 |
ROLL=-0.01 deg, PITCH=0.01 deg
ROLL=-0.03 deg, PITCH=-0.02 deg
ROLL=-0.03 deg, PITCH=-0.01 deg
ROLL=-0.03 deg, PITCH=-0.03 deg
ROLL=-0.07 deg, PITCH=-0.04 deg
ROLL=-0.06 deg, PITCH=-0.00 deg센서를 들어올렸을 때. 잘 변하네요.
ROLL=43.14 deg, PITCH=-0.01 deg
ROLL=43.02 deg, PITCH=-0.02 deg
ROLL=42.99 deg, PITCH=-0.06 deg
ROLL=43.00 deg, PITCH=-0.11 deg
ROLL=42.69 deg, PITCH=-0.12 deg
음. 근데 다시 바닥에 놓으니까 빠르게 원점으로 돌아가지 않네요? 천천히 0으로 내려갑니다.
ROLL=1.20 deg, PITCH=0.95 deg
ROLL=1.16 deg, PITCH=0.93 deg
ROLL=1.15 deg, PITCH=0.91 deg
ROLL=1.13 deg, PITCH=0.88 deg
ROLL=1.14 deg, PITCH=0.83 deg
ROLL=1.13 deg, PITCH=0.86 deg
ROLL=1.11 deg, PITCH=0.84 deg
ROLL=1.11 deg, PITCH=0.81 deg
ROLL=1.10 deg, PITCH=0.78 deg
ROLL=1.07 deg, PITCH=0.78 deg
ROLL=1.05 deg, PITCH=0.80 deg
ROLL=1.04 deg, PITCH=0.79 deg
ROLL=1.01 deg, PITCH=0.76 deg
ROLL=1.00 deg, PITCH=0.75 deg
ROLL=0.98 deg, PITCH=0.69 deg
왜냐면 지금 샘플링 주기가 1초에 5번, 0.2초 입니다. dt가 너무 크기 때문에 Gyro 적분량이 너무 크게 튑니다.
그리고 Complementary Filter는 사실상 Acc가 Gyro 드리프트를 "서서히 빼주는 역할"인데, 5Hz에서는 1초에 겨우 5번만 보정됩니다. 보정이 너무 느리기 때문에 “원래 위치로 돌아왔는데도 각도가 제자리로 복구되지 않는 현상”이 나타납니다.
따라서 샘플링 레이트를 최소 50Hz 이상을 권장합니다. dt가 0.02초로 줄면 적분 오차가 감소하고, Acc 보정 빈도가 증가해서 원점 복귀가 매우 빨라집니다.
실제로 샘플링 레이트를 50Hz로 해봤더니 아주 빠르게 반응하는 것을 알 수 있었습니다.
마치면서
가속도계랑 자이로스코프 데이터를 이용해 실제 물리량 변환하는 방법. 데이터 보정하는 방법. 그리고 각도(기울기)를 구하기 위해서 각각을 이용해보면서 한계를 알아봤고. 마지막으로 센서 융합 - 가속도랑 자이로스코프 데이터를 합쳐서 정확도를 높이는 상보 필터에 대해 알아봤습니다.
각도를 구하면 앞으로 다양한 것을 해볼 수도 있겠네요. 다음 시간에는 Kalman Filter 에 대해 알아보도록 하겠습니다. 어려우려나...?
