Path Planning Using Differential Flatness

For this application, the controller presented in Section 2.2 alone will not be sufficient. Performance can be further improved by varying desired setpoints in time and by also considering the torque required to actuate the gripper. For a dynamic system, it is expected that a smooth transition between states (e.g., not a step input) is required, but the exact requirements may not be obvious. Adding to the complexity is the fact that the quadrotor is a trivially underactuated system, as it has only four actuators and six degrees of freedom. Further, we conclude that the system has a relative degree of4by observing that the thrust can only directly affect the linear acceleration in the b3 (also Re3) direction and that the moments can only directly affect the angular acceleration (which impacts b3). Because of this, planning dynamically feasible trajectories is a significant challenge, which is only exacerbated with the consideration of adding an actuated appendage.

The authors in [36] and [98] demonstrated a method for planning dynamically feasible trajectories for certain types of underactuated systems by leveraging a change of coordinates to express the system as adifferentially flat system in a space offlat outputs. Specifically, a system is said to bedifferentially flat if there exists a change of coordinates which allows the state, (q,q˙), and control inputs, u, to be written as functions of the flat outputs and their derivatives(yi,y˙i,y¨i, ...). An additional requirement is that the flat outputs are functions of

can be planned using the flat outputs and their derivatives in the flat space since there is a unique mapping to the full state space of the dynamic system. In the MAV literature, [16] and [85] demonstrated that a set offlat outputs exists for a quadrotor, enabling application of the method in [36] and [98]. Thus, we can plan in the simplified space of flat outputs

and their derivatives, in which dynamic feasibility can be guaranteed by satisfying certain smoothness constraints. Trajectories in the flat space can then be mapped uniquely to feasible trajectories in the configuration space, making the challenge of planning a feasible trajectory for a quadrotor a tractable problem.

Proposition 1. The coupled system comprising of the quadrotor and the actuated gripper,

whose dynamics is given by (3.5), is differentially flat with a set of flat outputs given by

y=

xq zq β

T

. (3.9)

Proof. Let us first definems=mq+mg so that the center of mass of the coupled system is rs =

mqrq+mgrg

ms

. (3.10)

We recall from (3.1) that rq = rq(xq, zq) and rg = rg(xq, zq, β). Thus, rs = rs(xq, zq, β)

and ¨rs is fully defined using the proposed flat outputs and their derivatives. In addition,

this motivates the choice of β instead ofγ defining the angle of the gripper. If the gripper angle was defined relative to the quadrotor attitude, then θ would appear in rs and we

would ultimately see that γ is not a flat output. Further, it makes sense to plan using β

because at pickup, we have strict position constraints on the end effector which should be invariant to the attitude of the quadrotor. Defining e3 as the third standard basis vector, the Newton-Euler equations of motion provide

revealing that f =msk¨rs+ge3k (3.12) and b3= ¨rs+ge3 k¨rs+ge3k (3.13) from which θ can be determined. In addition, (3.13) requires that _k¨rs+ge3k >0 or that

f >0. Since the system is restricted to the planar case, b2 =e2 and b1 =b2_×b3. Next, we differentiate (3.11) to obtain

ms

...

rs= ˙fb3+Ω×fb3 (3.14)

where we know that Ω= ˙θb2. The projection ontob3 reveals

˙

f =b3·ms

...

rs (3.15)

and, using this relationship,

Ω_×b3= ms f ( ... rs−(b3· ... r3)b3). (3.16)

We notice that this is purely in theb1−b2 plane and, more specifically, thatΩ×b3 = ˙θb1. Thus, ˙ θ= ms f (b1· ... rs). (3.17)

Taking the second derivative of (3.11), we obtain

msr(4)s = ˙fθ˙b2×b3+ ¨fb3+ ˙θb2×f˙b3+ ˙θb2×

˙

θb2×fb3+ ¨θb2×fb3. (3.18) Collecting terms and simplifying cross products,

msr(4)s =

The projections onto b1 andb3 reveal ¨ θ= 1 f msb1·r(4)s −2 ˙fθ˙ (3.20) and ¨ f =b3_·msr(4)s + ˙θ2f. (3.21) Next, we letFx andFz be reaction forces at the attachment point of the gripper so that the

translational and angular equations of motion of the gripper are

mgx¨g =Fx (3.22)

mgz¨g =Fz−mgg (3.23) Igβ¨=τ+FxLgsin(β) +FzLgcos(β). (3.24)

Solving for the gripper arm actuator torque,τ,

τ =Igβ¨−Lgmg(¨xgsin(β) + (¨zg+g) cos(β)). (3.25)

Finally, we know that

M2 = ¨θIq+τ. (3.26)

Now we have demonstrated that state and inputs of the coupled system are related through a diffeomorphic map to the flat outputs and their derivatives. Therefore, the system is differentially flat.

From the previous analysis, we can conclude that any sufficiently smooth trajectory in the space of flat outputs is automatically guaranteed to satisfy the equations of motion. Specifically, we see that the control inputs to the system are functions of the snap y(4)

of the trajectories. Thus, we require that trajectories planned in the flat space be smooth in position(y), velocity ( ˙y), acceleration (¨y), and jerk (...y), which is the same as requiring each flat output to be of class _C4.