Trajectory tracking and control

To present the controller, recall from Section4.3.2 the system of equations for the dynamic model of a quadrotor in inertial frame,{A}

˙ ζ

ζζ =v, (6.33)

§6.5 High-Level Control 143

Substituting for the drag forceD=−TKrVfrom (4.20), then RD= ₋TRKRR>v, given thatv= R>V Kr=   ¯ c 0 0 0 c¯ 0 0 0 0  . From Bangura and Mahony [2014],

TRKrR>v=Tc¯v−TcR¯ ~e3Vz, sinceVz =~e>₃R>v. If ¯ T= T(1−cV¯ z), then (6.34) becomes mζζζ¨ =mg~e3−TR¯ ~e3−Tc¯v. (6.35)

From (6.35), the error dynamics for the linear dynamics is given by

mv˙˜ =mg~e3−T¯dR~e3−Tdc¯v−mv˙d. (6.36)

Theorem 6.4. Let Kp,Kd ∈ R3×3 be diagonal positive definite gain matrices. Consider the trajectory tracking with the following the control law

¯

TdRd~e3 =mg~e3+Kpζζζ˜+Kdv˜ −cT¯ dvd−mv˙d. (6.37) Based on Assumption 6.3, then the error dynamics of (6.36)are locally exponentially stable around the equilibrium pointζζζ˜ =v˜ = (0, 0, 0)>under the control law of (6.37).

Proof. From Assumption 6.2 and error dynamics (6.36), the new error dynamics un- der the control law of (6.37) is

mζζζ¨˜ =−Kpζζζ˜ −K_dv˜. (6.38)

This is an autonomous block diagonal linear system in error coordinates with Eigen values which are asymptotically stable and therefore globally exponentially stable. Hence the error dynamics are locally exponentially stable since for linear systems, asymptotic stability implies exponential stability.

Remark 6.3. It has been assumed thatζζζ˜ andv˜ are bounded (Assumption 6.3) such that the

attitude Rd and feedforward angular ratesΩΩΩd are also bounded, hence, the controllers can only be relied upon for local stability. Therefore, system(6.36)is locally exponentially stable under the control law of (6.37).

By the notion of input-to-state stability, given that the mid and high-level con- trollers are both locally exponentially stable, the entire system is locally exponentially stable around ˜ζζζ =v˜ =0 [Khalil, 1996, Lemma 5.6].

Unlike previous trajectory controllers such as Mellinger and Kumar [2011], the proposed control system does not use feedback linearisation and assumes availability of higher derivatives ofζζζandζζζdto determine feedforward terms for precise tracking.

Consider taking the derivative of (6.37),

¯ TdR˙d~e3+T˙¯dRd~e3=Kpv˜ +Kdv˙˜−cT¯ dv˙d−c¯T˙dvd−mv¨d. (6.39) From (4.1c) ˙ R_d = R_dΩΩΩ_d×. Thus ¯ TdRdΩΩΩd×~e3+T˙¯dRd~e3 =Kpv˜ +Kdv˙˜ −cT¯ dv˙d−c¯T˙dvd−mv¨d. (6.40) Since ¯T_d andR_d have been found from (6.37), by substituting them,Ωdx,Ωdy and ˙Td

can be determined. To obtain ˙Ωdx and ˙Ωdy, one has to differentiate further to use the

jerk ( ¨v) of the trajectory. Higher derivatives,...vd and beyond can be ignored as they

may not be available from the desired trajectory.

¯

T_d R˙_dΩΩΩ_d×~e3+RdΩΩΩ˙ d×~e3

+T˙¯_dR_dΩΩΩ_d×~e3+T¨¯dRd~e3+T˙¯dR˙d~e3 =Kpv˙˜ +Kdv¨˜+γγγ,

whereγγγ=−c¯T˙dv˙d−c¯T˙v¨d−c¯T¨dvd−c¯T˙dv˙d. Substituting for ˙Rd

¯ T_d R˙_dΩΩΩ_d×~e3+RdΩΩΩ˙ d×~e3 +2 ˙¯T_dR_dΩΩΩ_d×~e3 +T¨dRd~e3= Kpv˙˜+Kdv¨˜ +γγγ. (6.41) Therefore ¯ TdRdΩΩΩ˙ d×~e3+T¨¯dRd~e3= −T¯dRdΩΩΩd×ΩΩΩd×~e3−2 ˙¯TdRdΩΩΩd×~e3+Kpv˙˜+Kdv¨˜ +γγγ, (6.42) which is solved for ˙Ωdx, ˙Ωdy and ¨Td. Though ¨Td is never used, it gives an indica-

tion of the feasibility of the trajectory and is upper-bounded by the motor rise time of the low-level motor control response. With Rd~e3,Td,ΩΩΩd and ˙ΩΩΩd determined alge-

braically, they are then used as setpoints for the attitude controller of Section 6.4. In Section 6.5.3, determination of the full desired attitudeRd will be outlined using the

controller outputR_d~e3 and the desired~e1direction.