Udacity Flying Car Nanodegree - Project 3 - 3D Quadrotor Controller
In this project, I have implement and tuned a cascade PID controller for drone trajectory tracking. The theory behind the controller design using feed-forward strategy is explained in details on Udacity's instructor, Angela P. Schoellig, on her paper Feed-Forward Parameter Identification for Precise Periodic Quadrocopter Motions. The following diagram could be found in the course content describing the cascaded control loops of the trajectory-following controller:
The idea is to start with tuning and building the inner most controller first. Then the controllers dependent on it are tuned further. The project gives the intuition of how going from innermost to outer controller building takes place in practice.
A casaded controller needs to be implemented for this project using C++ so that the quad can follow the commanded trajectory. The controller should be able to handle deviations like heavier mass than what the model accounted for or an offset in the quad's center of mass. The quad should operate within the dynamic and legal constraints.
Udacity provides a seed project with the simulator implementation and placeholders for the controller code. The seed project README.md give guides to run the project and information of the task we need to execute for implementing the controller. There are five scenarios we need to cover. The simulator runs in a loop on the current scenario and show on the standard output an indication the scenario pass or not.
All the C++ code is in the /src directory. The more interesting files are:
- /config/QuadControlParams.txt: This file contains the configuration for the controller. While the simulator is running, you can modify and save changes to this file, and the simulator will "refresh" those parameters on the next loop execution.
- /src/QuadControl.cpp: This is where all the fun is because this is where the controller logic and dynamic model of the quad is implemented in code.
Nothing extra needs to install but the IDE is necessary to compile the code. In my case Microsoft Visual Studio 2019 because I am using Windows. Please, follow the instructions on the seed project README.md.
Following the instruction on the seed project, load the project on the IDE. Remember the code is on /src.
In this scenario, we adjust the mass of the drone in /config/QuadControlParams.txt until it hovers for a bit:
This video is Scenario_1.mp4
When the scenario is passing the test, you should see this line on the standard output:
PASS: ABS(Quad.PosFollowErr) was less than 0.500000 for at least 0.800000 seconds
We begin by establishing a relation between the rotor forces and (a) Required net force in z direction; (b) Required net torque along the quad's local frame's x, y, z axes. This will enable the drone to adjust the rotor speed according to the net forces and torque on the system commanded by the controller. The GenerateMotorCommands method needs to be programmed resolving these equations:
Where all kf*(omega_1^2) would be the motor's thrust at the first rotor, I_x*u_p_bar would be the net torque along the x-axis, c_bar*kf is the total thrust, kappa is the drag/thrust ratio (kappa=km/kf) and l is the drone arm length over square root of two. These equations come from the classroom lectures. There are a couple of things to consider. For example, on NED coordinates the z axis is inverted that is why the moment on z was inverted here. Another thing to note is the numbering of the rotors: front left->(1); front right->(2); rear right->(3); rear left->(4).
Now is the time to start coding the controller. Here we deal with the innermost Attitude controller which further contains the following three controllers within it:
In scenario 2, simulator gives the drone an initial disturbance: an initial angular velocity about x axis. Task is to do something so that the angular velocity goes back to zero (acheived by Body rate controller) and the angular positon about x axis (roll) goes back to zero (achieved by roll, pitch and yaw controllers).
Ideally the Body rate controller will receive the commanded angular velocity from roll, pitch and yaw controllers but that'll happen when all four of those controllers are functional. We get started by ignoring roll, pitch and yaw controllers and manually command the Body Rate Controller to maintain zero angular velocity and tune it. Once it responds to this command, we start setting up roll, pitch and yaw controllers to give it the angular velocities desired to make roll, pitch, yaw back to zero (which is again a manual command). These controllers will obtain commands from higher level controllers (Lateral and Altitude controllers) to compute the desired values of roll, pitch and yaw.
The first step is to implement the BodyRateControl method applying a P controller. At this point, the kpPQR parameter has to be tuned to stop the drone from flipping, but first, some thrust needs to be commanded in the altitude control because we don't have thrust commanded on the GenerateMotorCommands anymore. A good value is thurst = mass * CONST_GRAVITY (this is chosen to simply things by commanding the drone to just balance the gravitational force with no intent of acelerating in the z direction)
Once this is done, we move on to the RollPitchControl method (because the initial disturbance was given along the x axis). For this implementation, you need to apply a few equations. You need to apply a P controller to the elements R13 and R23 of the rotation matrix from body-frame accelerations and world frame accelerations:
Here the subscript a and c denote actual and commanded values respectively. Now, as we need to output roll and pitch rates, there is another equation to apply:
It is important to notice you have received the magnitude of thrust in the function arguments. This needs to be inverted and converted to acceleration with a proper sign before applying the equations (as z-acceleration is positive downwards).
Another thing to make sure is constraining the maximum tilt in the roll and pitch angle. This can be applied on b_x and b_y due to the fact that if you apply small angle approximations, they tend to theta and -phi respectively.
After the implementation is done, start tuning kpBank and kpPQR until the drone corrects its roll and angular velocity along x axis:
This video is Scenario_2.mp4
When the scenario is passing the test, you should see this line on the standard output:
PASS: ABS(Quad.Roll) was less than 0.025000 for at least 0.750000 seconds
PASS: ABS(Quad.Omega.X) was less than 2.500000 for at least 0.750000 seconds
There are three methods to implement here:
- AltitudeControl: This is a feedforward PID controller to control the acceleration meaning the thrust needed to control the altitude.
Pterm helps in correcting the deviation from the desired position.Dterm reduces the oscillations at the equilibrium position and slows down the quad to attain stability.Iterm helps in correcting the residual error from the equilibrium behaviour when PD controller is not able to bring the quad to desired position and velocity.
Following are the equations of motion related to this controller:
It is important to notice the difference between the constraints imposed on velocity vs constraints imposed on acceleration. Accelerations can be directly controlled by changing the net force by varying the omegas of rotors. So this constraint can be put after finding the output from the PID controller. If the controller asks for a=2.2 m/s^2 and quad is only capable of providing 2 m/s^2, the desired acceleration can be overwritten by this threshold. This is not the case for velocity constrains. Let's say, we need a velocity higher than maxAscentRate. We give it to the controller and it generates some acceleration to achieve it. Now, that would result in the increase in velocity. Now if it already has some acceleration, the quad's velocity cannot be changed immediately if it surpasses the maximum velocity threshold even by instantaneously changing the acceleration. To avoid that, the commanded velocity should be kept in check so that the quad never crosses the threshold.
To test this, go back to scenario 2 and make sure the drone doesn't fall. In that scenario, the desired z position is set at some constant value with zero net velocity and acceleration. In this case, the thrust should be mass * CONST_GRAVITY.
- LateralPositionControl: This is cascaded proportional controller (inner one to get velocity and an outer one to get acceleration) to control acceleration on
xandy. This helps us in tuning the parameters as there exists a relation between gains for a critically damped condition (Kv/Kp~ 4). Note that implementing a feedforward PD controller was also an option which could have implemented here but wasn't chosen asKp and Kddon't have a relation between them unlikeKp and Kv.
It is important to note the way velocity constraints and accleration constraints have been applied in the X-Y plane. Recall that while finding the magnitude of a vector, if each component is scaled by a factor k then the magnitude of the whole vector gets scaled by k. To limit the net velocity and acceleration in X-Y plane, a downscaling factor needs was calculated so that net vector's magnitude stays within the constrained values.
- YawControl: This is a simpler case because it is P controller (as yaw is being related to its first derivative only). It is better to optimize the yaw to be between
[0, 2*pi]. This will ensure that the difference between actual and commanded yaw would lie between[-2*pi, 2*pi]. Suppose you don't do this and consider the two example cases where the difference between the angles are2*pior4*pi. Both will mean the same mathematically, but would command higher magnitude of angular velocity along the z-axis.
Note: Avoid changing the gains of the controller that were already tuned. We go from tuning the innermost controller to the outermost chronologically. Here is a video of the scenario when it passes:
This video is Scenario_3.mp4
When the scenario is passing the test, you should see this line on the standard output:
PASS: ABS(Quad1.Pos.X) was less than 0.100000 for at least 1.250000 seconds
PASS: ABS(Quad2.Pos.X) was less than 0.100000 for at least 1.250000 seconds
PASS: ABS(Quad2.Yaw) was less than 0.100000 for at least 1.000000 seconds
This is a fun scenario. Everything is almost coded and tuned already (given that Integral term was added while designing the Altitude controller). Minor tweaking of the gains might be required here though. If the quad with off-center COM (yellow) is drifting way too much from the straight line joining the start and end point, increasing the proportional gain will help to apply stronger force to reduce deviation. If it starts overshooting the target, increasing the differential gain term would help to quickly change the velocity. If the heavier quad (red) is sinking downwards, increasing the proportional term will help in getting it up during its motion. Increasing the integral term will help in correcting the z-position at the final position if it is still lower than the desired height. Here's how the result of this scenario will look like:
This video is Scenario_4.mp4
When the scenario is passing the test, you should see this line on the standard output:
PASS: ABS(Quad1.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
PASS: ABS(Quad2.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
PASS: ABS(Quad3.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
At this point, the yellow quad will most likely follow the figure eight shaped trajectory. You can play with the gains to decrease the settling time (increasing the proportional gains will help). You might not be able to ace this scenario if the constraints are not put on the various state variables of the quad. This might result into random behavior of the drone once those constraints are surpassed.
The extra challenge in this scenario is to complete the trajectory planning for the red colored quad. It only has the position command at various time stamps. Various approaches can be adopted to calculate the velocity values at each waypoint the quad needs to follow. This can be done in Generate_pos_vel_Fig_8.ipynb by assigning values to vx, vy, vz. After running this python script, red drone's trajectory file will get updated with the velocity information. This additon will provide the feed-forward velocity terms in the controller equations. The yellow drone's trajectory file already contains the velocity information.
Here's how the result of this scenario will look like:
This video is Scenario_5.mp4
When the scenario is passing the test, you should see this line on the standard output:
PASS: ABS(Quad2.PosFollowErr) was less than 0.250000 for at least 3.000000 seconds
There is one more optional scenario on this project. If the above scenarios were programmed and tuned correctly, this would be a cake-walk. Here is the video of this scenario:
This video is Scenario_6.mp4
Provide a Writeup / README that includes all the rubric points and how you addressed each one. You can submit your write-up as markdown or pdf.
This markdown is the write-up.
The body rate control is implemented as proportional control in /src/QuadControl::BodyRateControl method from line 100 to 130 using C++.
The roll pitch control is implemented in /src/QuadControl::RollPitchControl method from line 133 to 189 using C++.
The altitude control is implemented in /src/QuadControl::AltitudeControl method from line 191 to 236 using C++.
The lateral position control is implemented in /src/QuadControl::LateralPositionControl method from line 239 to 305 using C++.
The yaw control is implemented in /src/QuadControl::YawControl method from line 309 to 337 using C++.
The calculation implementation for the motor commands is in /src/QuadControl::GenerateMotorCommands method from line 56 to 98.
Your C++ controller is successfully able to fly the provided test trajectory and visually passes the inspection of the scenarios leading up to the test trajectory.
Ensure that in each scenario the drone looks stable and performs the required task. Specifically check that the controller is able to handle the non-linearities of scenario 4 (all three drones in the scenario should be able to perform the required task with the same control gains used).
The implementation pass scenarios 1 - 5 on the C++ simulator:
# Scenario 1
PASS: ABS(Quad.PosFollowErr) was less than 0.500000 for at least 0.800000 seconds
# Scenario 2
PASS: ABS(Quad.Roll) was less than 0.025000 for at least 0.750000 seconds
PASS: ABS(Quad.Omega.X) was less than 2.500000 for at least 0.750000 seconds
# Scenario 3
PASS: ABS(Quad1.Pos.X) was less than 0.100000 for at least 1.250000 seconds
PASS: ABS(Quad2.Pos.X) was less than 0.100000 for at least 1.250000 seconds
PASS: ABS(Quad2.Yaw) was less than 0.100000 for at least 1.000000 seconds
# Scenario 4
PASS: ABS(Quad1.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
PASS: ABS(Quad2.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
PASS: ABS(Quad3.PosFollowErr) was less than 0.100000 for at least 1.500000 seconds
# Scenario 5
PASS: ABS(Quad2.PosFollowErr) was less than 0.250000 for at least 3.000000 seconds