Modern Techniques for Improving Aerial Vehicle Performance
In this project, we implement a few modern techniques for improving the performance of aerial vehicles. We build on the original Quadcopter Simulation and Control (Quad_SimCon) program developed by John Bass as follows:
- Implement multiple vehicles and obstacle avoidance
- Improve on standard potential fields using shifting planar inequality constraints
- Tune the proportional-integral-derivative (PID) gains in real-time using Reinforcement Learning
You can configure most of the simulation parameters in the config.py file.
Note: If you think this is cool and would like to build on this architecture, feel free to contact me at [email protected]
Motivation
Why implement shifting planar inequality constraints? Potential Fields is common strategy for obstacle avoidance. It is simple and effective but is not tailored for aerial vehicles. As opposed to ground vehicles, aerial vehicles require constant corrections to stay in flight. These unique energy characteristics motivate our work in improving on potential fields so that aerial vehicles can save energy when maneuvering around obstacles.
Why implement reinforcement learning? Designing a controller for aerial vehicles involves tuning a number of parameters (gains). These can be derived from first principles or, as we have done here, through data-driven methods like reinforcement learning.
Why quaternions? Quaternions are kind of an abstract concept, but they're awesome. By replacing the sine and cosine terms with a vector in the equations of motion, quaternions save computation time. They also help resolve some practical problems when operating in computationally inconvenient flight configurations (mainly at angles like 0, 90, 180, and 270 degrees). Thank you to John Bass for sharing this aspect of the code.
Relevant Publications
Planar Inequality Constraint Shifting described here (applied to the more sophisticated control technique, Model Predictive Control):
- P. T. Jardine, S. Givigi, and S. Yousefi, Planar Inequality Constraints for Stable, Collision-free Model Predictive Control of a Quadcopter , IFAC-PapersOnLine, Volume 50, Issue 1, July 2017, Pages 9095-9100
Potential Fields described here:
- A. Woods and H. La, A Novel Potential Field Controller for Use on Aerial Robots , arXiv, 2017
- Y. Koren and J. Borenstein, Potential Field Methods and Their Inherent Limitations for Mobile Robot Navigation , Proceedings of the IEEE Conference on Robotics and Automation, Sacramento, California, April 1991, pp. 1398-1404
Finite Action-set Learning Automata described here (from a system-of-systems perspective):
- P.T. Jardine, S.N. Givigi, and S.Yousefi, Leveraging Data Engineering to Improve Unmanned Aerial Vehicle Control Design, IEEE Systems Journal, July 2020
Smoother Trajectory Tracking Amidst Obstacles
Results
In Figure 1, we see shifting planar inequalities reduce oscillations when flying near obstacles. In Figure 2, we see this reduces error and energy consumption.
Figure 1 - (a) Potential Fields only; (b) With shifting planar inequalities
Figure 2 - (a) Improvements in Error Accumlation; (b) Energy Savings
Automatic Controller Tuning using Reinforcement Learning
We implemented Reinforcement Learning - specifically, Finite Action-set Learning Automata (FALA) - to tune the controller gains of a simulated Quadcopter. A detailed description of the learning technique is described in the publications above.
Results
In Figure 3, we provide an animated GIF showing the performance of the vehicle at the early phases of learning (i.e. with randomly selected gains). In Figure 4, we see that the vehicle performs much better when using gains learned with FALA.
Figure 3 - Poor performance with unlearned parameters
Figure 4 - Better performance with learned parameters
Animating n vehicles
We also provided an animation function that supports an arbitrary number of vehicles.
Figure 5 - Animation with 5 vehicles
key files
- run_3D_simulation.py - this is the main script and was substantially modified to integrate new components of the learning architecture.
- fala.py - totally new module that implements the reinforcement learning strategy as a new fala class
- cntrl.py - the controller files were slightly modified to integrate with the learning module
- waypoints.py - some minor modifications were made to waypoint generation in order to integrate with the learning module
Description of the Quadcopter Simulator
Detailed explaination of the simulator dynamics, control design, and trajectory generation is available here. Below is a summary pulled directly from the original repository README by John Bass.
PyDy Quadcopter
PyDy, short for Python Dynamics, is a tool kit made to enable the study of multibody dynamics. At it's core is the SymPy mechanics package, which provides an API for building models and generating the symbolic equations of motion for complex multibody systems.
In the PyDy Scripts folder, you will find multiple different scripts. Some express the drone's orientation in the NED frame, and some in the ENU frame.
NED frame : The world is oriented in such a way that the X direction is North, Y is East and Z is Down. The drone's orientation in this frame is front-right-down (frd). This is a conventional/classic frame used in aeronautics, and also the frame used for the PX4 multicopter controller.
ENU frame : The world is oriented in such a way that the X direction is East, Y is North and Z is Up. The drone's orientation in this frame is front-left-up (flu). This frame is widely used for its vizualizing simplicity (z is up), however it possesses a vizualizing complexity where "pitching up" actually results in a negative pitch angle.
The other difference in the provided scripts is that some use Euler angles phi (φ), theta (θ), psi (ψ) (roll, pitch, yaw) to describes the drone's orientation, while the other scripts uses a quaternion.
Euler angles : In the Euler angle scripts, the drone is first rotated about its Z axis by a certain yaw angle (heading), then about its new Y axis by a certain pitch angle (elevation) and then finaly about its new X axis by a certain roll angle (bank). The rotation order is thus a Body ZYX rotation. Using Euler angles, the resulting equations of motion possesses many sine and cosine functions, meaning that it requires more time to calculate. One must remember that these equations of motion are to be integrated in order to simulated the quadcopter's motion (using an ODE function for example). This means that the equations of motion are computed many time during a single timestep of the simulation.
Quaternion : The use of a quaternion to describe the drone's rotation significantly decreases computing time, because of the absence of sine and cosine functions in the equations of motion. The quaternion is formed with the angle value first, followed by the 3 axis values, like so : q = [q0, q1, q2, q3] = [qw, qx, qy, qz]. While it is sometimes complex to understand the rotation expressed by a quaternion, the quadcopter attitude control provided in this project uses quaternions (sets a desired quaternion, computes a quaternion error, ... ).
The quadcopter states are the following :
- Position (x, y, z)
- Rotation (φ, θ, ψ) or (q0, q1, q2, q3)
- Linear Velocity (x_dot, y_dot, z_dot)
- Angular Velocity (p, q, r) (The drone's angular velocity described in its own body frame, also known as Ω. This is not equivalent to phi_dot, theta_dot, psi_dot)
The PyDy scripts use the Kane Method to derive the system's equations and output a Mass Matrix (MM) and a right-hand-side vector (RHS). These outputs are used to obtain the state derivative vector s_dot in the equation MM*s_dot = RHS. To solve for s_dot, one must first calculate the inverse of the Mass Matrix, to be used in the equation s_dot = inv(MM)*RHS. Fortunately, for the quadcopter in this project, the Mass Matrix is a diagonal matrix and inverses quite easily. One can numerically solve for s_dot during the integration, but PyDy is able to analytically solve the state derivatives, which then can easily be copied into the ODE function.
Currently, I have seperated the PyDy scripts into 3 folders. The first is just a basic quadcopter. In the second, I have added gyroscopic precession of the rotors. And in the third, wind and aerodynamic drag was added.
NOTE: In my scripts, Motor 1 is the front left motor, and the rest are numbered clockwise. This is not really conventional, but is simple enough.
PyDy Installation
To be able to run the PyDy scripts of this project, you need to first install PyDy and its dependancies.
If you have the pip package manager installed you can simply type:
$ pip install pydy
Or if you have conda you can type:
$ conda install -c conda-forge pydy
Simulation and Control
First off, the world and body orientation can be switch between a NED or ENU frame in the config.py file. The other scripts then handle which equations to use, depending on the chosen orientation. It also has to be mentioned that both the PyDy scripts and the simulation aim to simulate the behaviour of a X configuration quadcopter (not a + configuration).
The only packages needed for the simulation part of this project are Numpy and Matplotlib.
Simulation
In quad.py, I've defined a Quadcopter Class and its methods are relatively simple : initialize the quadcopter with various parameters and initial conditions, update the states, calculate the state derivatives, and calculate other useful information. The simulation uses a quaternion in the state vector to express the drone's rotation, and the state derivative vector is copied from the corresponding PyDy script. However, 8 other states were added to simulate each motors dynamics (2nd Order System) :
- Motor Angular Velocities (wM1, wM2, wM3, wM4)
- Motor Angular Acceleration (wdotM1, wdotM2, wdotM3, wdotM4)
The current parameters are set to roughly match the characteristics of a DJI F450 that I have in my lab, and the rotor thrust and torque coefficients have been measured.
Trajectory Generation
Different trajectories can be selected, for both position and heading. In waypoints.py, you can set the desired position and heading waypoints, and the time for each waypoint. You can select to use each waypoint as a step, or to interpolate between waypoints, or to generate a minimum velocity, acceleration, jerk or snap trajectory. Code from Peter Huang was modified to allow for these 4 types of trajectories and to allow for segments between waypoints of different durations. There is also the possibility to have the desired heading follow the direction of the desired velocity.
Control
There are currently 3 controllers coded in this project. One to control XYZ positions, one to control XY velocities and Z position, and one to control XYZ velocities. In all 3 current controllers, it is also possible to set a Yaw angle (heading) setpoint. There are plans to add more ways of controlling the quadcopter.
The control algorithm is strongly inspired by the PX4 multicopter control algorithm. It is a cascade controller, where the position error (difference between the desired position and the current position) generates a velocity setpoint, the velocity error then creates a desired thrust magnitude and orientation, which is then interpreted as a desired rotation (expressed as a quaternion). The quaternion error then generates angular rate setpoints, which then creates desired moments. The states are controlled using a PID control. Position and Attitude control uses a simple Proportional (P) gain, while Velocity and Rate uses Proportional and Derivative (D) gains. Velocity also has an optional Integral (I) gain if wind is activated in the simulation.
There are multiple wind models implemented. One were the wind velocity, heading and elevation remain constant, one where they vary using a sine function, and one where they vary using a sine function with a random average value.
The mixer (not based from PX4) allows to find the exact RPM of each motor given the desired thrust magnitude and desired moments.