[드론] 가속도 센서를 이용한 각도 구하기_소스코드

가속도 센서를 이용한 각도 구하기

[기본 개념]

MPU-6050 가속도 자이로 센서

[처리루틴]

기본 루틴 구현하기

센서 보정 루틴 구현하기

시간 처리 루틴 구현하기

이번에는 가속도 센서를 처리해서 Roll, Pitch, Yaw의 각도를 구해보는 시간을 가지도록 하겠습니다.

일반적으로 기본 루틴 구현하기에서 구한 AcX, AcY, AcZ도 동일한 가속도 센서 값 입니다. 그러나 여기 소스코드에 보시면 아시겠지만 별도로 계산을 해서 accel_x, accel_y, accel_z의 값들을 구하는걸 볼 수 있을 것입니다.

그 이유는 MPU-6050의 가속도 센서를 이용한 드론을 지면 수평한 상태에 놓으면 미세한 떨림에 의해서 부유한 값을 얻게 됩니다. 그러한 현상들을 해결 하기 위해서 가속도 센서의 현재 위치하고 있는 드론의 상태에 따라 얻어지는 현재값 에다가 '센서 보정 루틴'해서 작성을 했던 초기 평균값들을 빼서 가속도 센서 값에 대한 최종적이 보정 값을 구하는 목적 입니다.

---

소스코드

//가속도 센서 처리 루틴
#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(); //가속도 센서 처리 루틴

  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;
}

다운로드 파일(2016.09.10)

sketch_sep10c.ino

---

코드 설명

loop구문에 calcAccelYPR() 이라는 제가 별도로 만들어준 함수를 선언해주었습니다.

loop 구문에다가 선언을 한 이유는, 매번 가속도 센서의 값을 해석해서 Roll, Pitch, Yaw에 대한 각도를 구하는 함수인데 setup()에다가 선언을 해버리면 맨 처음에만 해석을 하고 그 이후로는 안하게 됩니다. 이렇게 되면 드론을 조정하기 거의 불가능 해서 loop구문에다가 설정을 한것입니다.

그리고 아래칸에다가 진짜 가속도 처리 센서 함수를 표현해주었습니다.

해석을 자세하게 해보겠습니다.(너무 자세하게는 말고..ㅎㅎ)

127 : 위에서도 말씀 드렸다싶이 가속도 센서에 대한 최정적인 보정 값을 저장하는 변수를 선언하는 부분입니다.

128 : 드론이 기울어짐에 따라서 얻어지는 각도를 저장하는 변수 입니다.

여기 드론에서는 +Z축이 하늘을 바라보고 있으면 +X축은 비행기 머리부분 +Y축은 날개부분입니다.

[조건]  1. 직선축 +X는 비행기의 머리부분  2. 직선축 +Y는 비행기의 날개부분  accel_yz : 직선축 +X 기준으로 좌 우로 기울어 지는 각도를 저장하는 변수 accel_angel_y : Roll의 각도  accel_xz : 직선축 +Y 기준으로 비행기 머리가 위 아래로 기울어 지는 각도를 저장하는 변수 accel_angel_x = Pitch의 각도

129 : 이따 축에 따라 기울어진 각도를 구할때 사용되는 값들을 변수로 선언을 해주었습니다.

131 ~ 132 : 가속도 X축과 Y축에 대한 현재 값(AcX or AcY)에서 가속도 센서의 평균값(baseAcX or baseAcY)을 빼주고 있습니다. 이 경우는 가속도 센서에 대한 최종적인 보정 값을 저장해 줍니다.

133 : 왜 여기만 x,y축과 다르냐고요?? 그 이유는 이상적인 기본 루틴에서 얻은 AcZ(가속도 센서 Z값_의 값이 중력 가속도 g에 해당하는 값인 16384입니다. 그러나 말 그대로 이상적인 값이지  실제로는 +-얼마의 오차값의 범위를 가지고 있스빈다. 따라서 이러한 경우에 대해 보정을 해주고 있는 것입니다.

135~136 : 직선축 +X이 기울어진 각도를 구하고 있습니다.

직선 +X축이 곡선 +Y축에 따라 기울어진 각도를 구하는 공식[angel(Y) : Pitch값]

138 ~ 139 : 직선 +Y축이  기울어진 각도를 구하고 있습니다.

직석 +Y축이 곡선 +X축에 따라 기울어진 각도를 구하는 공식[angel(X) : Roll값]

141 : 현재 드론은 지면이 평평한 면에 둘 경우로 가정하고 하고 있습니다. 그러면 직선 +Z축이 중력 가속도와 정 반대방향이므로 중력 가속도는 양수의 값을 얻게 되지만 +Z축을 중심으로 한 회전각을 구할 수 없게 됩니다.

그래서 변수를 0으로 저장한 것 입니다.