{'fraction_non_alphanumeric': 0.0374899220639613, 'fraction_numerical': 0.011287288363343187, 'mean_word_length': 4.675394001016777, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 4, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
HM-DDP: A Hybrid Multiple-shooting Differential Dynamic Programming Method for Constrained Trajectory Optimization

Trajectory optimization has been used extensively in robotic systems. In particular, Differential Dynamic Programming (DDP) has performed well as an off-line planner or an online nonlinear model predictive control solver, with a lower computational cost compared with other general-purpose nonlinear programming solvers. However, standard DDP cannot handle any constraints or perform reasonable initialization of a state trajectory. In this paper, we propose a hybrid constrained DDP variant with a multiple-shooting framework. The main technical contributions are twofold: 1) In addition to inheriting the simplicity of the initialization in multiple shooting, a two-stage framework is developed to deal with state and control inequality constraints robustly without loss of the linear feedback term of DDP. Such a hybrid strategy offers a fast convergence of constraint satisfaction. 2) An improved globalization strategy is proposed to exploit the coupled effects between line-searching and regularization, which is able to enhance the numerical robustness of DDP-like approaches. Our approach is tested on three constrained trajectory optimization problems with nonlinear inequality constraints and outperforms the commonly-used collocation and shooting methods in terms of runtime and constraint satisfaction.


I. INTRODUCTION
Trajectory optimization (TO) has been well studied in control and robotic communities. It is built on optimization theory and techniques, which makes such TO-based approaches enjoy the feature of good generality. By minimizing objective functions and incorporating state and control constraints, an implicit and high-level formulation exploits the underlying dynamical properties and achieve efficient behaviors. To solve the formulated TO, there are two approaches: indirect and direct methods [3]. Indirect methods adopt principles of optimal control, like Pontryagin's Minimum Principle, to obtain analytical solutions without discretization [4]. However, indirect approaches can hardly scale with the system dimensions and complexity. In contrast, direct methods approximate the original TO through transcription [5]. Then commercial or free nonlinear programming (NLP) solvers, like SNOPT, are employed to solve the transcribed optimization problems. The most common direct method are shooting methods and direct collocation (DIRCOL). Although direct methods inherit the numerical robustness and easy-implementation features of the NLP solvers, they are often slow and require high computational power, which limits their applications in some online motion planning tasks.
Alternatively, Mayne [7] proposed Differential Dynamic Programming (DDP) to solve unconstrained TO problems The authors are with the Department of Mechanical and Automation Engineering, The Chinese University of Hong Kong, Hong Kong. in 1960s. DDP and its variant Sequential Linear Quadratic (SLQ) [8] are similarly based on Bellman's Principle of Optimality [5]. Compared with the collocation methods, DDP effectively solves TO through parameterized control variables. It requires a stable initial control policy to generate a state trajectory via forward integration. Additionally, DDP is sensitive to initial policies and can hardly be warm-started with dynamically infeasible state trajectories, since states are not optimized directly [22]. In contrast, multiple shooting (MS) approaches show a better flexibility in initialization and distribute nonlinearity by introducing extra state decision variables. Motivated by this, [10] proposed an unconstrained multiple-shooting framework for the DDP approach. Within this framework, [13] considered box constraints on control and [23] took into account of contacts (equality constraints) in legged locomotion. Although these works enjoyed the mentioned advantages of the MS algorithm and DDP-like methods, they is able to only handle specific constraints, such as box-input constraints and holonomic contact constraints.
The effort of extending DDP to generic constrained problems has been made. Unlike only focusing on either input constraints [14] or state constraints [15], [16] and [19] dealt with both state and control constraints with Karush-Kuhn-Tucker (KKT) conditions, which solved series of quadratic problems (QP) in the forward pass to guarantee feasibility. [17] and [18] combined DDP with Augmented Lagrangian approaches (AL-DDP). [21] (ALTRO) proposed a hybrid method which employed AL approaches firstly and post-processed the obtained solution with a direct active-set projection method. Similarly, [16] applied AL and polish the coarse solution with an interior-point method with slack variables (s-KKT). However, such methods either get numerical unstable easily [16]- [18], or increase the complexity by introducing factorization and projection [14] [21], or sacrifice the linear feedback policy in the optimal solution [15] [16] [21]. Hence, a generic algorithm framework, which can maintain the advantages of Multiple-shooting DDP (MDDP) and constraint-handling, is desirable.
In this paper, we propose a Hybrid Multiple-shooting DDP (HM-DDP) algorithm to solve state/control constrained trajectory optimization problems. The main contributions are twofold. First, inheriting the simplicity of the initialization in MS, we propose a two-stage framework to handle inequality constraints robustly without loss of the linear feedback term. Second, we propose an improved globalization strategy which exploits the coupled effects of line-searching and regularization. The paper is organized as follows: Section II recalls the mathematical background of the multiple-shooting DDP framework. Section III presents the proposed hybrid algorithm in detail. In Section IV, we numerically validate the proposed approach on several constrained TO problems and compare the performances with other TO methods.

A. DDP Preliminaries
Consider a discrete finite-time optimal control problem, where x k P R n and u k P R m denote the state and control at time t k , respectively. f : R nˆRm Þ Ñ R n is the transition dynamics. x init is the initial state and J is the total cost. The scalar-valued functions p¨,¨q and f p¨q denote the running and terminal objective functions. Let X :" px 0 , x 1 , . . . , x N q, U :" pu 0 , u 1 , . . . , u N´1 q be the state and control sequences over the horizon N . The value function V k is the optimal cost-to-go function starting at x k : Based on Bellman's Principle of Optimality, the optimal value function can be evaluated recursively, The value at the end state is V N " f px N q. In each iteration, DDP performs a backward pass (BP) and forward pass (FP) along a nominal state/control trajectory pX,Ū q. 1) Backward Pass: Q : R nˆRm Þ Ñ R function evaluates the cost-to-go of taking some action at a given state, Q k px, uq " lpx, uq`V k`1 pf px, uqq.
With the second-order approximation of the value functions, the optimal control modification δu is obtained by solving the following quadratic problem w.r.t., δu: δu˚" arg min A locally linear control update is obtained by solving (5), Plugging this control policy back into Q, recursive back propagation of the derivatives of the value function are done as follows: (8c) 2) Forward Pass: Integrating the dynamics with the updated feedforward and feedback control policy in (7) yeildŝ wherex 0 " x 0 and pX,Û q is the improved state/control nominal sequences.

B. Generalization to Multiple Shooting Framework
The multiple-shooting formulation introduces the intermediate state decision variables [22]. Additional matching constraints [10] or additional cost term [21] are required to ensure the dynamical feasibility. Our proposed algorithm is based on the former strategy. More details can be found in [10] [23].
end for 8: end for 9: Compute defects d and initial cost J 0 ; J Ð J 0 10: for i " 1 Ñ M axIter do 11: Backward Pass in each segment: 12: Forward Pass: 14: rX,Û ,Ĵs Ð F P pk f f , K f b , X, U , ∆V 1 , ∆V 2 , Jq 15: Update defectsd, compute costĴ 16: if (J´Ĵ ă v or allpk f f q ă q q and |d| 2 ă d max then 17: J ÐĴ, X ÐX, U ÐÛ 20: end for 21: return X sol , U sol and K sol 1) State Defect: By breaking a long trajectory into M segments and keeping the control sequence, the starting state of each shooting phase is a node state. Without loss of generality, we assume there is a node state at each time step, i.e., M " N´1. The defect between the end state of previous sub-trajectory and the node state of next successive segment, x k`1 , is defined as, The linearized dynamical constraint at the node state is, where f x and f u are the sensitives w.r.t the state and control. Taking into account of the defects, the modified TO is, where X node is the state decision variable, which is initialized as X init node . Initialization of the node states can be a simple interpolation between starting state and goal state or be from some sampling-based planner. To solve (11), the recursion procedure in the backward pass becomes, where Vx ,k`1 " V x,k`1`Vxx,k`1 d k is the Jacobian of the state-value function. The Hessian matrix V xx keeps the same as in (6c-6e).
2) State and Control Update: The control update keeps the same as (7). Based on the assumption of linearzied system dynamics, the node states are updated as below, Starting from the nodex k`1 , we can rewrite the subtrajectory within each shooting segment through forward integration with the updated control law. The detailed algorithm of MDDP is outlined in Algorithm 1.

III. HM-DDP: HYBRID MULTIPLE-SHOOTING DIFFERENTIAL DYNAMIC PROGRAMMING
In this section, we present our main contribution: a hybrid multiple-shooting Differential Dynamic Programming (HM-DDP) approach, to deal with both state and control inequality constraints and keep the linear feedback policy. As shown in Fig. (1), this hybrid algorithmic framework has two stages. Firstly, we utilize MDDP with the Augmented Lagrangian method to obtain a coarse solution rapidly. Then, the coarse solution is used to warm-start the second stage which uses a Relaxed Log Barrier (RLB) function. The second stage can eliminate the constraint violations and make an infeasible trajectory feasible. Since constraints are penalized as additional objective terms in both stages, the linear feedback policy can be naturally reserved.
Consider following additional inequality constraints on the basis of problem in (11).
where g P R q k is the inequality constraints and q k is the number of constraints at the k-th time instant.

A. Coarse Solution from MDDP with Augmented Lagrangian Method
Since Augmented Lagrangian method has a fast convergence rate at first several iterations, coarse trajectories with slight constraint violation can be obtained rapidly. In the first stage, we wrap up MDDP with Augmented Lagrangian (AL-MDDP) as in [21], then update the multipliers after one iteration of the inner MDDP solver. AL-MDDP is explained in Algorithm 2.
1) Inner MDDP reformulation: Based on (14), we can write the augmented Lagrangian as below, where h " maxp0, gq, λ is the dual variable, and µ is the penalty weight. The backward pass absorbs the derivatives of the penalty terms by fixing the multipliers. The recursive updates become, x,k h u,k . (16f) All the constraints are linearized, which is the same as the linearization of dynamics.
2) Update Augmented Lagrangian: When one inner iteration is done with the fixed λ and µ, the Lagrangian multipliers are updated as, and the penalty is increased by multiplying a scaling gain, µ k Ð φµ k , φ ą 1. If update: update λ, µ using (17) and (18)   is still a good initial guess to warm start the second stage.
To refine the solution, we applied the relaxed log barrier function method 1 , RLB-MDDP, to approximate the original problem. RLB-MDDP is outlined in Algorithm 3.
For β, we utilize the quadratic extension proposed in [24], and it is twice differentiable. Adding the barrier function terms Bpgq at each time step to the objective function yields, 2) Update RLB parameters: To get a better approximation of the original constrained problem, we adapt a new strategy to update the parameters of the relax log barrier function, ψ Ð ω 1 ψ, ψ ą 0, ω 1 ă 1, (20a) δ Ð maxpδ min , ω 2 δq, δ min ą 0, ω 2 ă 1. (20b) The optimal solution pX˚, U˚q is approached by decreasing ψ and δ gradually over iterations. In fact, as ψ Ñ 0, the RLB function becomes a good approximation of the indicator function of the subset defined by the constraints. Initialized by the solution from AL-MDDP, RLB-MDDP obtains a polished solution with a better constraints satisfaction and can even refine an infeasible trajectory into a 1 The feasibility in the forward pass cannot be guaranteed due to K f b , which causes the penalty term to be undefined. Consequently, we use a relaxed extension.

Algorithm 4 HM-DDP
Require: X init node , U init , f , N , T , M , and f 1: Initialize: Set required parameters 2: rX al , U al , K al s Ð AL-MDDPpX init node , U init q using Algorithm 2 3: rX sol , U sol , K sol s Ð RLB-MDDPpX al , U al , K al q using Algorithm 3 4: return X sol , U sol and K sol dynamically feasible one. The whole HM-DDP algorithm is shown in Algorithm 4.

C. Improved Globalization Strategies
In DDP methods, the inaccurate approximation may cause the Hessian matrix to be not positive definite. Then the descent direction cannot be guaranteed in Newton's method. Therefore, globalization strategies are required to improve the numerical stability. In this paper, we proposed an improved globalization strategy which exploits the coupled effects of line-searching and regularization.

1) Using Line Search (LS):
Once the new trajectory is updated with a full step, the state can stride out of the approximated region. Therefore, we adapt a backtracking line search method with Armijo condition [1] in HM-DDP. The state trajectory is integrated with, where α is an introduced scalar parameter. HM-DDP line-searches over an expected cost reduction as in [26], but an additional term related to the defects is suggested as below, where w is the weight of the additional cost term. dV plays a role of the merit function which trades off between minimizing cost and closing defects. The ratio between the actual and expected reduction is, r " pL p¨q pX,Û q´L p¨q pX, U qq{dV , where L p¨q is L 1 or L 2 . The iteration is accepted if r satisfies specific conditions, such as lying in a certain interval.
2) Regularization Schedule: For nonlinear dynamics and complex constraints, regularization is another way to enhance the stability of DDP algorithms. Following the strategy proposed by [26], we add a diagonal matrix I n to the Hessian of state value to penalize the deviations from the nominal state trajectory, instead of the control sequence, The introduced term makes the update of state sequence more conservative, which can keep the consistency of the state solution if warm-starting trajectories are available in MDDP.

3) Coupled Effect between Regularization and LS:
Previously, the regularization and LS techniques were used separately. Specifically, DDP-type methods like [11] [13] [20] [26] only require regularization if the backward pass failed, i.e., a non-positive definite Q uu exists. In the forward pass, LS is used for guaranteeing an adequate cost reduction. Iterations would terminate with a failed LS. However, we found a coupled effect between LS and regularization and a resulting strategy can accelerate the convergence. The main idea is to trigger the regularization when a failed LS happens in the forward pass, instead of only triggering the regularization in the backward pass. As shown in (21), the coefficient α only affects the feedforward term, and it (even a small value) may cause x N away from the goal state, especially when the trajectories are close to the local minimum or nearly diverge. The reason is the feedforward term dominates the forward integration and the feedback control policy cannot correct the state deviations. In this case, our solution is returning back to the backward pass and then increasing the regularization µ V . A linear modification law is proposed, A larger µ V can render a higher K to dominate and stabilize the next forward integration. Such a strategy can accelerate the convergence rate by avoiding small line-search factors and thus improve the numerical stability of MDDP.

IV. RESULTS
We firstly evaluated HM-DDP on several dynamical systems and compared the performance with DIRCOL, direct single shooting (SS) and pure AL-MDDP in terms of constraint satisfaction and computational time. HM-DDP was implemented with MATLAB and DIRCOL was implemented with an open-source trajectory optimization library of Op-timTraj [5]. OptimTraj solved the NLP using SQP [28] in MATLAB. SS was implemented using the MATLAB interface of CasADi [29] with IPOPT solver. All simulations were run in MATLAB2021a on a standard computer with an Intel(R)7-8700 CPU and 16GB RAM. Each example used a quadratic objective function as and the stop criterion was identical: dV ď 10´7, maxpgq ď 10´8, and M axIter " 100.
A. Dynamic System Examples 1) CartPole: The task is to swing up the pinned pole and move the cart [5] to a desired position with limited force (ď 5 Nm) and rail length (1.6 meters). Time horizon and time step are T " 3s and dt " 0.01s. The shooting phase is M " 10. The initial node states and initial control policy for this task are a dynamically infeasible solution from DIRCOL with a low discrete resolution (N " 5, Iter " 3).
2) 2D Car: A simplified unit mass point with a discrete nonholonomic vehicle dynamics [15] performs a task moving to the goal position x g without hitting obstacles. Limited actuation is imposed. Time horizon is T " 5s and dt " 0.01s in this example. The initialization for X init node is a simple linear interpolation (M " 5) between x 0 and x g . The initial control sequence is all zero.
3) Planar Quadrotor: A planar quadrotor is an underactuated nonlinear dynamic system [25] with state variable px, y, θ, 9 x, 9 y, 9 θq and input pu L , u R q. The task is to search for optimal collision-free trajectories reaching the goal position. The tilt angle of body is limited within r´π 6 , π 6 s and the direction of thrust force is forced to be positive. The node states are initialized with the linear interpolation (M " 30) and the initial policy is a hovering controller.

B. Performance
Different goal or initial states were validated for each system and the optimized trajectories of HM-DDP for the three systems are shown in the first column in Fig. 2 (a, d  and g), respectively. HM-DDP successfully generated smooth trajectories without violating any constraints for all tasks.
To further demonstrate the performance of HM-DDP, we compared HM-DDP with three common practices: DIRCOL, direct Single Shooting (SS) and AL-MDDP. All methods were with the same objective functions and constraints. DIRCOL and SS used a lower discretization resolution with N " 200 to take the advantages of the NLP solvers reasonably. Moreover, DIRCOL and SS were initialized with zero policy. The initial state guess for DIRCOL was a linear interpolation between x 0 and x g . We presented one of the simulation results for each system in detail. The constraint satisfaction and the total cost profiles were compared in the (a)  Table I. For CartPole, with a coarse solution, HM-DDP took two more iterations in RLB stage to eliminate constraint violations. AL-MDDP took over 60 iterations to satisfy all the constraints. Though DIRCOL and SS got low costs, both failed to satisfy the constraint tolerance within 100 iterations. Moreover, HM-DDP obtained the lowest cost value. In the 2D car example, HM-DDP eliminated constraint violations within 10 iterations. AL-MDDP got stuck with large constraint violation and failed to find a feasible trajectory. DIRCOL also satisfied the tolerance of constraints while SS exited with large constraint violations due to reaching the M axIter. SS got the best solution in this case and HM-DDP was very close to the optimal solution of SS. For the quadrotor system, HM-DDP rapidly eliminated constraint violations in the second stage, while AL-MDDP failed to find a feasible trajectory even though the cost is low. Both SS and DIRCOL got solutions within the tolerance of constraint. HM-DDP obtained the best solution while SS converged to another local minimum with a higher cost. When comparing the runtime, we excluded AL-MDDP because it cannot obtain a feasible solution stably. HM-DDP was much faster than DIRCOL and SS methods as shown in Table I.

C. Discussion
Pure AL-MDDP fails easily if the initialization is not welldesigned and shows a slow tailed convergence in 2D car and planar quadrotor systems. By switching to RLB-MDDP after obtaining a coarse solution, HM-DDP outperforms AL-MDDP in rapidly eliminating the constraint violations. Compared with SS, HM-DDP has the advantage of initialization with infeasible trajectories. Although DIRCOL performs robustly, it takes much more iterations and longer runtime (see Table I). Finally, future works include implementing HM-DDP in C/C++ to realize a high re-planning frequency in online tasks as [27] did, combining it with sample-based planner, and extending it to switched systems like legged robots.

V. CONCLUSION
In this paper, we proposed a Hybrid Multiple-shooting Differential Dynamic Programming (HM-DDP) approach to incorporate inequality constraints for constrained trajectory optimization. HM-DDP can be accelerated with an infeasible initialization of the state trajectory due to the multipleshooting framework. Then, a novel globalization strategy is introduced to improve the stability of DDP-like approaches. We validated HM-DDP and compared the performance with other approaches. HM-DDP outperformed DIRCOL and direct shooting methods on a variety of constrained TO problems in terms of computational time and constraint satisfaction. HM-DDP was a fast-converged and good constraintsatisfied nonlinear solver for constrained optimal control problems, and has great potential in the application of online motion planning and model predictive control.