[UVA] Pixhawk's Attitude Solution (4) _ Supplementary article

First, the outset

Everyone has been looking forward to the fourth article, but this article may be compared to water AH ~ ~ ~ ~ ~ ~

First of all, the last week there is no harvest, although read a lot of papers, but there is no quality of the leap ~ ~ ~ ~

Look at the paper is about the attitude of the calculation, most of the algorithms are based on the expansion of the Mahony algorithm, it is a deep understanding of the attitude of the process of solving, a little bit of writing last week summed up the slightest knowledge.

Yesterday opened a full day of the meeting, the final decision to figure out the attitude of the part or need to use EKF do, slowly, but I still take the attitude control part of the solution, feel this part is also very important.

Second, the copyright notice

Bo Master: Summer

Statement: Water do not forget to dig well people, reproduced please indicate the source.

Original address: http://blog.csdn.net/qq_21842557

Contact information: [Email protected]

Technical Exchange qq:1073811738

Third, the experimental platform

Software Version:px4firmware

Hardware Version:pixhawk

Ide:eclipse Juno (Windows)

Iv. Basic Knowledge

In the early stage of multi-rotor UAV, it is necessary to understand its aerodynamic layout and complex dynamic model, then the attitude calculation and controller design. In order to realize the attitude estimation of the precision four-rotor aircraft, the first is to understand the reasons for the existence of the data and the errors, then the various mathematical operations (all based on a certain mathematical platform), such as the change of DCM with time, the four-number differential equation, the change of four-yuan time, the re-orthogonal and so on.

In understanding the construction of the four-rotor dynamics model, it is necessary to understand that it is a four-input six-output of the under-drive system, under-driving is mainly input only four motor pull to achieve control angular velocity, angle, rise speed, descent speed, rise distance and fall distance. the kinetic model is still in the study .

To realize the autonomous flight of four-rotor aircraft is mainly by attitude calculation and attitude control. Attitude control is the core of the four-rotor aircraft, and the position control is based on the perfect attitude control. As already mentioned, it is unreliable to obtain the angle value only by gyroscope integral, because of the inherent drift of the gyroscope, the error caused by the integral becomes more and more large, and the accelerometer is susceptible to noise and vibration disturbance. Therefore, the two kinds of data are fused using various algorithms to obtain the ideal attitude data. The more common fusion algorithms are EKF (Extended Kalman filter), UKF (unscented Kalman filter), gradient descent method, CKF (Cubature Kalman filter), PF (particle Filter) and CF (complementary filter).

The Mahony attitude solving algorithm is mainly based on the CF design of data fusion filter, the algorithm needs to focus on the principle of its thinking is divided into ahrs (gyroscope, accelerometer, magnetometer) and IMU (gyroscope, accelerometer) two.

At present, the research is more concentrated in the field of nonlinear control, because the nonlinear control has a strong dependence on the accuracy of the model, the PID control is more practical under the condition that the model error exists, so the existing algorithm based on the Mahony complementary filter design is based on the PID feedback controller.

The next step is to use Simulink for modeling and simulation of the controller, as well as in the study .

v. Text

1, the sensor's numerical model (Mahony complementary filter)

First or the stickers, Word edit formula still can't get in.

2, the design of complementary filters (based on four yuan)

Still the picture ~ ~ ~

Finally not the picture ~ ~ ~

The complementary filter is the optimal value of the angle obtained by gyroscope in a short time, and the acceleration value of acceleration sampling is averaged to correct the angle of the gyroscope. In a short period of time with the gyroscope is more accurate, mainly by it; for a long time with an accelerometer more accurate, the increase in its proportion, this is complementary, but the accelerometer to filter out the high-frequency signal, the gyroscope to filter out the low-frequency signal, the complementary filter is based on the sensor characteristics, through different filters (high or low pass, complementary And then add the signal to the whole band. For example, the accelerometer measured the acceleration value, its dynamic response is slow, the signal is not available at high frequency, so the high-frequency interference can be suppressed through a low-pass filter, gyro response is fast, the integration can be measured dip, but due to 0-point drift, low-frequency signal is not good, through a high-pass filter can suppress low-frequency Combining the two, the advantages of the gyroscope and the accelerometer are fused together to obtain a better signal at both the high and low frequencies, and the complementary filter needs to select the frequency point of the switch, that is, Qualcomm and lowpass frequencies.

six, in a little deeper

1, the Mahony complementary filtering algorithm and Px4firmware in the source code of the Attitude solution algorithm of a little insight

The first is the most primitive Mahony filter algorithm in the accelerometer data correction gyroscope data, the source code is as follows.

1.//measured by accelerate Sensor2.  Ax = ax/norm;  3.  ay = ay/norm;  4.  az = az/norm;  5.6.//estimated direction of gravity  (v)   7.  VX = (Q1Q3-Q0Q2);  8.  VY = (q0q1 + q2q3);  9.  vz = q0q0-q1q1-q2q2 + q3q3;  11.//error is sum Ofcross product between reference direction of fields and direction measured by accelerate sensor
   12.  ex = Ay*vz-az*vy;  ey = az*vx-ax*vz;  ez = AX*VY-AY*VX;  

Understanding of the above code: [Ax,ay,az] means that the accelerometer is actually measured by the body of the acceleration data, according to the data fusion correction principle, the accelerometer in the case of low-frequency, the gyroscope effect is good at high frequency, at low frequencies, ignoring the body's motion acceleration, That is, the data measured by the accelerometer is the approximate pure gravitational acceleration vector. [Vx,vy,vz] represents the gravitational acceleration vector obtained by the gyroscope's attitude matrix after integration (how to get no more repeat). And both vectors are in the body coordinate system, the two vector of the gravitational acceleration vector is the so-called error vector (12, 13, 14 lines of code), and the size of the cross product is proportional to the gyroscope's integral error, Using this error vector to correct the next gyroscope data can achieve the effect of correcting gyro data error with accelerometer.

Then is the px4firmware source in the accelerometer data correction gyroscope data, the source code is as follows.

1.//Accelerometer correction  2.//Project ' k ' unit vector of Earth frame to body frame  3.//vector<3> k = _ Q.conjugate_inversed (vector<3> (0.0f, 0.0f, 1.0f)); N series to 4.//B-series 5.//Optimized version with dropped zeros  6.    Vector<3> K (  7.        2.0f * (_Q (1) * _Q (3)-_q (0) * _Q (2)),  8.        2.0f * (_Q (2) * _Q (3) + _q (0) * _Q (1)),  9.        (_q (0) * _Q (0)-_q (1) * _Q (1)-_Q (2) * _Q (2) + _Q (3) * _Q (3))  (Ten    );  

The,vector<3> k in this algorithm is the gravitational acceleration vector (equivalent to [VX,VY,VZ] in the Mahony algorithm) obtained by the Gyro integral attitude matrix, which is similar, the main difference is the following accelerometer measurement data processing, First, the total acceleration vector is obtained by the accelerometer, then the motion acceleration vector is obtained with the GPS or the airspeed, and then the pure gravitational acceleration is obtained by the difference between the two vectors, namely: the total acceleration = Motion acceleration + acceleration of gravity (The motion acceleration is considered in the low frequency case, the correction effect is better). The _w_accel represents the weight, which is similar to KP.

The "%" operator in the preceding code is defined as a cross-product operation:

1.vector<3> operator% (const vector<3> &v) const {  2.        Return vector<3> (  3.                   DATA[1] * v.data[2]-data[2] * v.data[1],  4.                   DATA[2] * v.data[0]-data[0] * v.data[2],  5.                   DATA[0] * v.data[1]-data[1] * V.data[0]  6.               );  7.    }  8.};  

Vii. Summary
did not see how many things, may be because the Qingming holiday too Hi, the heart has not received back. All right, I admit, it's me too water ~ ~ ~

Today, we have built a group, I hope that we can discuss technical issues, for the country's UAV industry to contribute more.

[UVA] Pixhawk's Attitude Solution (4) _ Supplementary article