Aerial Robotics Lecture 3B_3 Motion Planning for Quadrotors

 Lecture 3B.3

Motion Planning for Quadrotors We're now ready to apply what we have learned to quadrotors. Let's recall again the dynamic equations-of-motion from previous lessons:

u1 and u 2 are the two sets of inputs. We're now trying to generate trajectories in terms of the quadrotor position and trying to figure out how to control the motors to drive the vehicle to those trajectories. he quadrotor controller consists of two loops. !irst" the outer position-control-loop that specifies u1 and then second" an inner attitude-control-loop that determines u 2:

#n order to control the position" the inner loop" the attitude-control-loop" has to wor$  perfectly. %ac$ to the equations. u 1 and u2 are the two inputs. We can see from the first set of equations that the second-derivative of position depends on u 1. u1 is a scalar quantity. here's only one input to u 1" &ut we're trying to control the " y and ( components of acceleration.

)lso" notice that the rotation matri *" depends on the roll" pitch" and yaw angles. he second set of equations relates the angular velocities" p" q" and r to the derivatives of the roll" pitch" and yaw angles" and the third set of equations at the &ottom relates the input u 2 to the rate of change of angular velocity. !rom the two sets of equations at the &ottom" it is clear that the second derivative of the rotation matri depends on u 2.  +ow" if we turn our attention &ac$ to the first set of equations" we can see that the derivative of velocity" or the acceleration" depends on u 1" and that this dependence is linear. %ut through * it also depends on u 2. #f we ta$e into account the fact that this dependence of * on u 2 is through the second-derivative" as we see on the &ottom" then we can com&ine these facts to show that the fourth-derivative of position depends on u2. his might &e easier to see if we consider the linearised model of the system

#n the linearised model" the rotation matri is the identity matri. he angularvelocities are related to the rates of change of roll" pitch and yaw" through an identity matri. )nd the non-linear inertia-dependent terms at the &ottom disappear. We should go through these equations and convince ourselves that the second-derivative of position is proportional to u 1 and the fourth-derivative of position is proportional to u2. #n summary" the position control loop involves attitude control" and &ecause of that" the position control system yields a fourth-order system. o specify trajectories" we only want to consider those trajectories that are differentia&le at least four times. his motivates the use of a minimum-snap trajectory" which tries to minimi(e the fourthderivative of position integrated over the time history. T 

/



 x .t -  arg min . x .iv - - 2 dt   x . t -

,

We use minimum-snap trajectories for motion-planning for quadrotors. he video shows several eamples of minimum-snap trajectories. *emem&er that this all only wor$s if we have a ro&ust inner attitude-control loop. Let's consider what happens when we have o&stacles in the environment. 0ere is an eample of a safe minimum-snap trajectory:

) rectangular ro&ot is guided through a set of walls from an initial configuration to a final configuration. he trajectory is o&tained &y splicing together many minimumsnap trajectories" and is very reminiscent of what we saw earlier when we synthesi(ed cu&ic splines or nth-order splines to go through many different waypoints. #n this case" there are no waypoints. #nstead" the ro&ot $nows where the o&stacles are" so the challenge is to synthesi(e minimum-snap trajectories in a $nown environment. #magine that we have o&stacles in the environment" and in this case" that there's more than one quadrotor flying through the o&stacle-filled environment at the same time:

We're going to &o off each quadrotor inside virtual rectangular parallelepipeds. We assume that these rectangular parallelepipeds are $nown" and we want to ensure that they never intersect each other. #n addition" there are many o&stacles. We assume that every o&stacle is conve and that there are polyhedral models that can characterise the etent of each o&stacle. #f the o&stacles are non-conve" we can always descri&e them as a union of conve o&stacles. Let's consider a generic o&stacle" o. %ecause each o&stacle is a conve polyhedron" we want to thin$ a&out collisions in terms of the rectangular parallelepipeds that characterise the quadrotors" and each of the faces characterising the o&stacle. !or a generic face" f" on an o&stacle" o" for a generic time instant t $ " we want to ensure that the conve polyhedron characteri(ing the ro&ot" and the face f" do not intersect. hat fact is written as a linear inequality: n of  1 r.t$-   sof   he inequality restricts the positions * that can &e occupied &y the ro&ot. #t of course assumes that we $now eactly where the o&stacle is and we $now the normal + for each face" f" for all o&stacles" in this case considering o&stacle o. We can write similar equations for every face" for every o&stacle" and every ro&ot. his gives us the set of equations at the top of the figure:

he equation has &een modified &y adding a positive term on the right hand side. his term consists of a large" positive constant  and a &inary varia&le &. his &inary varia&le is characteri(ed &y the o&stacle o" the face f" and the time instant t$ . he reason for the etra term is quite simple. #n order for the ro&ot to avoid a particular o&stacle o" it is enough that this inequality &e satisfied for any one of the faces. #t is not necessary that the inequality &e satisfied for every face on the o&stacle. hat fact is captured in the inequality at the &ottom of the figure. #f there are nf  faces"  faces" we want to ensure that at least one of these &inary varia&les is ,. )nd that is captured in this inequality. his video shows an eample where this approach has &een used to synthesi(e trajectories for suspended payloads" where the vehicle is carrying a payload and the length of the suspended payload is longer than the height of the window it needs to go through. %y carefully designing minimum-

snap trajectories" the ro&ot is a&le to carry the payload through the narrow window without colliding with the environment. he same technique can &e etended to multiple quadrotors. #magine we have an environment with o&stacles. We want quadrotor 1 to go to position 1" quadrotor 2 to go to position 2" and quadrotor 3 to go to position 3" and we don't want these to intersect:

he video shows a simple eample in which two quadrotors coordinate their flight in an o&stacle filled environment" doing so safely. 4nce again" every trajectory shown is a minimum-snap trajectory. he approach has &een etended to 15 to 2, quadrotors" again" all trajectories are minimum-snap trajectories" and they're guaranteed to &e collision free.