Cooperative Quadrocopter Ball Throwing and Catching
Online Appendix
Robin Ritz, Mark W. Müller, Markus Hehn, and Raffaello D’Andrea
I. M INIMUM M AXIMUM ACCELERATION T RAJECTORY
A. Maneuver Structure
In order to find a maneuver that minimizes the maximum acceleration of a one-dimensional system with constrained jerk,
we have to solve the optimizing problem
minimize
max (|a|)
subject to ṡ = f (s, k),
(1)
s(t0 ) = s0 ,
s(tf ) = sf ,
k ∈ [−kmax , kmax ] ∀t ∈ [t0 , tf ],
with the system dynamics
ṡ = f (s, k) = (ṗ, v̇, ȧ) = (v, a, k).
(2)
We use Pontryagin’s minimum principle to determine the structure of the optimal solution. The maximum acceleration
magnitude within the interval [t0 , tf ] can be written as
s
Z tf
1
2n
max (|a|) = lim
a2n dt,
(3)
n→∞
tf − t0 t0
p
where n is a positive integer. Since the function 2n 1/(tf − t0 )(.) is strictly increasing, we can minimize its argument
instead of minimizing the function value. Therefore, we define the cost function to be
Z tf
Z tf
J=
g(s, k)dt =
a2n dt ⇒ g(s, k) = a2n ,
(4)
t0
t0
where we later increase n towards infinity. The Hamiltonian is then given by
H(s, k, p) = g(s, k) + pT f (s, k) = a2n + p1 v + p2 a + p3 k,
(5)
where p denotes the costates. We can see that the Hamiltonian is linear in k, meaning that the optimal control input jumps
between ±kmax if p3 switches its sign (the optimal solution minimizes the Hamiltonian). Additionally, for each switch a
singular arc might occur, meaning that p3 stays at zero for a nontrivial interval of time. Within these intervals, the input k
is determined by the condition that p3 and all its time-derivatives stay at zero. The adjoint equation
ṗ = −∇s H(s, k, p)
(6)
yields for our problem:
ṗ1 = 0
⇒
p1 = c1
ṗ2 = −p1
⇒
p2 = c2 − c1 t
(7)
ṗ3 = −p2 − 2na2n−1
By solving ṗ3 = 0 for a, we get the acceleration within a singular arc:
a(t) = ±c1
t − c2 /c1
±
2n
1
2n−1
.
(8)
1
For n > 1, the imaginary part of (8) only vanishes for positive values within the brackets, we therefore factor out ±c1 such
that the bracket is positive. To find the acceleration within a singular interval of a maneuver that minimizes max (|a|), we
increase n towards infinity, yielding
lim ±c1
n→∞
t − c2 /c1
±
2n
1
2n−1
= ±c1 .
(9)
Hence, the acceleration is constant if we are in a singular arc, and consequently the jerk stays at zero. Furthermore, knowing
that the acceleration is constant, it can be verified that more than two switches always increase the required maximum
acceleration.
Summing up, the optimal jerk trajectory switches twice between the jerk boundaries, and at each switch it might stay at
zero for a nontrivial interval of time; the maneuver has at most five intervals:
• [t0 , t1 ): k = ±kmax ,
• [t1 , t2 ): k = 0 and a = ±amax ,
• [t2 , t3 ): k = ∓kmax ,
• [t3 , t4 ): k = 0 and a = ∓amax ,
• [t4 , tf ]: k = ±kmax ,
with amax = c1 .
B. Maneuver Parameters
Each of the two singular intervals might vanish, hence we do not know the number of intervals beforehand. Furthermore,
the sign of the jerk within the first interval is not known, either. We therefore compute the solutions for all possible cases,
and pick the right solution at the end. The right solution is the one with the smallest amax among all solutions that make
physically sense.
1) Case t2 > t1 and t4 > t3 : In order to find the parameters of such a maneuver, we have to find the five unknowns
t1 , t2 , t3 , t4 , amax ,
(10)
satisfying the five conditions
p(tf ) = pf ,
(11)
v(tf ) = vf ,
(12)
a(tf ) = af ,
(13)
a(t1 ) = amax ,
(14)
a(t3 ) = −amax .
(15)
By inspecting the structure of the acceleration trajectory, the conditions (13)-(15) can be used to express the unknowns t1 ,
t3 , and t4 as linear functions of the two unknowns amax and t2 :
amax −a0
kmax ,
amax
t2 + 2 kmax ,
a
+af
tf − max
kmax .
t1 = t0 +
t3 =
t4 =
(16)
2
In order to obtain the closed-form solutions of the maneuver parameters, we integrate the different maneuver intervals
analytically, and subsequently substitute the unknowns t1 , t3 , and t4 by inserting (16). We get
p(tf ) = −a0
+
3
−24amax 3 +af 3 −3a0 2 (amax +kmax T )+3amax 2 (af +7kmax T −8kmax T2 )+3a0 amax (7amax −2kmax T +4kmax T2 )
6kmax 2
3amax (af 2 −kmax 2 (T 2 −4T T2 +2T2 2 ))+6kmax 2 (p0 +T v0 )
,
6kmax 2
v(tf ) = −a0
2
(17)
−2a0 amax +8amax 2 +af 2 +2amax (af −kmax T +2kmax T2 )+2kmax v0
,
2kmax
a(tf ) =af ,
(18)
(19)
where the notations T2 = t2 − t1 and T = tf − t0 were used. The resulting final velocity (18) is linear in T2 , hence we can
insert (18) into condition (12) and subsequently solve for T2 :
T2 =
a0 2 +2a0 amax −8amax 2 −2amax af −af 2 +2amax kmax T −2kmax v0 +2kmax vf
4amax kmax
.
(20)
By inserting (20) into (17), the final position condition (11) results in a cubic equation for the remaining unknown amax :
c3 amax 3 + c2 amax 2 + c1 amax + c0 = 0,
(21)
with
a0 −af +kmax T
,
2kmax 2
2
(a0 −af +kmax T )
,
4kmax 2
a0 3 −af 3 −3af 2 kmax T +3a0 2 (af −kmax T )−3a0 (af 2 +2kmax (v0 −vf ))+6af kmax (vf −v0 )+6kmax 2 (2p0 −2pf +T (v0 +vf ))
,
12kmax 2
2
(a0 2 −af 2 +2kmax (vf −v0 ))
.
−
16kmax 2
c3 = −
c2 =
c1 =
c0 =
(22)
This equation can be solved using Cardano’s method for general cubic equations, yielding three possible solutions for amax .
Afterwards, we can use (16) and (20) to obtain the other four unknowns.
2) Case t2 = t1 and t4 > t3 : In order to find the maneuver parameters for this case, we have to find the four unknowns
t1 , t3 , t4 , amax ,
(23)
satisfying the four conditions
p(tf ) = pf ,
(24)
v(tf ) = vf ,
(25)
a(tf ) = af ,
(26)
a(t3 ) = −amax .
(27)
Similar to the derivations above, we can use the conditions (26) and (27) to express t1 and t4 as linear functions of t3
and amax :
t1 = 12 t0 + 12 t3 −
t4 = tf −
amax +a0
2kmax ,
amax +af
kmax
(28)
.
3
Again, we integrate the system over all intervals and insert the substitution (28). We get for the final state:
p(tf ) = −a0
+
3
+af 3 +6kmax 2 p0 +3amax 2 (af −kmax T )−3a0 2 (amax +kmax T )+6kmax 3 T T3 2 −6kmax 3 T3 3
6kmax 2
3amax (af 2 −kmax 2 (T 2 −2T3 2 ))−3a0 (amax 2 +2amax kmax T −2kmax 2 T3 2 )+6kmax 2 T v0
,
6kmax 2
v(tf ) = −a0
2
−2a0 amax +af 2 +2amax (af −kmax T )+2kmax 2 T3 2 +2kmax v0
,
2kmax
a(tf ) =af ,
(29)
(30)
(31)
with T3 = t3 − t2 . In this case, the final velocity (30) is quadratic with respect to T3 , and we get two solutions when we
solve condition (25):
√ 2
a0 +2a0 amax −2amax af −af 2 +2amax kmax T −2kmax v0 +2kmax vf
(32)
√
.
T3 = ±
2k
max
Since T3 must be positive for a valid trajectory, we insert the positive solution of (32) into (29), and the final position
condition (24) yields a radical equation:
p
c2 amax 2 + c1 amax + c0 + d3 amax 3 + d2 amax 2 + d1 amax + d0 = 0,
(33)
with
c2 =
c1 =
c0 =
d3 =
d2 =
d1 =
d0 =
a0 −af +kmax T
,
2kmax 2
2a0 2 −2a0 (af −kmax T )+kmax (−2af T +kmax T 2 −2v0 +2vf )
,
2kmax 2
2a0 3 +af 3 −3af 2 kmax T −3a0 (af 2 +2kmax (v0 −vf ))+6kmax 2 (p0 −pf +T vf )
,
6kmax 2
(a0 −af +kmax T )3
,
kmax 4
3(a0 −af +kmax T )2 (a0 2 −af 2 +2kmax (−v0 +vf ))
,
2kmax 4
2
3(a0 −af +kmax T )(a0 2 −af 2 +2kmax (−v0 +vf ))
,
4kmax 4
3
(a0 2 −af 2 +2kmax (−v0 +vf ))
.
8kmax 4
(34)
Equation (33) can be converted to a quartic equation
c22 amax 4 + (2c1 c2 + d3 )amax 3 + (c21 + 2c0 c2 + d2 )amax 2 + (2c0 c1 + d1 )amax + c20 + d0 = 0,
(35)
which can be solved for a general case using Ferrari’s method. Four possible solutions for amax exist.
3) Case t2 > t1 and t4 = t3 : In order to find the parameters of such a maneuver, we have to find the four unknowns
t1 , t2 , t3 , amax ,
(36)
satisfying the four conditions
p(tf ) = pf ,
(37)
v(tf ) = vf ,
(38)
a(tf ) = af ,
(39)
a(t1 ) = amax .
(40)
4
Again, we can use the conditions (39) and (40) to express t1 and t2 as linear functions of t3 and amax :
amax −a0
kmax ,
a −amax
2t3 − tf + fkmax
.
t1 = t0 +
t2 =
(41)
Integrating the system over all intervals, and subsequently applying substitution (41) yields
p(tf ) = −a0
+
3
+af 3 +6kmax 2 p0 −3a0 amax (amax −2kmax T )+3a0 2 (amax −kmax T )+3amax 2 (af −kmax T )
6kmax 2
amax (−3af 2 +3kmax 2 (T 2 +2T3 2 ))+6kmax 2 T v0 −6af kmax 2 T3 2 −6kmax 3 T3 3
,
6kmax 2
v(tf ) = −a0
2
+2a0 amax −2amax af +af 2 +2amax kmax T −2kmax 2 T3 2 +2kmax v0
,
2kmax
a(tf ) =af .
(42)
(43)
(44)
Again, the final velocity (43) is quadratic in T3 , and solving the final velocity condition (38) for T3 yields:
√ 2
−a0 +2a0 amax −2amax af +af 2 +2amax kmax T +2kmax v0 −2kmax vf
√
T3 = ±
.
2k
(45)
max
We insert the positive solution of (45) into (42), and the final position condition (37) yields a radical equation:
p
c2 amax 2 + c1 amax + c0 + d3 amax 3 + d2 amax 2 + d1 amax + d0 = 0,
(46)
with
c2 =
c1 =
c0 =
d3 =
d2 =
d1 =
d0 =
a0 −af +kmax T
,
2kmax 2
2af 2 −2af kmax T −2a0 (af −kmax T )+kmax (kmax T 2 +2v0 −2vf )
,
2kmax 2
−a0 3 −2af 3 +3a0 2 (af −kmax T )+6kmax 2 (p0 −pf +T v0 )+6af kmax (−v0 +vf )
,
6kmax 2
(a0 −af +kmax T )3
,
kmax 4
3(a0 −af +kmax T )2 (−a0 2 +af 2 +2kmax (v0 −vf ))
,
2kmax 4
2
3(a0 −af +kmax T )(a0 2 −af 2 +2kmax (−v0 +vf ))
,
4kmax 4
3
(−a0 2 +af 2 +2kmax (v0 −vf ))
.
8kmax 4
(47)
Finally, we convert (46) to a quartic equation
c22 amax 4 + (2c1 c2 + d3 )amax 3 + (c21 + 2c0 c2 + d2 )amax 2 + (2c0 c1 + d1 )amax + c20 + d0 = 0,
(48)
which can be solved for general cases. Again, four possible solutions for amax result.
4) Case t2 = t1 and t4 = t3 : In this case, we have two unknowns (t1 and t3 ) and three final state conditions; we cannot
find a solution in general, meaning that this case only occurs as limit case of one of the other solutions above.
5) Negative Initial Jerk: All derivations above were done for positive jerk in the first interval. For negative initial jerk, we
substitute kmax = −kmax and amax = −amax and apply the same formulas.
5
II. T HROW T RAJECTORY
A. First Interval, t0 < t ≤ t1
Within the first interval of the throw, the acceleration is constant and given by
r̈net (t) = ath1 sin (φ0 ),
(49)
z̈net (t) = ath1 cos (φ0 ) − g,
where φ0 = φ(t0 ) is the steady-state roll angle. The velocity and position trajectories yield:
ṙnet (t) = ath1 sin (φ0 )t,
rnet (t) = r0 + 21 ath1 sin (φ0 )t2 ,
(50)
żnet (t) = (ath1 cos (φ0 ) − g)t,
znet (t) = z0 + 12 (ath1 cos (φ0 ) − g)t2 ,
where r0 = rnet (t0 ) denotes the initial net radius, and z0 = znet (t0 ) the initial net reference height.
B. Second Interval, t1 < t ≤ t2
Within the second interval, the vehicles turn outwards and the acceleration trajectories are
r̈net (t) = ath2 sin (φ0 + φ̇th2 (t − t1 )),
(51)
z̈net (t) = ath2 cos (φ0 + φ̇th2 (t − t1 )) − g.
By integration of the acceleration trajectories, we get
ath2 (cos (φ0 )−cos (φ0 +φ̇th2 (t−t1 )))
,
φ̇th2
ath2 (sin (φ0 )−sin (φ0 +φ̇th2 (t−t1 ))+φ̇th2 (t−t1 ) cos (φ0 ))
,
rnet (t1 ) + ṙnet (t1 )(t − t1 ) +
φ̇2th2
(φ0 +φ̇th2 (t−t1 )))
żnet (t1 ) − g(t − t1 ) − ath2 (sin (φ0 )−sin
,
φ̇th2
(t−t1 ))−φ̇th2 (t−t1 ) sin (φ0 ))
.
znet (t1 ) + żnet (t1 )(t − t1 ) − 12 g(t − t1 )2 + ath2 (cos (φ0 )−cos (φ0 +φ̇th2
φ̇2th2
ṙnet (t) = ṙnet (t1 ) +
rnet (t) =
żnet (t) =
znet (t) =
(52)
The parameters φ̇th2 and t2 are determined by the two conditions
rnet (t2 ) = lnet ,
φ(t2 )
(53)
= π/2.
We can insert (52) into (53) and solve for the unknowns. Two solutions exist; we pick the one yielding a positive roll rate:
√
−2ṙnet (t1 )φ0 +ṙnet (t1 )π+ (ṙnet (t1 )π−2ṙnet (t1 )φ0 )2 +8ath2 (lnet −rnet (t1 ))((π−2φ0 ) cos (φ0 )+2 sin (φ0 )−2)
φ̇th2 =
,
4(lnet −rnet (t1 ))
(54)
π/2−φ0
t2 = φ̇
.
th2
6
C. Third Interval, t2 < t ≤ t3
The trajectories during the stretching interval follow the trajectories of a linear spring-mass system, hence the accelerations
are given by
r̈net (t) = −astretch sin (λn (t − t2 ) + τstretch ),
(55)
z̈net (t) = −g,
where λn is the natural frequency of the system. The velocity and position trajectories yield
ṙnet (t) =
rnet (t) =
astretch
cos (λn (t − t2 ) + τstretch ),
λn
ath3
+ astretch
sin (λn (t − t2 ) + τstretch ),
λn 2
λn 2
(56)
żnet (t) = żnet (t2 ) − g(t − t2 ),
znet (t) = znet (t2 ) + żnet (t2 )(t − t2 ) − 12 g(t − t2 )2 .
We can insert (56) into the three conditions
rnet (t2 + ) = rnet (t2 − ) = lnet ,
ṙnet (t2 + ) = ṙnet (t2 − ),
(57)
rnet (t3 − ) = rnet (t3 + ) = lnet ,
and solve for the three unknowns astretch , τstretch , and t3 . Since multiple solutions exist, we pick the one with t3 > t2
and τstretch > 0:
q
2
astretch = − ṙnet (t2 )2 λn 2 + ath3 − λn 2 lnet ,
ṙnet (t2 )λn
√
,
τstretch = arccos −
(58)
2
2
2
2
th3 −λn lnet )
ṙnet (t2 ) λn +(a
ath3 −λn 2 lnet
.
t3
= t2 + λ1n τstretch − arcsin √
2
2
2
2
ṙnet (t2 ) λn +(ath3 −λn lnet )
D. Fourth Interval, t3 < t ≤ tf
In the fourth interval, the vehicles turn back and decelerate. The acceleration trajectories are given by
r̈net (t) = ath4 sin (π/2 + φ̇th2 (t − t3 )),
(59)
z̈net (t) = ath4 cos (π/2 + φ̇th2 (t − t3 )) − g.
The position and velocity trajectories yield
ath4 sin (φ̇th4 (t−t3 ))
,
φ̇th4
ath4 (1−cos (φ̇th4 (t−t3 )))
lnet + ṙnet (t3 )(t − t3 ) +
,
φ̇2th4
żnet (t3 ) − g(t − t3 ) − ath4 (1−cosφ̇(φ̇th4 (t−t3 ))) ,
th4
1
3 ))−φ̇th4 (t−t3 ))
znet (t3 ) + żnet (t3 )(t − t3 ) − 2 g(t − t3 )2 + ath4 (sin (φ̇th4 (t−t
.
φ̇2
ṙnet (t) = ṙnet (t3 ) +
rnet (t) =
żnet (t) =
znet (t) =
(60)
th4
The three final conditions to be satisfied are
rnet (tf ) = rf ,
(61)
ṙnet (tf ) = 0,
φ(tf )
= φf .
7
By inserting (60) into (61), and solving for the three unknowns ath4 , φ̇th4 , and tf , we get
ath4 =
φ̇th4 =
tf
=
ṙnet (t3 )2 sec (φtf )(π−2φtf −2 sec (φtf )+2 tan (φtf ))
,
2(lnet −rf )
ṙnet (t3 )(π−2φtf −2 sec (φtf )+2 tan (φtf ))
,
2(lnet −rf )
π
−φ
t
t3 + 2−φ̇ f .
(62)
th4
8