MPU6050 상보필터 적용하기

 이전 포스팅에서 MPU6050 센서에서 값을 측정하는 방법에 대해서 알아 보았다. 이번 포스팅에서는 센서에서 측정 되는 값에서 노이즈를 제거하기 위하여 필터를 적용하는 방법에 대해서 이야기 한다.

 MPU6050 센서 값을 Processing 이라는 툴을 가지고 Visual 하게 보여주는 프로젝트가 있다. 매우 유명한 사이트로 한번 쯤 가서 내용과 영상을 봐도 좋다.

http://www.geekmomprojects.com/gyroscopes-and-accelerometers-on-a-chip/

 MPU6050 센서는 가속도와 자이로 값을 측정하는데, 가속도는 중력 가속도에 대한 기울어진 각도를 측정한다. 

 가속도 센서는 움직이거나 회전시킬 때 민감하게 반응하기 때문에 노이즈가 심하가 나타난다. 특히 드론에서 모터 회전은 고속의 미세 움직임을 발생시키는데, 가속도 센서에서는 이런 노이즈에 취약하다. 가속도 센서는 정적 상태에서 각속도를 비교적 정확하게 측정 가능하지만 움직임에 따른 노이즈에 취약하다고 볼 수 있다.

 자이로 센서는 센서의 움직임에 대한 각속도를 측정하게 되는데, +X, +Y, +Z 방향에 대한 회전 각속도를 측정하게 되는 것이다. 각속도는 시간에 누적되는데, 반복적인 누적에 따라 오차가 발생하게 된다. 즉 시간이 지날 수록 오차가 많이 발생하게 되는 것이다. 결과적으로 자이로 센서는 짧은 시간에 대하여 정확한 데이터를 제공하지만 오랜 시간에 대한 데이터는 오류가 발생한다. (반복적 시간 누적에 따른 적분 값인데, 축적할 수록 값에 오류가 발생하는 현상으로 드리프트라 부른다.)

 즉 가속도 센서는 가만히 있으면 정확하지만 움직이면 노이즈에 취약하고, 평균값이 정확하며

 자이로 센서는 순간 순간의 움직임은 정확하지만 장시간에 대하여 오류가 발생하고 평균값은 오류가 있는 두 센서의 보정이 필요하다.

 이 두가지를 합쳐서 값을 보정하는 것을 상보 필터라고 한다. 위 Gekkmonproject 사이트에 보면 이 필터에 대한 계산식이 나온다.

Filtered Angle = α × (Gyroscope Angle) + (1 − α) × (Accelerometer Angle)  α = τ/(τ + Δt)   (Gyroscope Angle) = (Last Measured Filtered Angle) + ω×Δt Δt = sampling rate, τ = time constant greater than timescale of typical accelerometer noise

 위 사이트에서는  α = 0.96 으로 Sampling Rate는 0.04 초로 Time Constant 는 1 초로 하였다고 한다. 가속도 센서의 각도는 다음의 공식으로 구한다. (위 참조 사이트에서 가져 옴)

 

 다음은 위 사이트의 아두이노 코드 중 주요한 사항들을 분석해 보도록 하겠다.

1. calibrate_sensors

void calibrate_sensors() {   int                   num_readings = 10;   float                 x_accel = 0;   float                 y_accel = 0;   float                 z_accel = 0;   float                 x_gyro = 0;   float                 y_gyro = 0;   float                 z_gyro = 0;   accel_t_gyro_union    accel_t_gyro;      //Serial.println("Starting Calibration");   // Discard the first set of values read from the IMU   read_gyro_accel_vals((uint8_t *) &accel_t_gyro);      // Read and average the raw values from the IMU   for (int i = 0; i < num_readings; i++) {     read_gyro_accel_vals((uint8_t *) &accel_t_gyro);     x_accel += accel_t_gyro.value.x_accel;     y_accel += accel_t_gyro.value.y_accel;     z_accel += accel_t_gyro.value.z_accel;     x_gyro += accel_t_gyro.value.x_gyro;     y_gyro += accel_t_gyro.value.y_gyro;     z_gyro += accel_t_gyro.value.z_gyro;     delay(100);   }   x_accel /= num_readings;   y_accel /= num_readings;   z_accel /= num_readings;   x_gyro /= num_readings;   y_gyro /= num_readings;   z_gyro /= num_readings;      // Store the raw calibration values globally   base_x_accel = x_accel;   base_y_accel = y_accel;   base_z_accel = z_accel;   base_x_gyro = x_gyro;   base_y_gyro = y_gyro;   base_z_gyro = z_gyro;      //Serial.println("Finishing Calibration"); }

 가속도 및 자이로 센서의 초기 값을 조정하는 함수이다. 바닥에 그대로 두고, 가속도와 자이로의 평균 값을 구해서 초기 상태의 값을 calibration 한다

2. 자이로 센서를 각속도로 변환하기

 loop 코드에서 자이로 센서의 값을 각속도로 변환하는 코드가 있다.

// Convert gyro values to degrees/sec float FS_SEL = 131; float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro) / FS_SEL; float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro) / FS_SEL; float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro) / FS_SEL;  // Compute the (filtered) gyro angles float dt = (t_now - get_last_time()) / 1000.0; float gyro_angle_x = gyro_x*dt + get_last_x_angle(); float gyro_angle_y = gyro_y*dt + get_last_y_angle(); float gyro_angle_z = gyro_z*dt + get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + get_last_gyro_z_angle();

 앞선 포스팅에서 MPU6050 의 각속도의 각 1도/Sec 은 131 이 된다고 하였다. 따라서 측정되는 값에서 기본 값을 빼고 131로 나누면 값이 나온다.

3. 가속도 센서에 각 측정하기

// Get raw acceleration values //float G_CONVERT = 16384; float accel_x = accel_t_gyro.value.x_accel; float accel_y = accel_t_gyro.value.y_accel; float accel_z = accel_t_gyro.value.z_accel; // Get angle values from accelerometer float RADIANS_TO_DEGREES = 180 / 3.14159; //  float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); float accel_angle_y = atan(-1 * accel_x / sqrt(pow(accel_y, 2) + pow(accel_z, 2)))*RADIANS_TO_DEGREES; float accel_angle_x = atan(accel_y / sqrt(pow(accel_x, 2) + pow(accel_z, 2)))*RADIANS_TO_DEGREES; float accel_angle_z = 0;

4. 상보필터 적용

float alpha = 0.96; float angle_x = alpha*gyro_angle_x + (1.0 - alpha)*accel_angle_x; float angle_y = alpha*gyro_angle_y + (1.0 - alpha)*accel_angle_y; float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle