Optimal Keyframe Navigation

Here we define akeyframe as a position in space along with a yaw angle. Consider the problem of navigating throughmkeyframes at specified times. In between each keyframe there is a safe corridor that the quadrotor must stay within. This sequence of keyframes could be generated by a planning algorithm. A trivial trajectory that satisfies these con- straints is to simply fly in straight lines between keyframes. However this trajectory is inefficient because it has infinite curvature at the keyframes which requires the quadrotor to come to a stop at the keyframes.

Our method generates an optimal trajectory that passes through the keyframes at the given times while staying within the safe corridors. We generate trajectories that smoothly transition through the keyframes with the most natural velocities and accelerations. The optimization program to solve this problem while minimizing the integral of snap squared (without corridor constraints) is shown below.

min Rtm t0 µr d4_r dt4 2 +µψψ¨2Tdt (6.3) s.t. rT(ti) =ri, i= 0, ..., m ψT(ti) =ψi, i= 0, ..., m dp_x T dtp |t=tj = 0or free, j = 0, m, p= 1,2,3,4 dp_y T dtp |t=tj = 0or free, j = 0, m, p= 1,2,3,4 dp_z T dtp |t=tj = 0or free, j = 0, m, p= 1,2,3,4 dp_ψ T dtp |t=tj = 0or free, j = 0, m, p= 1,2

HererT = [xT, yT, zT]T and ri = [xi, yi, zi]. We assume that t0 = 0without loss of generality. We use piecewise polynomial functions to define the trajectory and put this problem in the form of (7.3). In between piecewise polynomial functions (at t1,...,tm−1) we enforce continuity between the first four derivatives ofrT and first two derivatives of

ψT.

Nondimensional problem

We note that in (7.1) the quantities xT, yT, zT, and ψT are independent in both the cost function and the constraints so this problem can be separated into four separate optimiza- tion problems. We now consider a general form of the optimization problem for the nondi- mensional variablew˜(τ)whereτ represents nondimensionalized time:

min R₀1 dk_dτw(τ)˜k 2 dτ (6.4) s.t. w˜(τi) = ˜wi, i= 0, ..., m dp_w(τ)˜ dτp |τ=0 = 0or free, p= 1,2,3,4 dp_w(τ)˜ dτp |τ=1 = 0or free, p= 1,2,3,4

We will show that this nondimensional problem can be transformed into one to solve for any of the variablesxT,yT,zT, orψT. First we introduce the dimensional time,t=ατ, and the dimensional variable,w, defined as:

w(t) =w(ατ) = β1 +β2w˜(τ) Next we rewrite (6.4) usingwandt:

min α2_β2k−1 R₀α dk_dtw(t)k dt (6.5) s.t. w(ti) = β1+β2w˜i, i= 1, ..., m dp_w(t) dtp |t=0 = 0or free, p= 1,2,3,4 dp_w(t) dtp |t=α = 0or free, p= 1,2,3,4

Note that in this problem the boundary conditions are spatially shifted byβ1 and scaled by

β2 and time is scaled byα. Letting the solution to the nondimensional problem bew˜∗the solution to the new problem is:

w∗(t) =β1+β2w˜∗(t/α)

Now let’s consider the nondimensional form of (7.1) where the nondimensional ˜r, ˜

ψ, and τ replace r, ψ, and t. One can solve four nondimensional problems by letting

˜rT = [ ˜w1,w˜2,w˜3]T andψ˜T = ˜w4. Then the optimal nondimensional solutions,w˜∗i(t), can be mapped to the optimal solutions forxT, yT, zT, andψT in the original problem (7.1). The time scale,α, must the same for each variable but the spatial transformation,β1 and

β2, can be unique.

Adding corridor constraints

We will now add corridor constraints to (7.1). First we defineti as the unit vector along the segment fromri tori+1:

ti =

ri+1−ri

kri+1−rik

The perpendicular distance vector,δi(t), from segmentiis defined as

δi(t) = (rT(t)−ri)−((rT(t)−ri)·ti)ti A corridor width on the infinity norm,di, is defined for each corridor.

−0.5

0

0.5

1

1.5

−0.5

0

0.5

1

1.5

2

2.5

x (m)

y (m)

−0.5

0

0.5

1

1.5

−0.5

0

0.5

1

1.5

2

2.5

x (m)

y (m)

Figure 6.1: Optimal trajectories (red) to pass through 4 keyframes (black). Left: no cor- ridor constraints. Right: corridor constraint between keyframes 2 and 3 forces changes from the unconstrained trajectory on the left.

kδi(t)k∞ ≤diwhileti ≤t≤ti+1

This constraint can be approximately satisfied in the QP by introducing nc intermediate points as follows

|xW ·δi(ti+

j

1 +nc

(ti+1−ti))| ≤di forj = 1, ..., nc

and equivalently for yW and zW. Note that the absolute value constraints can each be written as two linear constraints. The use of corridor constraints is shown in Fig. 6.1. In the left figure the optimization problem is solved without any corridor constraints and in the right figure a corridor constraint is added for the 2nd segment (d2 = 0.05andnc= 8). The trajectory stays within the dotted lines that illustrate the corridor.

Temporal Scaling

Next we consider changing the time to navigate the keyframes by a factor of α so that the new times to reach the keyframes are ti = ατi. We let the nondimensional desired

boundary conditions be˜ri =riandψ˜i =ψi in the nondimensional form of (7.1) with the corridor constraints. The solution to the true problem is simply a time-scaled version of the nondimensional solution.

r∗_T(t) =˜r∗_T(t/α), ψ_T∗(t) = ˜ψ_T∗(t/α)

This property can be used to tradeoff between fast, aggressive trajectories and slow, safe trajectories. Asαis increased the plan takes longer to execute and becomessafer. As

αgoes to infinity all the derivatives of position and yaw angle go to zero which leads to:

u(t)→[mg,0,0,0]T, ωBW,T(t)→[0,0,0]T

We can therefore satisfy arbitrary constraints of safeness by makingαlarge enough. Con- versely, asαis decreased the trajectory takes less time to execute, the derivatives of posi- tion increase, and the trajectory becomes more aggressive.

Optimal segment times

In some cases the arrival times at different keyframes is important and may be specified. However, in other cases these arrival times may not matter and we can try to find a more optimal solution by allowing more time for some segments while taking the same amount of time away from the others. Here we describe a method for finding the optimal relative segment times for a given set of keyframes. For this part it is more convenient to think of the time allowed for segmenti, Ti, rather than the arrival time for keyframei,tiwhere

Ti =ti−ti−1. We then solve the minimization problem:

min f(T) (6.6)

s.t. P

Ti =tm

Ti ≥0

wheref(T)is the solution the optimization problem (7.1) for segment timesT. We solve (6.6) via a constrained gradient descent method. The vectorsgiare constructed so that the

0 1 2 3 −0.5 0 0.5 1 1.5 2 2.5 y (m) x (m) 1 2 3 4 5 6 7 0.4 0.6 0.8 1 1.2 1.4 1.6x 10 5 f(T) Iteration

k=1

k=7

Figure 6.2: Illustration of relative time scaling. Left: Trajectory for different iterations. Right: Cost function vs. iteration.

ith element has a value of 1 and all other elements have the value _m−₋1₂. This is done so that

P

gi = 0andgi can be added or subtracted fromTand the final time does not change. Next we numerically compute the directional direction for eachgias follows.

∇gif =

f(T+hgi)−f(T)

h

where h is some small number. Given the estimates of the directional derivatives we perform gradient descent using backtracking line search. An illustration of this method for a trajectory in the x −y plane where the keyframes are simply points is shown in Fig. 6.2. The first choice of segment times allowed too much time for the 2nd segment and the trajectory for this segment deviates significantly from the convex hull formed by the keyframes. After 7 iterations the cost function converges to a minimum. The optimal trajectory appears to be a very natural trajectory for passing through all keyframes which qualitatively validates the choice of the cost function.