Attitude Control Algorithm for drone

드론은 attitude 에 대한 estimation 알고리즘으로 complemetary filtering, extended Kalman filter, gradient descent 알고리즘등을 사용한다. 

이 알고리즘들의 기본 아이디어는 gyroscope를 사용한다는 것이다. 실시간으로 드론의 attitude를 예측함에 있어서 동적 안정성이 좋기 때문이다. 

이와 함께 gyro scope는 inherent하게 시간이 흐를 수록 drift error가 누적된다는 한계를 가진다. 그래서 시간이 흐르더라도 변하지 않도록 correction과 calibration을 적용해주는 것이 중요하다. 

연산속도나 연산정확도에 따라서 각기 다른 필터 알고리즘을 사용하는데, 우리가 잘 알고 있는 오픈소스 프로젝트 드론인 Arducopter와 PX4 드론은 complemetary와 EKF를 사용한다.

PX4 소스코드는 기존의 complemetary filtering Algorithm을 사용한다. 

기본적으로 accelerometer callibration을 통해서 gyroscope를 보정하고, 추가적으로 Magnetometer와 GPS 데이터를 사용해서 quaternion calibration compensation. 일 수행한다. 

자 다음의 예시는 mahony 알고리즘을 통해서 드론의 attitude를 estimation 하는 과정에 대한 설명이다. 

 

 

용어 정리 

Coordinate System(방향 시스템)

• Navigation coordinate system: In the multi-rotor, it is also called the earth coordinate system and the geographic coordinate system. Usually, the North East (NED) is used to form the X, Y, and Z axes of the coordinate system.

-> 지상 기준으로 북쪽 동쪽,

• Body coordinate system : Solid on a multi-rotor aircraft, ie, the origin of the coordinate system is located at the center of the aircraft (assuming the center point coincides with the center of gravity).

-> 드론의 body를 기준으로 북쪽 동쪽

Attitude representation(자세 표현)

• Euler: More intuitive, describing the pose of a rigid body in a three-dimensional European space. At this time, the coordinate system is the body coordinate system, and rotates as the rigid body rotates. Disadvantages: universal joint lock.

-> 롤, 피치, 요, θ ,φ ,ϕ

• Quaternion: A set of four-dimensional vectors that represent the three-dimensional rotation of a rigid body. Suitable for computer calculations.

->쿼터니안 방식의 표현

• Direction cosine matrix DCM: The rotation of the coordinate system represented by the Euler angle cosine or quaternion.

->x, y, z 축 각속도를 quaternion으로 변환 해주기 위한 회전 행렬, 

Filtering principle

Complementary filtering은 서로 다른 noise 간섭 주파수를 가지는 두 개의 신호에 대해 cutoff frequency를 적절하게 설정해서 우리에게 fusion된 센서 데이타가 요구되는 frequency를 cover 할 수 있도록 해야 한다. 

In IMU pose estimation, the complementary filter uses high-pass filtering for the gyroscope (low-frequency noise) and low-frequency filtering for the acceleration/magnetometer (high-frequency noise).
(The sensor data has not been measured here, the noise and useful frequency are unknown... To be added later)

In the complementary filtering, the cutoff frequencies of the two filters are the same, and it is necessary to trade off the values ​​according to the useful frequencies. 같은 cut-off frequency를 가진다. 

When the body is level, the accelerometer cannot measure the amount of rotation around the Z axis, which is the yaw angle. The magnetometer has the same problem, and the amount of rotation of the magnetic axis cannot be measured.

Therefore, an accelerometer and a magnetometer are required to simultaneously correct the gyroscope.

Accelerometer와 Magnetometer를 사용해서 gyroscope를 correct 해준다.

 

Main process of filtering

여기서 q^ 은 드론 시스템의 자세의 예측을 표현하는 qaternion이다. 그리고 δ는 PI 조정기를 통해서 생성된

innovation이다. 그리고 e 는 측정 된 관성 벡터 v̄와 예측 벡터 v ^ 사이의 상대 회전 (오차)을 나타냅니다. 즉 측정과 예측값 사이의 오차다. 

P는 쿼터니안 에서 허수만 존재하는 부분으로 오로지 회전만을 나타낸다.

b)식의 PI 조정기에서 Kp는 가속도계와 자이로센서 사이의 Crossover Frequnecy를 말하며, KI는 자이로 스코프의 오류 correctio을 해주는 역할을 한다. 

Prerequisite knowledge

드론의 body를 기준으로 측정한 각속도를 Euler 각으로 변환하는 관계는 아래와 같다.

 

The relationship between the rotation matrix and the angular velocity of the body:

The relationship between quaternion and angular velocity of the body:

이러한 각 관계들은 아래의 도식을 보면 쉽게 이해 할 수 있다. 

자 이제 알고리즘으로 들어가 보자

prediction

칼만필터와 비슷하게, complementary filtering도  predection 과 correction으로 구성된다. 예측단계에서 3축 자이로를 통해 측정되는 각속도는 아래의 식을 통해  quaternion 자세를 계산한다. 

결과 값은 아래와 같이 얻어진다. 이전의 quaternion에 시간 경과에 따른 quaternion 변화를 더한 값이다. 

Correction

prediction을 통해 구한 quaternoin 값은 accelerometer와 magnetometer를 통해서 보정한다.

 

이때 , roll, pitch는 accelerometer를 통해서 얻은 quaternion 값으로 수행하고  magnetometer로 부터 구한 quaternion으로는 yaw를 보정한다.

 

 

Accelerometer correction

The accelerometer signal first passes through a low-pass filter (eliminating high-frequency noise):

Then, the resulting result is normalized

Calculate the deviation:

where v represents the vector of the gravity vector in the body coordinate system.

At this point, v is multiplied by the vertical component of the accelerometer vector to get the error value. [Two vectors of the same physical meaning, theoretically multiplied by zero]

Magnetometer correction

The data preprocessing is the same as the accelerometer, first filtering, then normalizing, to get

1. When there is no GPS calibration:
calculation error:

In the formula, w is calculated as follows:

The output of the magnetometer (m) is converted to the navigation coordinate system in the body coordinate system:

The x-axis of the navigation coordinate system is aligned with the true north, so the projection of the magnetometer on the xoy plane can be converted to the x-axis.

Change to the body coordinate system again:

2. When there is GPS calibration:
In px4, the magnetometer is calibrated using the GPS information [0,0,mag], so the formula is the same as the accelerometer.

At this point, w is multiplied by the magnetometer vector to get the error value. [Two vectors of the same physical meaning, theoretically multiplied by zero]

Update quaternion

Error values ​​obtained by accelerometer and magnetometer calibration:

The correction value is obtained from the error value: (only ki corrects bias)

Corrected angular velocity values:

Solve the first-order differential equation according to the first-order Runge-Kutta method:

The difference form of the quaternion differential equation can be found:

Quaternion normalization: