[드론] 상보필터 처리 루틴 구현하기(상보필터 근사공식 이용)

가속도,자이로 센서에 대한 상보필터 적용시키기

가속도 센서와 자이로 센서의 단점들을 보안하고자 상보필터를 이용해서 값을 추출하는 방법에 대해 한번 알아보도록 하겠습니다. 일단 상보필터를 구하는 근사공식에 대해 알아보겠습니다.

---

상보필터 근사공식

위 공식을 참고해서 상보필터 처리 루틴을 한번 구현해보도록 하겠습니다.

---

소스코드

//상보필터 적용시키기
#include 
   
const int MPU_addr = 0x68;
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
   
void setup() {
  initMPU6050(); //가속도 자이로 센서 값을 읽음
  Serial.begin(115200);
  calibAccelGyro(); //센서 보정 루틴
  initDT();// 시간 간격에 대한 초기화
}
   
void loop() {
  readAccelGyro();
  calcDT(); //시간 간격 계산
  calcAccelYPR(); //가속도 센서 처리 루틴
  calcGyroYPR(); //자이로 센서 처리 루틴 --> 가속도 센서의 값을 해석하기 위해 Roll, Pitch, Yaw에 대한 각도 구하는 함수
  calcFilteredYPR(); //상보필터 적용
 
  static int cnt;
  cnt++;
  if(cnt%2==0)
    SendDataToProcessing(); //Roll, Pitch, Yaw에 대한 각도 정보를 보냄
}
  
//MPU-6050초기화 루틴
void initMPU6050()
{
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}
  
 //가속도 자이로 센서를 읽는 루틴
void readAccelGyro()
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
}
  
float dt;
float accel_angle_x, accel_angle_y, accel_angle_z;
float gyro_angle_x, gyro_angle_y, gyro_angle_z;
float filtered_angle_x, filtered_angle_y, filtered_angle_z;
float baseAcX, baseAcY, baseAcZ;
float baseGyX, baseGyY, baseGyZ;
  
//센서 들의 기본값들의 평균을 내야하는 루틴(센서 보정 루틴)
void calibAccelGyro() 
{
  float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
  float sumGyX = 0, sumGyY = 0, sumGyZ = 0;
  
  //가속도 자이로 센서를 읽어드림
  readAccelGyro();
 
  //읽어드렸으면 이제 읽어드린 값을 토대로 평균값을 구하면 됨
  for(int i=0; i<10; i++)
  {
    readAccelGyro();
    sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
    sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
    delay(100);//0.1초
  }
  //맨 처음 기본 센서 값들을 보여지고 그다음에 평균값을 구하는 함수
  baseAcX = sumAcX / 10;
  baseAcY = sumAcY / 10;
  baseAcZ = sumAcZ / 10;
  
  baseGyX = sumGyX / 10;
  baseGyY = sumGyY / 10;
  baseGyZ = sumGyZ / 10;
}
  
 //프로세싱 스케치로 각도 정보를 보내는 루틴
void SendDataToProcessing()
{
  Serial.print(F("DEL:"));
  Serial.print(dt,DEC);
  Serial.print(F("#ACC:"));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angle_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(gyro_angle_x, 2);
  Serial.print(F(","));
  Serial.print(gyro_angle_y, 2);
  Serial.print(F(","));
  Serial.print(gyro_angle_z, 2);
  Serial.print(F("#FIL:"));
  Serial.print(filtered_angle_x, 2);
  Serial.print(F(","));
  Serial.print(filtered_angle_y, 2);
  Serial.print(F(","));
  Serial.print(filtered_angle_z, 2);
  Serial.println(F(""));
  delay(5);
}
 
unsigned long t_now;
unsigned long t_prev;
 
void initDT(){
  t_prev = millis();
}
 
void calcDT(){
  t_now = millis();
  dt = (t_now - t_prev) / 1000.0;
  t_prev = t_now;
}
 
void calcAccelYPR() //가속도 센서 처리 루틴
{
  float accel_x, accel_y, accel_z; //x, y, z 축에 대한 각도 저장 변수
  float accel_xz, accel_yz;
  const float RADIANS_TO_DEGREES = 180 / 3.14159;
 
  accel_x = AcX - baseAcX;
  accel_y = AcY - baseAcY;
  accel_z = AcZ + (16384 - baseAcZ);
 
  accel_yz = sqrt(pow(accel_y, 2) + pow(accel_z, 2));
  accel_angle_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
 
  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angle_y = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;
 
  accel_angle_z = 0;
}

float gyro_x, gyro_y, gyro_z;

void calcGyroYPR()
{
  const float GYROXYZ_TO_DEGREES_PER_SEC = 131; // 각속도를 저장하는 변수

  gyro_x = (GyX - baseGyX) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_y = (GyY - baseGyY) / GYROXYZ_TO_DEGREES_PER_SEC;
  gyro_z = (GyZ - baseGyZ) / GYROXYZ_TO_DEGREES_PER_SEC;

  gyro_angle_x += gyro_x * dt;
  gyro_angle_y += gyro_y * dt;
  gyro_angle_z += gyro_z * dt;  
}

void calcFilteredYPR()
{
   const float ALPHA = 0.96;
   float tmp_angle_x, tmp_angle_y, tmp_angle_z; //임시 각도

   tmp_angle_x = filtered_angle_x + gyro_x * dt; // 이전 보정 각도 + 현재 자이로 센서를 이용해 얻은 각도(gyro_x * dt)
   tmp_angle_y = filtered_angle_y + gyro_y * dt;
   tmp_angle_z = filtered_angle_z+ gyro_z * dt;

   filtered_angle_x = ALPHA * tmp_angle_x + (1.0 - ALPHA) * accel_angle_x;
   filtered_angle_y = ALPHA * tmp_angle_y + (1.0 - ALPHA) * accel_angle_y;
   filtered_angle_z = tmp_angle_z;
}

다운로드 파일(2016.09.10)

sketch_sep10c.ino

+

프로세싱 파일

ShowGY521Data.pde

---

해석

#1 calcFilteredYPR() 함수를 loop구문에다가 적용시키기!

#2 tmp_angel_x는 무엇인가?

상보필터 근사 공식에서 보면 (이전 필터 각도 + 자이로로 얻은 각도)라고 나와 있습니다. 이 부분이 바로 tmp_angel_x y z 변수에 대항하는 부분입니다.

자이로 센서를 이용해서 얻은 각속도를 단위시간을 곱해주는 이유는 적분을 해서 자이로 값(각도)을 얻기 위한 것 입니다.

# filtered_angle_x

상보필터 근사 공식을 적용시킨 것 입니다.

근데 가만 보면 filtered_angle_z만 자이로 센서만을 이용하고 있는데요 이것은 기준 축이 Z축이여서 그런거 같습니다.