Development of quadrocopter angular stabilization

This article is rather a logical continuation of my article about the balancer: “Creating a balancer robot on an arduino” .
It will very briefly highlight: a simple model of quadrocopter angular stabilization using quaternions, linearization, building control for an object and checking it in Matlab Simulink, as well as checking on a real object. Crazyflie 1.0 will be the test subject.

Now it flies like this (at the time of filming, I didn’t set the controls very well):

Building a dynamic system

We introduce 2 coordinate systems: local, tied to the ground, and the second, associated with the copter.
')


The rotation of the body is more convenient to represent using quaternions, due to the smaller amount of necessary calculations. About them many articles are written, including on Habré. I recommend reading the book “Branets V.N., Shmyglevsky I.P. Using quaternions in orientation tasks ”, thanks to Slovak from the MathWorks Competence Center for the hint.

We use the basic law of the dynamics of rotational motion:

where
- moments acting on the body,
I is the inertia tensor, and
- angular velocities along the main axes (in a related coordinate system).
In this way:
.

By virtue of the theorem on reducing the inertia tensor to the principal axes, the inertia tensor can be represented as: .

We define external moments through controls: where


Thus, the equations of angular velocity in a related coordinate system:


I would note that if we took into account the position of the copter, it would be possible not to introduce separate control functions, but to immediately use as a force traction, which is more convenient and faster in the calculations. In this case, the stabilization system does not have any data on the required amount of thrust forces, so it is necessary to use just such controls ...

The propeller thrust can be roughly described as . Then the equations can be written through the angular frequencies of the propellers, if you can control the frequency of the motors directly and know the concrete b:
Where
- Euler angles
I note that the selection of the coefficient b I have done manually, a simple selection.

You also need to write the equation for the rotation quaternion. From the properties of quaternions it follows that
where are the angular velocities in the coordinate system associated with the aircraft, in it the gyroscopes measure the angular velocity [1].

Let's try to stabilize only the angles and angular velocities:


Or more


We introduce the state space vector:
.
It should be noted that if a component is included in the space vector the system ceases to be managed. However, we can assume that and remove it from the state vector, thereby reducing the number of coordinates [2].



Control Vector:
,

The system is presented in the standard form.

.

In our case

, but

Linearization and control construction

By linearizing the system near the origin, we obtain the following matrices A and B:

,



Like last time, we use a linear-quadratic regulator. Let me remind the Matlab team to calculate it:

[K,S,e]=lqr(A,B,Q,R) 

Matrices Q and R are weight matrices. Q penalizes for deviation from zero, and R for power consumption by control.
As a result, we got the matrix K. In my coefficient matrix, all non-diagonal elements were very small (about 10 ^ -4) and I did not take them into account.
Let me remind you that in order to obtain control, it is necessary to multiply the matrix K by the vector X. Of course, in the code, you can omit the concept of a matrix and simply multiply each coordinate by a certain factor for speed.

Model checking

To check the results, a model was created in Matlab Simulink. Run it with non-zero initial conditions.



The first graph shows how the angular velocities behave, the second - the change in the components of the quaternion. Notice that the scalar magnitude of the quaternion comes in unit, despite the fact that it does not enter into the equations of the linearized system. As can be seen from the graphs - the model is stabilized.

Code

Crazyflie uses the Free RTOS system, where all the code is divided into modules, we are interested in the code sensfusion6.c and stabilizer.c.
Fortunately, the accelerometer and gyroscope filtering is done in quaternions, the problem is that the sensors on the copter are located for the + circuit. The model I expected for the X scheme. The only difference is in the choice of controls U1 and U2.



You must add a quaternion acquisition code to sensfusion6.c:

 void sensfusion6GetQuaternion(float* rq0,float* rq1,float* rq2,float* rq3){ *rq0=q0; *rq1=q1; *rq2=q2; *rq3=q3; } 

I did not add a separate module for the LQR controller, instead I changed stabilizer.c. Yes, perhaps this is not the most intelligent way, but it will be suitable for testing the model.

You should start by adding the variables of the current and the desired position of the device, as well as controls:

 static float q0Actual; static float q1Actual; static float q2Actual; static float q3Actual; static float q1Desired; static float q2Desired; static float q3Desired; int16_t actuatorU1; int16_t actuatorU2; int16_t actuatorU3; 

We do not indicate the desired position for q0 due to the fact that we do not need to stabilize it.

We make changes to the code for receiving commands. The copter gets an angle in degrees, it is mathematically more correct to do this:

 ommanderGetRPY(&q1Desired, &q2Desired, &q3Desired); q1Desired=cos((-q1Desired/2+90)*0.01745);//*3.14/180/2; q2Desired=cos((q2Desired/2+90)*0.01745); q3Desired=cos((q3Desired/2+90)*0.01745); 

Let's change the “fast” cycle (250Hz) of the stabilizer:

 sensfusion6UpdateQ(gyro.x, gyro.y, gyro.z, acc.x, acc.y, acc.z, FUSION_UPDATE_DT); sensfusion6GetEulerRPY(&eulerRollActual, &eulerPitchActual, &eulerYawActual); sensfusion6GetQuaternion(&q0Actual, &q1Actual,&q2Actual,&q3Actual); sensfusion6UpdateP(FUSION_UPDATE_DT); sensfusion6UpdateV(acc.x, acc.y, acc.z, FUSION_UPDATE_DT); actuatorU1=50*(1*(-gyro.x)+245*(q1Actual-q1Desired)); actuatorU2=50*(1*(gyro.y)-200*(q2Actual-q2Desired)); actuatorU3=50*(1.5*(gyro.z)+0*(q3Actual-q3Desired)); 

The selection of coefficients was made empirically, since it was not possible to find out the relationship between the team sent to the motors and the force given by the motor unit.

I also changed the power distribution function of the motors:

 static void distributePower(const uint16_t thrust, const int16_t u2, const int16_t u3, const int16_t u4) { motorPowerM1=limitThrust((thrust/4+u3/2+u4/4)*5); motorPowerM2=limitThrust((thrust/4-u2/2-u4/4)*5); motorPowerM3=limitThrust((thrust/4-u3/2+u4/4)*5); motorPowerM4=limitThrust((thrust/4+u2/2-u4/4)*5); motorsSetRatio(MOTOR_M1, motorPowerM1); motorsSetRatio(MOTOR_M2, motorPowerM2); motorsSetRatio(MOTOR_M3, motorPowerM3); motorsSetRatio(MOTOR_M4, motorPowerM4); } 

Conclusion

Based on the fact that the copter stabilizes its corners, we can conclude that the mathematical model is designed correctly. Unfortunately, it is not yet possible to get your coordinates and speeds (integrating an accelerometer makes a huge mistake), so the copter does not dampen the initial speed and does not return to the initial position.
To solve this problem, MIT, for example, uses cameras and tags on its copters.

Additional materials and sources

1. Branets V.N., Shmyglevsky I.P. "The use of quaternions in orientation problems"
2. Yaguang Yang "Analytic LQR Design for Spacecraft Control System Based on Quaternion Model"
3. Modified firmware branch on github

PS Unfortunately, I can not share the model, as well as talk about the extended model with autopilot and coordinate stabilization due to the fact that this is part of my future diploma, and all diplomas are now checked for novelty and anti-plagiarism.
PPS I publish this article on Habré, and not on the new GT due to the fact that the rest of my articles of similar subjects remained exactly on Habré.