60,506-axis sensor software attitude calculation for four-axis aircraft

Using the MPU6050 hardware DMP to solve the attitude is very simple, the following describes the three-axis gyroscope and the value of the accelerometer to use the four-number software to solve the attitude of the method.

Let's take a look at how to describe a plane rotation (coordinate transformation) with Euler's angle:

A coordinate system is arranged around the rotation of the α angle to obtain a coordinate system, in space there is a vector in the coordinate system projection, the projection is due to rotation around, so the z coordinate is unchanged, that is.

Converted to matrix form is represented as:

Make a clean look:

So from spinning to being able to write

The above is only a rotation around one axis, if the Euler angle rotation in three-dimensional space to go three times:

A cosine matrix representing the direction of rotation is given above.

But in order to use Euler's angle to solve the attitude, we can actually apply the Euler angular differential equation:

On the left side of the above-described, it is the Euler angle after this update, corresponding to the Row,pit,yaw. On the right, is the angle measured from the last cycle,,, three angular velocity from the three-axis gyroscope directly installed in the four-axis aircraft in this cycle of rotation angle, in radians, calculate the interval when the T- gyroscope angular velocity, such as 0.02 seconds 0.01 radians/second =0.0002 radians. Thus solving the differential equation can solve the current Euler angle.

The above describes what is Euler angle, and the Euler angular differential equation solver attitude relationship is simple and straightforward, the concept is intuitive and easy to understand, then why we do not use Euler angle to represent the rotation of the introduction of four yuan?

On the one hand, the Euler angular differential equation contains a large number of trigonometric operations, which brings some difficulties to the real-time solution. And when the pitch angle is 90 degrees, the equation will appear magical "Gimballock". So the Euler angle method is only applicable to the situation where the horizontal attitude change is not very large, but not for the attitude determination of the whole attitude aircraft.

The four-tuple method is a practical engineering method for solving only four linear differential equations with unknown quantity, which is small in calculation and easy to operate.

We know that the rotation in the plane (x, y) can be represented by a complex number, and the same three-dimensional rotation can be described in units of four yuan. Let's define a four-dollar number:

We can write it, of which,. So is the vector, which represents the axis of rotation in three-dimensional space. W is a scalar that represents the rotation angle. So it rotates the W degree around the axis, so a four-dollar number can represent a complete rotation. Only the unit four can represent the rotation, as for why, because this is the four-dollar number represents the rotation of the constraint conditions.

and the direction cosine matrix, which was described by Euler angle, is described by a four-dollar number:

So in the software solution, we have to first convert the value of the accelerometer (three-dimensional vector) to the unit vector, that is, the vector divided by the module, the incoming parameter is the gyroscope x, Y, z values and the accelerometer x, Y and Z values:

<code>void imuupdate (float GX, float Gy, float gz, float ax, float ay, float az) {float norm; float VX, vy, VZ; flo At ex, EY, EZ;           Norm = sqrt (Ax*ax + Ay*ay + az*az);       Ax = ax/norm; ay = ay/norm; AZ = az/norm;  

The following translates a four-dollar number into the three elements of the third row in the direction cosine. Just Vx,vy,vz is actually the unit vector of gravity that was converted from the body coordinate reference system of the last Euler's angle (four-dollar number).

AXYZ is the body coordinate reference frame, the accelerometer is measured by the gravitational vector, which is actually measured by the gravity vector.

AXYZ is a gravitational vector that is measured, and vxyz is a vector of gravity calculated from the attitude of the gyro integral, all of which are gravity vectors on the body coordinate reference frame.

The error vector between them is the error between the attitude of the gyro integral and the attitude measured by the accelerometer.

The error between vectors can be expressed by means of a vector cross product (also called an outer product of a vector, a cross-multiplication), and exyz is the cross product of two gravitational vectors.

This cross-product vector is still located in the body coordinate system, and the gyro integral error is also in the body coordinate system, and the size of the cross product is proportional to the gyro integral error, just take to correct gyro. (You can think for yourself) because the Gyro is the direct integration of the body, so the correction of the gyro is directly reflected in the correction of the body coordinate system.

Using cross product error to make PI correction gyro 0 deviation

<code>//integral error scaled integral gain Exint = Exint + Ex*ki; Eyint = Eyint + Ey*ki; Ezint = Ezint + Ez*ki;  

The four-tuple differential equation, where T is the measurement period, is the gyroscope angular velocity, and the following are known quantities, where the first-order Rongocuta is used to solve the four-element differential equation:

<code>//integrate quaternion rate and normalise q0 = q0 + (-q1*gx-q2*gy-q3*gz) *halft; Q1 = Q1 + (Q0*GX + q2*gz-q3*gy) *halft; Q2 = q2 + (Q0*gy-q1*gz + q3*gx) *halft; Q3 = Q3 + (Q0*gz + q1*gy-q2*gx) *halft;   

At last, we convert the four-dollar number into Euler angle according to the conversion relationship between cosine array and Euler angle in four-element direction:

So there are:

Q_angle. Yaw = atan2 (2 * Q1 * q2 + 2 * q0 * Q3,-2 * q2*q2-2 * q3* Q3 + 1) * 57.3; Yaw Q_angle. Y  = ASIN ( -2 * Q1 * q3 + 2 * q0* Q2) * 57.3;//Pitch q_angle. X = atan2 (2 * Q2 * q3 + 2 * q0 * Q1,-2 * Q1 * q1-2 * q2* Q2 + 1) * 57.3; Roll

60,506-axis sensor software attitude calculation for four-axis aircraft