Lecture 4.2
Nonlinear Control Up until now, we've considered the problem of controlling the quadrotor at states that are not too far from the hover equilibrium configuration. Because of this we’ve been able to make certain assumptions. We said that the roll and pitch angles were close to zero and that all the velocities were close to zero. owever, as we can see from these pictures, as the vehicle manoeuvres at high speeds and e!hibits roll and pitch angles far awa" from zero, these kinds of controllers are unlikel" to work. We want to overcome these limitations. #pecificall", we want to consider non$linear controllers that allow us to control the vehicle far awa" from the equilibrium %hover& state. et's recall the tra(ector"$control problem. )"picall" we're given a tra(ector", its derivatives up to the second order, we’re specified !%t&, "%t&, z%t&, and the "aw angle %t&.
x %t & y %t & r T %t & z %t & %t &
We're also able to differentiate them to get higher$order derivatives. We define an error vector that describes where the vehicle is, relative to where it's supposed to be, and then we tr" to drive that error e!ponentiall" to zero. B" insisting on this e!ponential convergence, we're able to determine the commanded accelerations, which then get translated to inputs that can be applied b" the quadrotor through its motors.
)his is shown in the figure below*
We approached the controller design problem in two stages. #pecificall" there's an inner loop that addresses the attitude control problem and an outer loop that then addresses the position control problem*
)he inner loop compares the commanded orientation with the actual orientation and the angular velocities and determines the input u +. )he outer loop looks at the specified position and its derivatives, compares that to the actual position and its derivative, and computes the input u . We want to do the same thing, e!cept now we want to look at tra(ectories that can e!hibit high velocities and high roll$ and pitch$angles. )hat is, tra(ectories that are not close to the equilibrium or hover state.
We start with a ver" simple control law for the outer position loop*
)his is essentiall" a proportional plus derivative control law. We have the proportional term, - p, which multiplies the error, e r , and the derivative term, - v, that multiplies the error, er . n addition, there's a feed$forward term, r des which is essentiall" the acceleration of the specified tra(ector". We're also compensating for the gravit" vector. f the s"stem were full" actuated, in other words, if the s"stem could move in an" direction, this would essentiall" solve the control problem. u would be a three dimensional vector and we could control the vehicle, so that the error in the position would go e!ponentiall" to zero. owever, u is a scalar quantit". n fact, the onl" direction in which the thrust can be applied is along the b / direction. Because of that, we do the ne!t$best thing. We take this vector and pro(ect it along the b/ direction.
)hat gives us a pro(ection that is close to the desired input, but not quite, because t is never perfectl" aligned with b3.
0e!t, we turn our attention to the attitude$control problem. -nowing -nowing that we want t to be aligned with b/, and knowing that we have ver" little fle!ibilit" in the directions that t can point in1 we do the ne!t best thing which is to rotate the vehicle, so that b / is aligned with t. R desb/
t t
)his gives us the constraint. )he desired rotation matri! should be such that the b / vector should point in the t direction. We also know what the desired "aw angle needs to be. 2rmed with these two pieces of information, it's possible to solve for the desired rotation matri!, R des. Using this information, we can now compute the error in rotation b" comparing the actual rotation, 3, and the desired rotation, 3 des*
We ne!t follow the moral equivalent of a proportional$plus$derivative control law* u+ I I % K R e R K e & )here are two terms in the parenthesis. 4ne is proportional to the error in rotation, the other is proportional to the error in angular velocit". We also have to compensate for the fact that the vehicle has a non$zero inertia, , and there are g"roscopic terms, as we know from the 5uler equations of motion. )his gives us the control inputs u and u +. )here are a couple of steps in the derivation of the control$law that are not straightforward. 6irst, how do we determine the desired rotation matri!7 We have two pieces of information. 6irst, we want the vector b / to be aligned with the vector, t. t is known and we want to find 3 des, so that this equation is satisfied* R desb/
t t
#econd, we want the "aw angle to be equal to the desired "aw angle des. We also know from the theor" of 5uler$angles that the rotation matri! has the form shown here*
cos cos sin sin sin cos sin cos sin cos sin sin R cos sin cos sin sin cos cos sin cos cos sin cos sin cos cos cos sin #o the question is* can we use the two equations shown above to solve for the two unknown angles, the roll angle and pitch angle. )he second non trivial step in the derivation of the controller is the calculation of the des error term in the rotation matri!* e R % R R & 8iven the current rotation, 3, and the desired rotation, 3 des, what is the rotation error7
We cannot simpl" subtract one matri! from another. f we do, we will get a /!/ matri!, but that matri! is not orthogonal, and therefore is not a rotation matri!. )he correct wa" to calculate the error is b" asking* what is the magnitude of the rotation required to go from the current rotation to the desired rotation7 R
des
R
)he required rotation is 3, given b"*
R RT R des We can multipl" 3 b" 3 to verif" that this is, in fact, true. 4nce we have 3, then the angle of rotation which is reall" the magnitude of rotation, and the a!is of rotation can both be determined using 3odrigues formula. t turns out that this controller guarantees that the vehicle is stable through a wide range of angles and velocities. n fact, it's basin of attraction, which is the region from which it will converge to a desired equilibrium point, is almost all of the space of rotations, and a ver" large set of angular velocities and linear velocities.
)he inequalities here characterise this basin of attraction. )he first inequalit" specifies the set of rotations from which it can converge to the hover configuration, and the second inequalit" tells us the range of angular velocities from which it can converge to the hover configuration. 0otice that the second inequalit" has a term in the denominator which relates to the eigenvalues of the inertia tensor. min is essentiall" the smallest eigenvalue of the inertia matri!. )he smaller this eigenvalue, the larger the quantit" on the right hand side, which tells us that as we scale down the inertia matri!, the set of angular velocities from which the robot can converge to the hover state increases. n other words, the vehicle becomes more stable. We see this in nature. )here are man" e!amples where the advantage of small size can be seen. We saw a video showing the abilit" of hone" hone" bees to react to collisions and recover from them. 2gain, because their inertia is small, the" have a controller whose base of attraction is fairl" large, and the"'re able to e!ploit the advantages of small size 9 small inertia to recover from large perturbations. )his is one of the motivations for us to build smaller quadrotors. )his picture shows protot"pe of the :ico ;uadrotor, which is onl" cm tip$ to$tip, weighs onl" +
We saw a video clip demonstrating the advantage of small size in which two vehicles were commanded to collide at a relative speed of + ms $. )he" were able to recover from the collision because of the large basin of attraction for the non$linear controller, and the vehicles are robust to perturbations like the ones we saw. n fact, the size of the basin of attraction scales at* ?
, >
L+ )he smaller the characteristic length, , the larger the basin of attraction. )he video showed some e!periments that illustrated the advantages of the non$linear controller over the linear controller. n the videos, the roll and pitch angles deviated significantl" from the hover position with velocities that scaled up to > ms $. n one e!periment, the non$linear tra(ector"$controller was used along with two other controllers*
)he first of these was the linearised hover$controller, and the second was a rotation$ controller or an attitude$controller. B" piecing these three controllers together, the robot was able to gather momentum before twisting its bod" to go through a window, whose width was onl" a few centimetres more than the height of the robot.
)he use of nonlinear controllers also allows us to design more aggressive tra(ectories. )o see that, it's useful to e!ploit the geometric structure that's inherent in these quadrotors.
We want to construct two vectors. 4n the right$hand side "ou see the state, q, q , and the inputs u and u +, but we've now augmented the state 9 inputs with two additional terms, u, and u, . )he vector on the left$hand side consists of the position, velocit", and acceleration, together with the (erk, (, and snap, s. 2gain, the (erk is the derivative the rate$ of the acceleration and the snap the derivative of (erk. is the "aw angle, is the second$derivative of "aw. of$change of "aw and t turns out that there's a one$to$one correspondence between the variables in the vector on the left and the variables in the vector on the right. 2nd we can see this from the equations$of$motion* u, m% a/ g .b/& u, mj/ u, ms/ u, % q + rp & p
mj+
q
mj,
u, u,
p q r
6or e!ample, if we know the acceleration in the z$direction a /, we can calculate u and b" differentiating that equation, we get an e!pression for u, . @ifferentiating it again "ields an e!pression for u, . 2ll the terms on the right$hand side of these equations are obtained from the vector consisting of position, velocit", acceleration, (erk, snap, the "aw angle, its derivative, and its second derivative. )his shows how we can get the terms in the vector on the right$hand side from the terms in the vector on the left$hand side. We can go through a similar e!ercise to go the other wa" around. What are the implications of this in terms of tra(ector" planning7 What it reall" means is that if we know the position, the "aw angle, 9 its derivatives, we can determine the state and the input. et's consider the equations for a planar quadrotor in which it's easier to visualize the mapping between these two vectors*
)he equivalence between the two vectors is due to a propert" called differential flatness. )he ke" idea is that all the state variables and the inputs can be written as smooth functions of the so$called flat outputs and their derivatives. )he flat outputs for a quadrotor are simpl" the "$ and z$position. )his equivalence works both wa"s.
y y z z y z y z y z y u , u, z u y % iv & , % iv & z u+ f we know the state vector, the input u , its first$ and second$derivative, and the input u+, we can recover the flat outputs and their derivatives*
)his relationship between the two vectors is called a diffeomorphism. )he term diffeomorphism refers to a smooth map between these two vectors, and the definition can be found in most differential$geometr" te!tbooks. #imilarl", for a three$ dimensional quadrotor, "ou have a longer vector on both sides, but once again, there's a one$to$one relationship between these vectors. f we know the flat outputs and their derivatives, "ou can recover the state variables and the inputs. )he three$dimensional quadrotor is differentiall" flat, 9 the implication for tra(ector" planning is quite simple. 3ather than think about the the d"namics of the s"stem when we plan tra(ectories, we can focus our attention on the flat space on the left$hand side. We can think about mapping obstacles on to this flat space and planning tra(ectories that are safe, provided that these tra(ectories are smooth.
f the" are smooth, since these are the flat outputs, we're guaranteed to be able to find feasible state$vectors and inputs that correspond to these tra(ectories. #o once we find smooth tra(ectories, and, as we've discussed before, we prefer to make these smooth tra(ectories minimum-snap trajectories , we can then transform them into the real space with state$vectors and inputs. 2gain, we're guaranteed that these state$vectors and inputs will satisf" the d"namic equations$of$motion.
)he video showed an e!ample of a minimum$snap tra(ector" from a starts position to a goal via an intermediate wa"point. )he tra(ector" was generated in the flat$space, consisting of the flat$outputs, without specific knowledge of the d"namics, but because these tra(ectories are smooth, the" could be automaticall" transformed into tra(ectories that are realistic and can be e!ecuted b" a real robot. )his reduces the motion$planning problem to a problem in computational geometr". f we know how to s"nthesize smooth curves going through specified wa"points, a sub(ect that we've alread" addressed in previous lectures, we can easil" transform these curves into d"namicall" feasible tra(ectories that a non$linear controller can faithfull" e!ecute.
