Lecture 3A.2
3-D Quadrotor Control Having looked at the 2-dimensional quadrotor, we now turn our attention to the dynamics of the 3-dimensional model, and we'll talk about trajectory planning and control !ecall that we have a body-fi"ed frame attached to the quadrotor and we have an inertial frame#
$he state of the quadrotor consists of position and orientation $he position is the position-vector of the centre-of-mass, and the roll, pitch and yaw angles tell us the orientation $his state is composed of a si"-dimensional vector and its rate of change %in other words, the velocity&
x y z q q , x q $he roll, pitch and yaw angles follow the usual convention# first the yaw above the a"is, and then the roll ( pitch as we've seen before $he angular velocity components in the body frame, p q and r, are related to the derivatives of the roll, pitch and yaw angles through this coefficient matri"#
p cos ) cos sin q ) * sin r sin ) cos cos
$his set of equations tells us everything we need to know about the kinematics of the vehicle +hat wed like to do is to track arbitrarily-specified trajectories in three-dimensions
+e assume were given a trajectory $his trajectory consists of a position vector that varies as a function of time, and a yaw angle, which also varies as a function of time#
x %t & y %t & r T %t & z %t & %t & +e want this four-dimensional vector to be differentiable, and we want to be able to obtain not only its derivative, but also its second derivative s before, we're interested in the difference between the desired position and the actual position .ut now we're also interested in the difference between the desired yaw angle and the actual yaw angle $hat gives us the error vector and its derivative# e p r $ %t& - r
$ %t& - r ev r nd again, we want the error vector to go e"ponentially to ero %r $ %t& - r c & K d ev K p e p ) $his lets us calculate the commanded acceleration, r c , whether it's the 2nd-derivative of the position vector, or the 2nd-derivative of the yaw angle /et's take another look at the equations of motion, and the nested-feedback-loop we described for the two-dimensional quadrotor model#
s in the planar case, we have nested feedback loops $he inner loop corresponds to attitude control, and the outer loop corresponds to position control 0n the inner loop, we specify the orientation either using a rotation matri" or a series of roll, pitch, and yaw angles +ell feedback the actual attitude and angular velocity, or the roll, pitch and yaw angles and the angular rates 1rom that, we calculate the input u 2 u2 is a function of the thrusts and moments that we get from the motors 0n the equations-of-motion at the bottom, we have to calculate the value of u 2 based on the desired attitude +e now turn our attention to the outer-loop, which is a position feedback loop 0n this loop, we take the specified position vector ( specified yaw angle from the $rajectory lanner +e compare that with the actual position and velocity, and from that we calculate u*, u* is essentially the sum of all the thrust forces !ather than looking at the general trajectory-following problem, let's initially focus on a very specific case, the case of hovering#
o in hovering, the robots position and orientation are fi"ed 0n other words, all velocities are ero 1urther, the roll and pitch angles are also equal to ero +e want to consider small perturbations around the hover position ccordingly, we'll linearise the dynamics around this current configuration 0n order for the hover position to be one of equilibrium, the input u 2 has to be ), the angular velocity components p, q, and r are equal to ), and their derivatives are also equal to ) /ikewise, the sum of the thrusts has to compensate for the weight of the robot, but the yaw angle can be non-ero as long as it's fi"ed /inearising around this point, means that we are assuming u * is almost equal to mg +e assume the roll and pitch angles are close to ero, and we assume that the yaw angle is fi"ed, or close to fi"ed at a given value ) %u* 4 mg , 4 ), 4 ), 4 ) & +hen we linearise the equations, we end up with these simplified equations#
* x g % cos sin & r 2 y g % sin cos & r +e can now design the inner feedback loop, by simply specifying u 2 using a proportional plus derivative controller#
K p , % c & K d , % pc p & u2 K p , % c & K d , % qc q& K p , % c & K d , %r c r & Here we assume that we have the commanded roll, pitch, and yaw angles and their derivatives nd all we require for feedback are the actual roll, pitch, and yaw angles, and their derivatives 5ow, let's consider the outer feedback loop 0f we linearise the equations-of-motion, the e"pressions for the first two components of acceleration can be written in the form shown below#
* x g % cos sin & r 2 y g % sin cos & r +e can now turn our attention to the error equation describing the error in position, using these linearised equations of motion !emember, we want this error to satisfy the second-order differential equation#
i , des r i , c & K d ,i %r i , des r i & K p ,i %r i , des r i & ) %r $his equation contains terms to do with the desired trajectory, r i ,des , and terms to do with the actual trajectory being followed, r i +e will use this equation to calculate the
i ,c $his commanded acceleration will, in turn, tell us what commanded acceleration, r thrust we need to apply with the rotors u* m% g r 3, c &
1inally, we need to determine the commanded roll, pitch, and yaw using the linearised equations 0f we know that the commanded acceleration needs to be in the 6 and 7 direction, we can calculate the roll and pitch angles and their derivatives c c
* g * g
% r *, c sin des r 2, c cos des & %r *, c cos des r 2, c sin des &
+e can determine the commanded yaw angles in a similar manner c des
