Quadrotor with a Cable-Suspended Load

July 22, 2013

mQ , mL R
JQ R
f R
M R
lR
T R
xQ , xL R2
vQ , vL R2
p S1
R SO(2)
e2 , e3
b2 , b3
Q (, ]
L (, ]
Q , L R

Mass of quadrotor, load

Inertia of quadrotor
Magnitude of thrust for quadrotor
Magnitude of moment for quadrotor in body frame
Length of suspension cable
Magnitude of tension in cable
Position vector of center of mass of quadrotor, load in inertial frame
Velocity vector of center of mass of quadrotor, load in inertial frame
Unit vector from quadrotor to load, p = [sin(L ) cos(L )]T
Rotation matrix of quadrotor from body to inertial frame, R = [cos(Q ) sin(Q ); sin(Q ) cos(Q )]
Axes of the inertial frame, e2 = [1 0]T , e3 = [0 1]T
Axes of quadrotor body frame,b2 = Re2 , b3 = Re3
Angle of quadrotor counter-clockwise from horizontal
Angle of load counter-clockwise from vertical
Angular velocity of the quadrotor, load

Equations of Motion

x 1 = f1 (x1 ) + g1 (x1 )u1 ,

x+ = (x ),
1 1
2
:

=
f
(x
2
2 2 ) + g2 (x2 )u2 ,

+
x1 = 2 (x
2 ),

0
vL

x L

cos(Q L )

mQ l 2L

v L (mQ +mL ) p ge3 (mQ +mL ) p

L
0

=
x
=

+ sin(Q L )
1

0
L
mQ l

0
0
Q

x
x

L
L

v
v

L
L

xQ xL lp

x
=
=

2
v+ v lp ,

Q L

Q
Q

+
Q
Q

:
0
0

x
v

L
L

0
v L ge3 0
" #

0
0

x
v

x 2 = Q = Q + 1
,

v Q ge3 mQ b3 0 M

Q Q 0
0

Q
0
0

JQ

x
L

xL

mL vL +mQ vQ

vL
(m
+m
)

L
Q

x
x

L
Q
1

+
e3 )

L = cos ( l

x
=
,
1

L
0

Q
Q

x1 6 S1
x
1 S1
x2 6 S2
x
2 S2

0
" #
0
f
, x1 6 {x1 | T kmL (v L + ge3 )k = 0}

0 M

1
JQ

x
1 {x1 | T = 0}

x2 6 {x2 | kxQ xL k = L}

x
2 {x2 | kxQ xL k = L}

1.1

x1 system (when cable is taut)

Let e2 = [1 0]T and e3 = [0 1]T be unit vectors in the plane; let e1 be a unit vector out of the plane
Constraint: xQ = xL lp
Applied forces: f1 = f b3 at r1 = xQ = xL lp, f2 = M e1 at r2 = Q e1

Coordinates: q = [xL L Q ]T
n
X
ri
Forces: Qj =
fi
q
j
i=0
QxL = f b3

Q e1
(xL lp)
+ M e1
xL
xL

= f b3
(xL lp)
Q e1
+ M e1
L
L
 cos(L ) 
= f b3 L sin(L )

QL = f b3

= Lf sin(Q L )
QQ = f b3

Q e1
(xL lp)
+ M e1
Q
Q

=M

L=T U
1
1
1
T = mQ vQ vQ + mL vL vL + JQ 2Q
2
2
2
1
1
1
(vL lp)
+ mL vL vL + JQ 2Q
= mQ (vL lp)
2
2
2
1
1
1
+ mQ l2 (p p)
+ JQ 2Q
= (mQ + mL )(vL vL ) mQ (vL lp)
2
2
2
 cos(L ) 
1
1
1
2 2

= (mQ + mL )(vL vL ) mQ (vL lL sin(L ) ) + mQ l L + JQ 2Q

2
2
2
U = mQ ge3 xQ + mL ge3 xL
= mQ ge3 (xL lp) + mL ge3 xL
= (mQ + mL )ge3 xL + mQ lge3 p
= (mQ + mL )ge3 xL mQ lg cos(L )


1
1
1
L)
) + mQ l2 2L + JQ 2Q
L = (mQ + mL )(vL vL ) mQ (vL lL cos(
sin(
)
L
2
2
2
(mQ + mL )ge3 xL + mQ lg cos(L )

d L
L

= f b3
dt vL
xL


d 
L)
(mQ + mL )vL mQ lL cos(
(mQ + mL )ge3 = f b3
sin(L )
dt
 cos(L ) 


2
L )
(mQ + mL )vL mQ lL sin(L ) + mQ lL sin
(mQ + mL )ge3 = f b3
cos(L )

L
d L

= Lf sin(Q L )

dt L
L
 

 cos( ) 
 sin L ) 
d 
mQ lvL sin(LL) + mQ l2 L mQ lL vL cos(
+
m
lg
sin(
)
= Lf sin(Q L )
Q
L
L)
dt
 cos(L ) 
 sin L ) 
mQ lv L sin(L ) + mQ lL vL cos(L ) + mQ l2 L

L ) 
mQ lg sin(L ) = Lf sin(Q L )
mQ lL vL sin
cos(L )
 cos(L ) 
mQ lv L sin(L ) + mQ l2 L mQ lg sin(L ) = Lf sin(Q L )
d L
L

=M
dt Q
Q

d 
JQ Q = M
dt
JQ Q = M
Decoupling equations:
(mQ + mL )(v L + ge3 ) = (f cos(Q L ) mQ l 2L )p
mQ lL = f sin(Q L )
JQ Q = M

x1 = [xL vL L L Q Q ]T

vL
x L

(f cos(Q L )mQ l 2L )
p ge3
v L
(mQ +mL )

L
L

=
x 1 =
f sin(Q L )

mQ l

Q
M
Q
JQ

Note the tension force in the cable can be explicitly calculated by taking Newtons Law on the body of the
load:
F = mL v L
T p mL ge3 = mL v L
T p = mL v L + mL ge3
kT k = kmL v L + mL ge3 k

1.2

x2 system (when cable is slack)

Let e2 = [1 0]T and e3 = [0 1]T be unit vectors in the plane; let e1 be a unit vector out of the plane
Applied forces: f1 = f b3 at r1 = xQ , f2 = M e1 at r2 = Q e1

Coordinates: q = [xL xQ Q ]T
n
X
ri
Forces: Qj =
fi
qj
i=0
QxL = f b3

xQ
Q e1
+ M e1
xL
xL

=0
QxQ = f b3

Q e1
xQ
+ M e1
xQ
xQ

= f b3
QQ = f b3

(xL lp)
Q e1
+ M e1
Q
Q

=M

L=T U
1
1
T = mQ vQ vQ + mL vL vL +
2
2
U = mQ ge3 xQ + mL ge3 xL
1
1
L = mQ vQ vQ + mL vL vL +
2
2

1
JQ 2Q
2
1
JQ 2Q mQ ge3 xQ mL ge3 xL
2

d L
L

=0
dt vL
xL
d
(mL vL ) + mL ge3 = 0
dt
mL v L + mL ge3 = 0
d L
L

= f b3
dt vQ
xQ
d
(mQ vQ ) + mQ ge3 = f b3
dt
mQ v Q + mQ ge3 = f b3
d L
L

=M
dt Q
Q

d 
JQ Q = M
dt
JQ Q = M

x2 = [xL vL xQ vQ Q Q ]T

vL
x L

ge3
v L

vQ
x Q

x 2 = v = f b ge
3
mQ 3
Q

Q
Q

M
Q

JQ

2
2.1

Differential Flatness
x1 system:

Recall the equations of motion:

(mQ + mL )(v L + ge3 ) = (f cos(Q L ) mQ l 2L )p
mQ lL = f sin(Q L )
JQ Q = M
Note the tension force in the cable can be explicitly calculated by taking Newtons Law on the body of the
load:
F = mL v L
T p mL ge3 = mL v L
T p = mL v L + mL ge3
kT k = kmL v L + mL ge3 k
Choose flat outputs y = [xL ]T = [yL zL ]T
Derive y L = vyL , zL = vzL , and all higher derivatives from differentiation of yL , zL
From equation of motion:
T p = (mL x
L + mL ge3 )
T = kmL x
L + mL ge3 k
1

2
= mL (
yL
+ (
zL + g)2 ) 2

(mL x
L + mL ge3 )
 T  

mL
yL
sin(L )
=
=
cos(L )
T zL + g


p e2
L = tan1
p e3
p=

Differentiating this equation of motion:

...
T p + T p = mL x L
1
...
p = (mL x L + T p)
T
Where:
...
...
yL y L + (
zL + g) z L
T = mL
1
2 + (
(
yL
zL + g)2 ) 2
From p,
we can find the state L :


cos(L )

p = L
sin(L )


1
...
cos(L )

L = (mL x L + T p)
sin(L )
T
mL ...
...
=
( y L cos(L ) + z L sin(L ))
T
...
...
zL + g) mL z L yL
mL mL y L (
=
(

)
T
T
T
m2 ...
...
= 2L ( y L (
zL + g) z L yL )
T
zL yL yL (
zL + g)
=
2 + (
yL
zL + g)2
Differentiating again to find higher derivatives of p and L :
(4)

2T p + T p
+ Tp = mL xL
1
p
= (mL x(4) L + Tp + 2T p)

T
Where:
...2
...
...2
... 2 !
(4)
(4)
y L + yL yL
y L + (
z
z L)
)
+
z
(
z
+
g)
+
(
(
y
z
+
g)
L
L
L
L
L
T = mL

1
3
2 + (
2 + (
zL + g)2 ) 2
zL + g)2 ) 2
(
yL
(
yL
...
1
...
(4)
(4)
2
= mL (( y 2L + yL yL + zL (
zL + g) + z 2L )(
yL
+ (
zL + g)2 ) 2
...
3
...
2
yL
+ (
zL + g)2 ) 2 )
(
yL y L + (
zL + g) z L )2 (
From p
, we can find the state L :




cos(L )
sin(L )
p
= L
+ 2L
sin(L )
cos(L )


cos(L )
L = p

sin(L )

... ...
(5)
3Tp + 3T p
+ T p + T p = mL xL
... 
...
1
(5)
p =
mL xL + 3Tp + 3T p
+ Tp
T
Where:
...
T = (mL (
...
...
3(2 z L (
zL + g) + 2 y L yL )3
...
...
...
(4)
(4)
2
2
12((
zL + g)2 + yL
)(zL (
zL + g) + yL
+ yL yL + z 2L )(2 z L (
zL + g) + 2 y L yL )
... (4)
... (4)
(5)
(5)
2 2
+ 4((
zL + g)2 + yL
) (2zL (
zL + g) + 2yL yL + 6 y L yL + 6 z L zL )))
5

2 2 1
(8((
zL + g)2 + yL
) )

...
...
From p , we can find the state L :




...
...
p = ( L 3L ) cos(L ) + 3 L L sin(L )
sin(L )
cos(L )


...
... cos(L )
L = p
+ 3L
sin(L )
...
...
(6)
4 T p + 6Tp
+ 4T p + T p(4) + T (4) p = mL xL
...
...
1
(6)
+ 4T p + T (4) p)
p(4) = (mL xL + 4 T p + 6Tp
T
T (4) = (mL (
...
...
zL + g) + 2 y L yL )4
15(2 z L (
...
...
...
...
(4)
(4)
2
zL + g) + 2 y L yL )2
+ 72((
zL + g)2 + yL
)(zL (
zL + g) + y 2L + yL yL + z 2L )(2 z L (
...
...
(4)
2 2 (4)
48((
zL + g)2 + yL
) (zL (
zL + g) + y 2L + yL yL + z 2L )2
...
... (4)
... (4) ...
(5)
(5)
2 2
zL + g) + 2 y L yL )
16((
zL + g)2 + yL
) (2zL (
zL + g) + 2yL yL + 6 y L yL + 6 z L zL )(2 z L (
...
...
(6)
(4) 2
(6)
(4) 2
2 3
+ 8((
zL + g)2 + yL
) (2zL (
zL + g) + 6yL + 2yL yL + 8 y L y (5) + 6zL + 8 z L z (5) )))
7

2 2 1
(16((
zL + g)2 + yL
) )

(4)

From p(4) , we can find L :

(4)

(4)





cos(L )
sin(L )
2
2
4

=
6L L )
+ (3L L )
sin(L )
cos(L )


cos(L )
= p(4)
+ 6 2L L
sin(L )
(4)
(L

Using Newtons Equations on the quadrotor and the load to eliminate T p and solve for f b3 :
mQ x
Q = f b3 mQ ge3 + T p
mL x
L = T p mL ge3
f b3 = mQ x
Q + mL x
L + mQ ge3 + mL ge3
Using the constraint x
Q = x
L l
p:
f b3 = mQ (
xL l
p) + mL x
L + mQ ge3 + mL ge3
= (mQ + mL )(
xL + ge3 ) mQ l
p

b3 =



(mQ + mL )(
xL + ge3 ) mQ l
p
sin(Q )
=
cos(Q )
k(mQ + mL )(
xL + ge3 ) mQ l
pk

f = ((mQ + mL )(
xL + ge3 ) mQ l
p) b3


f b3 e2
Q = tan1
f b3 e3


(mQ + mL )
yL + mQ l
p e2
= tan1
(mQ + mL )(
zL + g) mQ l
p e3
...
...
...
...

Q = ((lmQ p 1 (mQ + mL ) y L )((mQ + mL )(

zL + g) lmQ p2 ) + (lmQ p 2 (mQ + mL ) z L )(lmQ p1 (mQ + mL )
yL ))
"
#


1
(k)
(lmQ p1 (mQ + mL )
yL )2
p
(lmQ p2 (mQ + mL )(
zL + g))2 (
+ 1)
, where p(k) = 1(k)
(lmQ p2 + (mQ + mL )(
zL + g))2
p2
...
...
...
...
(4)
(4)
lmQ p1 (mQ + mL )yL
2(lmQ p 1 (mQ + mL ) y L )((mQ + mL ) z L lmQ p 2 )
Q = [(

(mQ + mL )(
zL + g) lmQ p2
((mQ + mL )(
zL + g) lmQ p2 )2
...
...
(4)
(4)
((mQ + mL )zL lmQ p2 )(lmQ p1 (mQ + mL )
yL ) 2((mQ + mL ) z L lmQ p 2 )2 (lmQ p1 (mQ + mL )
yL )
+
)

((mQ + mL )(
zL + g) lmQ p2 )2
((mQ + mL )(
zL + g) lmQ p2 )3
(lmQ p1 (mQ + mL )
yL ) 2
+ 1)1 ]
((mQ + mL )(
zL + g) lmQ p2 )2
...
...
...
...
yL )2
2(lmQ p 1 (mQ + mL ) y L )(lmQ p1 (mQ + mL )
yL ) 2((mQ + mL ) z L lmQ p 2 )(lmQ p1 (mQ + mL )

)
[(
((mQ + mL )(
zL + g) lmQ p2 )2
((mQ + mL )(
zL + g) lmQ p2 )3
...
...
...
...
yL )
lmQ p 1 (mQ + mL ) y L
((mQ + mL ) z L lmQ p 2 )(lmQ p1 (mQ + mL )
(

)
((mQ + mL )(
zL + g) lmQ p2 )
((mQ + mL )(
zL + g) lmQ p2 )2

(lmQ p1 (mQ + mL )
yL ) 2
+ 1)2 ]
((mQ + mL )(
zL + g) lmQ p2 )2

10

2.1.1

Control Laws

Using the control laws from the paper:

Load Position Control
kp = kd = [1 ; 10]
F = mL (kp (ex ) kd (e x ) + x
T + ge3 ) + mQ x
dQ + mQ ge3
= mL (kp (xL xT ) kd (x L x T ) + x
T + ge3 ) + mQ (
xT l
p) + mQ ge3
(kp (xL xT ) kd (x L x T ) + x
T + ge3 )
k kp (xL xT ) kd (x L x T ) + x
T + ge3 k


d
p

e
2
dL = tan1
pd e3


sin(Q )
f =F
cos(Q )
pd =

Load Attitude Control

Tnom pnom = (mL x
T + mL ge3 )
(mL x
T + mL ge3 )
k (mL x
T + mL ge3 )k
Tnom = Tnom pnom


Tnom pnom e2
1
Lnom = tan
Tnom pnom e3
...
...
1
yT y T + (
zT + g) z T
...

p nom =
(mL x T + Tnom pnom ), Tnom = mL
1
Tnom
(
yT2 + (
zT + g)2 ) 2


cos(Lnom )
dL = p nom
sin(Lnom )
pnom =

p
nom =

1
Tnom

(mL x(4) T + Tnom pnom + 2Tnom p nom ),

Tnom = mL

...
...
(4)
(4)
zT + g) + z 2T )
( y 2T + yT yT + zT (
1

(
yT2 + (
zT + g)2 ) 2


cos(Lnom )
dL = p
nom
sin(Lnom )

!
...
...
(
yT y T + (
zT + g) z T )2
3

(
yT2 + (
zT + g)2 ) 2

kpL = 0.7, kdL = 0.1

dQ

dL

kpL eL

kdL e L

+ sin

dL mQ l
f

= dL kpL (L dL ) kdL ( L dL ) + sin1

dL mQ l
f

dL mQ l
f

Intuition:
kp is negative, so the actual control law is:
dQ

dL

kpL (dL

L )

kdL ( L

11

dL ) + sin1

Let axes +y point left, +z point up.

A positive L is to the right, a positive L is the load swinging to the right, a positive Q is the quad
turning counter-clockwise/going left.
If +
If Term
L
Right
Left
L
Swinging to right Swinging to left
d
d
Let the desired be dL = L = L = 0.
When the load starts at rest at an angle too far right, the quad turns left to correct for it. The quad is
pulling the load towards the center.
When the load starts at L = 0, but is moving right, the quad turns right, to counteract the velocity. Look
at the EOM for the load angle:
f sin(Q L )
L =
mQ l
If the load velocity is to the right, we want to have a negative acceleration (ie. slowing down the positive
velocity), thus we need a negative Q . If the load (NOT veloctiy) is too far right, we want the load velocity
to be negative, so we want the load acceleration to the positive (ie. speed up in the negative velocity), so
Q is positive.
Now, we can consider combinations of the above. For example, if the load is too far right and swinging out,
the speed of the swing, the gains, etc. will determine the quad behavior.
dQ
Term
Error Sign
L
+ (too far right)
+
L
- ( too far left)

L
+ (Swinging to right)
L
- (Swinging to left)
+

12

Quadrotor Attitude Control

...
T nom = (mL (
...
...
3(2 z T (
zT + g) + 2 y T yT )3
...
...
...
(4)
(4)
12((
zT + g)2 + yT2 )(zT (
zT + g) + yT2 + yT yT + z 2T )(2 z T (
zT + g) + 2 y T yT )
... (4)
... (4)
(5)
(5)
+ 4((
zT + g)2 + yT2 )2 (2zT (
zT + g) + 2yT yT + 6 y T yT + 6 z T zT )))
5

(8((
zT + g)2 + yT2 ) 2 )1

...
...
1 
(5)
p nom =
mL xT + 3Tnom p nom + 3Tnom p
nom + T nom pnom
Tnom
(4)
Tnom = (mL (
...
...
15(2 z T (
zT + g) + 2 y T yT )4
...
...
...
...
(4)
(4)
+ 72((
zT + g)2 + yT2 )(zT (
zT + g) + 2 y T yT )2
zT + g) + y 2T + yT yT + z 2T )(2 z T (
...
...
(4)
(4)
48((
zT + g)2 + yT2 )(zT (
zT + g) + y 2T + yT yT + z 2T )2
...
... (4)
... (4) ...
(5)
(5)
zT + g) + 2 y T yT )
16((
zT + g)2 + yT2 )2 (2zT (
zT + g) + 2yT yT + 6 y T yT + 6 z T zT )(2 z T (
...
...
(6)
(4) 2
(6)
(4) 2
+ 8((
zT + g)2 + yT2 )3 (2zT (
zT + g) + 6yT + 2yT yT + 8 y T y (5) + 6zT + 8 z T z (5) )))
7

(16((
zT + g)2 + yT2 ) 2 )1
...
...
1
(6)
(4)
p(4)
pnom )
(mL xT + 4 T nom p nom + 6Tnom p
nom + 4Tnom p nom + Tnom
nom =
Tnom
...
...
dQ = ((lmQ p 1 (mQ + mL ) y T )((mQ + mL )(
z + g) lmQ p2 )
...
...
yT ))
+ (lmQ p 2 (mQ + mL ) z T )(lmQ p1 (mQ + mL )
#
"

1
(k)
2
(lm
p

(m
+
m
)
y
)
p
Q
1
Q
L
T
1 nom
(lmQ p2 (mQ + mL )(
zT + g))2 (
+ 1)
, where p(k)
nom =
(k)
(lmQ p2 + (mQ + mL )(
zT + g))2
p2 nom

...
...
...
...
(4)
(4)
lmQ p1 (mQ + mL )yT
2(lmQ p 1 (mQ + mL ) y T )((mQ + mL ) z T lmQ p 2 )
dQ = [(

(mQ + mL )(
zT + g) lmQ p2
((mQ + mL )(
zT + g) lmQ p2 )2
...
...
(4)
(4)
((mQ + mL )zT lmQ p2 )(lmQ p1 (mQ + mL )
yT ) 2((mQ + mL ) z T lmQ p 2 )2 (lmQ p1 (mQ + mL )
yT )
+
)

2
3
((mQ + mL )(
zT + g) lmQ p2 )
((mQ + mL )(
zT + g) lmQ p2 )
(lmQ p1 (mQ + mL )
yT )2
+ 1)1 ]
((mQ + mL )(
zT + g) lmQ p2 )2
...
...
...
...
2(lmQ p 1 (mQ + mL ) y T )(lmQ p1 (mQ + mL )
yT ) 2((mQ + mL ) z T lmQ p 2 )(lmQ p1 (mQ + mL )
yT )2
[(

)
2
3
((mQ + mL )(
zT + g) lmQ p2 )
((mQ + mL )(
zT + g) lmQ p2 )
...
...
...
...
lmQ p 1 (mQ + mL ) y T
((mQ + mL ) z T lmQ p 2 )(lmQ p1 (mQ + mL )
yT )
(
)

2
((mQ + mL )(
zT + g) lmQ p2 )
((mQ + mL )(
zT + g) lmQ p2 )

(lmQ p1 (mQ + mL )
yT )2
+ 1)2 ]
((mQ + mL )(
zT + g) lmQ p2 )2
kpQ = 160, kdQ = 56
M = JQ (kpQ eQ kdQ e Q + dQ )
= JQ (kpQ (Q dQ ) kdQ ( Q dQ ) + dQ )

13

2.2

x2 system:

Recall the equations of motion:

mL v L + mL ge3 = 0
mQ v Q + mQ ge3 = f b3
JQ Q = M
Flat outputs y = [xQ ]T = [yQ zQ ]T
xL and vL are known from initial conditions because load is in free fall:
x L = vL
v L = ge3
Derive y Q = vyQ , zQ = vzQ , and all higher derivatives from differentiation of yQ , zQ
From equation of motion:
f b3 = mQ x
Q + mQ ge3
f = kmQ x
Q + mQ ge3 k
2
= mQ yQ
+ (
zQ + g)2

b3 =

21



mQ x
Q + mQ ge3
sin(Q )
=
cos(Q )
kmQ x
Q + mQ ge3 k
mQ
yQ
sin(Q ) =
f
mQ
(
zQ + g)
cos(Q ) =
f


f b3 e2
Q = tan1
f b3 e3



yQ
= tan1
zQ + g

Differentiating the equation of motion:

...
mQ x Q = fb3 + f

B b3



= fb3 + f Q b1 b3
= fb3 f Q b2
mQ ...
Q =
( x Q b2 )
f
mQ ...
...
( y Q cos(Q ) + z Q sin(Q ))
=
f
m2Q ...
...
= 2 ( y Q (
zQ + g) z Q yQ )
f
...
...
( z Q yQ y Q (
zQ + g))

= 
2
yQ + (
zQ + g)2

14

Differentiating the equation of motion again:

....
mQ x Q = fb3 + f




B b3 (f Q + f Q )b2 + f Q (I B b2 )

 

= fb3 + f Q b1 b3 (f Q + f Q )b2 + f Q ( Q b1 b2 )


= fb3 f Q b2 (f Q + f Q )b2 + f 2Q b3
I

= (2f Q + f Q )b2 + (f f 2Q )b3

f Q
mQ ....
( x Q b2 ) 2
Q =
f
f
Where:
...
...
(
yQ y Q + (
zQ + g) z Q )
f = m2Q
f
....
m
....
Q
Q =
( y Q cos(Q ) + z Q sin(Q ))
f
...
...
yQ y Q + (
zQ + g) z Q )
2m2Q (

Q
f2
m2Q ....
....
zQ + g) z Q yQ )
= 2 ( y Q (
f
2m4Q
...
...
... ...
+ 4 (
zQ + g) z Q yQ )
yQ y Q + (
zQ + g) z Q )( y Q (
f
....
....
zQ + g))
( z Q yQ y Q (


=
2
yQ + (
zQ + g)2
...
...
... ...
zQ + g) z Q yQ )
2(
yQ y Q + (
zQ + g) z Q )( y Q (
+

2
2 + (
yQ
zQ + g)2
The moment input be found from:
M = JQ Q

15

2.2.1

Control Laws

Use control laws from paper with desired trajectory: T (t) = [xT (t)] = [yT (t) zT (t)]T :
F1 = mQ ge3 + mQ x
T
F = Kp (x xT ) Kd (x x T ) + F1
f = F b3
dQ = tan1

F e2
F e3

fnom b3nom = mQ x
T + mQ ge3
mQ x
T + mQ ge3
b3nom =
kmQ x
T + mQ ge3 k


fnom b3nom e2
Qnom = tan1
fnom b3nom e3
fnom = fnom b3nom b3nom


mQ ...
cos(Qnom )
d

( x T b2nom ) , where b2nom =

Q =
sin(Qnom )
fnom
...
...
f dQ
(
yT y T + (
zT + g) z T )
mQ ....
d

( x T b2nom ) 2
,f =
Q =
fnom
fnom
fnom
Md = JQ dQ
M = JQ (Kp (Q dQ ) Kd ( Q dQ )) + Md

16

2.3

Differential Flatness of Hybrid System

A Differentially-Flat Hybrid System is a hybrid system where each subsystem is differentially-flat, the
switching surfaces are functions of the flat outputs and their derivatives, and moreover the flat outputs
map from one subsystem to a subsequent subsystem through the sufficiently smooth transition maps.
The switching surfaces are S1 = {x1 | T kmL (v L + ge3 )k = 0}, which is in terms of the flat outputs xL .
The second switching surface is S2 = {x2 | kxQ xL k = l}, which is in terms of the flat output xQ and xL
which can be determined from initial conditions.

17