Starting from:
$30
MEAM 620 Project 1 Phase 1-Modeling and Control of a Quadrotor Solved
In this project you will put your knowledge of quadrotor dynamics to work by developing algorithms for control. A good controller is the backbone of any quadrotor autonomy stack and thus it is crucial that you understand the theory and implementation in this project. You will be reusing the code throughout this course; during the lab session you will use it to fly a real, physical quadrotor! All code written in this project will be in Python. Refer to the instructions on Piazza for more information about which version to use and getting set up.
Figure 1: The CrazyFlie 2.0 robot that will be used in the lab and exercises. An Euler Z-X-Y transformation takes the inertial frame A to the body-fixed frame B. First a rotation by ψ around the a3 axis is performed, then a roll by φ around the (rotated!) a1 axis, and finally a pitch by θ around the (now twice rotated!) a2 axis. A translation r then produces the coordinate system B, coinciding with the center of mass C of the robot, and aligned along the arms.
2 Modeling
2.1 Coordinate Systems and Reference frames
The coordinate systems and free body diagram for the quadrotor are shown in Fig. 1. The inertial frame, A, is defined by the triad a1, a2, and a3 with a3 pointing upward. The body frame, B, is attached to the center of mass of the quadrotor with b1 coinciding with the preferred forward direction and b3 being perpendicular to the plane of the rotors pointing vertically up during perfect hover (see Fig. 1). These vectors are parallel to the principal axes. More formally, the coordinates of a vector x that is expressed in A as x = Pi Axiai and in B as x = Pi Bxibi are transformed into each other by the rotation matrix ARB and translation vector ATB:
A A B A
x = RB x + TB . (1)
To express the rotational motion of the moving frame B, it is useful to introduce the angular velocity vector ω that describes how the basis vectors bi move:
d
bi = ω × bi . (2)
dt
Note that this equation is coordinate free, meaning ω is not yet expressed explicitly in any particular coordinate system. We will denote the components of angular velocity of the robot in the body frame by p, q, and r:
ω = pb1 + qb2 + rb3. (3)
The heading (yaw) angle of the robot plays a special role since we can choose it freely without directly affecting the robot’s dynamics. For this reason we use Z − X − Y Euler angles to describe the transform from A to B: first a yaw rotation by ψ around the a3 axis is performed, then a roll by φ around the (rotated!) a1 axis, and finally a pitch by θ around the (now twice rotated!) a2 axis [1] From the Euler angles one can compute the rotation matrix as follows:
The angular velocity and Euler angle velocities are related by:
(5)
2.2 Motor Model
This section describes how we model the motors. Each rotor has an angular speed ωi and produces a vertical force Fi according to:
Fi = kF ωi2. (6)
Experimentation with a fixed rotor at steady-state shows that kF ≈ 6.11 × 10−8 N/rpm2. The rotors also produce a moment according to
Mi = kMωi2. (7)
The constant, kM, is determined to be about 1.5 × 10−9 Nm/rpm2 by matching the performance of the simulation to the real system.
Data obtain from system identification experiments suggest that the rotor speed is related to the commanded speed by a first-order differential equation ω˙i = km(ωides − ωi).
This motor gain, km, is found to be about 20s−1 by matching the performance of the simulation to the real system. The desired angular velocities, ωides, are limited to a minimum and maximum value determined through experimentation.
However, as a first approximation, we can assume the motor controllers to be perfect and the time constant km associated with the motor response to be arbitrarily small. In other words, we can assume the actual motor velocities ωi are equal to the commanded motor velocities, ωides. Notes:
1. In simulation, you will be able to directly command torque and thrust. The thrust limit is available as a parameter inside the simulator, although you don’t need it for this project.
2. In the lab, we will provide the code to translate torque and thrust to motor commands. You will however encounter saturation of your motors and may have to tweak your gains.
2.3 Rigid Body Dynamics: Newton’s Equations of Motion for the Center of Mass
We will describe the motion of the center of mass (CoM) in the inertial (“world”) coordinate frame A. This makes sense because we will want to specify our waypoints (where the robot should fly), trajectories and controller targets (where the robot should be at this moment) in the inertial frame.
Newton’s equation of motion for the robot’s CoM r is determined by the robot’s mass m, the gravitational force Fg = mg, and the sum of the motor’s individual forces Fi:
m¨r = Fg + XFi .
i
In the coordinates of the inertial frame A this reads:
(8)
. (9)
where ARB is the rotation matrix used in Eq (1) constructed via Eq (4). If the robot is tilted, the above equation will mix the propeller forces into the x and y plane and so generate horizontal acceleration of the robot. In other words, we can accomplish movement in all three directions by manipulating the attitude of the vehicle and its thrust.
Equation (9) further suggests that rather than using the forces Fi as control inputs, we should define our first input u1 as the sum:
. (10)
2.4 Euler’s Equations of Motion for the Attitude
Since from Eq (9) we know that attitude control is necessary to generate horizontal motion, in this section we will examine how the motors affect the orientation of the vehicle.
In the inertial frame, the rate at which the robot’s angular momentum L = Iω changes is determined by the total moment M generated by the propellers:
L˙ = M . (11)
While this equation looks clean and simple in the inertial frame, it is actually hard to work with because the inertial tensor I changes with the attitude of the robot, and is thus time dependent and non diagonal! For control purposes, this equation is best expressed in the body frame that is aligned with the principal axis, where the inertia tensor I is constant, and diagonal. However, since now the basis vectors bi are time-dependent, the equation for the time derivative of the angular momentum in the body frame becomes:
L˙ = XBL˙ ibi + XBLib˙ i = XBL˙ ibi + Xω × BLibi = XBL˙ ibi + ω × L .
i i i i i
Combining (11) and (12), and using the fact that I is constant in B yield’s Euler’s Equation:
(12)
IBω˙ = BM − Bω × IBω ,
(13)
where depending on context I means alternatingly a tensor or a matrix expressed in the body frame.
How do the motors come into play? First, by generating a force Fi that is a distance l away from the CoM, each motor can exert a torque that is in the b1, b2 plane. In addition, each rotor produces a moment Mi perpendicular to the plane of rotation of the blade. Rotors 1 and 3 rotate clockwise in the −b3 direction while 2 and 4 rotate counter clockwise in the b3 direction. Since the moment produced on the quadrotor is opposite to the direction of rotation of the blades, M1 and M3 act in the b3 direction while M2 and M4 act in the −b3 direction. In contrast to this, the forces Fi are all in the positive b3 direction due to the fact that the pitch is reversed on two of the propellers, see Fig
1.
Using this geometric intuition, and expressing the angular velocity in the body frame by p, q, and r as in Eq (3), the Euler equation (13) becomes:
. (14)
Note that the total net torque along the yaw (b3) axis of the robot is simply the signed sum of the motor’s torques Mi (why does the distance l to the center of the robot play no role?). We can rewrite this as:
l
0
−γ
0 l γ
.
(15)
where is the relationship between lift and drag given by Equations (6-7). Accordingly, we will define our second set of inputs to be the vector of moments u2 given by:
u . (16)
Dropping the reference frame superscripts for brevity, we can now write the equations of motion for center of mass and orientation in compact form:
(17)
, (18)
where (17) is in inertial coordinates, and (18) is in body coordinates. The inputs u1 and u2 are related to the motor forces Fi via the linear system of equations formed by (10) and (16). We are thus facing a system governed by two coupled second order differential equations which we will exploit in section 3 to design controllers.
3 Robot Controllers
3.1 Trajectory Generation
Our ultimate goal is to make the robot follow a trajectory. For the time let’s assume we are given a trajectory generator that for any given time t produces a trajectory target zdes(t) consisting of target position rT (t) and target yaw ψT (t):
zdes (19)
and its first and second derivatives z˙des and z¨des. To hover for example, the trajectory generator would produce a constant r(t) = r0 and e.g. a fixed ψ(t) = ψ0, with all derivatives being zero.
The controller’s job will then be to produce the correct torque u2 and thrust u1 to bring the robot to the desired state specified by the trajectory generator.
3.2 Linear Backstepping Controller
For this controller we will make the following assumptions:
1. The robot is near the hover points, meaning the roll angle φ and pitch angle θ are small enough to allow linearization of trigonometric functions, i.e. cos(φ) = 1, sin(φ) = φ, cos(θ) = 1, sin(θ) = θ. This will allow us to linearize the rotation matrix in Eq (17).
2. The robot’s angular velocity is small enough for the cross product term between angular momentum and velocity in (18) to be negligible. This is usually a good assumption for almost any quadrotor.
3. The attitude of the quadrotor can be controlled at a much smaller time scale than the position. This “backstepping” approach to controller design allows a decoupling of position and attitude control loops. In practice this is generally warranted since the attitude controller usually runs almost an order of magnitude faster than the position controller.
Making the backstepping approximation is equivalent to assuming that R in Eq (17) can be commanded instantaneously. This further implies that it is possible to directly command the acceleration rdes. Figure 2 shows how trajectory generator, position, and attitude controller play together.
Figure 2: The position and attitude control loops.
Define the position error in terms of components by:
ei = (ri − ri,T ).
In order to guarantee that this error goes exponentially to zero, we require
(r¨ides − r¨i,T ) + kd,i(r˙i − r˙i,T ) + kp,i(ri − ri,T ) = 0 . (20)
In Eq (20), ri,T and its derivatives are given by the trajectory generator, and ri and r˙i are provided by the state estimation system, allowing for the commanded acceleration r¨ides to be calculated:
r¨ides = r¨i,T − kd,i(r˙i − r˙i,T ) − kp,i(ri − ri,T ) . (21)
Now the attitude of the quadrotor must be controlled such that it will generate ¨rdes. For this, the linearized version of (17) is used:
(22a)
(22b)
(22c)
From the third equation we can directly read off the thrust control u1. The first two equations can be solved for θdes and φdes, since ψdes = ψT is given directly by the trajectory generator. Linearizing (18), we get:
, (23)
and by further exploiting that via Eq (5) Euler angle velocities are to linear approximation equal to angular velocities, it becomes evident that we can directly command the acceleration of the Euler angles:
. (24)
Thus the “inner loop” attitude control can also be done with a simple PD controller:
u , (25)
where the desired roll and pitch velocities pdes and qdes can be computed from the equations of motion and the specified trajectory [1], but in practice can be set to zero. [2]In summary, the controller then works as follows:
1. Use Eq (21) to compute the commanded acceleration ¨rdes.
2. Use Eq (22c) to compute u1 and the desired angles θdes and φdes from (22a) and (22b).
3. Use Eq (25) to compute u2.
3.3 A Geometric Nonlinear Controller
Nonlinear controllers are generally built based on geometric intuition, i.e. the quadrotor’s b3 axis is tilted to point in the desired direction, and thrust is applied. Such controllers are suitable for very aggressive maneuvers and will allow for faster flight and sharper turns, in particular in the simulator. Note that we have considerable freedom to choose a control algorithm so long as it results in a stable system.
The following section is based on the controller developed in [1], which in turn is a simplified version of the controller in [2]. For a thorough treatment and stability analysis please refer to [2].
It turns out that the basic layout of the controller remains as shown in Figure 2, i.e. there is an attitude and a position controller, although we no longer make the backstepping assumption.
We start out from the PD controller in Eq (21), but now write it in more compact vector form:
¨rdes = ¨rT − Kd(r˙ − r˙T ) − Kp(r − rT ) , (26)
where Kd and Kp are diagonal, positive definite gain matrices.
We are again faced with the question how to compute the inputs u1, u2 to generate ¨rdes. We first determine the input u1. By rearranging Eq (17) one obtains:
. (27)
Note that the lhs of (27) is just the total commanded force Fdes (including gravity):
Fdes = m¨rdes . (28)
On the right hand side is u1, multiplied with the quadrotor’s axis b3 = R[0,0,1]T , expressed in the inertial frame. We obtain the input u1 by projecting Fdes onto b3:
Fdes . (29)
To compute u2, we observe that a quadrotor can only produce thrust along its b3 axis. It makes sense to align b3 with Fdes, and align b1 to match the desired yaw ψT . Please refer to Fig. 3. In the following steps we find a triad
Figure 3: Geometry based attitude control. First b3des is aligned along Fdes. Then b2des is chosen to be perpendicular to the plane spanned by the desired yaw heading vector aψ and b3des. This ensures that b1des and b3des are in the plane containing aψ.
Rdes = [b1des,b2des,b3des] that has the proper alignment, then proceed to develop a control input u2 that produces the corresponding torque to drive the attitude towards Rdes.
As indidicated, the b3des axis should be oriented along the desired thrust: Fdes
des
b3 = || Fdes|| . (30)
Next, we want the b2des axis to be perpendicular to both b1des and the vector aψ that defines the yaw direction in the plane (a1, a2) plane:
a . (31)
This can be accomplished by forming the cross product between b3des and aψ:
des 3des × a
b2(32)
Rdes = [b2des × b3des,b2des,b3des] .
Next, we need a measure for the error in orientation RdesT R. The following error vector (see [1])
(33)
R 1 desT − RT Rdes)∨
e = (R R
(34)
which guarantees that the plane formed by b3des and the axis b1des representing the head of the robot contains the aψ. Finally, b1des is obtained by cross product and the desired rotation matrix is:
2
is obtained from the matrices by taking the ∨ operator that maps elements of so(3) to R3. This error vector is zero when Rdes = R. It is the rotation vector that generates the error in orientation. Consequently, by applying a torque along its direction it can be decreased, suggesting the control input:
u2 = I(−KReR − Kωeω) , (35)
where eω = ω − ωdes is the error in angular velocities and KR and Kω are diagonal gains matrices. The desired angular velocities ωdes can be computed from the output of the trajectory generator zT and its derivatives, but setting them to zero will work for the purpose of this project.
To summarize, the steps to implement the controller are:
1. calculate Fdes from Eq (28), (27), and (26).
2. compute u1 from Eq (29)
3. determine Rdes from Eq (33) and the definitions of bides.
4. find the error orientation error vector eR from Eq (34) and substitute ω for eω.
5. compute u2 from Eq (35).
4 System Description
4.1 Quadrotor Platform
For this project we will be using the CrazyFlie 2.0 platform made by Bitcraze, shown in Fig. 1. The CrazyFlie has a motor to motor (diagonal) distance of 92mm, and a mass of 30g, including a battery. A microcontroller allows lowlevel control and estimation to be done onboard the robot. An onboard IMU provides feedback of angular velocities and accelerations.
4.2 Software and Integration
Position control and other high level commands are computed in Python and sent to the robot via the CrazyRadio (2.4GHz). Attitude control is performed onboard using the microcontroller, though you will control it in simulation.
4.3 Inertial Properties
Since bi are principal axes, the inertia matrix referenced to the center of mass along the bi reference triad, I, is a diagonal matrix. In practice, the three moments of inertia can be estimated by weighing individual components of the quadrotor and building a physically accurate model in SolidWorks. The key parameters for the rigid body dynamics for the CrazyFlie platform are as follows:
(a) mass: m = 0.030kg;
(b) the distance from the center of mass to the axis of a motor: l = 0.046m; and (c) the components of the inertia dyadic using bi in kg m2:
.
Note that these constants have already been incorporated into the provided Python simulator.
5 Project Work
5.1 Code Packet
We have provided you with a project packet that contains all the code you need to complete this assignment. The flightsim directory contains the quadrotor simulator; you will not need to make any changes to the enclosed files but you may want to poke around to see how it works under the hood. Inside the proj11 directory you will find two folders: code contains files with code stubs for you to complete for this assignment and util contains a test suite for judging the performance of your controller offline.
The file code/sandbox.py will run the simulator with your controller on the trajectory specified within and produce plots of the state of the quadrotor over time useful for performance tuning as well as an animation of the result. Additionally, a test script (util/test.py) and a few test cases specified as util/*.json files will help you judge the performance of you implementation offline and gain familiarity with the testing system (more on this later). Additionally, at the root of the packet is an important file called setup.py. This file specifies the additional python packages necessary to run the simulator and can be used to automatically install those dependencies in your python environment. For more of this see the piazza post on Python and Pycharm.
Before implementing your own functions, you should first try running code/sandbox.py. It should produce a number of plots displaying useful information about the state of the quadrotor over time as well as a short animation showing the quadrotor in action. Because you haven’t implemented your controller yet you will get an error in the console and the animation will show your quadrotor falling out of the sky. This is because the outputs of the update function in se3control.py are all zeros and thus no thrust is generated.
5.2 Tasks
For this project you will complete the implementation of a controller in se3control.py and a waypoint trajectory generator in waypointtraj.py:
1. waypoint traj.py
You will first implement a waypoint trajectory class. This class holds a set of desired waypoints for the quadrotor to visit and subsequently must prescribe a state for each instant in time that if followed by the quadrotor will ensure it visits each point. In particular, you will populate the dict flatoutput containing position (x), velocity (xdot), acceleration (xddot), jerk (xdddot), snap (xddddot), yaw angle (yaw), and angular rate (yawdot). For more information see the doc strings for each function in waypointtraj.py.
2. se3 control.py
Next you will implement a controller that drives the quadrotor towards the state specified by the waypoint trajectory class. As mentioned before, this controller will be used throughout project 1. The controller is implemented as a class that holds the physical parameters of the robot and produces motor speed commands (cmd motorspeeds) given the current time and state of the quadrotor as well as the desired state (flatoutput). For more information see the doc strings for each function in se3control.py.
Although in this phase of the project you will only need to output motor speeds, you should also set the corresponding commanded thrust (cmdthrust), moment (cmdmoment), and attitude (cmdq). Although they will be ignored in the simulator, they can be useful for debugging purposes and will be used when you fly in the lab.
Hints:
1. sandbox.py ships with a hover trajectory generator built in; use it to first implement and tune your controller then move on to filling out waypointtraj.py.
2. Make sure that your controller can also handle arbitrary yaw targets, and test that it works! Beware of yaw angle wrap-around.
3. Tune attitude and position gains separately. Which ones should you do first?
[1] In aerospace engineering, the direction of rotation for yaw and pitch are opposite to our definition, which follows the conventional right-hand rule.
[2] On the RHS of Eq. (25) there should also be the trajectory angular accelerations, analogous to r¨i,T in Eqn (20). By exploiting “Differential Flatness” [1] the trajectory angular accelerations can be computed from the equations of motion and zdes(t) and its derivatives. However, in practice these terms can be neglected.
Figure 1: The CrazyFlie 2.0 robot that will be used in the lab and exercises. An Euler Z-X-Y transformation takes the inertial frame A to the body-fixed frame B. First a rotation by ψ around the a3 axis is performed, then a roll by φ around the (rotated!) a1 axis, and finally a pitch by θ around the (now twice rotated!) a2 axis. A translation r then produces the coordinate system B, coinciding with the center of mass C of the robot, and aligned along the arms.
2 Modeling
2.1 Coordinate Systems and Reference frames
The coordinate systems and free body diagram for the quadrotor are shown in Fig. 1. The inertial frame, A, is defined by the triad a1, a2, and a3 with a3 pointing upward. The body frame, B, is attached to the center of mass of the quadrotor with b1 coinciding with the preferred forward direction and b3 being perpendicular to the plane of the rotors pointing vertically up during perfect hover (see Fig. 1). These vectors are parallel to the principal axes. More formally, the coordinates of a vector x that is expressed in A as x = Pi Axiai and in B as x = Pi Bxibi are transformed into each other by the rotation matrix ARB and translation vector ATB:
A A B A
x = RB x + TB . (1)
To express the rotational motion of the moving frame B, it is useful to introduce the angular velocity vector ω that describes how the basis vectors bi move:
d
bi = ω × bi . (2)
dt
Note that this equation is coordinate free, meaning ω is not yet expressed explicitly in any particular coordinate system. We will denote the components of angular velocity of the robot in the body frame by p, q, and r:
ω = pb1 + qb2 + rb3. (3)
The heading (yaw) angle of the robot plays a special role since we can choose it freely without directly affecting the robot’s dynamics. For this reason we use Z − X − Y Euler angles to describe the transform from A to B: first a yaw rotation by ψ around the a3 axis is performed, then a roll by φ around the (rotated!) a1 axis, and finally a pitch by θ around the (now twice rotated!) a2 axis [1] From the Euler angles one can compute the rotation matrix as follows:
The angular velocity and Euler angle velocities are related by:
(5)
2.2 Motor Model
This section describes how we model the motors. Each rotor has an angular speed ωi and produces a vertical force Fi according to:
Fi = kF ωi2. (6)
Experimentation with a fixed rotor at steady-state shows that kF ≈ 6.11 × 10−8 N/rpm2. The rotors also produce a moment according to
Mi = kMωi2. (7)
The constant, kM, is determined to be about 1.5 × 10−9 Nm/rpm2 by matching the performance of the simulation to the real system.
Data obtain from system identification experiments suggest that the rotor speed is related to the commanded speed by a first-order differential equation ω˙i = km(ωides − ωi).
This motor gain, km, is found to be about 20s−1 by matching the performance of the simulation to the real system. The desired angular velocities, ωides, are limited to a minimum and maximum value determined through experimentation.
However, as a first approximation, we can assume the motor controllers to be perfect and the time constant km associated with the motor response to be arbitrarily small. In other words, we can assume the actual motor velocities ωi are equal to the commanded motor velocities, ωides. Notes:
1. In simulation, you will be able to directly command torque and thrust. The thrust limit is available as a parameter inside the simulator, although you don’t need it for this project.
2. In the lab, we will provide the code to translate torque and thrust to motor commands. You will however encounter saturation of your motors and may have to tweak your gains.
2.3 Rigid Body Dynamics: Newton’s Equations of Motion for the Center of Mass
We will describe the motion of the center of mass (CoM) in the inertial (“world”) coordinate frame A. This makes sense because we will want to specify our waypoints (where the robot should fly), trajectories and controller targets (where the robot should be at this moment) in the inertial frame.
Newton’s equation of motion for the robot’s CoM r is determined by the robot’s mass m, the gravitational force Fg = mg, and the sum of the motor’s individual forces Fi:
m¨r = Fg + XFi .
i
In the coordinates of the inertial frame A this reads:
(8)
. (9)
where ARB is the rotation matrix used in Eq (1) constructed via Eq (4). If the robot is tilted, the above equation will mix the propeller forces into the x and y plane and so generate horizontal acceleration of the robot. In other words, we can accomplish movement in all three directions by manipulating the attitude of the vehicle and its thrust.
Equation (9) further suggests that rather than using the forces Fi as control inputs, we should define our first input u1 as the sum:
. (10)
2.4 Euler’s Equations of Motion for the Attitude
Since from Eq (9) we know that attitude control is necessary to generate horizontal motion, in this section we will examine how the motors affect the orientation of the vehicle.
In the inertial frame, the rate at which the robot’s angular momentum L = Iω changes is determined by the total moment M generated by the propellers:
L˙ = M . (11)
While this equation looks clean and simple in the inertial frame, it is actually hard to work with because the inertial tensor I changes with the attitude of the robot, and is thus time dependent and non diagonal! For control purposes, this equation is best expressed in the body frame that is aligned with the principal axis, where the inertia tensor I is constant, and diagonal. However, since now the basis vectors bi are time-dependent, the equation for the time derivative of the angular momentum in the body frame becomes:
L˙ = XBL˙ ibi + XBLib˙ i = XBL˙ ibi + Xω × BLibi = XBL˙ ibi + ω × L .
i i i i i
Combining (11) and (12), and using the fact that I is constant in B yield’s Euler’s Equation:
(12)
IBω˙ = BM − Bω × IBω ,
(13)
where depending on context I means alternatingly a tensor or a matrix expressed in the body frame.
How do the motors come into play? First, by generating a force Fi that is a distance l away from the CoM, each motor can exert a torque that is in the b1, b2 plane. In addition, each rotor produces a moment Mi perpendicular to the plane of rotation of the blade. Rotors 1 and 3 rotate clockwise in the −b3 direction while 2 and 4 rotate counter clockwise in the b3 direction. Since the moment produced on the quadrotor is opposite to the direction of rotation of the blades, M1 and M3 act in the b3 direction while M2 and M4 act in the −b3 direction. In contrast to this, the forces Fi are all in the positive b3 direction due to the fact that the pitch is reversed on two of the propellers, see Fig
1.
Using this geometric intuition, and expressing the angular velocity in the body frame by p, q, and r as in Eq (3), the Euler equation (13) becomes:
. (14)
Note that the total net torque along the yaw (b3) axis of the robot is simply the signed sum of the motor’s torques Mi (why does the distance l to the center of the robot play no role?). We can rewrite this as:
l
0
−γ
0 l γ
.
(15)
where is the relationship between lift and drag given by Equations (6-7). Accordingly, we will define our second set of inputs to be the vector of moments u2 given by:
u . (16)
Dropping the reference frame superscripts for brevity, we can now write the equations of motion for center of mass and orientation in compact form:
(17)
, (18)
where (17) is in inertial coordinates, and (18) is in body coordinates. The inputs u1 and u2 are related to the motor forces Fi via the linear system of equations formed by (10) and (16). We are thus facing a system governed by two coupled second order differential equations which we will exploit in section 3 to design controllers.
3 Robot Controllers
3.1 Trajectory Generation
Our ultimate goal is to make the robot follow a trajectory. For the time let’s assume we are given a trajectory generator that for any given time t produces a trajectory target zdes(t) consisting of target position rT (t) and target yaw ψT (t):
zdes (19)
and its first and second derivatives z˙des and z¨des. To hover for example, the trajectory generator would produce a constant r(t) = r0 and e.g. a fixed ψ(t) = ψ0, with all derivatives being zero.
The controller’s job will then be to produce the correct torque u2 and thrust u1 to bring the robot to the desired state specified by the trajectory generator.
3.2 Linear Backstepping Controller
For this controller we will make the following assumptions:
1. The robot is near the hover points, meaning the roll angle φ and pitch angle θ are small enough to allow linearization of trigonometric functions, i.e. cos(φ) = 1, sin(φ) = φ, cos(θ) = 1, sin(θ) = θ. This will allow us to linearize the rotation matrix in Eq (17).
2. The robot’s angular velocity is small enough for the cross product term between angular momentum and velocity in (18) to be negligible. This is usually a good assumption for almost any quadrotor.
3. The attitude of the quadrotor can be controlled at a much smaller time scale than the position. This “backstepping” approach to controller design allows a decoupling of position and attitude control loops. In practice this is generally warranted since the attitude controller usually runs almost an order of magnitude faster than the position controller.
Making the backstepping approximation is equivalent to assuming that R in Eq (17) can be commanded instantaneously. This further implies that it is possible to directly command the acceleration rdes. Figure 2 shows how trajectory generator, position, and attitude controller play together.
Figure 2: The position and attitude control loops.
Define the position error in terms of components by:
ei = (ri − ri,T ).
In order to guarantee that this error goes exponentially to zero, we require
(r¨ides − r¨i,T ) + kd,i(r˙i − r˙i,T ) + kp,i(ri − ri,T ) = 0 . (20)
In Eq (20), ri,T and its derivatives are given by the trajectory generator, and ri and r˙i are provided by the state estimation system, allowing for the commanded acceleration r¨ides to be calculated:
r¨ides = r¨i,T − kd,i(r˙i − r˙i,T ) − kp,i(ri − ri,T ) . (21)
Now the attitude of the quadrotor must be controlled such that it will generate ¨rdes. For this, the linearized version of (17) is used:
(22a)
(22b)
(22c)
From the third equation we can directly read off the thrust control u1. The first two equations can be solved for θdes and φdes, since ψdes = ψT is given directly by the trajectory generator. Linearizing (18), we get:
, (23)
and by further exploiting that via Eq (5) Euler angle velocities are to linear approximation equal to angular velocities, it becomes evident that we can directly command the acceleration of the Euler angles:
. (24)
Thus the “inner loop” attitude control can also be done with a simple PD controller:
u , (25)
where the desired roll and pitch velocities pdes and qdes can be computed from the equations of motion and the specified trajectory [1], but in practice can be set to zero. [2]In summary, the controller then works as follows:
1. Use Eq (21) to compute the commanded acceleration ¨rdes.
2. Use Eq (22c) to compute u1 and the desired angles θdes and φdes from (22a) and (22b).
3. Use Eq (25) to compute u2.
3.3 A Geometric Nonlinear Controller
Nonlinear controllers are generally built based on geometric intuition, i.e. the quadrotor’s b3 axis is tilted to point in the desired direction, and thrust is applied. Such controllers are suitable for very aggressive maneuvers and will allow for faster flight and sharper turns, in particular in the simulator. Note that we have considerable freedom to choose a control algorithm so long as it results in a stable system.
The following section is based on the controller developed in [1], which in turn is a simplified version of the controller in [2]. For a thorough treatment and stability analysis please refer to [2].
It turns out that the basic layout of the controller remains as shown in Figure 2, i.e. there is an attitude and a position controller, although we no longer make the backstepping assumption.
We start out from the PD controller in Eq (21), but now write it in more compact vector form:
¨rdes = ¨rT − Kd(r˙ − r˙T ) − Kp(r − rT ) , (26)
where Kd and Kp are diagonal, positive definite gain matrices.
We are again faced with the question how to compute the inputs u1, u2 to generate ¨rdes. We first determine the input u1. By rearranging Eq (17) one obtains:
. (27)
Note that the lhs of (27) is just the total commanded force Fdes (including gravity):
Fdes = m¨rdes . (28)
On the right hand side is u1, multiplied with the quadrotor’s axis b3 = R[0,0,1]T , expressed in the inertial frame. We obtain the input u1 by projecting Fdes onto b3:
Fdes . (29)
To compute u2, we observe that a quadrotor can only produce thrust along its b3 axis. It makes sense to align b3 with Fdes, and align b1 to match the desired yaw ψT . Please refer to Fig. 3. In the following steps we find a triad
Figure 3: Geometry based attitude control. First b3des is aligned along Fdes. Then b2des is chosen to be perpendicular to the plane spanned by the desired yaw heading vector aψ and b3des. This ensures that b1des and b3des are in the plane containing aψ.
Rdes = [b1des,b2des,b3des] that has the proper alignment, then proceed to develop a control input u2 that produces the corresponding torque to drive the attitude towards Rdes.
As indidicated, the b3des axis should be oriented along the desired thrust: Fdes
des
b3 = || Fdes|| . (30)
Next, we want the b2des axis to be perpendicular to both b1des and the vector aψ that defines the yaw direction in the plane (a1, a2) plane:
a . (31)
This can be accomplished by forming the cross product between b3des and aψ:
des 3des × a
b2(32)
Rdes = [b2des × b3des,b2des,b3des] .
Next, we need a measure for the error in orientation RdesT R. The following error vector (see [1])
(33)
R 1 desT − RT Rdes)∨
e = (R R
(34)
which guarantees that the plane formed by b3des and the axis b1des representing the head of the robot contains the aψ. Finally, b1des is obtained by cross product and the desired rotation matrix is:
2
is obtained from the matrices by taking the ∨ operator that maps elements of so(3) to R3. This error vector is zero when Rdes = R. It is the rotation vector that generates the error in orientation. Consequently, by applying a torque along its direction it can be decreased, suggesting the control input:
u2 = I(−KReR − Kωeω) , (35)
where eω = ω − ωdes is the error in angular velocities and KR and Kω are diagonal gains matrices. The desired angular velocities ωdes can be computed from the output of the trajectory generator zT and its derivatives, but setting them to zero will work for the purpose of this project.
To summarize, the steps to implement the controller are:
1. calculate Fdes from Eq (28), (27), and (26).
2. compute u1 from Eq (29)
3. determine Rdes from Eq (33) and the definitions of bides.
4. find the error orientation error vector eR from Eq (34) and substitute ω for eω.
5. compute u2 from Eq (35).
4 System Description
4.1 Quadrotor Platform
For this project we will be using the CrazyFlie 2.0 platform made by Bitcraze, shown in Fig. 1. The CrazyFlie has a motor to motor (diagonal) distance of 92mm, and a mass of 30g, including a battery. A microcontroller allows lowlevel control and estimation to be done onboard the robot. An onboard IMU provides feedback of angular velocities and accelerations.
4.2 Software and Integration
Position control and other high level commands are computed in Python and sent to the robot via the CrazyRadio (2.4GHz). Attitude control is performed onboard using the microcontroller, though you will control it in simulation.
4.3 Inertial Properties
Since bi are principal axes, the inertia matrix referenced to the center of mass along the bi reference triad, I, is a diagonal matrix. In practice, the three moments of inertia can be estimated by weighing individual components of the quadrotor and building a physically accurate model in SolidWorks. The key parameters for the rigid body dynamics for the CrazyFlie platform are as follows:
(a) mass: m = 0.030kg;
(b) the distance from the center of mass to the axis of a motor: l = 0.046m; and (c) the components of the inertia dyadic using bi in kg m2:
.
Note that these constants have already been incorporated into the provided Python simulator.
5 Project Work
5.1 Code Packet
We have provided you with a project packet that contains all the code you need to complete this assignment. The flightsim directory contains the quadrotor simulator; you will not need to make any changes to the enclosed files but you may want to poke around to see how it works under the hood. Inside the proj11 directory you will find two folders: code contains files with code stubs for you to complete for this assignment and util contains a test suite for judging the performance of your controller offline.
The file code/sandbox.py will run the simulator with your controller on the trajectory specified within and produce plots of the state of the quadrotor over time useful for performance tuning as well as an animation of the result. Additionally, a test script (util/test.py) and a few test cases specified as util/*.json files will help you judge the performance of you implementation offline and gain familiarity with the testing system (more on this later). Additionally, at the root of the packet is an important file called setup.py. This file specifies the additional python packages necessary to run the simulator and can be used to automatically install those dependencies in your python environment. For more of this see the piazza post on Python and Pycharm.
Before implementing your own functions, you should first try running code/sandbox.py. It should produce a number of plots displaying useful information about the state of the quadrotor over time as well as a short animation showing the quadrotor in action. Because you haven’t implemented your controller yet you will get an error in the console and the animation will show your quadrotor falling out of the sky. This is because the outputs of the update function in se3control.py are all zeros and thus no thrust is generated.
5.2 Tasks
For this project you will complete the implementation of a controller in se3control.py and a waypoint trajectory generator in waypointtraj.py:
1. waypoint traj.py
You will first implement a waypoint trajectory class. This class holds a set of desired waypoints for the quadrotor to visit and subsequently must prescribe a state for each instant in time that if followed by the quadrotor will ensure it visits each point. In particular, you will populate the dict flatoutput containing position (x), velocity (xdot), acceleration (xddot), jerk (xdddot), snap (xddddot), yaw angle (yaw), and angular rate (yawdot). For more information see the doc strings for each function in waypointtraj.py.
2. se3 control.py
Next you will implement a controller that drives the quadrotor towards the state specified by the waypoint trajectory class. As mentioned before, this controller will be used throughout project 1. The controller is implemented as a class that holds the physical parameters of the robot and produces motor speed commands (cmd motorspeeds) given the current time and state of the quadrotor as well as the desired state (flatoutput). For more information see the doc strings for each function in se3control.py.
Although in this phase of the project you will only need to output motor speeds, you should also set the corresponding commanded thrust (cmdthrust), moment (cmdmoment), and attitude (cmdq). Although they will be ignored in the simulator, they can be useful for debugging purposes and will be used when you fly in the lab.
Hints:
1. sandbox.py ships with a hover trajectory generator built in; use it to first implement and tune your controller then move on to filling out waypointtraj.py.
2. Make sure that your controller can also handle arbitrary yaw targets, and test that it works! Beware of yaw angle wrap-around.
3. Tune attitude and position gains separately. Which ones should you do first?
[1] In aerospace engineering, the direction of rotation for yaw and pitch are opposite to our definition, which follows the conventional right-hand rule.
[2] On the RHS of Eq. (25) there should also be the trajectory angular accelerations, analogous to r¨i,T in Eqn (20). By exploiting “Differential Flatness” [1] the trajectory angular accelerations can be computed from the equations of motion and zdes(t) and its derivatives. However, in practice these terms can be neglected.
1 file (5.5KB)
