The goal of the project is to first implement a prototype controller in Python and then translate that code into C++ with some modifications that will improve its robustness and performance.
Program, script, configuration, and log:
Python script: controller.py
Log file: Logs/TLog-20180417.2007.txt
Located in: https://github.com/jwdunn1/FCND-Controls-CPP:
C++ program: src/QuadControl.cpp
C++ header: src/QuadControl.h
Configuration: config/QuadControlParams.txt
The following report consists of 4 sections:
01 Implementation
01.1 Body rate control
01.2 Altitude control
01.3 Roll-pitch control
01.4 Yaw control
01.5 Lateral position control
01.6 Motor commands in C++
02 Flight evaluation
02.1 Python controller performance
02.2 C++ controller performance
03 References
Books, research papers, and tools
04 Appendix
04.1 Rotation matrix
04.2 Angular velocity
04.3 Thrust computation
Each of the implemented methods of the architecture fit together as illustrated in Figure 1. Beginning with the code from the Full 3D Control exercise presented in class, the methods were refactored several times to gain a full understanding of the mathematics, physics, and data flow.
Figure 1: Control structure
The controller is a proportional controller on body rates to commanded moments. The controller takes into account the moments of inertia of the drone when calculating the commanded moments. For the Python version, this controller operates at a frequency of 40 hertz through the gyro callback. (The C++ version operates all controllers synchronously at 500 Hz.)
Python: [see lines 105-112 in controller.py]
C++: [see lines 111-117 in QuadController.cpp]
Part one of attitude control, the altitude controller uses both the down position and the down velocity to command thrust. The drone's mass is accounted for to ensure that the output value is a thrust value in newtons. The thrust includes the non-linear effects from non-zero roll/pitch angles.
Note: The Python version of the attitude controller (which includes altitude, roll-pitch, and yaw controllers) operates at 40 Hz through the attitude callback.
Python: [see lines 135-144 in controller.py]
Additionally, the C++ altitude controller contains an integrator to handle the weight non-idealities presented in scenario 4.
C++: [see lines 149-159 in QuadController.cpp]
Part two of attitude control, the roll-pitch controller uses the acceleration and thrust commands, in addition to the vehicle attitude to output a body rate command. The controller accounts for the non-linear transformation from local accelerations to body rates. The drone's mass is accounted for when calculating the target angles. See also the rotation matrix and angular velocity in the appendix below. Lateral acceleration is limited by a maximum tilt angle.
Python: [see lines 158-170 in controller.py]
C++: [see lines 190-199 in QuadController.cpp]
Part three of attitude control, the yaw controller is a linear/proportional heading mechanism which outputs yaw rate commands. Further, the yaw error is checked for a valid range between -π and π.
Python: [see lines 179-185 in controller.py]
C++: [see lines 222-228 in QuadController.cpp]
Finally, the lateral position controller uses the local NE position and velocity to generate a commanded local acceleration. The Python version operates at 80 Hz through the velocity callback. The trajectory_control routine optionally returns an acceleration feed-forward value to pass along to the lateral position controller (a testing script mentioned in section 07 below makes use of this feed forward value). Additionally, to smooth the sharp corners of the test trajectory and improve computation of the commanded acceleration, an integrator uses the time duration since the last call.
Python: [see lines 208-219 in controller.py]
C++: [see lines 263-274 in QuadController.cpp]
The thrust and moments are converted to the appropriate four different desired thrust commands for the motors. The dimensions of the drone arm length (L) and motor torque coefficient (kappa) are accounted for when calculating thrust from desired rotation moments. See also the thrust computation in the appendix below.
C++: [see lines 77-86 in QuadController.cpp]
The Python controller successfully follows the provided test trajectory, meeting the minimum flight performance metrics. See the log file (TLog.txt) in the Logs folder. The control gains were discovered manually through trial and error.
For this, the drone passes the provided evaluation script with the default parameters. These metrics being:
- the drone flies the test trajectory faster than 20 seconds
- the maximum horizontal error is less than 2 meters
- the maximum vertical error is less than 1 meter
Figures 2-4 below are graphic results from a successful flight.
Figure 2: Example results from the test script.
Figure 3: Overhead view of test trajectory (gray line), flight path (dashed blue), start (green), goal (red). The light gray area is a 2-meter lateral tolerance.
Figure 4: Horizontal and vertical error metrics from a successful flight
The NonlinearController class can operate with the default controls_flyer.py script, or with the controls_flyer-TEST.py script. The TEST version improves performance by slowing the attitude and position controllers to 20 Hz. A comparison sequence of 20 successful runs of each script is plotted in Figure 5 and demonstrates vertical error reduction by 31.5% (blue) and horizontal error reduction by 2.3% (red). The TEST version also includes alternative trajectories useful for tuning the control gains. A hover stability test indicates the Unity simulator contains a GPS noise radius less than 0.25 meter.
Figure 5: Error metrics from 40 successful flights, comparing the default script (dashed) with reduced rate script (solid). Vertical error is blue and horizontal error is red. Lower values are better.
The C++ controller successfully follows the provided test trajectory and passes (numerically and visually) all scenarios. See Figures 6-11 below.
In each scenario, the drone looks stable and performs the required task. The controller is able to handle the non-linearities of scenario 4 (all three drones in the scenario perform their required tasks with the same control gains). The control gains were discovered manually through trial and error, finding workable ranges and setting to mid-points, except yaw gain which is intentionally set to a lower value for higher stability.
Figure 6: Metrics from successful scenarios (note all are PASS)
Figure 7: Scenario 1 - tuning mass to hover for at least 0.8 seconds
Figure 8: Scenario 2 - stabilize the rotational motion and bring the vehicle back to level attitude
Figure 9: Scenario 3 - move to a destination (yellow yaws in flight)
Figure 10: Scenario 4 - nonidealities and robustness (red: overweight, orange: ideal, green: shifted mass)
Figure 11: Scenario 5 - trajectory following (red: no feed-forward, orange: with feed-forward)
An additional scenario tests hover stability. In this case, position following error is less than 1.2 millimeters for at least 3.33 seconds.
[1] Quad Rotorcraft Control
Carrillo, Lopez, Lozano, and Pegard
https://www.springer.com/us/book/9781447143987
[2] The GRASP Multiple Micro UAV Testbed
Michael, Mellinger, Lindsey, and Kumar
https://pdfs.semanticscholar.org/20b0/f0268bc11c55389816223d712d85203e2936.pdf
[3] Feedback Systems, 2nd Ed
Karl J. Åström and Richard M. Murray
http://www.cds.caltech.edu/~murray/amwiki/index.php
[4] Feed-Forward Parameter Identification for Precise Periodic Quadrocopter Motions
Schoellig, Wiltsche, and D’Andrea
http://www.dynsyslab.org/wp-content/papercite-data/pdf/schoellig-acc12.pdf
[5] On-board Model Predictive Control of a Quadrotor Helicopter
Patrick Bouffard
https://www2.eecs.berkeley.edu/Pubs/TechRpts/2012/EECS-2012-241.pdf
[6] Quadcopter Dynamics and Simulation
Andrew Gibiansky
http://andrew.gibiansky.com/blog/physics/quadcopter-dynamics
[7] Propeller Thrust and Drag in Forward Flight
Rajan Gill and Raffaello D’Andrea
http://flyingmachinearena.org/wp-content/publications/2017/gilIEEE17.pdf
[8] Thrust Mixing, Saturation, and Body-Rate Control
Faessler, Falanga, and Scaramuzza
http://rpg.ifi.uzh.ch/docs/RAL17_Faessler.pdf
[9] Modelling and Control of the Crazyflie Quadrotor
Marcus Greiff
http://lup.lub.lu.se/student-papers/record/8905295/file/8905299.pdf
[10] Control of VTOL Vehicles with Thrust-direction Tilting
Hua, Hamel, and Samson
https://arxiv.org/pdf/1308.0191.pdf
[11] A platform for aerial robotics research and demonstration: The Flying Machine Arena
Lupashin, Hehn, Mueller, Schoellig, Sherback, and D’Andrea
http://flyingmachinearena.org/wp-content/publications/2014/lupashin2014platform.pdf
[12] Matrix Equations Solver
https://www.symbolab.com/solver/matrix-equations-calculator
[13] Servo Tuning
http://support.motioneng.com/downloads-notes/tuning
Figure 12: Body coordinates and forces
End of report