In this project, x, y, z is in NED and the thrust upward is positive. The controller for the quad uses the collective thrust and rotational moments as inputs and those inputs are converted to each rotor thrust. (F1: front left, F2: front right, F3: rear left, F4: rear right)
The translational motion in the inertial frame is described by
where R(t) is the rotation matrix from the body frame to the inertial frame and c(t) is the collective thrust normalized by the vehicle mass.
z_dot_dot is commanded using this equation. This controller also assume the weight non-idealities and uses an integrator to handle that so it's PID controller.
float QuadControl::AltitudeControl(float posZCmd, float velZCmd, float posZ, float velZ, Quaternion<float> attitude, float accelZCmd, float dt)
{
// Calculate desired quad thrust based on altitude setpoint, actual altitude,
// vertical velocity setpoint, actual vertical velocity, and a vertical
// acceleration feed-forward command
// INPUTS:
// posZCmd, velZCmd: desired vertical position and velocity in NED [m]
// posZ, velZ: current vertical position and velocity in NED [m]
// accelZCmd: feed-forward vertical acceleration in NED [m/s2]
// dt: the time step of the measurements [seconds]
// OUTPUT:
// return a collective thrust command in [N]
// HINTS:
// - we already provide rotation matrix R: to get element R[1,2] (python) use R(1,2) (C++)
// - you'll need the gain parameters kpPosZ and kpVelZ
// - maxAscentRate and maxDescentRate are maximum vertical speeds. Note they're both >=0!
// - make sure to return a force, not an acceleration
// - remember that for an upright quad in NED, thrust should be HIGHER if the desired Z acceleration is LOWER
Mat3x3F R = attitude.RotationMatrix_IwrtB();
float thrust = 0;
////////////////////////////// BEGIN STUDENT CODE ///////////////////////////
float z_err = posZCmd - posZ;
velZCmd += kpPosZ * z_err;
velZCmd = CONSTRAIN(velZCmd, -maxAscentRate, maxDescentRate);
integratedAltitudeError += z_err * dt;
float z_err_dot = velZCmd - velZ;
accelZCmd += KiPosZ * integratedAltitudeError + kpVelZ * z_err_dot;
float b_z = R(2, 2);
thrust = mass * ((float)CONST_GRAVITY - accelZCmd) / b_z;
/////////////////////////////// END STUDENT CODE ////////////////////////////
return thrust;
}
The R_dot_commanded has the following relation with the body rates. The roll pitch controller commands the desired body rates for the body rate controller using this equation.
V3F QuadControl::RollPitchControl(V3F accelCmd, Quaternion<float> attitude, float collThrustCmd)
{
// Calculate a desired pitch and roll angle rates based on a desired global
// lateral acceleration, the current attitude of the quad, and desired
// collective thrust command
// INPUTS:
// accelCmd: desired acceleration in global XY coordinates [m/s2]
// attitude: current or estimated attitude of the vehicle
// collThrustCmd: desired collective thrust of the quad [N]
// OUTPUT:
// return a V3F containing the desired pitch and roll rates. The Z
// element of the V3F should be left at its default value (0)
// HINTS:
// - we already provide rotation matrix R: to get element R[1,2] (python) use R(1,2) (C++)
// - you'll need the roll/pitch gain kpBank
// - collThrustCmd is a force in Newtons! You'll likely want to convert it to acceleration first
V3F pqrCmd;
Mat3x3F R = attitude.RotationMatrix_IwrtB();
////////////////////////////// BEGIN STUDENT CODE ///////////////////////////
float R11 = R(0, 0);
float R12 = R(0, 1);
float R13 = R(0, 2);
float R21 = R(1, 0);
float R22 = R(1, 1);
float R23 = R(1, 2);
float R33 = R(2, 2);
float c = -collThrustCmd / mass;
V3F b_c = V3F(accelCmd.x / c, accelCmd.y / c, 0.f);
b_c.constrain(-maxTiltAngle, maxTiltAngle);
V3F b_error = b_c - V3F(R(0, 2), R(1, 2), 0.f);
V3F b_c_dot = kpBank * b_error;
pqrCmd.x = (R21 * b_c_dot.x - R11 * b_c_dot.y) / R33;
pqrCmd.y = (R22 * b_c_dot.x - R12 * b_c_dot.y) / R33;
pqrCmd.z = 0.f;
/////////////////////////////// END STUDENT CODE ////////////////////////////
return pqrCmd;
}
It is better to implement and test the inner controller first. So the body rate controller and then the roll pitch controller should be implemented. This proportional body rate controller commands the rotation moments using the error of the body rates.
V3F QuadControl::BodyRateControl(V3F pqrCmd, V3F pqr)
{
// Calculate a desired 3-axis moment given a desired and current body rate
// INPUTS:
// pqrCmd: desired body rates [rad/s]
// pqr: current or estimated body rates [rad/s]
// OUTPUT:
// return a V3F containing the desired moments for each of the 3 axes
// HINTS:
// - you can use V3Fs just like scalars: V3F a(1,1,1), b(2,3,4), c; c=a-b;
// - you'll need parameters for moments of inertia Ixx, Iyy, Izz
// - you'll also need the gain parameter kpPQR (it's a V3F)
V3F momentCmd;
////////////////////////////// BEGIN STUDENT CODE ///////////////////////////
V3F I = V3F(Ixx, Iyy, Izz);
momentCmd = I * kpPQR * (pqrCmd - pqr);
/////////////////////////////// END STUDENT CODE ////////////////////////////
return momentCmd;
}
V3F QuadControl::LateralPositionControl(V3F posCmd, V3F velCmd, V3F pos, V3F vel, V3F accelCmdFF)
{
// Calculate a desired horizontal acceleration based on
// desired lateral position/velocity/acceleration and current pose
// INPUTS:
// posCmd: desired position, in NED [m]
// velCmd: desired velocity, in NED [m/s]
// pos: current position, NED [m]
// vel: current velocity, NED [m/s]
// accelCmdFF: feed-forward acceleration, NED [m/s2]
// OUTPUT:
// return a V3F with desired horizontal accelerations.
// the Z component should be 0
// HINTS:
// - use the gain parameters kpPosXY and kpVelXY
// - make sure you limit the maximum horizontal velocity and acceleration
// to maxSpeedXY and maxAccelXY
// make sure we don't have any incoming z-component
accelCmdFF.z = 0;
velCmd.z = 0;
posCmd.z = pos.z;
// we initialize the returned desired acceleration to the feed-forward value.
// Make sure to _add_, not simply replace, the result of your controller
// to this variable
V3F accelCmd = accelCmdFF;
////////////////////////////// BEGIN STUDENT CODE ///////////////////////////
const V3F error = posCmd - pos;
velCmd += kpPosXY * error;
velCmd.x = CONSTRAIN(velCmd.x, -maxSpeedXY, maxSpeedXY);
velCmd.y = CONSTRAIN(velCmd.y, -maxSpeedXY, maxSpeedXY);
const V3F error_dot = velCmd - vel;
accelCmd += kpVelXY * error_dot;
accelCmd.x = CONSTRAIN(accelCmd.x, -maxAccelXY, maxAccelXY);
accelCmd.y = CONSTRAIN(accelCmd.y, -maxAccelXY, maxAccelXY);
accelCmd.z = 0.0F;
/////////////////////////////// END STUDENT CODE ////////////////////////////
return accelCmd;
}
float QuadControl::YawControl(float yawCmd, float yaw)
{
// Calculate a desired yaw rate to control yaw to yawCmd
// INPUTS:
// yawCmd: commanded yaw [rad]
// yaw: current yaw [rad]
// OUTPUT:
// return a desired yaw rate [rad/s]
// HINTS:
// - use fmodf(foo,b) to unwrap a radian angle measure float foo to range [0,b].
// - use the yaw control gain parameter kpYaw
float yawRateCmd=0;
////////////////////////////// BEGIN STUDENT CODE ///////////////////////////
float error = yawCmd - yaw;
error = fmodf(error, 2.*F_PI);
yawRateCmd = kpYaw * error;
/////////////////////////////// END STUDENT CODE ////////////////////////////
return yawRateCmd;
}
This is the readme for the C++ project.
For easy navigation throughout this document, here is an outline:
Regardless of your development platform, the first step is to download or clone this repository.
Once you have the code for the simulator, you will need to install the necessary compiler and IDE necessary for running the simulator.
Here are the setup and install instructions for each of the recommended IDEs for each different OS options:
For Windows, the recommended IDE is Visual Studio. Here are the steps required for getting the project up and running using Visual Studio.
- Download and install Visual Studio
- Select Open Project / Solution and open
<simulator>/Simulator.sln - From the Project menu, select the Retarget solution option and select the Windows SDK that is installed on your computer (this should have been installed when installing Visual Studio or upon opening of the project).
- To compile and run the project / simulator, simply click on the green play button at the top of the screen. When you run the simulator, you should see a single quadcopter, falling down.
For Mac OS X, the recommended IDE is XCode, which you can get via the App Store.
- Download and install XCode from the App Store if you don't already have it installed.
- Open the project from the
<simulator>/projectdirectory. - After opening project, you need to set the working directory:
- Go to (Project Name) | Edit Scheme
- In new window, under Run/Debug on left side, under the Options tab, set Working Directory to
$PROJECT_DIRand check ‘use custom working directory’. - Compile and run the project. You should see a single quadcopter, falling down.
For Linux, the recommended IDE is QtCreator.
- Download and install QtCreator.
- Open the
.profile from the<simulator>/projectdirectory. - Compile and run the project (using the tab
Buildselect theqmakeoption. You should see a single quadcopter, falling down.
NOTE: You may need to install the GLUT libs using sudo apt-get install freeglut3-dev
Now that you have all the code on your computer and the simulator running, let's walk through some of the elements of the code and the simulator itself.
For the project, the majority of your code will be written in src/QuadControl.cpp. This file contains all of the code for the controller that you will be developing.
All the configuration files for your controller and the vehicle are in the config directory. For example, for all your control gains and other desired tuning parameters, there is a config file called QuadControlParams.txt set up for you. An import note is that while the simulator is running, you can edit this file in real time and see the affects your changes have on the quad!
The syntax of the config files is as follows:
[Quad]begins a parameter namespace. Any variable written afterwards becomesQuad.<variablename>in the source code.- If not in a namespace, you can also write
Quad.<variablename>directly. [Quad1 : Quad]means that theQuad1namespace is created with a copy of all the variables ofQuad. You can then overwrite those variables by specifying new values (e.g.Quad1.Massto override the copiedQuad.Mass). This is convenient for having default values.
You will also be using the simulator to fly some difference trajectories to test out the performance of your C++ implementation of your controller. These trajectories, along with supporting code, are found in the traj directory of the repo.
In the simulator window itself, you can right click the window to select between a set of different scenarios that are designed to test the different parts of your controller.
The simulation (including visualization) is implemented in a single thread. This is so that you can safely breakpoint code at any point and debug, without affecting any part of the simulation.
Due to deterministic timing and careful control over how the pseudo-random number generators are initialized and used, the simulation should be exactly repeatable. This means that any simulation with the same configuration should be exactly identical when run repeatedly or on different machines.
Vehicles are created and graphs are reset whenever a scenario is loaded. When a scenario is reset (due to an end condition such as time or user pressing the ‘R’ key), the config files are all re-read and state of the simulation/vehicles/graphs is reset -- however the number/name of vehicles and displayed graphs are left untouched.
When the simulation is running, you can use the arrow keys on your keyboard to impact forces on your drone to see how your controller reacts to outside forces being applied.
There are a handful of keyboard / mouse commands to help with the simulator itself, including applying external forces on your drone to see how your controllers reacts!
- Left drag - rotate
- X + left drag - pan
- Z + left drag - zoom
- arrow keys - apply external force
- C - clear all graphs
- R - reset simulation
- Space - pause simulation
When you run the simulator, you'll notice your quad is falling straight down. This is due to the fact that the thrusts are simply being set to:
QuadControlParams.Mass * 9.81 / 4
Therefore, if the mass doesn't match the actual mass of the quad, it'll fall down. Take a moment to tune the Mass parameter in QuadControlParams.txt to make the vehicle more or less stay in the same spot.
Note: if you want to come back to this later, this scenario is "1_Intro".
With the proper mass, your simulation should look a little like this:
For this project, you will be building a controller in C++. You will be implementing and tuning this controller in several steps.
You may find it helpful to consult the Python controller code as a reference when you build out this controller in C++.
-
Comparison to Python: Note that the vehicle you'll be controlling in this portion of the project has different parameters than the vehicle that's controlled by the Python code linked to above. The tuning parameters that work for the Python controller will not work for this controller
-
Parameter Ranges: You can find the vehicle's control parameters in a file called
QuadControlParams.txt. The default values for these parameters are all too small by a factor of somewhere between about 2X and 4X. So if a parameter has a starting value of 12, it will likely have a value somewhere between 24 and 48 once it's properly tuned. -
Parameter Ratios: In this one-page document you can find a derivation of the ratio of velocity proportional gain to position proportional gain for a critically damped double integrator system. The ratio of
kpV / kpPshould be 4.
First, you will implement the body rate and roll / pitch control. For the simulation, you will use Scenario 2. In this scenario, you will see a quad above the origin. It is created with a small initial rotation speed about its roll axis. Your controller will need to stabilize the rotational motion and bring the vehicle back to level attitude.
To accomplish this, you will:
- Implement body rate control
- implement the code in the function
GenerateMotorCommands() - implement the code in the function
BodyRateControl() - Tune
kpPQRinQuadControlParams.txtto get the vehicle to stop spinning quickly but not overshoot
If successful, you should see the rotation of the vehicle about roll (omega.x) get controlled to 0 while other rates remain zero. Note that the vehicle will keep flying off quite quickly, since the angle is not yet being controlled back to 0. Also note that some overshoot will happen due to motor dynamics!.
If you come back to this step after the next step, you can try tuning just the body rate omega (without the outside angle controller) by setting QuadControlParams.kpBank = 0.
- Implement roll / pitch control We won't be worrying about yaw just yet.
- implement the code in the function
RollPitchControl() - Tune
kpBankinQuadControlParams.txtto minimize settling time but avoid too much overshoot
If successful you should now see the quad level itself (as shown below), though it’ll still be flying away slowly since we’re not controlling velocity/position! You should also see the vehicle angle (Roll) get controlled to 0.
Next, you will implement the position, altitude and yaw control for your quad. For the simulation, you will use Scenario 3. This will create 2 identical quads, one offset from its target point (but initialized with yaw = 0) and second offset from target point but yaw = 45 degrees.
- implement the code in the function
LateralPositionControl() - implement the code in the function
AltitudeControl() - tune parameters
kpPosZandkpPosZ - tune parameters
kpVelXYandkpVelZ
If successful, the quads should be going to their destination points and tracking error should be going down (as shown below). However, one quad remains rotated in yaw.
- implement the code in the function
YawControl() - tune parameters
kpYawand the 3rd (z) component ofkpPQR
Tune position control for settling time. Don’t try to tune yaw control too tightly, as yaw control requires a lot of control authority from a quadcopter and can really affect other degrees of freedom. This is why you often see quadcopters with tilted motors, better yaw authority!
Hint: For a second order system, such as the one for this quadcopter, the velocity gain (kpVelXY and kpVelZ) should be at least ~3-4 times greater than the respective position gain (kpPosXY and kpPosZ).
In this part, we will explore some of the non-idealities and robustness of a controller. For this simulation, we will use Scenario 4. This is a configuration with 3 quads that are all are trying to move one meter forward. However, this time, these quads are all a bit different:
- The green quad has its center of mass shifted back
- The orange vehicle is an ideal quad
- The red vehicle is heavier than usual
-
Run your controller & parameter set from Step 3. Do all the quads seem to be moving OK? If not, try to tweak the controller parameters to work for all 3 (tip: relax the controller).
-
Edit
AltitudeControl()to add basic integral control to help with the different-mass vehicle. -
Tune the integral control, and other control parameters until all the quads successfully move properly. Your drones' motion should look like this:
Now that we have all the working parts of a controller, you will put it all together and test it's performance once again on a trajectory. For this simulation, you will use Scenario 5. This scenario has two quadcopters:
- the orange one is following
traj/FigureEight.txt - the other one is following
traj/FigureEightFF.txt- for now this is the same trajectory. For those interested in seeing how you might be able to improve the performance of your drone by adjusting how the trajectory is defined, check out Extra Challenge 1 below!
How well is your drone able to follow the trajectory? It is able to hold to the path fairly well?
You will notice that initially these two trajectories are the same. Let's work on improving some performance of the trajectory itself.
-
Inspect the python script
traj/MakePeriodicTrajectory.py. Can you figure out a way to generate a trajectory that has velocity (not just position) information? -
Generate a new
FigureEightFF.txtthat has velocity terms Did the velocity-specified trajectory make a difference? Why?
With the two different trajectories, your drones' motions should look like this:
For flying a trajectory, is there a way to provide even more information for even better tracking?
How about trying to fly this trajectory as quickly as possible (but within following threshold)!
To assist with tuning of your controller, the simulator contains real time performance evaluation. We have defined a set of performance metrics for each of the scenarios that your controllers must meet for a successful submission.
There are two ways to view the output of the evaluation:
- in the command line, at the end of each simulation loop, a PASS or a FAIL for each metric being evaluated in that simulation
- on the plots, once your quad meets the metrics, you will see a green box appear on the plot notifying you of a PASS
The specific performance metrics are as follows:
-
scenario 2
- roll should less than 0.025 radian of nominal for 0.75 seconds (3/4 of the duration of the loop)
- roll rate should less than 2.5 radian/sec for 0.75 seconds
-
scenario 3
- X position of both drones should be within 0.1 meters of the target for at least 1.25 seconds
- Quad2 yaw should be within 0.1 of the target for at least 1 second
-
scenario 4
- position error for all 3 quads should be less than 0.1 meters for at least 1.5 seconds
-
scenario 5
- position error of the quad should be less than 0.25 meters for at least 3 seconds
Thanks to Fotokite for the initial development of the project code and simulator.