Nonlinear Trajectory Optimization Problems

Unconstrained Problem

The unconstrained nonlinear trajectory optimization problem is most commonly solved in the discrete-time form $$\begin{array}{rl}\underset{x_{0:N},u_{0:N-1}}{\min }& \Phi (x_N)+\sum _{k=0}^{N-1}l(x_k,u_k,k)\\ \text{s.t.}& x_{k+1}=f(x_k,u_k,k),k\in \{0,\dots ,N-1\}\\ & x_0={\hat{x}}_0\end{array}$$ is certainly more typical than its continuous-time counterpart. We will cover direct transcription and iterative LQR (iLQR) / differential dynamic programming (DDP) as means of solving this problem.

Direct Transcription

Direct transcription simply solves the problem as any nonlinear program, typically using sequential quadratic programming (SDP). In SDP we start by initializing the algorithm with a nominal trajectory ${\bar{x}}_{0:N}$, ${\bar{u}}_{0:N-1}$ (important if the problem is non-convex), around which we form a QP that locally approximates the original problem: $$\begin{array}{rl}\underset{\begin{array}{c}\delta x_{0:N}\\ \delta u_{0:N-1}\end{array}}{\min }& {\Phi }_{x_N}^{\top }\delta x_N+\frac{1}{2}{\delta x_N}^{\top }{\Phi }_{x{x}_N}\delta x_N+\sum _{k=0}^{N-1}\left({\left[\begin{array}{c}{l}_{x_k}\\ l_{u_k}\end{array}\right]}^{\top }\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]}^{\top }\left[\begin{array}{cc}{l}_{x{x}_k}& l_{x{u}_k}\\ l_{u{u}_k}& l_{u{u}_k}\end{array}\right]\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]\right)\\ \text{s.t.}& \delta x_{k+1}=f_{x_k}\delta x_k+f_{u_k}\delta u_k+f_k-{\bar{x}}_{k+1},\forall k\in \{0,\dots ,N-1\}\\ & \delta x_0={\hat{x}}_0-{\bar{x}}_0,\end{array}$$ Its terms are obtained using first and second order Taylor expansions of the system’s dynamics and cost functions, evaluated at ${\bar{x}}_{0:N}$, ${\bar{u}}_{0:N-1}$. The solution $\delta x_{0:N}$, $\delta u_{0:N-1}$ represents a search direction that is already scaled bases on the local curvature of the problem. Direct transcription faces challenges common to general nonlinear programming (NLP) in general.

• The quadratic approximation is non-convex
• Taking the full step does not improve the objective function
• Constraints are not satisfied after updating the trajectory

As a resource for suitable strategies I generally point to Nocedal2006.

iLQR & DDP

An entirely different philosophy for solving this problem is followed in the closely linked iLQR and DDP algorithms. Both algorithms have a two pass structure. First in the backward pass quadratic approximations of the Bellman equation $$V(x_k,k)=\underset{u_k}{\min }\left(l(x_k,u_k,k)+V(f(x_k,u_k,k),k+1)\right)$$ are solved at each timestep along the receding horizon. This not only produces a local approximation of value function but also produces a policy update $$\delta u_k(\delta x_k)=d_k+K_k\delta x_k$$ as a by-product. In the forward pass the updated controlled policy is applied while simulate the system’s behavior forwards in time: $$\begin{array}{rl}{x}_0& ={\hat{x}}_0\\ u_k& ={\bar{u}}_k+d_k+K_k(x_k-{\bar{x}}_k)\\ x_{k+1}& =f(x_k,u_k,k).\end{array}$$

Backward Pass

$$V_k+V_{x_k}^{\top }\delta x_k+\frac{1}{2}\delta x_k^{\top }{V}_{x{x}_k}\delta x_k=\underset{\delta u_k}{\min }\left(Q_k+{\left[\begin{array}{c}{Q}_{x_k}\\ Q_{u_k}\end{array}\right]}^{\top }\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]+\frac{1}{2}{\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]}^{\top }\left[\begin{array}{cc}{Q}_{x{x}_k}& Q_{x{u}_k}\\ Q_{u{x}_k}& Q_{u{u}_k}\end{array}\right]\left[\begin{array}{c}\delta x_k\\ \delta u_k\end{array}\right]\right)$$ where $$\begin{array}{rl}{Q}_{x_k}& =l_{x_k}+f_{x_k}^{\top }{V}_{x_{k+1}}\\ Q_{u_k}& =l_{u_k}+f_{u_k}^{\top }{V}_{x_{k+1}}\\ Q_{x{x}_k}& =l_{x{x}_k}+f_{x_k}^{\top }{V}_{x{x}_{k+1}}{f}_{x_k}+V_{x_{k+1}}\cdot f_{x{x}_k}\\ Q_{u{u}_k}& =l_{u{u}_k}+f_{u_k}^{\top }{V}_{x{x}_{k+1}}{f}_{u_k}+V_{x_{k+1}}\cdot f_{u{u}_k}\\ Q_{u{x}_k}& =l_{u{x}_k}+f_{u_k}^{\top }{V}_{x{x}_{k+1}}{f}_{x_k}+V_{x_{k+1}}\cdot f_{u{x}_k}\end{array}$$ with $$\delta u_k(\delta x_k)=\underset{d_k}{\underbrace{-Q_{u{u}_k}^{-1}{Q}_{u_k}}}\underset{K_k}{\underbrace{-Q_{u{u}_k}^{-1}{Q}_{u{x}_k}}}\delta x_k.$$

Unconstrained Continuous-Time Problem

Let us now tackle the continuous-time problem $$\begin{array}{rl}\underset{x(t),u(t)}{\min }& \Phi (x(T))+{\int }_{t=0}^{T}l(x(t),u(t),t)\\ \text{s.t.}& \dot{x}(t)=f(x(t),u(t),t),t\in [0,T]\\ & x(0)={\hat{x}}_0\end{array}$$ As optimization of continuous variables is rather impractical, we generally need to discretize the problem at some point. This is often done even before stating the trajectory optimization problem. For instance, we may discretize $\dot{x}=f(t,x,u)$ using Runge-Kutta methods (with a zero-order hold on the input $u$) as : $$x_{n+1}=x_n+\sum _{i=1}^s{b}_i{k}_{i,n},k_{i,n}=f\left(t_n+c_{i}h,x_n+h\sum _{j=1}^s{a}_{ij}{k}_{n,j},u_n\right)$$ resulting in standard discrete-time dynamics. This is typically done using explicit schemes $$\begin{array}{ccccc}0& & & & \\ c_2& a_{21}& & & \\ ⋮& ⋮& \ddots & & \\ c_s& a_{s1}& \cdots & a_{s,s-1}& \\ & b_1& \cdots & b_{s-1}& b_s\end{array}$$ Then, approximating the integral with a sum we get the discrete-time trajectory optimization problem that can be solved using direct transcription or iLQR/DDP.

Alternatively, we can aim for a higher degree of accuracy (and improved stability) by using implicit RK schemes $$\begin{array}{ccccc}{c}_1& a_{11}& a_{12}& \cdots & a_{1s}\\ c_2& a_{21}& a_{22}& \cdots & a_{2s}\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ c_s& a_{s1}& a_{s2}& \cdots & a_{s,s}\\ & b_1& b_2& \cdots & b_s\end{array}$$ In standard forward simulation this comes with the disadvantage of having to solve a set of nonlinear equations to acquire $k_i$ at each step. However, as we are already solving a nonlinear optimization problem, we may directly incorporate these equations into its constraints.

Direct Collocation

This exactly is utilized by direct collocation methods. Here we typically reserve the symbol $k$ for timesteps and use ${\dot{x}}_{i,k}$ and $x_{i,k}$ as a substitute for $k_{i,n}$ and $x_k+h{\sum }_{j=1}^s{a}_{ij}{\dot{x}}_{j,k}$. This is in-line with the idea of fitting a polynomial that approximates the value of $x(t)$ on each interval which behind the formulation of many implicit RK methods. Additionally, control inputs $u(t)$, $t\in [t_k,t_{k+1}]$ take the form of polynomial functions: $$\Pi (\tau ;u_{1:r,k})=\sum _{j=1}^r{\sigma }_j(\tau )u_{j,k},\tau \in [0,1],$$ where ${\sigma }_j(\tau )$ are polynomial bases, however the running cost is for convenience evaluated only at the end-points of timesteps in most cases. The overall problem can then be stated as $$\begin{array}{rlrl}\underset{\begin{array}{c}{x}_{0:N}\\ u_{1:r,0:N-1}\\ x_{1:s,0:N-1}\\ {\dot{x}}_{1:s,0:N-1}\end{array}}{\min }& \Phi (x_N)+\sum _{k=0}^{N-1}l(x_k,u_k,k)& & \\ \text{s.t.}& x_{k+1}=x_k+h{\sum }_{i=1}^s{b}_i{\dot{x}}_{i,k},& & n\in \{0,\dots ,N-1\}\\ & {\dot{x}}_{i,k}=f\left(t_k+c_{i}h,x_{i,k},\Pi (c_i;u_{1:r,k})\right),& & \{i,k\}\in \{1,\dots ,s\}\times \{0,\dots ,N-1\}\\ & x_{i,k}=x_k+h{\sum }_{j=1}^s{a}_{ij}{\dot{x}}_{j,k},& & \{i,k\}\in \{1,\dots ,s\}\times \{0,\dots ,N-1\}\\ & x_0={\hat{x}}_0.& & \end{array}$$ This problem is then typically solved again using SQP, similarly to direct collocation.

Constrained Problems

Similarly to how we linearized the system’s dynamics it is common to also linearize additional constraints, e.g. $$\begin{array}{rl}g(x_k,u_k,k)& \le 0\\ h(x_k,u_k,k)& =0.\end{array}$$ to fit within SQP. However, for some types of constraints, such as second-order cone constraints, there are specialized ways in which they can be effectively handled (e.g. ADMM).

Due to the special structure of the trajectory optimization problem, many specialized solvers capable of handling additional constraints exist. As nonlinear MPC is still extremely computationally demanding, these solvers are often tailored to handle specific types of constraints, also making deliberate choices when it comes to how the algorithms converge (Altro, Crocoddyl, MJPC).