가속도계(Accelerometer)는 움직임이 있을 때마다 값이 튈 수 밖에 없는데 이는 움직임에 따른 가속도와 중력가속도가 합해져 마치 중력가속도 방향이 변한 것처럼 보이기 때문입니다. 이러한 고주파 잡음을 제거하기 위해서는 저역 통과필터(Low pass filter; LPF)를 사용해야 합니다. 반면에 자이로(Gyroscope)는 잡음이 긴 시간에 대해서 누적(적분)되어 드리프트(혹은 바이어스)되므로 짧은 시간인 고역 통과필터(High pass filter; HPF)를 사용해야 합니다. 이러한 아이디어를 구체화한 것이 아래의 블록선도와 같으며 이를 IMU(Inertia Measurement Unit) 데이터를 융합하는데 가장 간단한 방법인 보상필터(Complementary filter) 혹은 상보필러라 부릅니다.




여기서 과 는 각각 1차 저역 통과필터, 1차 고역 통과필터로 그 합은 안정성의 문제로 '1'이 성립되어야만 합니다. 2차 필터도 구현할 수 있지만 편의상 1차 필터로만 제한합니다. 위 그림으로부터 수식을 정리하면 다음과 같습니다.



여기서 는 차단주파수(cut off frequency)를 결정합니다. 위 식을 정리하면 다음과 같습니다.



위 수식으로 블록선도를 다시 그리면 다음과 같습니다.



위 수식을 MCU로 처리하기 위해서는 연속시간에 대한 이산화가 필요하고 하드웨어 상에 아날로그 혹은 디지털 필터(예를 들어 IIR, FIR 등)를 사용하지 않는 이상 코드로 구현해야 합니다. MCU 성능 개선으로 외장 디지털 필터나 디지털 필터 알고리즘을 사용하지 않고 수식을 이산화하여 구현하였습니다.


동일한 연속 시간 전달 함수(continous time transfer function) G(s)를 이산화하는 방법에는 여러가지가 있으며 안정도와 고주파 잡음 등과 같은 성능이 동일하지는 않습니다. 전형적으로 연속 시간 전달 함수(continous time transfer function) G(s)를 이산(불연속 시간 전달 함수(discrete time transfer function) H(z)로 변환하는 방법은 전형적으로 다음과 같은 방법이 있습니다.


1) 후방 차분법(backward difference) : 

2) Bilinear transformation, expansion of ln(z) : 

3) Impulse invariance transformation : 


가장 기본적인 후방 차분법(backward difference)을 적용하면 이므로 이를 대입하여 다시 정리하면 추정 각도 는 다음과 같이 나타낼 수 있습니다.



여기서 편의상 가속계인 accelerometer의 첫글자 , 자이로 측정치로부터의 각속도(rotation rate)라 놓았습니다.


위 식에 z 변환인 은 이산 시스템에서 이므로 위 식을 이산 신호 시스템으로 변경하면 다음과 같습니다. 다른 말로 는 단위 시간 지연(unit time delay)과도 같기 때문입니다.



위 식에서 이면 다음과 같습니다.



정성적으로 위 수식에서 만일 가속도계에 측정치가 튄다면 즉, 고주파 잡음이 들어갔다면, 0.01이 곱하여져 새로운 각도가 계산됩니다. 그러나 0.01이 곱하여지므로 기여도는 작아 각도는 프로그램 루프(loop)가 상당히 반복되어도 매우 천천히 증가하게 됩니다. 이는 가속도계 입력이 추정 각도 계산에 실시간으로 반영되기 어렵고 이는 출력이 반응하지 않는다는 의미로 고주파 잡음은 제거되고 서서히 증감되는 저주파 측정치만 반영되게 된다는 의미입니다.


는 weight factor라 부르며 통상 0.95~0.98 범위의 값을 사용합니다.



Appendix A.


Reference:

http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf

http://www.olliw.eu/2013/imu-data-fusing/#chapter21

http://www.dummies.com/education/science/signals-and-systems-working-with-transform-theorems-and-pairs/(z-transform)




Posted by Nature & Life
Embedded Lecture/Arduino2017. 11. 30. 18:04


아두이노에서 기본적으로 제공하는 Wire 라이브러리를 이용해도 되지만 I2C 라이브러리를 이용하여 MPU6050의 X축, Y축 그리고 Z축의 기울기를 구하였습니다.


i2c.ino

const uint8_t IMUAddress = 0x68; // AD0 is logic low on the PCB

const uint16_t I2C_TIMEOUT = 1000; // Used to check for errors in I2C communication


uint8_t i2cWrite(uint8_t registerAddress, uint8_t data, bool sendStop) {

  return i2cWrite(registerAddress,&data,1,sendStop); // Returns 0 on success

}


uint8_t i2cWrite(uint8_t registerAddress, uint8_t* data, uint8_t length, bool sendStop) {

  Wire.beginTransmission(IMUAddress);

  Wire.write(registerAddress);

  Wire.write(data, length);

  return Wire.endTransmission(sendStop); // Returns 0 on success

}


uint8_t i2cRead(uint8_t registerAddress, uint8_t* data, uint8_t nbytes) {

  uint32_t timeOutTimer;

  Wire.beginTransmission(IMUAddress);

  Wire.write(registerAddress);

  if(Wire.endTransmission(false)) // Don't release the bus

    return 1; // Error in communication

  Wire.requestFrom(IMUAddress, nbytes,(uint8_t)true); // Send a repeated start and then release the bus after reading

  for(uint8_t i = 0; i < nbytes; i++) {

    if(Wire.available())

      data[i] = Wire.read();

    else {

      timeOutTimer = micros();

      while(((micros() - timeOutTimer) < I2C_TIMEOUT) && !Wire.available());

      if(Wire.available())

        data[i] = Wire.read();

      else

        return 2; // Error in communication

    }

  }

  return 0; // Success

}


Kalman.h

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics-> All rights reserved->

 This software may be distributed and modified under the terms of the GNU

 General Public License version 2 (GPL2) as published by the Free Software

 Foundation and appearing in the file GPL2->TXT included in the packaging of

 this file-> Please note that GPL2 Section 2[b] requires that all works based

 on this software must also be made publicly available under the terms of

 the GPL2 ("Copyleft")->


 Contact information

 -------------------

 Kristian Lauszus, TKJ Electronics

 Web      :  http://www->tkjelectronics->com

 e-mail   :  kristianl@tkjelectronics->com

 */


#ifndef _Kalman_h

#define _Kalman_h


struct Kalman {

    /* Kalman filter variables */

    double Q_angle; // Process noise variance for the accelerometer

    double Q_bias; // Process noise variance for the gyro bias

    double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise


    double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector

    double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector

    double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate


    double P[2][2]; // Error covariance matrix - This is a 2x2 matrix

    double K[2]; // Kalman gain - This is a 2x1 vector

    double y; // Angle difference

    double S; // Estimate error

};


void Init(struct Kalman* klm){

    /* We will set the variables like so, these can also be tuned by the user */

    klm->Q_angle = 0.001;

    klm->Q_bias = 0.003;

    klm->R_measure = 0.03;


    klm->angle = 0; // Reset the angle

    klm->bias = 0; // Reset bias

   klm->P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en->wikipedia->org/wiki/Kalman_filter#Example_application->2C_technical

    klm->P[0][1] = 0;

    klm->P[1][0] = 0;

    klm->P[1][1] = 0;

}


// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds

double getAngle(struct Kalman * klm, double newAngle, double newRate, double dt) {

    // KasBot V2  -  Kalman filter module - http://www->x-firm->com/?page_id=145

    // Modified by Kristian Lauszus

   // See my blog post for more information: http://blog->tkjelectronics->dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it


    float P00_temp;

    float P01_temp;


    // Discrete Kalman filter time update equations - Time Update ("Predict")

    // Update xhat - Project the state ahead


   /* Step 1 */

    klm->rate = newRate - klm->bias;

    klm->angle += dt * klm->rate;


    // Update estimation error covariance - Project the error covariance ahead


    /* Step 2 */

    klm->P[0][0] += dt * (dt*klm->P[1][1] - klm->P[0][1] - klm->P[1][0] + klm->Q_angle);

    klm->P[0][1] -= dt * klm->P[1][1];

    klm->P[1][0] -= dt * klm->P[1][1];

    klm->P[1][1] += klm->Q_bias * dt;


    // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")

    // Calculate Kalman gain - Compute the Kalman gain


    /* Step 4 */

    klm->S = klm->P[0][0] + klm->R_measure;


    /* Step 5 */

    klm->K[0] = klm->P[0][0] / klm->S;

    klm->K[1] = klm->P[1][0] / klm->S;


    // Calculate angle and bias - Update estimate with measurement zk (newAngle)


    /* Step 3 */

    klm->y = newAngle - klm->angle;

 

   /* Step 6 */

    klm->angle += klm->K[0] * klm->y;

    klm->bias += klm->K[1] * klm->y;


    // Calculate estimation error covariance - Update the error covariance


    /* Step 7 */

    P00_temp = klm->P[0][0];

    P01_temp = klm->P[0][1];


    klm->P[0][0] -= klm->K[0] * P00_temp;

    klm->P[0][1] -= klm->K[0] * P01_temp;

    klm->P[1][0] -= klm->K[1] * P00_temp;

    klm->P[1][1] -= klm->K[1] * P01_temp;

 

   return klm->angle;

}

// Used to set angle, this should be set as the starting angle

void setAngle(struct Kalman* klm, double newAngle) { klm->angle = newAngle; }


// Return the unbiased rate

double getRate(struct Kalman* klm) { return klm->rate; }

 

/* These are used to tune the Kalman filter */

void setQangle(struct Kalman* klm, double newQ_angle) { klm->Q_angle = newQ_angle; }


/* Default value is (0.003f), raise this to follow input more closely, lower this to smooth result of kalman filter */

void setQbias(struct Kalman* klm, double newQ_bias) { klm->Q_bias = newQ_bias; }

void setRmeasure(struct Kalman* klm, double newR_measure) { klm->R_measure = newR_measure; }

double getQangle(struct Kalman* klm) { return klm->Q_angle; }

double getQbias(struct Kalman* klm) { return klm->Q_bias; }

double getRmeasure(struct Kalman* klm) { return klm->R_measure; }

#endif


mpu6050.ino

/* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.

This software may be distributed and modified under the terms of the GNU

General Public License version 2 (GPL2) as published by the Free Software

Foundation and appearing in the file GPL2.TXT included in the packaging of

this file. Please note that GPL2 Section 2[b] requires that all works based

on this software must also be made publicly available under the terms of

the GPL2 ("Copyleft").

Contact information

-------------------

Kristian Lauszus, TKJ Electronics

Web : http://www.tkjelectronics.com

e-mail : kristianl@tkjelectronics.com

Updated by Joe YOON in 2017.12.02

*/


#include <Wire.h>

#include "Kalman.h"


struct kalman Kal_struct_X, Kal_struct_Y; // Create the Kalman instances


/* IMU Data */

int16_t accX, accY, accZ; // 3-axis accelerometer

int16_t tempRaw;

int16_t gyroX, gyroY, gyroZ; // 3-axis gyroscope


double accXangle, accYangle; // Angle calculate using the accelerometer

double temp; // Temperature

double gyroXangle, gyroYangle; // Rate calculate using the gyro

double compAngleX, compAngleY; // Calculate the angle using a complementary filter

double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter


uint32_t timer;

uint8_t i2cData[14]; // Buffer for I2C data


void setup() {

  Init(&Kal_struct_X); // Initialize Kalman filter for X-axis

  Init(&Kal_struct_Y); // Initialize Kalman filter for Y-axis

  Serial.begin(9600);

  Wire.begin();

  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz

  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling

  i2cData[2] = 0x00; // Set Gyro Full Scale Range to +/-250[deg/s]

  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to +/-2[g]

  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once

  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode

  while(i2cRead(0x75,i2cData,1));

  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register

    Serial.print(F("Error reading sensor"));

    while(1);

  }

  

  delay(100); // Wait for sensor to stabilize

  

  /* Set kalman and gyro starting angle */

  while(i2cRead(0x3B,i2cData,6));

  accX = ((i2cData[0] << 8) | i2cData[1]);

  accY = ((i2cData[2] << 8) | i2cData[3]);

  accZ = ((i2cData[4] << 8) | i2cData[5]);


  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2

  // We then convert it to 0 to 2? and then from radians to degrees

  accYangle = atan2(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;

  accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;

   

  setAngle(&Kal_struct_X, accXangle); // Set starting angle

  setAngle(&Kal_struct_Y, accYangle);


  gyroXangle = accXangle;

  gyroYangle = accYangle;

  compAngleX = accXangle;

  compAngleY = accYangle;

  

  timer = micros();

}


void loop() {

  /* Update all the values */

  while(i2cRead(0x3B,i2cData,14));

  accX = ((i2cData[0] << 8) | i2cData[1]);

  accY = ((i2cData[2] << 8) | i2cData[3]);

  accZ = ((i2cData[4] << 8) | i2cData[5]);

  tempRaw = ((i2cData[6] << 8) | i2cData[7]);

  gyroX = ((i2cData[8] << 8) | i2cData[9]);

  gyroY = ((i2cData[10] << 8) | i2cData[11]);

  gyroZ = ((i2cData[12] << 8) | i2cData[13]);

  

  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2

  // We then convert it to 0 to 2? and then from radians to degrees

  accYangle = atan2(-1*accX/sqrt(pow(accY,2) + pow(accZ,2)))*RAD_TO_DEG;

  accXangle = atan2(accY/sqrt(pow(accX,2) + pow(accZ,2)))*RAD_TO_DEG;

   

  double gyroXrate = (double)gyroX/131.0;

  double gyroYrate = -((double)gyroY/131.0);

  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter

  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);

  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate

  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);

  

  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter

  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);

  

  kalAngleX = getAngle(&Kal_struct_X, accXanglegyroXrate(double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter

  kalAngleY = getAngle(&Kal_struct_Y, accYanglegyroYrate(double)(micros()-timer)/1000000);

  timer = micros();

  

  temp = ((double)tempRaw + 12412.0) / 340.0;

  

  /* Print Data */

  display_formatted_float(accX, 5, 0, 3, false);

  display_formatted_float(accY, 5, 0, 3, false);

  display_formatted_float(accZ, 5, 0, 3, false);

  display_formatted_float(gyroX, 5, 0, 3, false);

  display_formatted_float(gyroY, 5, 0, 3, false);

  display_formatted_float(gyroZ, 5, 0, 3, false);


  Serial.print("\t");


  display_formatted_float(accXangle, 5, 2, 3, false);

  display_formatted_float(gyroXangle, 5, 2, 3, false);

  display_formatted_float(compAngleX, 5, 2, 3, false);

  display_formatted_float(kalAngleX, 5, 2, 3, false);


  Serial.print("\t");


  display_formatted_float(accYangle, 5, 2, 3, false);

  display_formatted_float(gyroYangle, 5, 2, 3, false);

  display_formatted_float(compAngleY, 5, 2, 3, false);

  display_formatted_float(kalAngleY, 5, 2, 3, false);


  //Serial.print(temp);Serial.print("\t");


  Serial.print("\r\n");

  delay(1);

}


void display_formatted_float(double val, int characteristic, int mantissa, int blank, boolean linefeed) {

  char outString[16];

  int len;


  dtostrf(val, characteristic, mantissa, outString);

  len = strlen(outString);

  for(int i = 0; i < ((characteristic+mantissa+blank)-len); i++) Serial.print(F(" "));

  Serial.print(outString);

  if(linefeed)

    Serial.print(F("\n"));

}


Posted by Nature & Life


아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783


Step 1)



위 식에서 각도(angle)의 추정치 는 이전 상태의 추정치 에 bias 되지 않은 각속도(rate)에 곱하기 미소 시간 를 더한 것과 같습니다. bias 되지 않은 각속도(rate)에 곱하기 미소 시간 은 결국 드리프트 되지 않은 각도의 증분이 됩니다. 또한 좌측항은 아직 보정되지 않았음을 기억합니다.

게다가 우리는 bias를 직접 측정할 수 없기 때문에 bias의 추정치는 이전 것을 사용합니다.


이는 다음과 같이 C 언어로 쓸 수 있습니다.

rate = newRate - bias;

angle += dt * rate;


Step 2)



위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);

P[0][1] -= dt * P[1][1];

P[1][0] -= dt * P[1][1];

P[1][1] += Q_gyroBias * dt;


Step 3)



참고로 우측에 현재 상태변수는 보정되지 않았기 때문에 angle 변수를 그대로 사용합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.


y = newAngle - angle;


Step 4)




위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

S = P[0][0] + R_measure;


Step 5)



다른 경우에 S는 행렬이 될 수 있습니다. 그 경우에는 여러분은 간단히 S로 P를 나눌 수 없으며 역행렬을 구해서 곱해야 합니다.

위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

K[0] = P[0][0] / S;

K[1] = P[1][0] / S;


Step 6)



위 식은 다음과 같이 C 언어로 쓸 수 있습니다.

angle += K[0] * y;

bias += K[1] * y;


Step 7)



상태 추정 오차가 감소되었기 때문에 오차 공분산 행렬을 다시 감소시킴을 기억하세요.

C 코드는 다음과 같습니다.

float P00_temp = P[0][0];

float P01_temp = P[0][1];


P[0][0] -= K[0] * P00_temp;

P[0][1] -= K[0] * P01_temp;

P[1][0] -= K[1] * P00_temp;

P[1][1] -= K[1] * P01_temp;


참고로 대부분의 IMU에 대해서 다음의 변수들이 완벽하게 동작합니다.

float Q_angle = 0.001;

float Q_gyroBias = 0.003;

float R_measure = 0.03;


초기치로서 각도를 설정하는 것을 기억하세요 왜냐하면 필터가 안정화되는데 시간이 걸리기 때문입니다. 반대로 칼만 필터가 안정화되기 전까지는 상태의 추정치를 믿을 수 없습니다.



Posted by Nature & Life


3축 가속도와 3축 자이로 센서를 조합한 후 각각의 센서 출력을 내보내는 장치를 관성측정장치(IMU; Inertial Measurement Unit)이라고 부릅니다. 스마트폰에도 탑재되어 있을 정도로 그 용도가 근래에 흔하며, 비행기의 항법 장치에 필수적인 요소입니다. 기체의 자세 제어에 요구되는 롤(Roll), 피치(Pitch), 요(Yaw)의 기울어진 각도를 알기 위함으로, 롤은 좌우로 기울어짐, 피치는 앞뒤로 기울어짐, 요는 z축 방향으로 기울어짐(회전각)을 의미합니다. 여기서 롤과 피치는 중력방향을 기준으로 얼마나 기울어져 있는지를 나타냅니다.



이처럼 롤과 피치 그리고 요와 같은 자세 측정을 위해서는 필요한 장치가 자이로와 가속도 센서입니다.


MEMS 기반의 3축 가속도 센서(Accelerometer)는 x축, y축, z축 방향의 가속도를 측정할 수 있으며 단위는 [g]입니다. 가속도 센서는 정지한 상태에서 중력 가속도를 감지하기 때문에 z축 방향으로 -g 만큼의 값을 출력합니다. 센서의 초기 출력은 모두 '0'이라고 가정하고 센서를 y축 기준으로 45도 기울여 보면, 기울어진 상태에서 z축 방향과 x축 방향으로 동일한 값의 가속도가 측정되며 중력방향으로 g가 측정되어야 하므로 0.707g 만큼 z축과 x축 방향으로 값이 출력됩니다. 결과적으로는 z축과 x축 값의 비율을 arctan으로 계산하여 기울어진 값을 구할 수 있습니다.



그러나 정지 상태가 아닌 움직이는 가속 상태의 경우, 또다른 힘의 영향으로 중력 방향이 변한 것처럼 올바른 값을 얻을 수가 없습니다. 즉, 정지하지 않은 움직임 상태에서는 가속도 센서만으로 기울기 값을 측정할 수는 없습니다. 뿐만 아니라 z축상에 회전각인 요는 중력 방향(중력가속도)이 전혀 변하지 않으므로 측정이 불가능합니다.


3축 자이로 센서(Gyroscope)는 가속도를 측정하는 가속도 센서와 달리 각속도를 측정하므로 단위는 [degree/sec]입니다. 자이로는 각속도를 재는 장치이기에 이를 이용해서 각도를 알려면 전체 시간에 대하여 적분을 하여 얻게 됩니다. 그러나 센서에서 측정되는 각속도는 노이즈나 어떠한 이유에 의해 측정값에 에러가 계속 생기는데, 이 오차가 적분시에는 누적이 되어서 최종 값이 드리프트 되는 현상이 발생합니다. 게다가 시간이 지날수록 이 오차는 커져 각도가 변하게 된다는 것입이다.


결과적으로 정지상태의 긴 시간의 관점에서 보면 가속도 센서에 의해 계산된 각도는 올바른 값을 보여주지만, 자이로 센서에서는 시간이 지날 수록 틀린 값을 내보내게 됩니다. 반대로 움직이는 짧은 시간의 관점에서 보면 자이로 센서는 올바른 값을 보여주지만 가속도 센서는 다른 값을 내보내게 됩니다.


그러므로 가속도 센서와 자이로 센서를 모두 사용해서 각각의 단점을 보상할 수 있는 알고리즘을 적용하여 롤 또는 피치 값을 계산하여야 한다는 것입니다. 많이 사용하는 보상 방법 및 필터링으로는 칼만 필터(Kalman filter)의 적용입니다.


요의 회전축은 z축방향, 즉 중력방향과 같으므로 가속도 센서가 아닌 자이로 센서의 z축 값을 측정해서 이 값을 이용해 요값을 계산하여야 하고 드리프트되는 오차를 보상하는 다른 센서를 추가적으로 사용하는데, 이것이 지자기 센서(magnetometer or compass)입니다. 자이로는 온도가 변하면 그 값이 같이 변하는 특성이 있어 정교한 측정을 위해서는 온도 센서도 함께 사용해서 오차를 보상하기도 합니다. 이를 모두 고려하면 3축 가속도 센서, 3축 자이로 센서, 3축 지자기 센서를 내장한 IMU 센서를 9축 센서라 부르는 이유에서 입니다.



위 그림의 칩은 Invensense사의 9DOF IMU인 MPU-9150입니다. 이는 3축 자이로 센서와 3축 가속도 센서인 MPU-6050과 3축 지자기 센서(digital compass)를 One Pakage(SiP)하였으며, I2C(TWI)를 지원합니다. 여기서 DOF는 'Degree Of Freedom'의 약자로 '축(Axis)'을 의미합니다.



'Flight Controller 이해 > 센서' 카테고리의 다른 글

MPU6050 센서  (1) 2017.12.01
드론에 요구되는 각종 센서들  (0) 2017.02.26
Posted by Nature & Life


각종 센서들이 측정한 상태측정치들은 각 센서들 고유의 오차 및 잡음이 포함되기 때문에 비행제어기에서 바로 사용할 수 없습니다. 센서융합기는 자이로 센서, 가속도 센서 및 지자기 센서가 측정한 드론의 회전운동 상태측정치와 GPS 수신기 및 기압 센서가 측정한 드론의 병진운동 상태측정치들을 적절히 융합하여 각종 오차 및 잡음이 최소화 된 상태추정치를 계산합니다.


자이로 센서(Gyroscope)

관성측정장치(IMU) 내부에 있는 3축 자이로 센서를 이용해 드론 기체좌표 x, y, z 세 축이 지구관성좌표에 대하여 회전하는 각속도를 측정한 후 고정좌표로 변환된 값(Wx.gyro, Wx.gyro, Wx.gyro)을 계산합니다. 자이로 센서 측정치는 선형 미분방정식을 이용해 오일러 각도 (Φgyro, ϴgyro, ψgyro)로 변환될 수 있다. 자이로 센서 측정치는 저주파수 대역에서 바이어스(bias) 오차를 포함하기 때문에(즉, 드론이 정지해 있을 때에도 자이로 센서의 측정치가 '0'이 되지 않음) x, y, z 세 축에 대한 자이로 센서의 바이어스 오차가 제거되어야 합니다.


가속도 센서(Accelerometer)

자이로 센서의 스코프의 오차를 제거하기 위해 가속도 센서가 이용됩니다. 관성측정장치(IMU) 내부에 있는 가속도 센서를 이용해 드론 기체좌표 x, y, z 세 축의 지구관성좌표에 대한 가속도를 측정한 후 고정좌표로 변환된 값 (fx,acc, fy,acc, fz,acc)을 계산합니다. 가속도센서 측정치 역시 오일러 각도의 '롤(Φacc)'과 '피치(ϴacc)'로 변환될 수 있으며, 이 값들은 자이로 센서의 측정치를 이용해 계산한 '롤(Φgyro)'과 '피치(ϴgyro)'에 포함된 바이어스 오차를 제거하는데 이용됩니다. 하지만, 가속도 센서는 '요(yaw)'를 측정할 수 없기 때문에 자이로 센서를 이용해 측정한 '요(ψgyro)'에 포함된 바이어스 오차를 제거할 수 없습니다.


지자기 센서(Magetometer or Compass)

지자기 센서를 이용해 드론 기체좌표 x, y, z 세 축의 자북점에 대한 방향을 측정합니다. 이 값을 이용해 기체좌표의 NED 좌표에 대한 '요' 값을 계산할 수 있으며, 센서융합기는 지자기 센서로 측정한 '요(ψmag)'를 이용해 자이로 센서 측정치 '요(ψgyro)'에 포함된 바이어스 오차를 제거합니다. 고정날개 드론의 경우 몸체 전면에 피토관(Pitot Tube)을 부착해 좀 더 정확한 '요' 값을 측정할 수 있으나, 드론의 경우 몸체가 회전하면서 날아가기 때문에 피토관을 이용하기가 어렵다는 것입니다.


GPS 수신기 

GPS 수신기는 GPS 위성들로부터 수신한 신호를 이용해 NED 좌표 상에서 드론의 병진운동상태, 즉 위도(Pn.GPS), 경도(Pe.GPS), 고도(hMSL.GPS), 위도상의 속도(Vn.GPS), 경도상의 속도(Ve.GPS) 및 고도상의 속도(Vd.GPS)를 계산합니다. 여기서 첨자 MSL은 해수면(MSL: Mean Sea Level)을 의미합니다.


기압 센서(Barometer)

GPS 수신기를 통해 수신한 위치 좌표에는 항상 5~10m의 오차가 존재합니다. 민수용 GPS 수신기는 L1 주파수밴드(1.5GHz)의 C/A(Coarse-Acquisition) 코드 혹은 L2 주파수밴드(1.2GHz)의 C/A 코드 둘 중의 하나만을 수신할 수 있습니다. 하지만 군사용 GPS 수신기는 L1 C/A와 L2 C/A를 동시에 수신할 수 있어 Diversity로 인한 이득을 얻을 수 있으며, 추가로 암호화 신호(Encrypted Signal) P(Y)를 수신할 수 있어 GPS 신호가 지구의 이온층을 통과할 때 교란되는 것을 보정할 수 있습니다. 이를 이온층 보정(Ionospheric Correction라 일컫습니다. 5~10m의 GPS 고도 오차는 주로 저공비행을 하는 드론의 지상시설물들과의 충돌 위험을 야기시킵니다. 따라서 별도의 기압 센서를 이용하여 고도(hALP.baro)를 측정하기도 한다는 것입니다. 여기서, 첨자 ALP는 기압(Air-Level Pressor)를 의미하며 드론의 이륙시 기압과 현재 비행고도에서의 기압을 비교해 이륙 지점으로부터의 현재 고도를 계산합니다.


센서융합기는 회전운동상태(ΦE, ϴE, ψE)만을 추정하거나, 회전운동 상태와 병진운동상태(Plon.E, Plat.E, hE)를 동시에 추정할 수 있습니다. 회전운동상태 만을 추정하는 센서융합기를 AHRS(Attitude & Heading Reference System)라고 부르고, 회전운동상태와 병진운동상태를 동시에 추정하는 센서융합기를 관성항법기(INS; Inertial Navigation System)라고 부릅니다.


AHRS를 이용한 센서융합기

AHRS 센서융합기는 보상필터(Complimentary Filter; 상보필터)를 이용하거나 확장 칼만 필터(EKF: Extended Kalman Filter)를 이용합니다. AHRS 보상필터는 고주파 대역 특성이 좋은 자이로 센서의 상태측정치(Φgyro, ϴgyro)를 고주파 대역 필터로 추출하고, 저주파 대역 특성이 좋은 가속도 센서의 상태측정치(Φacc, ϴacc)를 저주파 대역 필터로 추출한 후 합쳐서 자이로 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE)를 비행제어기(FC)로 전달해 줍니다.


AHRS 확장칼만필더(AHRS-EKF)는 드론의 비행역학(Flight Dynamics)을 이용해 각종 오차를 제거하는 방법입니다. 자이로 센서를 이용한 측정치(Φgyro, ϴgyro, ψgyro), 가속도 센서를 이용한 측정치(Φacc, ϴacc), 지자기 센서를 이용한 측정치(ψmag) 및 GPS 수신기를 이용한 측정치(Vn.GPS, Ve.GPS, Vd.GPS)를 이용해 실시간으로 드론의 회전운동역학(Rotational Dynamics)을 확장 칼만 필터를 이용해 모델링하면서 자이로 센서 및 가속도 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE, ψE)를 계산해 비행제어기로 전달해 줍니다. GPS 수신기로 측정한 병진운동상태 측정치(Pn.GPS, Pe.GPS) 및 기압 센서로 측정한 고도측정치(hALP.baro)는 융합과정을 거치지 않고 그대로 비행제어기로 전달된다.




Posted by Nature & Life


과거 모형 헬기와 같은 전통적 RC는 메인 로터가 양력을 얻어 부양하고 메인 로터가 회전할 때 회전각에 따른 로터의 피치를 조절하여 헬기가 원하는 방향으로 나아가는데, 메인 로터로 인한 헬기 동체의 반동 토크를 상쇄시킬 목적으로 테일 로터도 함께 회전시키게 됩니다(안정성을 강조한 동축반전 헬기는 제외). 이때 헬기 동체가 정숙하게 방향성을 유지하고 호버링하거나 이동하기 위해서는 자이로(gyroscope) 센서의 도움을 받아 실시간 보상하였는데, 이것이 사실 비행 자동화의 전부였으며 나머지는 오직 조종자의 오랜 비행 경험을 토대로 한 자동 반사적인 키감에 의존하여 매우 역동적인 스포츠를 즐기게 되었습니다.


반면에 드론은 쿼드콥터를 예를 들어, 4개의 프로펠러로 양력을 얻고 원하는 방향으로 나아가기 위해서는 개별 로터의 회전속도를 정교히 제어해야 하는데 이는 컴퓨터의 도움없이는 거의 불가능하다는 것입니다. 이러한 이유로 드론의 비행제어기(FC; Flight Controller)는 사람의 심장과도 유사하여 수신모듈로 부터 수신된 명령 신호를 처리하여 각 암(ARM)의 모터를 제어하고, 게다가 가속도계/자이로 센서를 포함하는 관성측정장치(IMU), 바로미터, 컴파스/지자계 등의 센서 데이터를 기반으로 안정적인 비행이 가능하도록 한다는 것입니다.


최근에는 GPS 센서를 탑재하여 GPS 데이터에 기반하여 사전에 입력된 경유지(waypoint)를 순차적으로 운항하거나 RTL(Return to Launch)라는 자동 회귀 기능 등의 탑재로 조종자의 명령이나 각종 기체 이상 등을 감지하여 이륙 장소로 스스로 귀환시키거나, 영상 및 소리 센서들을 활용한 충돌회피 등등 다양한 기능들이 추가되면서 FC는 날로 매우 빠른 연산을 수행하는 MCU가 필요한 추세라는 것입니다.


이를 증명하듯 수 년전에는 오픈 소스에 기반한 APM(AutoPilot Mega) 보드나 multiwii 보드는 8bit 16MHz의 ATmega328이나 ATmega2560의 MCU가 사용되었는데, 그 후로 AruPilot의 PixHawk(3DR)은 훨씬 강력한 32bit 168MHz의 STMicro사의 ARM Cortex M4를 사용하게 되었습니다. 현재의 오픈 소스의 드론 플랫폼으로 가장 인기있는 PX4는 64bit quad-core 2.26GHz의 퀄컴사 SOC(System on Chip) 기반 스냅드래곤 SOC(System on Chip)을 채용하고 있는 실정입니다.



사실 드론이 안정적인 비행으로 대중화를 선언한 그 이면에는 고성능의 MCU 채용만큼이나 FC에서 중요한 것은 센서 기술의 진화에 있다고 해도 지나치지 않다라는 것입니다. 각종 센서들로부터 드론은 비행 속도/각도, 좌표, 위치 데이타 등을 실시간으로 MCU에 제공하여 상당히 안정적인 비행을 가능하게 하지만, 최근에는 저고도에서의 정확한 고도 유지와 포지션홀드 기능을 위해 초음파센서, 옵티컬플루우(Optical Flow) 센서 등이 사용되고 있으며, 또한 충돌회피를 위해 카메라 센서 기반한 SLAM(SImultaneous Localization and Mapping)등의 알고리즘들이 활발히 연구되고 있다는 것입니다.



Posted by Nature & Life


AutoQuad 6 Flight Controller


What makes the AutoQuad flight controllers special?


무엇이 AutoQuad 비행제어기(FC)를 특별하게 만들까요?


Besides using top quality sensors and MCU, AutoQuad differs in its approach of using sensor calibration techniques and calculating the magnetic profile of the entire multirotor creating the ultimate stable flying and navigation experience.


최고 양질의 센서들과 MCU를 사용한 것 외에도 AutoQuad는 센서 보정 기술을 사용하는 그리고 최고의 안정된 비행과 조종 경험을 갖게 하는 전체 멀티로터의 자기 프로파일을 계산하는 접근 방식에서부터 차이가 납니다.





Summary of specifications of the AQ 6.x flightcontroller hardware

    • 2″ x 2.5″ main board with 4.5×4.5cm mounting hole pattern

    • Input voltage: 6.5v => 18v

    • 2 High efficiency DC/DC converters, separate power to flight controller logics and accessories.

    • STM32F407 32bit Cortex M4 microcontroller (1MB flash)

    • Standard Arm 10 pin 0.05″ pitch SWD connector

    • 14 general purpose PWM controllers / receivers (powered or un-powered)

    • Dedicated Spektrum satellite (remote receiver) 2.4Ghz R/C radio connector

    • uSD card slot driven by 4bit SDIO capable of 100Mb/s transfer (up to 32GB storage)

    • onboard uBlox LEA-6T GPS module with battery backup and time pulse capture

    • u.FL active GPS antenna connector

    • optional external bi-directional telemetry radio via standard 6 pin FTDI connector – powered up to 1A

    • I2C bus connector for I2C ESC’s (or other I2C devices)

    • X, Y Mag: HMC6042

    • Z Mag: HMC1041Z

    • X, Y Gyro: IDG500

    • Z Gyro: ISZ500

    • Accel: ADXL325

    • Pressure Sensor: MP3H6115A (optional 2. tube sensor)

    • Battery voltage monitor


Onboard IMU options:


Summary of capabilities of the AQ 6.x firmware

    • Fly extremely stable yet offer full dynamic control to the pilot.

    • Limit flying angles.

    • Mavlink 1.0x compatible protocol, Ground Stations for Win, Mac, Linux & Android

    • Very accurate position hold, depending on GPS reception a hold within 15-30 cm is possible.

    • Altitude override (ascend / descend) during position hold with controlled vertical speed.

    • Full mission flight, speed, heading and 'loiter time' is settable.

    • Return to home position, altitude is also recorded during home position set.

    • POI (point of interest), autonomous circling around given coordinates

    • Heading Free flight mode, Follow Me, Click & Go (needs exp. firmware)

    • Radio loss detection and event triggering.

    • Low battery detection and event triggering.

    • 2 axis gimbal control with pitch angle override on transmitter.

    • Gimbal working angle and response time is settable.

    • Gimbal servos can use 50Hz (analog) to 400+ Hz (digital) settable.

    • Radio options: Spektrum satellite (10 & 11bit), S-bus receiver, (C)PPM input, SUMD (Graupner) M-Link (Multiplex)

    • User Addon Waypoint recording and playback using transmitter switch.

    • User Addon External LED / Piezo signaling for status and events, MAVLink telemetry for Graupner HOTT radios





'Radio Control > Flight Controller' 카테고리의 다른 글

비행제어기(FC)의 역사와 종류(1)  (1) 2020.03.28
비행제어기(FC)란?  (0) 2017.03.07
AutoQuad 사이트에서 소소 코드를 확인하는 법  (0) 2015.12.02
AutoQuad란?  (0) 2015.11.29
APM v2.5 vs. Crius AIOP  (0) 2014.03.04
Posted by Nature & Life



ARM 기반에 32 비트 MCU(STM32 계열)을 사용하여 비행제어기 보드(Flight Control Board)와 ESC를 만든 AutoQuad 프로젝트를 소개합니다. MCU가 갖는 빠르고 강력한 이점 외에도 C언어 기반의 ESC를 포함한 완전한 오프 소스 프로젝트라는 것입니다.


AutoQuad is an Open Source firmware project with closed source hardware. It is aimed at providing a flight controller board with stabilization, dynamic flight and autopilot features. The community can enhance the AutoQuad code. The powerful hardware with a 32 bits MCU and a selection of the best IMU sensors form the base for the AutoQuad flight controller, the possibilities are endless and only limited by the imagination and skills of the coders.


AutoQuad는 비공개 하드웨어를 사용한 오픈 소스 펨웨어 프로젝트입니다. 이는 안정성과 역동적인 비행 그리고 자동 항법 특징을 갖는 비행 제어 보드를 공급하는데 목적이 있습니다. 이 커뮤니티는 AutoQuad 코드를 진보시킬 수 있습니다. 32 비트 MCU와 가장 훌륭한 IMU 센서들의 장착한 강력한 하드웨어로 AutoQuad 비행제어기의 기본을 갖추었습니다. 그 가능성은 무한하고 단지 코드를 프로그램하는 사람의 상상력에 의해서 제한될 뿐입니다.


The current code base is targeting multicopters up to 14 motors. However the platform is flexible and can be adapted to fixed wing or single rotor applications.


현재의 코드는 14 모터까지 장착한 멀티콥터를 목표로 합니다. 그러나 개발환경은 유연하고 고정익 혹은 단일 로터를 갖는 기체에도 적용될 수 있습니다.


AutoQuad has recognized the need for specially adapted speed controllers or ESC’s. During the initial project setup, an 32bit ESC is developed: the ESC32. As with AutoQuad, the ESC32 is based on open source code, but closed source hardware.


AutoQuad는 특별한 속도 제어기 혹은 ESC를 요구해왔습니다. 초기 프로젝트 착수 동안에 32 비트 ESC가 개발되었습니다: ESC32. AutoQuad와 함께 사용되어지기 때문에 ESC32는 오픈 소스 코드 기반이지만 비공개 하드웨어에 사용되어집니다.



다음은 실제 프로젝트의 로드맵(Actual Project Timeline)니다.

Actual Project Timeline




'Radio Control > Flight Controller' 카테고리의 다른 글

AutoQuad 6 비행제어기의 스펙  (0) 2015.12.02
AutoQuad 사이트에서 소소 코드를 확인하는 법  (0) 2015.12.02
APM v2.5 vs. Crius AIOP  (0) 2014.03.04
다양한 Multiwii 보드  (0) 2014.03.03
Multiwii란?  (0) 2014.02.26
Posted by Nature & Life