1

Robust Multi-Robot Trajectory Optimization Using Alternating Direction Method of MultiplierRuiqi Ni1, Zherong Pan2 and Xifeng Gao2

Abstract—We propose a variant of alternating directionmethod of multiplier (ADMM) to solve constrained trajectoryoptimization problems. Our ADMM framework breaks a jointoptimization into small sub-problems, leading to a low iterationcost and decentralized parameter updates. Starting from acollision-free initial trajectory, our method inherits the theoreticalproperties of primal interior point method (P-IPM), i.e., guaran-teed collision avoidance and homotopy preservation throughoutoptimization, while being orders of magnitude faster. We haveanalyzed the convergence and evaluated our method for time-optimal multi-UAV trajectory optimizations and simultaneousgoal-reaching of multiple robot arms, where we take into considerkinematics-, dynamics-limits, and homotopy-preserving collisionconstraints. Our method highlights an order of magnitude’sspeedup, while generating trajectories of comparable qualitiesas state-of-the-art P-IPM solver.

I. INTRODUCTION

This paper focuses on trajectory optimization problems,a fundamental topic in robotic motion planning. Althoughthe problem finds countless domains of applications, theirpivotal common feature could be illustrated through the lensof two applications: goal-reaching of multiple UAVs andarticulated robot arms. UAV trajectory optimization has beenstudied vastly [1]. Due to their small size and differential-flatdynamics [2], point-mass models can be used and Cartesian-space trajectories are linear functions of configuration vari-ables. Furthermore, the quality of a UAV trajectory could bemeasured via convex metrics such as jerk or snap, castingtrajectory optimization as convex programs. However, whenflying in obstacle-rich environments and among other UAVs,non-convex, collision constraints must be considered [3].Failing to satisfy these constraints can render a generatedtrajectory completely useless. Handling articulated robot armsposes an even more challenging problem, where the lineardynamic assumption must be replaced with a nonlinear for-ward kinematic function that maps from configuration- toCartesian-space, rendering all the Cartesian-space constraintsnon-convex. In summary, trajectory optimizer should pertainthree properties: (versatility) handle non-convex constraintsand kinematic models; (robustness) guarantee to satisfy allthe constraints throughout optimization; (efficacy) rapidlyrefine feasible initial trajectories into nearby, locally optimalsolutions.

We observe that prior trajectory optimization techniques ex-hibit remarkable performs under certain assumptions but havepartial coverage of the three features above. For example, off-the-shelf primal-dual optimizers can solve general constrainedprograms and have been applied to trajectory optimization [4,3, 5]. However, these methods violate robustness by allowinga feasible trajectory to leave the constraint manifold. Similarly,penalty methods [6, 7, 8] have been used for trajectoryoptimization by replacing hard constraints with soft energies,

1 Department of Computer Science, Florida State University,rn19g@my.fsu.edu. 2 Lightspeed & Quantum Studios, Tencent America,{zherong.pan.usa, gxf.xisha}@gmail.com.

which cannot guarantee robustness. On the other hand, weproposed a new optimizer in our prior work [9] for UAVtrajectory planning with perfect versatility and robustness,where all the constraints are converted into primal-only log-barrier functions with finite duality gap. As a result, all theconstraints are satisfied throughout the optimization with theContinuous Collision Detection (CCD) bounded line searchstep. With the improved robustness, however, comes a signifi-cant sacrifice in efficacy. For the same benchmarks, our primal-only methods can take 3−5× more computations to converge ascompared with primal-dual counterparts. This is due to the log-barrier functions introducing arbitrarily large gradients nearthe constraint boundaries. As a result, an optimizer needs touse a costly line-search after each iteration to ensure a safesolution that satisfies all the stiff constraints. The gradient-flows of such objective functions are known as stiff dynamics,for which numerical time-integration can have ill-convergenceas studied [10].

Main Results: We propose a variant of ADMM-type solverthat inherits the versatility and robustness from [9], while weachieve orders of magnitude higher performance. Intuitively,ADMM separates non-stiff and stiff objective terms intodifferent sub-problems using slack variables, so that each sub-problem is well-conditioned. Moreover, since sub-problemsare independent and involve very few decision variables, anADMM iteration can be trivially parallelized and incurs amuch lower cost. Existing convergence analysis, however, onlyguarantees that ADMM converges for convex problems or non-convex problems with linear or affine constraints [11]. Wepresent improved analysis which shows that our ADMM vari-ant converges for both UAV and articulated trajectory planningproblems under nonlinear collision constraints, kinematic- anddynamic-limits. We have applied our method to large-scalemulti-UAV trajectory optimization and articulated multi-robotgoal-reaching problems as defined in Section III. During ourevaluations (Section V), we observe tens of times’ speedupover Newton-type methods. Our algorithms are detailed inSection IV.

II. RELATED WORK

We review related trajectory generation techniques andcover necessary backgrounds in operations research.

Trajectory Generation aims at computing robot trajec-tories from high-level goals and constraints. Their typicalscenarios of applications involve navigation [12], multi-UAVcoordination [13], human-robot interaction [14], tele-operation[15], trajectory following [16], etc., where frequent trajectoryupdate is a necessity to handle various sources of uncertainty.Due to the limited computational resources, early works usepre-computations to reduce the runtime cost. For example,Panagou, Shimoda et al. [12, 17] modulate a vector field toguide agents in a collision-free manner, while assuming pointrobots and known environments. Closed-form solutions suchas [18, 19] exist but are limited to certain types of dynamic

arX

iv:2

111.

0701

6v3

[cs

.RO

] 5

Feb

202

2

2

systems or problem paradigms. More recently, Belghith et al.,Karaman et al. [20, 21] have established anytime-variants ofsampling-based roadmaps that continually improve an initialfeasible solution during execution.

Trajectory Optimization refines robot trajectories givena feasible or infeasible initial guess. Trajectory optimizationdates back to [22, 4], but has recently gained significantly at-tention due to the maturity of nonlinear programming solvers.These methods have robots’ goal of navigation formulated asobjective functions, while taking various safety requirementsas (non)linear constraints. They achieve unprecedented successin real-time control of high-dimensional articulated bodies[23] and large swarms of UAVs [3]. In particular, trajectoryoptimizer can also be used for trajectory generation by settingthe initial trajectory to be a trivial solution [23, 24, 25]. Onthe down side, most trajectory optimization techniques sufferfrom a lack of robustness. Many works formulate constraintsas soft objective functions [26, 23] or use primal-dual interiorpoint methods to handle non-convex constraints [4, 27, 25],which does not guarantee constraint satisfaction. On the otherhand, some techniques [28, 29] restrict the solution space to adisjoint convex subset so that efficient solvers are available, butthese methods are limited to returning sub-optimal solutions.Instead, Ni et al. [9] starts from a strictly feasible initialguess and uses a primal-only method to transform non-convexconstraints into log-barrier functions with finite duality gap.Using a line-search with CCD as safe-guard, it is guaranteedto satisfy all constraints and no restrictions in solution spaceare needed. But optimizer in this case can make slow progress,being blocked by the large gradient of log-barrier functions.This dilemma between efficacy and robustness has beenstudied as stiff dynamic systems [10], for which dedicatedtechniques are developed for different applications such asnumerical continuation [30]. Unlike these methods, we showthat first-order methods can be combined with barrier methodsto achieve significant speedup.

Alternating Direction Method of Multiplier is a first-order optimization framework originally designed in convex-programming paradigm [31]. ADMM features a low iterationcost and moderate accuracy of solutions, making it a stellarfit for trajectory optimization where many iterations can beperformed within a short period of time. ADMM uses slackvariables to split the problem into small subproblems andapproximately maintains the consistency between slack andoriginal variables by updating the Lagrangian multipliers.Although theoretical convergence guarantee was only availableunder convex assumptions, ADMM has been adapted to solvenon-linear fluid dynamics control [32], collision-free UAVtrajectory generation [24], and bipedal locomotion [33], wheregood empirically performances have been observed. It was notuntil very recently that the good performances of ADMM innonlinear settings have been theoretically explained by [34,35, 11]. Unfortunately, even these latest analysis cannot covermany robotic applications such as [24]. Our new ADMMalgorithm correctly divides the responsibility between the slackand original variables, where the slack variables handle non-stiff function terms and the original variables handle stiff ones.By using line-search to ensure strict function decrease, we

can prove convergence (speed) of our algorithm to a robustsolution.

III. TRAJECTORY OPTIMIZATION PARADIGM

......

z1 z2

z4 z3

x0x1

xN−1

xN

xN+1

n ● +d = 0

Fig. 1: Illustrative problemwith the red dashed line beingthe separating plane.

We motivate our analysis us-ing a 2D collision-free trajec-tory optimization problem as il-lustrated in Figure 1, while ouralgorithm is applied to both 2Dand 3D workspaces alike. Con-sider a point robot traveling ina 2D workspace along a piece-wise linear trajectory discretizedusing N +1 linear segments andN + 2 vertices. We further as-sume the start and goal positionsare fixed, leaving the intermedi-ary N points as decision vari-ables. This trajectory can be pa-rameterized by a vector x ∈ R2N

where xi ∈ R2 is the ith vertex. For simplicity in this example,our goal is for the trajectory to be as smooth as possible.Further, the robot must be collision-free and cannot intersectthe box-shaped obstacle in the middle and we assume thatthe four vertices of the box are Z = {z1, z2, z3, z4}. Thesecollision constraints can be expressed as:

d(hull(Xi),hull(Z)) ≥ 0 ∀i = 1,⋯,N − 1,

where Xi = {xi, xi+1} is the ith line segment (Xi ∈ R4

in this illustrative toy example), hull(●) denotes the convexhull and d is Euclidean the distance between two convexobjects. A collision-free trajectory optimization problem canbe formulated as:

argminx

∑i

XTi LXi (1)

s.t. d(hull(Xi),hull(Z)) ≥ 0 ∀i = 1,⋯,N − 1,

where L is the Laplacian stencil measuring smoothness andXTi LXi measures the squared length of ith line segment.

However, Equation 1 only considers geometric or kinematicconstraints and a robot might not be able to traverse theoptimized trajectory due to the violation physical constraints.In many problems, including autonomous driving [36] andUAV path planning [37], a simplified physical model canbe incorporated that only considers velocity and accelerationlimits. We could approximate the velocity and accelerationusing finite-difference as:

Vi ≜ xi+1 − xi Ai ≜ xi+2 − 2xi+1 + xi,and formulate the time-optimal, collision-free trajectory opti-mization problem as:

argminx,∆t

∑i

XTi LXi/∆t2 +w∆t (2)

s.t. d(hull(Xi),hull(Z)) ≥ 0 ∀i = 1,⋯,N − 1

∥Vi∥ ≤ vmax∆t ∀i = 1,⋯,N − 1

∥Ai∥ ≤ amax∆t2 ∀i = 1,⋯,N − 2,

3

where we use ∑iXTi LXi/∆t2 to measure trajectory smooth-

ness with time, e.g. Dirichlet energy, and we use a coefficientw to balance between optimality in terms of trajectory lengthand arrival time. Here, v, amax are the upper bounds of veloc-ities and accelerations. Although the above example is onlyconsidering a single robot and piecewise linear trajectories,extensions to several practical problem settings are straight-forward, as discussed below.

θi

θi

θj

θj

xi

xjnx + d = 0

Fig. 2: The configuration of two robot arms includes joint anglesθi, θj . The set of Cartesian points on the robot, xi, xj (green), aremapped from θi, θj using forward kinematic functions. To avoidcollisions between the two robot arms, a convex hull is computedfor each robot link (blue), and a separating plane (red) is used toensure the pair of convex hulls stay on different sides.

A. Time-Optimal Multi-UAV Trajectory Optimization

We first extend our formulation to handle multiple UAVtrajectories represented using composite Bezier curves withN pieces of order M . In this case, the decision variable xis a set of N(M − 2) + 3 control points or x ∈ R3(N(M−2)+3)

(note that the two neighboring curves share 3 control points toensure second order continuity and we refer readers to [9] formore details). For the ith piece of Bezier curve, the velocityand acceleration are defined as:

Xi ≜⎛⎜⎝

xi(M−2)−M+3

⋮

xi(M−2)+3

⎞⎟⎠

Vi(s) ≜ I(s)Xi Ai(s) ≜ I(s)Xi,

where Xi are the M + 1 control points of the ith Beziercurve piece, and I(s), I(s) and I(s) are the Bezier curve’sinterpolation stencil for position, velocity and acceleration,respectively, with s ∈ [0,1] being the natural parameter. It canbe shown that Vi(s),Ai(s) are also Bezier curves of ordersM − 1 and M − 2, respectively. The velocity and accelerationlimits must hold for every s ∈ [0,1], for which a finite-dimensional, conservative approximation is to require all thecontrol points of I(s) and I(s) are bounded by vmax andamax. With a slight abuse of notation, we reuse I, I, I withoutparameter s to denote the matrices extracting the control pointsof I(s)Xi, I(s)Xi, I(s)Xi, respectively. In other words, wedefine the vectors Vi ≜ IXi and Ai ≜ IXi as the controlpoints of the ith Bezier curve, then the form of velocity andacceleration limits are identical to Equation 2.

For multiple UAVs, however, we need to consider theadditional collision constraints between different trajectories.To further unify the notations, we concatenate the controlpoints of different UAVs into a single vector x, i.e., two

Bezier curve pieces might correspond to different UAVs, andwe introduce collision constraints between different UAVs:

d(hull(Xi),hull(Xj)) ≥ 0 ∀i, j ∈ different UAV.

Our final formulation of time-optimal multi-UAV trajectoryoptimization takes the following form:

argminx,∆t

∑i

O(Xi,∆t) (3)

s.t. d(hull(Xi), hull(Z)) ≥ 0 ∀i = 1,⋯,N − 1

d(hull(Xi), hull(Xj)) ≥ 0 ∀i, j ∈ different UAV∥Vi∥ ≤ vmax∆t ∀i = 1,⋯,N − 1

∥Ai∥ ≤ amax∆t2

∀i = 1,⋯,N − 2,

where we generalize the objective function with smoothnessand time optimality to take an arbitrary, possibly non-convexform, O(Xi,∆t), which is a function of a single piece of sub-trajectory Xi. Almost all the objective functions in trajectoryoptimization applications can be written in this form. Forexample, smoothness can be written as the sum of totalcurvature, snap, or jerk of each piece, and the end-point costis only related to the last piece.

B. Goal-Reaching of Articulated Robot Arms

The position of UAV at any instance s on the ith Beziercurve is I(s)Xi, which is a linear function of decision variableXi and thus x. However, more general problem settingsrequire non-linear relationships, of which a typical case isarticulated robot arms as illustrated in Figure 2. Consider theproblem of multiple interacting robot arms in a shared 3Dworkspace. Each arm’s Cartesian-space configuration at theith time instance is represented by a triangle mesh with aset of vertices concatenated into the vector xi. However, weneed to maintain the corresponding configuration θi, where ∣θi∣is the degrees of freedom of each arm (DOF). Our decisionvariable is θ ∈ RDOF×N , each xi is a derived variable ofθi via the forward kinematics function xi ≜ FK(θi), anda linear interpolated Cartesian-space trajectory is: Xi(θ) ={xi, xi+1} = {FK(θi), FK(θi+1)}. We further define thevelocity and acceleration in configuration space as:

Vi(θ) ≜ θi+1 − θi Ai(θ) ≜ θi+2 − 2θi+1 + θi.In summary, the multi-arm goal-reaching problem can beformulated as:

argminθ,∆t

∑i

O(Xi(θ),∆t) (4)

s.t. d(hull(Xi(θ)), hull(Z)) ≥ 0 ∀i = 1,⋯,N − 1

d(hull(Xi(θ)), hull(Xj(θ))) ≥ 0 ∀i, j ∈ different arm∥Vi(θ)∥ ≤ vmax∆t ∀i = 1,⋯,N − 1

∥Ai(θ)∥ ≤ amax∆t2

∀i = 1,⋯,N − 2.

Equation 4 takes a more general form than all the previousproblems, and we would propose our variant of ADMMalgorithm assuming this formulation. If our method is appliedto UAV trajectory optimization, we can plug in the degeneraterelationship FK(θi) = θi.

Remark 3.1: By using separating planes to formulate col-lision constraints, we assume each robot link is convex. Thistreatment allows us to use only one separating plane betweena pair of robot links. If concave features of robot links

4

must be modeled accurately, then robot link must be furtherdecomposed into convex parts, and a separating plane must beused between each pair of parts, leading to more separatingplanes and iterations.

IV. ADMM-TYPE TRAJECTORY OPTIMIZATION

ADMM is a variant of the Augmented Lagrangian Method(ALM) that does not update the penalty parameter. The mainadvantage of ADMM is that, by introducing slack variables,each substep consists of either a small problem involving thenon-stiff part of the objective function or a large probleminvolving the stiff part of the objective function or constraints.As a result, the ADMM solver allows larger timestep sizes tobe taken for the non-stiff part, leading to faster convergence.Although ADMM has only first-order convergence rate, it canquickly approximate a locally optimal solution with moderateaccuracy, which is sufficiently for trajectory optimization.

X0X1

X2

X3

X4

X0

X1

X2

X3

X4

Fig. 3: ADMM main-tain two sets of variablesXi and Xi. Xi satisfies allconstraints throughout opti-mization, while Xi can vio-late constraints during inter-mediary iterations. On con-vergence, however, both Xiand Xi satisfy all constraints.

ADMM handles inequalityconstraints by reformulatingthem as indicator functions, butwe are handling possibly non-convex constraints for whichprojection operators, whichis associated with indicatorfunctions, do not have closed-form solutions. Instead, wefollow our prior work [9] andrely on a log-barrier relaxationswith non-zero duality gap. Forexample, if we have a hardconstraint g(x) ≥ 0 where gis some differentiable function,then the feasible domain canbe identified with the finitesub-level set of the log-barrierfunction: −log(g(x)). We applythis technique to the velocityand acceleration limits. Asimilar technique can be used for collision constraints withthe help of a separating plane, which is illustrated in Figure 1as nT ● +d = 0 (n is the plane normal, d is the position,and ● is the arbitrary point on the plane). Since we onlyconsider distance between convex hulls, two convex hulls arenon-overlapping if and only if there is a separating planesuch that the two hulls are on different sides. We proposeto optimize the parameters of the separating plane (n, d) asadditional slack variables. As a result, the collision constraintsbecome convex when fixing n, d and optimizing the trajectoryalone. Applying this idea to all the constraints and wecan transform Equation 4 into the following unconstrainedoptimization:

argminθ,∆t,di,dij∥ni∥,∥nij∥=1

L(θ,∆t, ni, di, nij , dij) ≜∑i

O(Xi(θ),∆t)−

γ∑i

log(vmax∆t − ∥Vi∥) − γ∑i

log(amax∆t2− ∥Ai∥)−

γ∑i

⎡⎢⎢⎢⎢⎣

∑x∈Xi

log(nix(θ) + di) + ∑z∈Z

log(−niz − di)

⎤⎥⎥⎥⎥⎦

−

γ∑ij

⎡⎢⎢⎢⎢⎣

∑x∈Xi

log(nijx(θ) + dij) + ∑x∈Xj

log(−nijx(θ) − dij)

⎤⎥⎥⎥⎥⎦

, (5)

where γ is the weight of log-barrier function that can be tunedfor each problem to control the exactness of constraint satis-faction. We use the same subscript convention as Section III.Specifically, for ith trajectory piece Xi and the obstacle Z, weintroduce a separating plane nTi ● +di = 0. For the pair of ithand jth trajectories pieces that might collide, we introduce aseparating plane nTij ● +dij = 0.

Equation 5 is a strongly coupled problem with six setsof decision variables, where the constraints (or log-barrierfunctions) and the objective O are added up. However, thesetwo kinds of functions have very different properties. The log-barrier functions are “stiff” and do not have a Lipschitz con-stant, which could generate arbitrarily large blocking gradientsnear the constraint boundaries, but the objective O is well-conditioned, oftentimes having a finite Lipschitz constant. Ourmain idea is to handle these functions in separate subproblems.

A. Alternating Minimization (AM)

Before we describe our ADMM-type method, we reviewthe basic alternating minimization scheme. AM has been usedin trajectory optimization to handle time-optimality [38] andcollision constraints [39]. A similar method can be appliedto minimize Equation 5 that alternates between updating theseparating plane ni, di, nij , dij and the robot configurations θas outlined in AM 1. AM can be used along with ADMMwhile being easier to analyze. Prior work [39] did not providea convergence analysis and Wang et al. [38] setup the firstorder convergence for a specific, strictly convex objectivefunction where each minimization subproblem is a single-valued map. In Section VII, we establish the convergence ofAM 1 for twice-differentiable objective functions with planenormals ni, nij constrained to the unit circle/sphere. In thenext section, we will combine AM and ADMM, specificallywe update robot configurations θ using ADMM and updateseparating planes ni, di, nij , dij using AM.

B. ADMM with Stiffness Decoupling

The key idea behind ADMM is to treat stiff and non-stifffunctions separately by introducing slack variables. Specifi-cally, we introduce slack variables Xi for each i = 1,⋯,Nand transforms Equation 5 into the following equivalent form:

argminθ,∆t,∆ti,Xi

di,dij ,∥ni∥,∥nij∥=1

∑i

[O(Xi,∆ti)]− (6)

γ∑i

log(vmax∆t − ∥Vi∥) −∑i

γ log(amax∆t2− ∥Ai∥)−

γ∑i

⎡⎢⎢⎢⎢⎣

∑x∈Xi

log(nix(θ) + di) + ∑z∈Z

log(−niz − di)

⎤⎥⎥⎥⎥⎦

−

γ∑ij

⎡⎢⎢⎢⎢⎣

∑x∈Xi

log(nijx(θ) + dij) + ∑x∈Xj

log(−nijx(θ) − dij)

⎤⎥⎥⎥⎥⎦

s.t. Xi(θ) = Xi ∧∆t = ∆ti.

5

Algorithm 1: AM

Input: θ0,∆t0, (ni, di )0, (nij , dij )

0, assuming L in Equation 5

1: for k = 0,1,⋯ do2: θk+1,∆tk+1

≈ argminθ,∆t

L(θ,∆t, nki , dki , n

kij , d

kij) ▷ Update θ,∆t

3: for collision constraint between ith piece of trajectory and environment do ▷ Optimize separating plane4: (ni, di )

k+1≈ argmin∥ni∥=1,di

L(θk+1,∆tk+1, ni, di, nkij , d

kij)

5: for collision constraint between ith and jth piece of trajectory do6: (nij , dij )

k+1≈ argmin∥nij∥=1,dij

L(θk+1,∆tk+1, nk+1i , dk+1

i , nij , dij)

Algorithm 2: ADMM with Stiffness Decoupling

Input: θ0,∆t0,∆t0i , X0i , λ

0i , (ni, di )

0, (nij , dij )

0, assuming L in Equation 7

1: for k = 0,1,⋯ do2: θk+1,∆tk+1

≈ argminθ,∆t

L(θ,∆t,∆ti, Xki , λ

ki , n

ki , d

ki , n

kij , d

kij) ▷ Update θ,∆t, Xi, λi

3: for ith piece of trajectory do4: ∆tk+1

i , Xk+1i ≈ argmin

∆ti,Xi

L(θk+1,∆tk+1,∆ti, Xi, λki , n

ki , d

ki , n

kij , d

kij) +

%2∥Xi − X

ki ∥

2

5: λk+1i ← λki + %(Xi(θ

k+1) − Xk+1

i )

6: Λk+1i ← Λki + %(∆t

k+1−∆tk+1

i )

7: for collision-free constraint between ith piece of trajectory and environment do ▷ Optimize separating plane8: (ni, di )

k+1≈ argmin∥ni∥=1,di

L(θk+1,∆tk+1,∆tk+1i , Xk+1

i , λk+1i , ni, di, n

kij , d

kij)

9: for collision-free constraint between ith and jth piece of trajectory do10: (nij , dij )

k+1≈ argmin∥nij∥=1,dij

L(θk+1,∆tk+1,∆tk+1i , Xk+1

i , λk+1i , nk+1

i , dk+1i , nij , dij)

By convention, we use a bar to indicate slack variables, i.e.Vi(s) = A(s)Xi and Ai(s) = A(s)Xi.

Remark 4.1: We choose to have all slack variables reside inCartesian space. As a result, if non-linear forward kinematicsfunctions are used, ADMM must handle nonlinear constraintXi(θ) = Xi.ADMM proceeds by transforming the equality constraints inEquation 6 into augmented Lagrangian terms. We arrive at thefollowing augmented Lagrangian function:

L(θ,∆t,∆ti, Xi, λi, ni, di, nij , dij) ≜∑i

O(Xi,∆ti)−

γ∑i

log(vmax∆t − ∥Vi∥) − γ∑i

log(amax∆t2 − ∥Ai∥)−

γ∑i

⎡⎢⎢⎢⎢⎣

∑

x∈Xi

log(nix(θ) + di) + ∑z∈Z

log(−niz − di)

⎤⎥⎥⎥⎥⎦

−

γ∑ij

⎡⎢⎢⎢⎢⎣

∑

x∈Xi

log(nijx(θ) + dij) + ∑x∈Xj

log(−nijx(θ) − dij)

⎤⎥⎥⎥⎥⎦

+

∑

i

%

2∥Xi(θ) − Xi∥

2+ λTi (Xi(θ) − Xi)+

∑

i

%

2∥∆t −∆ti∥

2+ΛTi (∆t −∆ti), (7)

where % is the penalty parameter, λi is the augmented La-grangian multiplier for Xi. We can now present our ADMMalgorithm seeking stationary points of Equation 7. Each itera-tion of our ADMM 2 is a five-way update that alternates be-tween {θ,∆t},{∆ti, Xi},{λi,Λi}, (ni, di ) , (nij , dij ). Notethat our objective function only appears in the {∆ti, Xi}-subproblem, which does not involve any stiff, log-barrierfunctions. Therefore, ADMM 2 achieves stiffness decoupling.

Remark 4.2: For each optimization subproblem of ADMM

2, we assume that the decision variable is initialized fromlast iteration. In Section VII,VIII, we show that subproblemsin AM 1 and ADMM 2 only need to be solved approxi-mately. Specifically, we update θ,∆t, ni, di, nij , dij using asingle (Riemannian) line search step, and we update ∆t, Xusing a linearized function L. For brevity, we denote theseapproximate oracles using an ≈ symbol.

Remark 4.3: ADMM always maintains two representationsof the trajectory, Xi and Xi, where Xi is used to satisfy thecollision constraints and Xi (the slack variable) focuses onminimizing the objective function O at the risk of violatingthe collision constraints, as illustrated in Figure 3. It is knowthat using slack variables can loosen constraint satisfaction. Asa result, we choose to formulate collision constraints and otherhard constraints on Xi instead of Xi, so that all the constraintscan be satisfied using a line-search step on Xi variables.For collision constraints in particular, we inherit the line-steptechnique from [9] that is safe-guarded by CCD. The CCDprocedure ensures that there always exists a separating plane tosplit each pair of convex objects and the Lagrangian functionL always takes a finite value. On convergence, however, thetwo representations coincide and both collision-free and localoptimal conditions hold.

Remark 4.4: When updating the separating plane, thenormal vector must be constrained to have unit length. Theseconstraints can be reparameterized as an optimization onSO(3). Specifically, given the current solution denoted asncurrij with ∥ncurrij ∥ = 1, we reparameterize nij by pre-multiplying a rotation matrix R = exp(r) by ncurrij , where weuse the Rodriguez formula to parameterize a rotation matrix as

6

102 103 1040

50

100

150

200 ADMMP-IPM

Computational Time(ms)

#Tra

ject

ory

(a) (c) (f)

(b) (d)

(e)

(g)

Fig. 4: Examples of trajectories gen-erated by Safe-Corridor [40] (blue) andADMM 2 (red) for a single UAV incomplex environments: indoor flight (a-c), outdoor flight (a-f). (g): The distribu-tions of computational time of ADMM2 and P-IPM [9] over 600 trajectoriescomputed for a synthetic environment inwhich we compute 600 random initialtrajectories using RRT-connect.

0 1 2 3 40

1000

2000

×1050 100 200 300 400 500

0

1000

2000 P-IPMAMADMM

Convergence History

Computational Time(ms) Iteration Number

Obje

ct V

alue

(a) (b)

Fig. 5: Convergence history of various algorithms for the examplein Figure 4 (f), comparing AM, ADMM, and Newton-type method(P-IPM) in terms of wall-time (a) and #Iterations (b).

the exponential of an arbitrary 3-dimensional vector r. Insteadof using nij as decision variables, we let nij = exp(r)ncurrij

and use r as our decision variables. Whichever value r takes,we can ensure ∥nij∥ = 1 (we refer reads to [41] for moredetails).

V. EVALUATIONS

Our implementation uses C++11. Experiments are per-formed on a workstation with a 3.5 GHz Intel Core i9processor. For experiments, we choose a unified set of pa-rameters vmax = 2m/s, amax = 2m/s2 for UAVs (resp.vmax = 0.1m/s, amax = 0.1m/s2 for articulated bodies),w = 108, % = 0.1 unless otherwise stated. We use the sameweight, γ = 10, for all the log-barrier functions. Althoughfine-tuning γ separately for each log-barrier function can leadto better results, we find the same γ = 10 achieves reasonablygood results over all examples. We use a locally supportedlog-barrier function as done in our prior work [9], which isactive only when the distance between two objects is less than0.1m for UAVs (resp. 0.04m for articulated bodies). Further,we set our clearance distance to be 0.1m for UAVs (resp.10−3m for articulated bodies), which can be plugged into CCDused by our line-search algorithm. Our algorithm terminateswhen ∥∇θk+1L∥∞ < ε and we choose ε = 10−2 for UAVs(resp. ε = 10−1 for articulated bodies). By comparing withour prior work [9], we demonstrate the ability of ADMMin terms of resolving stiff-coupling issues and boosting theoverall perform. We further compare with prior works [8, 40]to highlight the robustness of our approach.

Fast Separating Plane Update: We found that iterativeseparating plane updates is a major computational bottleneck.Fortunately, mature collision detection algorithms such as theGilbert–Johnson–Keerthi (GJK) algorithm [42] can quicklyreturn the optimal separating direction. We emphasize thatour separating planes minimizing the soft log-barrier penalties

do not match the separating directions returned by GJK ingeneral. However, we found that the separating plane returnby GJK oftentimes leads to a reduction in the Lagrangianfunction, while being orders of magnitude faster to computedue to their highly optimized implementation. Therefore, wepropose to use the GJK algorithm for updating the separatingplanes. After each iteration of of either AM 1 or ADMM 2, wecheck whether the Lagrangian function L is decreasing. If Lincreases, we fallback to our standard log-barrier functions sothe overall algorithm conforms to our convergence guarantee.

Trajectory Length(m) / Flying Time(s)Example AM ADMM P-IPM [9] Safe-Corridor [40]

(a) 16.0/9.6 16.0/9.6 16.1/9.7 18.0/14.0(b) 17.9/10.4 17.9/10.4 18.1/10.6 19.0/11.3(c) 13.2/8.9 13.2/9.0 13.8/9.1 14.6/11.7(d) 39.1/21.8 39.2/21.9 39.2/21.8 47.2/25.1(e) 66.8/44.7 66.5/44.6 68.1/45.5 71.2/57.9(f) 67.3/62.8 67.2/62.8 70.1/65.3 74.6/74.1

Computational Cost(ms)Example AM ADMM P-IPM [9] Safe-Corridor [40]

(a) 388 45 1.0K 41(b) 133 34 2.8K 50(c) 313 123 7.8K 79(d) 8.7K 938 34.3K 51(e) 39.0K 6.4K 175.0K 864(f) 92.9K 18.9K 420.1K 5.6K

TABLE I: The quality of results in terms of trajectory length/flyingtime and computational cost of the three methods (ours, P-IPM [9],and Safe-Corridor [40]) for the six examples of Figure 4.

Single-UAV: We first show six examples of trajectoryoptimization for a single UAV in complex environments asillustrated in Figure 4. For each example, we compare ourmethod against two baselines [9, 40]. We initialize all threemethods using the same feasible trajectory that is manuallydesigned. Our AM 1 takes from 133 to 93K(ms) to conver-gence and our ADMM 2 takes 34 to 18.9K(ms), as comparedwith our prior work [9] that takes 1K to 420K(ms). Theconvergence history of all three algorithms are summarizedin Figure 5. Although the convergence speed is comparablein terms of number of iterations, our two methods (AM andADMM) clearly outperform in terms of computational time.By not restricting the trajectory to precomputed corridors asdone in [28, 43, 44, 40, 45], our method allows a largersolution space and returns a shorter trajectory. We summarizethe quality of trajectory as computed by three methods inTable I. Finally, we conduct large-scale experiments using asynthetic problem illustrated in Figure 4g, where we randomlycompute feasible initial trajectories using RRT-connect for 600

7

0.0 0.5 1.00

50

100

150

200

250

300

×103

ADMMEGO-swarm

Computational Time(ms)

#Tra

ject

ory

(a) (b)

ADMM

Penalty-Method [8]

(c)

Fig. 6: Multiple UAV in com-plex environments, both with twogroups of UAVs switching posi-tions without (a) and with (b) ob-stacles. (c): The distributions ofcomputational time of ADMM 2and Penalty-Method [8] over 600trajectories computed for a syn-thetic environment in which werandomly generate initial trajecto-ries for the 4 UAVs in differenthomotopy classes.

times and compare the computational speed of ADMM 2 and[9]. The resulting plot Figure 4g shows more than an order ofmagnitude’s speedup using stiffness decoupling. Comparingwith P-IPM [9], our trajectories have similar quality, e.g.the mean/variance of trajectory length 14.7/2 × 10−2 (ours)vs 15.0/4 × 10−2 ([9]) and flying time 12.7/2 × 10−5 (ours) vs12.8/4 × 10−5 ([9]).

Multi-UAV: We assume the trajectory of each UAV isrepresented by composite Bezier curves with degree M = 5.Ideally, log-barrier functions should be added between everyUAV-UAV and UAV-environment pair, incurring a quadraticcost. Instead, we use a bounding-volume hierarchy and onlyadd collision constraints when the distance between two con-vex hulls are less than the activation distance of log-barrierfunction (0.1m). Note that this abrupt change in numberof collision constraints will not hinder the convergence ofADMM because it can happen at most finitely many times.Our algorithm requires collision-free initial-guesses for alltrajectories, and we compute these initial trajectories usingRRT connect. Our ADMM algorithm minimizes the jerk ofeach trajectory with time optimality as our objective functionO(Xi,∆ti). Figure 6 shows two challenging problems. Wefurther compare our method with penalty method [8], whichuses soft penalty terms to push trajectories out of the obstacles.This work is complementary to our method, which allowsinitial guesses to penetrate obstacles but cannot ensure finalresult to be collision-free. Instead, our method must start froma collision-free initial guess and maintain the collision-freeguarantee throughout the optimization. In Table II, we comparethe quality of solutions and computational cost of these twomethods. We have also conducted a large-scale comparisonwith penalty method [8] as illustrated in Figure 6c, where werandomly generate 600 initial trajectories that cover differenthomotopy classes. As summarized in Figure 6c, our methodgenerates trajectories with faster computing time and a 100%success rate, while [8] can only achieve a success rate of53%. Besides, Our results have higher quality than [8], e.g.the mean/variance of trajectory length 77.8/17.8 (ours) vs80.1/21.3 ([8]) and flying time 11.3/0.4 (ours) vs 20.6/8.0 ([8]).

Trajectory Length(m) / Flying Time(s) / Computational Cost(ms)Example ADMM Penalty-Method [8]

(a) 152.68/7.84/515 169.34/14.48/844.49(b) 71.33/14.57/2527 89.82/20.73/584.89

TABLE II: The quality of results in terms of trajectory length/flyingtime and computational cost, comparing our method and Penalty-Method [8].

Articulated Robot Arm: We highlight the performance ofour method via an example involving two arms. We approx-imate each robot joint as a single convex object to reducethe number of separating planes. Our example is inspired byprior work [46], as illustrated in Figure 7, where we have twoKUKA LWR robot arms (each with 8 links) switch positionsof their end-effectors. From an initial trajectory computed withthe length 6.38m using RRT-connect, our stiffness-decoupledADMM method can easily minimize acceleration of end-effects obtaining a trajectory with the length 1.63m in 16s.The convergence history is shown in Figure 8.

(a) (b)

Fig. 7: Trajectory optimization for two KUKA LWR robot armsswitching end-effector positions. (a): initial trajectory via RRT-connect; (b): optimized trajectory.

0 1 2 3

100

200

×1040 100 200 300

100

200AMADMM

Convergence History

Computational Time(ms) Iteration Number

Obje

ct V

alue

(a) (b)

Fig. 8: Convergence history for the example in Figure 7, comparingAM and stiffness-decoupled ADMM in terms of wall-time (a) and#Iterations (b).

VI. CONCLUSION & LIMITATION

We propose a variant of ADMM-type solver for trajectoryoptimization. We observe that the limited efficacy of our priorwork [9] is mainly due to the stiff log-barrier functions corre-sponding to various hard constraints. Therefore, we propose todecompose stiff and non-stiff objective function terms usingslack variables, while using additional constraints to ensuretheir consistency. ADMM was originally applied to convexoptimizations and we establish its convergence guarantee un-der non-convex objectives and constraints that arise from UAVand articulated robot kinematics. Our experiments confirm

8

that ADMM successfully resolves stiff-coupling issues andachieves tens of times’ speedup over Newton-type algorithms.The major limitation of our method is the requirement of astrictly feasible initial trajectory and our convergence rate canalso be dependent on the initial guess. As a result, our methodcannot be used for receding-horizon settings. If the horizonsare truncated, then there can be unforeseen obstacles, withwhich collisions are ignored, resulting in infeasible trajectoriesin future horizons. Finally, the convergence of ADMM-typesolvers for general control of nonlinear dynamic systems [32]remains an open question.

REFERENCES[1] C. Goerzen, Z. Kong, and B. Mettler, “A survey of motion planning algorithms

from the perspective of autonomous uav guidance,” Journal of Intelligent andRobotic Systems, vol. 57, no. 1, pp. 65–100, 2010.

[2] T. Lee, M. Leok, and N. H. McClamroch, “Geometric tracking control of aquadrotor uav on se (3),” in 49th IEEE conference on decision and control(CDC), IEEE, 2010, pp. 5420–5425.

[3] F. Augugliaro, A. P. Schoellig, and R. D’Andrea, “Generation of collision-free trajectories for a quadrocopter fleet: A sequential convex programmingapproach,” in 2012 IEEE/RSJ international conference on Intelligent Robotsand Systems, IEEE, 2012, pp. 1917–1922.

[4] J. T. Betts and W. P. Huffman, “Path-constrained trajectory optimization usingsparse sequential quadratic programming,” Journal of Guidance, Control, andDynamics, vol. 16, no. 1, pp. 59–68, 1993.

[5] C. Park, J. Pan, and D. Manocha, “Itomp: Incremental trajectory optimiza-tion for real-time replanning in dynamic environments,” in Proceedings ofthe Twenty-Second International Conference on International Conference onAutomated Planning and Scheduling, ser. ICAPS’12, Atibaia, Sao Paulo, Brazil:AAAI Press, 2012, 207–215.

[6] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M.Dellin, J. A. Bagnell, and S. S. Srinivasa, “Chomp: Covariant hamiltonianoptimization for motion planning,” The International Journal of RoboticsResearch, vol. 32, no. 9-10, pp. 1164–1193, 2013.

[7] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “Stomp:Stochastic trajectory optimization for motion planning,” in 2011 IEEE Interna-tional Conference on Robotics and Automation, 2011, pp. 4569–4574.

[8] X. Zhou, X. Wen, J. Zhu, H. Zhou, C. Xu, and F. Gao, “Ego-swarm:A fully autonomous and decentralized quadrotor swarm system in clutteredenvironments,” arXiv:2011.04183, 2020.

[9] R. Ni, T. Schneider, D. Panozzo, Z. Pan, and X. Gao, “Robust & asymptot-ically locally optimal uav-trajectory generation based on spline subdivision,”arXiv:2010.09904, 2020.

[10] L. F. Shampine and C. W. Gear, “A user’s view of solving stiff ordinarydifferential equations,” SIAM review, vol. 21, no. 1, pp. 1–17, 1979.

[11] W. Gao, D. Goldfarb, and F. E. Curtis, “Admm for multiaffine constrainedoptimization,” Optimization Methods and Software, vol. 35, no. 2, pp. 257–303,2020.

[12] D. Panagou, “Motion planning and collision avoidance using navigation vectorfields,” in 2014 IEEE International Conference on Robotics and Automation(ICRA), IEEE, 2014, pp. 2513–2518.

[13] Y. Choi, M. Chen, Y. Choi, S. Briceno, and D. Mavris, “Multi-uav trajectoryoptimization utilizing a nurbs-based terrain model for an aerial imaging mis-sion,” Journal of Intelligent & Robotic Systems, vol. 97, no. 1, pp. 141–154,2020.

[14] M. Ragaglia, A. M. Zanchettin, and P. Rocco, “Trajectory generation algorithmfor safe human-robot collaboration based on multiple depth sensor measure-ments,” Mechatronics, vol. 55, pp. 267–281, 2018.

[15] M. M. G. Ardakani, J. H. Cho, R. Johansson, and A. Robertsson, “Trajectorygeneration for assembly tasks via bilateral teleoperation,” IFAC ProceedingsVolumes, vol. 47, no. 3, pp. 10 230–10 235, 2014.

[16] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and controlfor quadrotors,” in 2011 IEEE International Conference on Robotics andAutomation, 2011, pp. 2520–2525.

[17] S. Shimoda, Y. Kuroda, and K. Iagnemma, “Potential field navigation of highspeed unmanned ground vehicles on uneven terrain,” in Proceedings of the2005 IEEE International Conference on Robotics and Automation, IEEE, 2005,pp. 2828–2833.

[18] Z. Qu, J. Wang, and C. E. Plaisted, “A new analytical solution to mobile robottrajectory generation in the presence of moving obstacles,” IEEE Transactionson Robotics, vol. 20, no. 6, pp. 978–993, 2004.

[19] M. Shomin and R. Hollis, “Differentially flat trajectory generation for adynamically stable mobile robot,” in 2013 IEEE International Conference onRobotics and Automation, IEEE, 2013, pp. 4467–4472.

[20] K. Belghith, F. Kabanza, L. Hartman, and R. Nkambou, “Anytime dynamicpath-planning with flexible probabilistic roadmaps,” in Proceedings 2006 IEEEInternational Conference on Robotics and Automation, 2006. ICRA 2006.,IEEE, 2006, pp. 2372–2377.

[21] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, “Anytime motionplanning using the rrt,” in 2011 IEEE International Conference on Robotics andAutomation, IEEE, 2011, pp. 1478–1483.

[22] O. Von Stryk and R. Bulirsch, “Direct and indirect methods for trajectoryoptimization,” Annals of operations research, vol. 37, no. 1, pp. 357–373, 1992.

[23] E. Todorov, T. Erez, and Y. Tassa, “Mujoco: A physics engine for model-basedcontrol,” in 2012 IEEE/RSJ International Conference on Intelligent Robots andSystems, IEEE, 2012, pp. 5026–5033.

[24] Z. Cheng, J. Ma, X. Zhang, and T. H. Lee, “Semi-proximal admm for modelpredictive control problem with application to a uav system,” in 2020 20thInternational Conference on Control, Automation and Systems (ICCAS), 2020,pp. 82–87.

[25] X. Zhou, Z. Wang, H. Ye, C. Xu, and F. Gao, “Ego-planner: An esdf-freegradient-based local planner for quadrotors,” IEEE Robotics and AutomationLetters, vol. 6, no. 2, pp. 478–485, 2020.

[26] F. Gao, Y. Lin, and S. Shen, “Gradient-based online safe trajectory generationfor quadrotor flight in complex environments,” in 2017 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), 2017, pp. 3681–3688.

[27] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotortrajectory generation for fast autonomous flight,” IEEE Robotics and AutomationLetters, vol. 4, no. 4, pp. 3529–3536, 2019.

[28] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, andV. Kumar, “Planning dynamically feasible trajectories for quadrotors using safeflight corridors in 3-d complex environments,” IEEE Robotics and AutomationLetters, vol. 2, no. 3, pp. 1688–1695, 2017.

[29] J. Tordesillas and J. P. How, “FASTER: Fast and safe trajectory planner fornavigation in unknown environments,” IEEE Transactions on Robotics, 2021.

[30] G. Vasudevan, L. T. Watson, and F. Lutze, “A homotopy approach for solvingconstrained optimization problems,” in 1989 American Control Conference,IEEE, 1989, pp. 780–785.

[31] S. Boyd, N. Parikh, and E. Chu, Distributed optimization and statistical learningvia the alternating direction method of multipliers. Now Publishers Inc, 2011.

[32] Z. Pan and D. Manocha, “Efficient solver for spacetime control of smoke,”ACM Trans. Graph., vol. 36, no. 5, Jul. 2017.

[33] Z. Zhou and Y. Zhao, “Accelerated admm based trajectory optimization forlegged locomotion with coupled rigid body dynamics,” in 2020 AmericanControl Conference (ACC), IEEE, 2020, pp. 5082–5089.

[34] B. Jiang, T. Lin, S. Ma, and S. Zhang, “Structured nonconvex and nonsmoothoptimization: Algorithms and iteration complexity analysis,” ComputationalOptimization and Applications, vol. 72, no. 1, pp. 115–157, 2019.

[35] Y. Wang, W. Yin, and J. Zeng, “Global convergence of admm in nonconvexnonsmooth optimization,” Journal of Scientific Computing, vol. 78, no. 1,pp. 29–63, 2019.

[36] C. Katrakazas, M. Quddus, W.-H. Chen, and L. Deka, “Real-time motionplanning methods for autonomous on-road driving: State-of-the-art and futureresearch directions,” Transportation Research Part C: Emerging Technologies,vol. 60, pp. 416–442, 2015.

[37] P. Cheng, J. Keller, and V. Kumar, “Time-optimal uav trajectory planning for3d urban structure coverage,” in 2008 IEEE/RSJ International Conference onIntelligent Robots and Systems, IEEE, 2008, pp. 2750–2757.

[38] Z. Wang, X. Zhou, C. Xu, J. Chu, and F. Gao, “Alternating minimization basedtrajectory generation for quadrotor aggressive flight,” IEEE RA-L, vol. 5, no. 3,pp. 4836–4843, 2020.

[39] W. Honig, J. A. Preiss, T. K. S. Kumar, G. S. Sukhatme, and N. Ayanian,“Trajectory planning for quadrotor swarms,” IEEE Transactions on Robotics,vol. 34, no. 4, pp. 856–869, 2018.

[40] Z. Han, Z. Wang, N. Pan, Y. Lin, C. Xu, and F. Gao, “Fast-racing: An open-source strong baseline for se (3) planning in autonomous drone racing,” IEEERobotics and Automation Letters, 2021.

[41] C. J. Taylor and D. J. Kriegman, “Minimization on the lie group so (3) andrelated manifolds,” 1994.

[42] M. Montanari, N. Petrinic, and E. Barbieri, “Improving the gjk algorithmfor faster and more reliable distance queries between convex objects,” ACMTransactions on Graphics (TOG), vol. 36, no. 3, pp. 1–17, 2017.

[43] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation forquadrotors using fast marching method and bernstein basis polynomial,” in2018 IEEE International Conference on Robotics and Automation (ICRA), 2018,pp. 344–351.

[44] J. Tordesillas, B. T. Lopez, and J. P. How, “FASTER: Fast and safe trajectoryplanner for flights in unknown environments,” in 2019 IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), IEEE, 2019.

[45] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Geometrically constrained trajectoryoptimization for multicopters,” arXiv:2103.00190, 2021.

[46] H. Ha, J. Xu, and S. Song, “Learning a decentralized multi-arm motion planner,”in Conference on Robotic Learning (CoRL), 2020.

[47] Y. Xu and W. Yin, “A globally convergent algorithm for nonconvex optimizationbased on block coordinate update,” Journal of Scientific Computing, vol. 72,no. 2, pp. 700–734, 2017.

[48] Q. Deng and C. Lan, “Efficiency of coordinate descent methods for structurednonconvex optimization,” arXiv:1909.00918, 2019.

[49] W. Ring and B. Wirth, “Optimization methods on riemannian manifolds andtheir application to shape space,” SIAM Journal on Optimization, vol. 22, no. 2,pp. 596–627, 2012.

[50] S. Lojasiewicz, “A topological property of real analytic subsets,” Coll. du CNRS,Les equations aux derivees partielles, pp. 87–89, 1963.

9

VII. CONVERGENCE ANALYSIS:ALTERNATING MINIMIZATION (AM)

Throughout our analysis, we assume the objective functionO is twice-differentiable. We rewrite the Lagrangian Equa-tion 5 as a function in the three (sets of) variables, Θ, Pi, Pij ,where we define the shorthand notation Θ ≜ (θ,∆t), Pi ≜(ni, di ), and Pij ≜ (nij , dij ). We also denote P ≜ (n, d)as an arbitrary plane, which can be some Pi or Pij . Duringthe kth iteration, these three variables will be updated inorder and Algorithm 1 will generate an infinite sequence{(Θk, P ki , P

kij )}. We further assume that each minimization

subproblem of Algorithm 1, e.g., the Θk-subproblem, is warm-started from its previous value Θk−1. The convergence ofAM in non-convex settings has been shown in prior work[47], but they assume the log-barrier functions are proximable,which does not hold in our case due to the non-linear for-ward kinematics x(θ). Other work such as [48] assumes theavailability of a global Lipschitz constant, which does nothold for a log-barrier function. We overcome this difficultyby using a line-search to ensure the satisfaction of Armijo’scondition for solving the three subproblems. In other words,Line 2 of Algorithm 1 is implemented in Algorithm 3. Toimplement Line 4 and Line 6 of Algorithm 1, we handle theadditional unit-norm constraint on ni and nij via Riemannianoptimization [49]. Taking an arbitrary P for example, we startfrom a feasible initial guess ∥n∥ = 1 and optimize on thetangent space v ∈ TnS2. The optimized v provides an updatedP = (expn v, d). Essentially, we use standard line-search asoutlined in Algorithm 4 to optimize the function L(expn v, d)denoted as L ○ expn. We further use a shorthand notationL○ exp(θ,∆t, vi, di, vij , dij) to denote the reparameterizationof L with each ni (w.r.t. nij) replaced by expni(vi) (w.r.t.expnij(vij)). A direct verification shows that a first-ordercritical point of L is a point with ∥∇L ○ exp ∥ = 0.

Algorithm 3: Line-Search Gradient Descent

Input: Initial guess Θ, c ∈ (0,1), γ ∈ (0,1)1: α← 12: while L(Θ − α∇ΘL(Θ)) > L(Θ) − cα∥∇ΘL(Θ)∥

2 do3: α← γα

4: Return Θ − α∇ΘL(Θ)

Algorithm 4: Riemannian Line-Search Gradient Descent

Input: Initial guess v = 0, c ∈ (0,1), γ ∈ (0,1)1: α← 1

2: whileL○expn((v, d)−α∇v,dL○expn)

>L(v, d)−cα∥∇v,dL○expn ∥2 do

3: α← γα

4: (v, d)← (v, d) − α∇v,dL ○ expn (v, d)

5: Return Pi ← (expn v, d)

To establish convergence to first-order critical point, webound the magnitude of α making use of the Lipschitzconstant of a function. In the following analysis, we omit someparameters of L for brevity, where the omitted parameters issame as Algorithm 1.

Lemma 7.1: Consider a function L(Θ). Assume that∇ΘL(Θ) is L-Lipschitz continuous within some convex set

B. If Algorithm 3 updates Θk to Θk+1 with Θk,Θk+1 ∈ B,then we have: α ≥ 2γ(1−c)/L and L(Θk+1)+c∥Θk+1−Θk∥2 ≤L(Θk).

Proof: When α ≤ 2(1 − c)/L, we have:

L(Θk+1) ≤L(Θk

) + ⟨∇ΘL(Θk),Θk+1

−Θk⟩ +

L

2∥Θk+1

−Θk∥2

=L(Θk) + (

Lα2

2− α)∥∇ΘL(Θ

k)∥

2

≤L(Θk) − cα∥∇ΘL(Θ

k)∥

2.

By the logic of Algorithm 3, we have the first inequality:

α ≥ γ⌈logγ(2(1−c)/L)⌉ ≥ 2γ(1 − c)/L. (8)

The second inequality follows from the Armijo’s condition:

L(Θk+1) + c∥Θk+1

−Θk∥2= L(Θk+1

) + cα2∥∇ΘL(Θ

k)∥

2

≤L(Θk) − cα∥∇ΘL(Θ

k)∥

2+ cα2

∥∇ΘL(Θk)∥

2≤ L(Θk

), (9)

where we have used α ≤ 1.Given the above results, it is straightforward to establish theconvergence guarantee for a convergent sub-sequence.

Lemma 7.2: If L is lower-bounded, then every accumulationpoint of the sequence {(Θk, P ki , P

kij )} generated by Algo-

rithm 1 is a critical point. Suppose the sequence is convergent,the local convergence speed of ∥∇L ○ exp ∥ is O(1/

√K).

Proof: The function {L(Θk, P ki , Pkij)} is monotonically

decreasing and bounded from below, so it converges toL(Θ, Pi, Pij) by continuity, where (Θ, Pi, Pij ) is an accu-mulation point with the convergent sub-sequence identified bythe index subset: K ⊂ {k∣k = 1,2,⋯}.

Applying Lemma 7.1: Invoke Equation 9 for L in Algo-rithm 3 and we will have:

L(Θk+1) + c∥Θk+1 −Θk∥2 ≤ L(Θk).Invoke Equation 9 for L ○ expn in Algorithm 4 and we willhave:

L(P k+1) + c∥vk∥2 + c∥dk+1 − dk∥2 ≤ L(P k).Summing up the above equations for all the iterations ofAlgorithm 1 and we have:

∞

∑k=0

∥Θk+1−Θk

∥2+

∞

∑k=0

∑i

∥ (vki , dk+1i ) − (0, dki ) ∥

2+

∞

∑k=0

∑ij

∥ (vkij , dk+1ij ) − (0, dkij ) ∥

2

≤L(Θ0, P 0

i , P0ij) −L(Θ, Pi, Pij)

c<∞.

We conclude that limk→∞ ∥Θk+1 −Θk∥ = 0, limk→∞ ∥vk∥ = 0,and limk→∞ ∥dk+1−dk∥ = 0 for any P . Here we use shorthandnotation vki (w.r.t. vkij) to denote v found by Algorithm 4 forPi (w.r.t. Pij).

Optimality & Convergence Speed of Θ: domL is dic-tated by the log-barrier function, which is an open set.L(Θ, Pi, Pij) < ∞ so that (Θ, Pi, Pij ) is within the openset. As a result, we can find some small neighborhoodBρ (Θ, Pi, Pij ) ⊂ domL (denoted as Bρ for short) suchthat (Θk, P ki , P

kij ) ∈ Bρ/2 and ∥Θk+1 − Θk∥ ≤ ρ/2 for any

sufficiently large K ∋ k > k0, where ρ is some small positivenumber. W.l.o.g., we can discard the sequence before k0 and

10

assume k0 = 0. We can choose a sufficiently small ρ such that∇ΘL is L-Lipschitz continuous within Bρ ⊂ domL. InvokeEquation 8 for L in Algorithm 3 and we will have:

0 = limK∋k→∞

∥Θk+1−Θk

∥ = limK∋k→∞

αk∥∇ΘL(Θk)∥

≥2γ(1 − c)/L limK∋k→∞

∥∇ΘL(Θk)∥ = 2γ(1 − c)/L∥∇ΘL(Θ)∥,

where αk is the α used in the line-search during the k−thiteration. If the entire sequence is convergent, then we havethe following result for the local convergence speed:

K(2γ(1 − c)/L)2 mink=0,⋯,K

∥∇ΘL(Θk)∥2

≤(2γ(1 − c)/L)2K

∑k=0

∥∇ΘL(Θk)∥2 ≤K

∑k=0

∥Θk+1 −Θk∥2

≤∞∑k=0

∥Θk+1 −Θk∥2 <∞.

Optimality & Convergence Speed of P : We canalso choose sufficiently small ρ such that for any fixed(Θk, P ki , P

kij ) ∈ Bρ ⊂ domL, the function ∇v,dL ○ expn is

L-Lipschitz continuous within Bρ. We invoke Equation 8 forL ○ exp in Algorithm 3 and following the same logic as thatof Θ.Note that we cannot have overall convergence speed dueto a lack of a global Lipschitz constant. To get rid of thewhole sequence convergence assumption and establish a localconvergence speed and the convergence of entire sequence{(Θk, P ki , P

kij )}, we need the KŁ property [50] of L, which

is a rather mild assumption that holds for all real analyticfunctions. Specifically, we assume the KŁ property [50] holdsfor the composite function L ○ exp defined below:

Definition 7.3: The function L ○ exp satisfies the KŁproperty at some point (Θ, Pi, Pij ), if there exists a neigh-borhood Bρ and a convex function φ such that, for any(Θ, Pi, Pij ) ∈ Bρ, we have:

φ′(L (Θ, Pi, Pij ) −L (Θ, Pi, Pij ))∥∇L (Θ, expni(0), di, expnij(0), dij ) ∥ ≥ 1.

We establish the convergence of sequence {(Θk, P ki , Pkij )}

and the local convergence speed assuming the KŁ property.Lemma 7.4: If L○exp is lower-bounded and pertains the KŁ

property (Definition 7.3), and the sequence {(Θk, P ki , Pkij )}

generated by Algorithm 1 has a finite accumulation point, thenthe sequence is convergent.

Proof: We denote (Θ, Pi, Pij ) as the accumulation point.By invoking Lemma 7.2, we know that ∇L○exp = 0. W.l.o.g.,we assume L (Θ, Pi, Pij ) = 0. By continuity, we can choosea small neighborhood B2ρ ⊂ domL where ∥∇L○exp ∥ < ε andthe the KŁ property holds. We further denote ∇L ○ exp asL-Lipschitz continuous within B2ρ. Finally, in a sufficientlysmall local neighborhood, we can setup strong equivalencebetween metrics in tangent space and ambient space. In otherwords, we can find two positive constants L and L such that:

L∥ expn v − expn v′∥ ≤ ∥v − v′∥ ≤ L∥ expn v − expn v

′∥,

for any point in B2ρ. If we start from (Θk, P ki , Pkij ) ∈ Bρ,

we choose sufficiently large k0 such that the subsequentpoint (Θk+1, P k+1

i , P k+1ij ) ∈ B2ρ for every k ≥ k0 due to

Lemma 7.2. W.l.o.g., we can assume k0 = 0. Let’s now definetwo constants:

L1 ≜max( L

2γ(1 − c)+∑

i

L +∑ij

L,LL

2γ(1 − c))

L2 ≜min(c, cL2).

Since there is a sub-sequence converging to (Θ, Pi, Pij ), wecan choose large enough k ≥ k0 such that:

∥ (Θk, Pki , Pkij ) − ( Θ, Pi, Pij ) ∥ ≤ ρ −L1/L2φ(L (Θk, Pki , P

kij )).

W.l.o.g., we can assume k0 = 0 and:

(Θ0, P 0i , P

0ij ) ∈ Bρ−L1/L2φ(L(Θ0, P 0

i , P0ij ))

. (10)

Next, we show that every (Θk, P ki , Pkij ) ∈ Bρ by induction. If

we already have (Θ0, P 0i , P

0ij ) ,⋯, (Θk, P ki , P

kij ) ∈ Bρ, then

the following holds for (Θk+1, P k+1i , P k+1

ij ) ∈ Bρ:

L (Θk, Pki , Pkij ) −L (Θk+1, Pk+1

i , Pk+1ij )

φ(L (Θk, Pki , Pkij )) − φ(L (Θk+1, Pk+1

i , Pk+1ij ))

≤1/φ′(L (Θk, Pki , Pkij ))

≤∥∇L(Θk, expnki(0), dki , expnk

ij(0), dkij)∥

=∥∇ΘL(Θk)∥+

∑

i

∥∇v,dL ○ expnki(0, dki )∥ +∑

ij

∥∇v,dL ○ expnkij(0, dkij))∥

≤∥Θk+1

−Θk∥

αkΘ+

∑

i

⎡⎢⎢⎢⎢⎣

∥ (vki , dk+1i − dki ) ∥

αkni,di

+L∥Θk+1−Θk∥

⎤⎥⎥⎥⎥⎦

+

∑

ij

⎡⎢⎢⎢⎢⎣

∥ (vkij , dk+1ij − dkij ) ∥

αknij ,dij

+L∥Θk+1−Θk∥

⎤⎥⎥⎥⎥⎦

≤

⎡⎢⎢⎢⎢⎣

L

2γ(1 − c)+∑

i

L +∑ij

L

⎤⎥⎥⎥⎥⎦

∥Θk+1−Θk∥+

L

2γ(1 − c)

⎡⎢⎢⎢⎢⎣

∑

i

∥ (vki , dk+1i − dki ) ∥ +∑

ij

∥ (vkij , dk+1ij − dkij ) ∥

⎤⎥⎥⎥⎥⎦

≤

⎡⎢⎢⎢⎢⎣

L

2γ(1 − c)+∑

i

L +∑ij

L

⎤⎥⎥⎥⎥⎦

∥Θk+1−Θk∥+

LL

2γ(1 − c)

⎡⎢⎢⎢⎢⎣

∑

i

∥Pk+1i − Pki ∥ +∑

ij

∥Pk+1ij − Pkij∥

⎤⎥⎥⎥⎥⎦

≤L1∥ (Θk+1, Pk+1i , Pk+1

ij ) − (Θk, Pki , Pkij ) ∥, (11)

where we use shorthand notation αkΘ (w.r.t. αkni,di , αknij ,dij

)to denote α found by Algorithm 3 for Θ (w.r.t. Pi, Pij).The first inequality above is due to the convexity of φ. Thesecond inequality is due to the KŁ property of L ○ exp. Thethird inequality is due to the logic of line-search algorithmsand L-Lipschitz continuity of gradient within B2ρ. The forthinequality is due to Lemma 7.2 and the fifth inequality isdue to the strong metric equivalently, both within B2ρ. Wefurther have the following estimate of function decrease dueto Lemma 7.2:L (Θk, P ki , P

kij ) −L (Θk+1, P k+1

i , P k+1ij )

≥c∥Θk+1−Θk

∥2+ c∑

i

∥vki ∥2+ c∑

i

∥dk+1i − dki ∥

2+

c∑ij

∥vkij∥2+ c∑

ij

∥dk+1ij − dkij∥

2

≥c∥Θk+1−Θk

∥2+ cL2

∑i

∥nk+1i − nki ∥

2+ c∑

i

∥dk+1i − dki ∥

2+

11

cL2∑ij

∥nk+1ij − nkij∥

2+ c∑

ij

∥dk+1ij − dkij∥

2

≥L2∥ (Θk+1, P k+1i , P k+1

ij ) − (Θk, P ki , Pkij ) ∥

2. (12)

Combining Equation 11 and Equation 12, we conclude that:

φ(L (Θk, P ki , Pkij )) − φ(L (Θk+1, P k+1

i , P k+1ij ))

≥L2/L1∥ (Θk+1, P k+1i , P k+1

ij ) − (Θk, P ki , Pkij ) ∥. (13)

The above property holds for all 0,⋯, k so we can sum theseequations up to derive:

∥ (Θk+1, P k+1i , P k+1

ij ) − (Θ, Pi, Pij ) ∥

=∥ (Θ, Pi, Pij ) − (Θ0, P 0i , P

0ij ) ∥+

∥ (Θk+1, P k+1i , P k+1

ij ) − (Θ0, P 0i , P

0ij ) ∥

≤∥ (Θ, Pi, Pij ) − (Θ0, P 0i , P

0ij ) ∥+

L1/L2 [φ(L (Θ0, P 0i , P

0ij )) − φ(L (Θk+1, P k+1

i , P k+1ij ))]

≤ρ −L1/L2φ(L (Θ0, P 0i , P

0ij ))+

L1/L2 [φ(L (Θ0, P 0i , P

0ij )) − φ(L (Θk+1, P k+1

i , P k+1ij ))] ≤ ρ,

where we have used Equation 10 in the second to lastinequality. By induction, we conclude that every element of(Θk, P ki , P

kij ) ∈ Bρ and we have from Equation 13:

∞∑k=0

∥ (Θk+1, P k+1i , P k+1

ij ) − (Θk, P ki , Pkij ) ∥ <∞,

which shows that the sequence {(Θk, P ki , Pkij )} is a Cauchy

sequence and is thus convergent. Since (Θ, Pi, Pij ) is a limitpoint of the sequence, we conclude that the entire sequenceconverge to (Θ, Pi, Pij ) with local convergence speed beingO(1/

√K) due to Lemma 7.2.

VIII. CONVERGENCE ANALYSIS:ADMM WITH STIFFNESS DECOUPLING

We prove the convergence of Algorithm 2 which is similarto [34] but we deviate from their prove in that 1) we considernonlinear constraints and 2) we consider a Lagrangian function(Equation 7) that is not Lipschitz continuous. To simplifynotation, we present our proof without variable ∆t and ∆t,i.e., we assume Θ = θ in the following proof. In fact,the identical proof can be extended to the case with time-optimality. Specifically, if we adopt the following change ofvariable, the case with time optimality is proved:

θ ← (θ

∆t) Xi(θ)← (

Xi(θ)∆t

) Xi ← (Xi∆ti

) λi ← (λiΛi

) .

We take the same assumptions on O as that of Section VII.The only different from Algorithm 1 lies in Line 4 and Line 5of Algorithm 2. First, it is not practical to solve the Xk+1

i -subproblem exactly and we adopt the linear proximal operatorinstead:

Xk+1i =argmin

Xi

β

2∥Xi − X

ki ∥+

⟨∇XiL(θk+1, Xi, λ

ki , n

ki , d

ki , n

kij , d

kij), Xi − X

ki ⟩

=Xki −

1

β[∇O(Xk

i ) + %(Xki −Xi(θ

k+1)) − λki ]

=Xki −

1

β[∇O(Xk

i ) − %(Xk+1i − Xk

i ) − λk+1i ] , (14)

with β determining the strength of regularization. The changeof L due to Line 5 can be bounded as follows:

Lemma 8.1: If ∇O is LO-Lipschitz continuous, then Line 5of Algorithm 2 will monotonically increase L by at most:

2(β − %)2

%∥Xk+1

i − Xki ∥2 + 2(β − % +LO)2

%∥Xk

i − Xk−1i ∥2.

Proof: Some minor rearrangement of Equation 14 wouldlead to:

(β − %)(Xk+1i − Xk

i ) = λk+1i −∇O(Xk

i ). (15)

We have the following identity due to LO-Lipschitz propertyof ∇O:

∥λk+1i − λki ∥ ≤ ∥∇O(Xk

i ) −∇O(Xk−1i )∥+

(β − %)∥Xk+1i − Xk

i ∥ + (β − %)∥Xki − X

k−1i ∥

≤(β − %)∥Xk+1i − Xk

i ∥ + (β − % +LO)∥Xki − X

k−1i ∥.

By the inequality (a+b)2 ≤ 2a2+2b2, we can derive the resultto be proved.Further, the change of L due to Line 4 can also be boundedas follows:

Lemma 8.2: If ∇O is LO-Lipschitz continuous, then Line 4of Algorithm 2 will monotonically increase L by at most:

[LO + %2

− β] ∥Xk+1i − Xk

i ∥2.

Proof: There are two terms related to Xi, namely O(Xi)and %/2∥Xi(θk+1) − Xi − λki /%∥2. Using Equation 14, thechange to the first term can be bounded as:

O(Xk+1i ) −O(Xk

i )

≤∇O(Xki )T(Xk+1

i − Xki ) +

LO2

∥Xk+1i − Xk

i ∥2

=λk+1i

T(Xk+1

i − Xki ) + [

LO2

− (β − %)] ∥Xk+1i − Xk

i ∥2. (16)

The change to the second term can be bounded as:%

2∥Xi(θ

k+1) − Xk+1

i −

λki%

∥2−%

2∥Xi(θ

k+1) − Xk

i −λki%

∥2

=%

2(Xk

i − Xk+1i )

T[

−2λk+1i

%+ Xk+1

i − Xki ]

= −%

2∥Xk+1

i − Xki ∥

2− λk+1

iT(Xk+1

i − Xki ). (17)

We can derive the result to be proved by summing upEquation 16 and Equation 17.We can then establish the Lyapunov candidate Lkκ ≜L(θk+1, Xi, λ

ki , n

ki , d

ki , n

kij , d

kij) + κ∥Xk+1

i − Xki ∥2 and prove

its monotonic property below:Lemma 8.3: If ∇O is LO-Lipschitz continuous, then the

sequence {Lkκ} is monotonically decreasing when:

% = β κ = β/4 β > 2√

2LO.

Proof: The other parts of Algorithm 2 are monotonicallydecreasing L except for Line 4 and Line 5. Combining theresults of Lemma 8.1 and Lemma 8.2, we have:Lk+1κ −L

kκ ≤ κ∑

i

∥Xk+1i − Xk

i ∥2− κ∑

i

∥Xki − X

k−1i ∥

2+

2(β − %)2

%∑

i

∥Xk+1i − Xk

i ∥2+

2(β − % +LO)2

%∑

i

∥Xki − X

k−1i ∥

2+

[LO + %

2− β]∑

i

∥Xk+1i − Xk

i ∥2

= [2(β − %)2

%+LO + %

2− β + κ]∑

i

∥Xk+1i − Xk

i ∥2+

[2(β − % +LO)

2

%− κ]∑

i

∥Xki − X

k−1i ∥

2.

12

It can be verified that both terms in the last equation arenegative using the parameter choices give above.Next, we show that the sequence {Lkκ} is convergent:

Lemma 8.4: We denote the following remainder as R:

R(θ, ni, di, nij , dij) ≜

γ∑i

⎡⎢⎢⎢⎣∑x∈Xi

log(nix(θ) + di) + ∑z∈Z

log(−niz − di)⎤⎥⎥⎥⎦−

γ∑ij

⎡⎢⎢⎢⎢⎣∑x∈Xi

log(nijx(θ) + dij) + ∑x∈Xj

log(−nijx(θ) − dij)⎤⎥⎥⎥⎥⎦.

If ∇O is LO-Lipschitz continuous, R ≥ R and O ≥ Oare lower-bounded and parameters are chosen according toLemma 8.3, then the Lyapunov candidate Lκ is lower-boundedand the sequence {Lkκ} is convergent.

Proof: We have the following result from Equation 15:

Lkκ ≥R +∑

i

O(Xki )+

∑i

%

2∥Xi(θ

k) − Xk

i ∥2+ λki

T(Xi(θ

k) − Xk

i )

=R +∑i

O(Xki ) +∇O(Xk

i )T(Xi(θ

k) − Xk

i )+

∑i

%

2∥Xi(θ

k) − Xk

i ∥2+ (β − %)(Xk+1

i − Xki )T(Xi(θ

k) − Xk

i )

≥R +∑i

O(Xi(θk)) −

LO2

∥Xi(θk) − Xk

i ∥2+

∑i

%

2∥Xi(θ

k) − Xk

i ∥2+ (β − %)(Xk+1

i − Xki )T(Xi(θ

k) − Xk

i )

≥R +∑i

O +% −LO

2∥Xi(θ

k) − Xk

i ∥2≥R +∑

i

O,

so the sequence {Lkκ} is monotonically decreasing, lower-bounded, and thus convergent.

Remark 8.5: If time optimality is considered, thenLemma 8.3 holds with the following choice of remainder:

R(θ,∆t, ni, di, nij , dij) ≜ w∆t−γ∑

i

log(vmax∆t − ∥Vi∥) − γ∑i

log(amax∆t2 − ∥Ai∥)−

γ∑i

⎡⎢⎢⎢⎣∑x∈Xi

log(nix(θ) + di) + ∑z∈Z

log(−niz − di)⎤⎥⎥⎥⎦−

γ∑ij

⎡⎢⎢⎢⎢⎣∑x∈Xi

log(nijx(θ) + dij) + ∑x∈Xj

log(−nijx(θ) − dij)⎤⎥⎥⎥⎥⎦.

Next, we establish the convergence guarantee for a convergentsub-sequence. The first-order critical point should satisfy thefollowing conditions:

∥ ∂L∂λi

∥ = ∥Xi(θ) − Xi∥ = 0 (18)

∥ ∂L∂Xi

∥ = ∥∇O(Xi) − λi∥ = 0 (19)

∥∇L ○ exp (Θ, expni(0), di, expnij(0), dij )∥ = 0, (20)

where we define L ○ exp with each ni (w.r.t. nij) replaced byexpni(vi) (w.r.t. expnij(vij)).

Lemma 8.6: If ∇O is LO-Lipschitz continuous, both R andO are lower-bounded and parameters are chosen according toLemma 8.3, then every accumulation point of the sequence{(Θk, Xk

i , Pki , P

kij )} generated by Algorithm 2 is a critical

point. Suppose the sequence is convergent, the local conver-gence speed of ∥∇L ○ exp ∥ is O(1/

√K).

Proof: The sequence {Lκ (Θk, Xki , λ

ki , P

ki , P

kij )} is con-

vergent due to Lemma 8.4. We assume the accumulation pointis (Θ, ¯Xi, Pi, Pij ) with convergent sub-sequence identified bythe index subset: K ⊂ {k∣k = 1,2,⋯}.

Applying Lemma 7.1 and Lemma 8.3: The reduction ofLκ over one iteration of Algorithm 2 can be bounded as:

Lk+1κ + c∥Θk+1 −Θk∥2+

c∑i

∥vki ∥2 + ∥dk+1i − dki ∥2 + c∑

ij

∥vkij∥2 + ∥dk+1ij − dkij∥2−

[2(β − %)2

%+ LO + %

2− β + κ]∑

i

∥Xk+1i − Xk

i ∥2−

[2(β − % +LO)2

%− κ]∑

i

∥Xki − Xk−1

i ∥2 ≤ Lkκ, (21)

from which we conclude that limk→∞ ∥Θk+1 − Θk∥ = 0,limk→∞ ∥vk∥ = 0, and limk→∞ ∥dk+1 − dk∥ = 0 for anyP , and limk→∞ ∥Xk+1

i − Xki ∥ = 0. Equation 20 is satisfied

at the accumulation point following the same reasoning asLemma 7.2. Taking limits on both sides of Equation 15 andwe have:

limk→∞

∥λk+1 −∇O(Xk+1i )∥

≤ limk→∞

∥λk+1 −∇O(Xki )∥ + ∥∇O(Xk+1

i ) −∇O(Xki )∥

≤LO limk→∞

∥Xk+1i − Xk

i ∥ = 0,

from which Equation 19 follows. We then take limits on bothsides of Line 5 and we have limk→∞ ∥Xi(θk)−Xk

i ∥ = 0, fromwhich Equation 18 follows.

Convergence Speed: Summing up Equation 21 over Kiterations and we have:

K mink=1,⋯,K

[∥Xk+1i − Xk

i ∥2+ ∥Xk

i − Xk−1i ∥

2]

≤L

1κ −Lκ

min(− [2(β−%)2

%+LO+%

2− β + κ] ,− [

2(β−%+LO)2

%− κ])

.

We plug these results into Equation 14 and Lemma 8.1 toderive:

∥Xi(θk+1

) − Xk+1i ∥

2=

1

%2∥λk+1i − λki ∥

2

≤2(β − %)2

%2∥Xk+1

i − Xki ∥

2+

2(β − % +LO)2

%2∥Xk

i − Xk−1i ∥

2

≤max(2(β − %)2,2(β − % +LO)

2)

%2[∥Xk+1

i − Xki ∥

2+ ∥Xk

i − Xk−1i ∥

2]

∥λki −∇O(Xki )∥

2≤ 2∥λk+1

i −∇O(Xki )∥

2+ 2∥λk+1

i − λki ∥2

≤4(β − %)2∥Xk+1

i − Xki ∥

2+ 2(β − % +LO)

2∥Xk

i − Xk−1i ∥

2

≤max(4(β − %)2,2(β − % +LO)2) [∥Xk+1

i − Xki ∥

2+ ∥Xk

i − Xk−1i ∥

2] ,

which establishes the O(1/√K) convergence speed of Equa-

tion 18 and Equation 19. The local convergence speed ofEquation 20 follows the same reasoning as Lemma 7.2.Finally, we establish whole sequence convergence and thuslocal convergence speed assuming KŁ property of L ○ exp.

Lemma 8.7: If ∇O is LO-Lipschitz continuous, R andO are lower-bounded and pertains the KŁ property (Defini-tion 7.3), and the sequence {(Θk, Xk

i , Pki , P

kij )} generated by

Algorithm 2 has a finite accumulation point, then the sequence

13

is convergent.

Proof: We denote (Θ, ¯Xi, λi, Pi, Pij ) as the accumula-tion point. By invoking Lemma 8.6, we know that ∇L○exp =0. W.l.o.g., we assume L (Θ, ¯Xi, λi, Pi, Pij ) = 0. By continu-ity, we can choose a small neighborhood B2ρ ⊂ domL where∥∇L ○ exp ∥ < ε and the the KŁ property holds. We furtherdenote ∇L ○ exp as L-Lipschitz continuous and every Xi isLXi -Lipschitz continuous within B2ρ. Finally, in a sufficientlysmall local neighborhood, we can setup strong equivalencebetween metrics in tangent space and ambient space for anypoint in B2ρ. If we start from (Θk, Xk

i , λki , P

ki , P

kij ) ∈ Bρ,

we choose sufficiently large k0 such that the subsequent point(Θk+1, Xk+1

i , λk+1i , P k+1

i , P k+1ij ) ∈ B2ρ for every k ≥ k0 due

to Lemma 7.2. W.l.o.g., we can assume k0 = 0. Since thereis a sub-sequence converging to (Θ, ¯Xi, λi, Pi, Pij ), we canchoose large enough k ≥ k0 (W.l.o.g., we can assume k0 = 0)so that:

(Θ0, X0i , λ

0i , P

0i , P

0ij ) ∈ Bρ−L1/L2φ(L(Θ

0, X0i , λ

0i , P

0i , P

0ij )

. (22)

Next, we show that every subsequent point (Θk, Xki , λ

ki , P

ki , P

kij ) ∈ Bρ

lies in by induction. If we already have this property for all previous points,i.e., (Θ0, X0

i , λ0i , P

0i , P

0ij ) ,⋯, (Θk, Xk

i , λki , P

ki , P

kij ) ∈ Bρ, then the

following holds for (Θk+1, Xk+1i , λk+1

i , Pk+1i , Pk+1

ij ) ∈ Bρ:

L (Θk, Xki , λki , P

ki , P

kij ) −L (Θk+1, Xk+1i , λk+1i , Pk+1i , Pk+1ij )

φ(L (Θk, Xki , λki , P

ki , P

kij )) − φ(L (Θk+1, Xk+1i , λk+1i , Pk+1i , Pk+1ij ))

≤1/φ′(L (Θk, Xki , λki , P

ki , P

kij ))

≤∥∇L(Θk, Xki , λki , exp

nki(0), dki , exp

nkij

(0), dkij)∥

=∆ +∑i

∥%(Xki −Xi(θk)) +∇O(Xki ) − λ

ki ∥ +∑

i

∥Xi(θk) − Xki ∥

≤∆ +∑i

∥∇O(Xki ) − λk+1i + λk+1i − λk∥ + (% + 1)∥Xi(θk) − Xki ∥

≤∆ +∑i

∥λk+1i − λk∥ + (β − %)∥Xk+1i − Xki ∥ + (% + 1)∥Xi(θk) − Xki ∥

≤∆ +∑i

∥λk+1i − λk∥ + (β − %)∥Xk+1i − Xki ∥+

(% + 1) [∥Xi(θk+1) − Xk+1i ∥ + ∥Xi(θk+1) −Xi(Θk)∥ + ∥Xk+1i − Xki ∥]

≤∆ +∑i

2% + 1

%∥λk+1i − λk∥+

(β + 1)∥Xk+1i − Xki ∥ + (% + 1)LXi∥Θk+1i −Θ

k∥≤L1∥ (Θk+1, Xk+1i , λk+1i , Pk+1i , Pk+1ij ) − (Θk, Xki , λ

ki , P

ki , P

kij ) ∥,

where we define:

∆ ≜

⎡⎢⎢⎢⎢⎣

L

2γ(1 − c)+∑

i

L +∑ij

L

⎤⎥⎥⎥⎥⎦

∥Θk+1−Θk∥+

LL

2γ(1 − c)

⎡⎢⎢⎢⎢⎣

∑

i

∥Pk+1i − Pki ∥ +∑

ij

∥Pk+1ij − Pkij∥

⎤⎥⎥⎥⎥⎦

L1 =max⎛

⎝

L2γ(1−c)

+∑i L +∑ij L + (% + 1)∑i LXi

max (LL

2γ(1−c), β + 1, 2%+1

%)

⎞

⎠

.

We have used Equation 14 in the forth inequality and the LXi -Lipschitz smoothness in the sixth inequality. We further havethe following estimate of function decrease due to Lemma 8.6:L (Θk, Pki , P

kij ) −L (Θk+1, Pk+1

i , Pk+1ij )

≥c∥Θk+1−Θk∥2

+ cL2∑

i

∥nk+1i − nki ∥

2+ c∑

i

∥dk+1i − dki ∥

2+

cL2∑

ij

∥nk+1ij − nkij∥

2+ c∑

ij

∥dk+1ij − dkij∥

2−

[2(β − %)2

%+LO + %

2− β + κ] ∥Xk+1

i − Xki ∥

2−

[2(β − % +LO)

2

%− κ] ∥Xk

i − Xk−1i ∥

2

≥L2∥ (Θk+1, Xk+1i , λk+1

i , Pk+1i , Pk+1

ij ) − (Θk, Xki , λ

ki , P

ki , P

kij ) ∥

2,

where we define:

L2 = min

⎛⎜⎜⎜⎝

min(c, cL2)

− [2(β−%)2

%+LO+%

2− β + κ]

− [2(β−%+LO)

2

%− κ]

⎞⎟⎟⎟⎠

.

The remaining argument is identical to Lemma 7.4.