3D Control for Quadcopter
In the Udacity Flying Car Nanodegree Lesson 12-14, I learned about the basic vehicle control, the control architecture and a full 3D cascaded control. In this project, I've learned how to implement the 3D control in C++ for a real quadcopter. The development setup and the project instruction are provided here.
A quadcopter in 3D space has 12 state parameters:
As described in the lesson, the 3D Control architecture for a quadcopter can be illustrated as the below image:
The Udacity team has provided a simulator with 6 scenarios, where I can test my control function.
The scenario 1 is the test scenario to see if all development setup is correctly installed and configured. With the default implementation of
QuadControlParams.Mass * 9.81 / 4
I can change the correct drone mass to 0.5 kg.
As described in the lesson, I have to implement the body-rate-control (inner loop block) first and tune the parameter. Once I can rely the parameters, I can continue to implement the roll-pitch-controller. In order to test both control in the simulator, the function
GenerateMotorCommands(float collThrustCmd, V3F momentCmd) has to be implemented correctly. Based on the given commanded collective thrust
collThrustCmd and the commanded moment
momentCmd, I need to solve these four linear equations:
f_total = f0 + f1 + f2 + f3 f_rot_x = f0 - f1 + f2 - f3 f_rot_y = f0 + f1 - f2 - f3 f_rot_z = -f0 + f1 + f2 - f3
f0is the thrust of the up-left propeller with clockwise rotation,
f1is the thrust of the up-right propeller with counterclockwise rotation,
f2is the thrust of the down-left propeller with counterclockwise rotation,
f3is the thrust of the down-right propeller with clockwise rotation,
which can be illustrated as following:
Since the rotation thrust in the z-axis is in the negative direction, I multiplied the above equation of
f_rot_z with -1.
The rotation thrusts can be calculated using this equations:
float f_rot_x = momentCmd.x / l; // float f_rot_y = momentCmd.y / l; // float f_rot_z = momentCmd.z / kappa;
float l = L / sqrt(2); float f_total = collThrustCmd;
Before I return the thrust of each propeller, the
CONSTRAIN function is call to constrain the thrust values like this:
cmd.desiredThrustsN = CONSTRAIN( f0, minMotorThrust, maxMotorThrust);
The result of the implementation can be seen in scenario 2 as below animation:
The next step is to implement the lateral-position-control, the altitude-control, and the yaw-control sequentially. Then, I tuned their gains. One important thing in the lateral-position-control is that the input parameters from the trajectory point (
curTrajPoint.accel have to be modified. I implemented PD-with-feedforward control in this lateral-position-control block based on this quadcopter dynamics taken from the section 3 of this paper:
In the altitude-control block, I implement the PID-with-feedforward. The integral term is integrated after observing the quad1 in the scenario4. For the yaw-control block, I implement the P control.
In this scenario, the parameter tuning of the integral controller kiPosZ was not trivial. I need to increase the speed of the lateral position by increasing the
kpPosXY and the
kpVelXY, in combination with the integral gain
kiPosZ. Lower lateral gain and high integral gain will overshoot the quad1 before approaching the target position.
Scenario 5 and 6
The scenario 5 and 6 are the extra challenge scenarios with more advance trajectories. Currently, I cannot pass all of the challenges. In the scenario 5, I still cannot suppress the error of the quad1:
In the scenario 6 with 9 quadcopters, I can see that 2 vehicles are crashed and other 7 vehicles can follow the trajectory.