[드론] 자이로 센서 처리 루틴 구현하기

자이로 센서 처리 루틴 구현

먼저 참고해야할 포스팅

[드론]자이로 센서 개념정리

[드론]자이로 센서 값 해석하기

이번 포스팅 목적

: 자이로 값을 이용해서 Roll, Pitch, Yaw의 각도를 구하는 루틴을 추가하기

---

소스코드

//자이로 센서 처리 루틴 구현
#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에 대한 각도 구하는 함수
 
  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_angel_x, accel_angel_y, accel_angel_z;
float gyro_angel_x, gyro_angel_y, gyro_angel_z;
float filtered_angel_x, filtered_angel_y, filtered_angel_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_angel_x, 2);
  Serial.print(F(","));
  Serial.print(accel_angel_y, 2);
  Serial.print(F(","));
  Serial.print(accel_angel_z, 2);
  Serial.print(F("#GYR:"));
  Serial.print(gyro_angel_x, 2);
  Serial.print(F(","));
  Serial.print(gyro_angel_y, 2);
  Serial.print(F(","));
  Serial.print(gyro_angel_z, 2);
  Serial.print(F("#FIL:"));
  Serial.print(filtered_angel_x, 2);
  Serial.print(F(","));
  Serial.print(filtered_angel_y, 2);
  Serial.print(F(","));
  Serial.print(filtered_angel_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_angel_y = atan(-accel_x / accel_yz)*RADIANS_TO_DEGREES;
 
  accel_xz = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
  accel_angel_x = atan(accel_y / accel_xz)*RADIANS_TO_DEGREES;
 
  accel_angel_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_angel_x += gyro_x * dt;
  gyro_angel_y += gyro_y * dt;
  gyro_angel_z += gyro_z * dt;  
}

다운로드 파일(2016.09.10)

sketch_sep10c.ino

---

해석

이 두 부분을 추가를 해주면 됩니다.

제가 이전에 말했던 내용이지만 다시 한번 언급을 하고 넘어갈게요

자이로 센서 

장점 : 매 순간 값은 정확함

단점 : 평균적인 값은 믿을 수 없다.

가속도 센서

장점 : 평균적인 값은 믿을 수 있다.

단점 : 매 순간 데이터는 확실하지 않다.

따라서 각 단점을 보안하기 위해서 상보필터라는 개념을 적용한다고 말을 했습니다.

[참고 포스팅]

상보필터란 무엇일까?

상보필터를 적용한 센서 보정 루틴 구현하기

#1 왜 calcGyroYPR 함수를 loop함수에 구현을 해주었을까?

: 그 이유는 loop구문은 매번 반복을 하는 무한loop이니까 매번 자이로값을 이용해서 Roll, Pitch, Yaw 각도를 구해야 해서 loop() 함수에 적용을 시켜준 것 입니다.

#2 자이로값(GYROXYZ_TO_DEGREES_PER_SEC)을 왜 131로 해주었을까?

위 표를 누르셔서 해당 포스팅이 적혀있는 링크를 들어가시면 참고를 하실 수 있습니다.

#3 각속도로 변환하기

위에 수식을 보시면 현재 각속도[ex) +-250'/s]를 구하는 수식 입니다. 각속도를 저장하는 전역변수(gyro_x)에 (현재 자이로 센서값 - 초기 자이로센서 평균 값)을 하고 각속도당 자이로 센서 값으로 나누어주어서 센서 값에 대한 보정을 해준 후 각속도를 저장하는 변수(gyro_x)에 저장하여 각속도로 변환을 해주 고 있는 작업입니다.

#4 현재 각 속도에 단위 시간을 곱해서 누적

1초당 250'를 돌게 된다면 자이로 값은 131입니다. 근데 매번 드론이 1초당 일정하게 얼마의 속도로 회전하라는 법은 없습니다. 따라서 매번 단위 시간을 각 속도에 곱해주는 것 입니다.

---

초기 가속도 & 자이로센서

자이로 센서의 장점이 보임

자이로 센서의 단점이 보임 [현재 가속도 센서를 보니까 지면에 드론이 위치하고 있음을 할 수 있는데 자이로 센서는 이상한 값을 보여주고 있음]

정리를 해보자면...

자이로 센서의 최종 목표는 회전하는 속도(자이로 값)을 적분해서 Roll, Pitch, Yaw에 대한각도를 얻는 것 입니다.

그래서 센서에 대한 보정을 한 각속도를 구해서 그 가속도를 단위 시간을 곱해서 적분을 한 후 각도를 구하는 과정 입니다.

제가 공부하면서 느낀점을 적은것인데 틀린점이 있다면 언제든지 피드백을 해주시면 감사하겠습니다^^