轨迹优化方法与实践

轨迹优化方法是在满足特定约束的前提下寻找使评价函数最小（或最大）的轨迹的方法。轨迹优化源于变分法(Calculus of Variations）。轨迹可以表示为系统状态变量和控制变量的函数，而评价函数是轨迹函数的函数，变分法是在当前轨迹附近进行轻微的扰 动寻找使轨迹代价下降的方法。在变分法中一个最重要的结论是欧拉-拉格朗日方程（Euler-Lagrange）。欧拉-拉格朗日方程给 出了轨迹最优的必要条件。充分理解欧拉-拉格朗日方程对于充分理解和灵活应用轨迹优化非常有帮助。

严格地讲轨迹优化解决的是以下一类优化问题：\[ \min_{x, u} \phi(x(t_f)) + \int_{t_0}^{t_f} l(x(t), u(t), t) dt \] s.t. \[ \dot{x}(t) = f(x(t), u(t), t) \] \[ x(t_0) - x_0 = 0 \] \[ h(x, u) \le 0\] 其中\(x\) 是系统的状态变量，如车辆的位置 和速度，\(u\) 是系统的控制变量，如油门大小和方向盘的转角，\(f\) 是微分形式的系统动力学方程，描述了系统的状态的变化与当前状 态和控制输入之间的关系。

根据变分法，无约束情况下轨迹最优的必要条件是：

\begin{equation*} l_x + \lambda^\top f_x + \dot{\lambda}^\top = 0 \end{equation*} \begin{equation*} l_u + \lambda^\top f_u = 0 \end{equation*} \begin{equation*} \phi_x(x(t_f)) - \lambda^\top(t_f) = 0 \end{equation*}

其中\(\lambda\) 称为系统的协状态。

轨迹优化跟最优控制有着紧密的联系。 两者的区别在于：最优控制通常研究的是状态反馈控制律， 即对于任意指定的系统状态计算对应的最优控制输出； 而轨迹优化研究的是从某个指定状态开始的最优控制序列。 两者的联系在于：动力学系统在最优反馈控制律的作用下形成向量场， 最优轨迹是最优控制向量场中起始于某个特定状态的流。 如果我们知道了最优状态反馈控制律就可以得到起始于任意状态的最优轨迹； 反之，如果我们得到了始于任意状态的最优轨迹， 就能够通过这些最优轨迹重构最优反馈控制律 ^{3 , 4}。 也可以通过在线优化构成反馈控制律⁵，或者将离线规划的结果与在线优化相结合构成反馈控制律⁶

根据求解的过程的不同，轨迹优化可分为间接方法和直接方法：

• 间接方法：依赖Pontryagin 最大值原理，将轨迹优化问题转化为两点边值问题，然后利用求解两点边值的方法求解最优轨迹， 最后对计算结果离散化。间接法在求解的最后进行离散化，因此结果的精度很高，但缺点是不容易收敛。
• 直接方法：直接方法首先将原问题离散化，从而将无限维连续优化问题转化为有限参数的非线性优化问题， 然后利用常规的非线性优化方法求解最优轨迹。 直接法在一开始对原问题进行离散化，其结果会存在一定的近似误差，但优点是比较容易收敛。

通常我们将把无限维连续轨迹优化问题转化为有限维常规非线性优化问题的过程称为改编（transcription）。 根据改编方法的不同，轨迹优化方法又可大致分成打靶法和配点法， 两者的主要不同在于对状态方程的处理。

• 打靶法 （shooting method）：打靶法只优化控制变量，状态变量和协状态通过对状态方程前向积分得到。 基本过程是首先采用初始的控制轨迹\(u(0,\ldots,N-1)\) 生成初始的状态轨迹，根据末端边值误差\(e_f\triangleq \lambda(N)-V_x(x(N))^\top\) 采用非线性优化方法求解\(\lambda(0)\) 使得\(e_f\) 接近于零。 打靶法的优点是能够得到非常精确的最优轨迹。打靶法适用于控制变量的维数较低且初始估计比较准确的应用场合。 打靶法存在的主要问题是对于有强约束或者动力学不稳定的系统优化不容易收敛。 这种情况下可以采用多目标打靶法将轨迹分成若干轨迹片段，然后在每条轨迹片段上反复采用打靶法， 直到所有断点满足连续条件。
• 配点法（collocation）：配点法采用条函数（spline）近似状态轨迹和控制轨迹， 然后采用常规的非线性优化方法（NLP）求解样条函数的参数。 伪谱方法（pseudospectral method）采用全局多项式函数近似状态轨迹和控制轨迹， 系统状态方程约束由正交配置法近似，从而将原问题参数化。 主要的伪谱方法有勒让德伪谱方法(the Legendre pseudospectral method) 和切比雪夫伪谱方法（the Chebyshev pseudospectral method）。

除此之外，还有很多其他轨迹优化方法，例如CHOMP 和 DDP 等。 每种轨迹优化方法都有其自身的特点，适用于不同的场合。没有适 合所有应用的最好的轨迹优化方法，实践中要根据对精度、对实时性的要求和优化问题的结构灵活选择。在非线性不强，对实时性要 求较高的场合，微分动态规划（Differential Dynamic Programming）提供了一种行之有效的方法，下面重点介绍一下。微分动态规 划方法大致属于直接打靶法。但和上面介绍的方法略有不同的是它不明确的区分改编和优化，而是沿轨迹反复地做前向和反向积分从 而满足系统的动力学方程和轨迹最优的必要条件。微分动态规划法有接近二次型的收敛速度。此外，微分动态规划方法在收敛的同时 给出了最优反馈控制律的一阶近似和值函数的二次阶近似，在很多分析中非常有用。

为了简单起见，我们首先来介绍有限时域离散状态轨迹优化问题： \[ U, X = \arg \min_{X, U} \big \{ \phi(x_N) + \sum_{i=0}^{N-1} L(x_i, u_i, i) \big \} \] s.t. \[ x_{i+1} = F(x_i, u_i, i) \] \[ x_0 = \bar{x}_0 \]

在介绍微分动态规划法之前，需要首先介绍一下贝尔曼方程和动态规划法（dynamic programming）。

最优性原理（也称为嵌入原理）：不论一个问题的初始状态为何，以最优控制所形成的状态作为初始条件，余下的控制对余下的问题也必构成最优策略： \[ u^*(i) = \arg \min_{u_i} \big \{ L(x_i, u_i, i) + \min \{ \sum_{k = i+1}^{N-1} L(x_k, u_k, k) + \Phi(x_N) \big \} \} \]

在最优控制中，值函数是一个被提到的函数。 对于有限时域轨迹优化问题，值函数是关于系统状态\(x\) 和阶段\(i\) 变化的时变函数: \[ V(x_i, i) = \min_{u_i} \big \{ \sum_{k=i}^{N-1} L(x_k, u_k, k) + \Phi(x_N) \big \} \]

对于无限时域问题，值函数是关于系统状态\(x\) 的稳态函数: \[ V(x_i) = \min_{u_i} \big \{ \sum_{k=i}^{\infty} L(x_k, u_k, k)\big \} \]

定义了值函数以后，贝尔曼方程可描述为如下的嵌套形式： \[ V(x_i, i) = \min_{u_i} \big \{ L(x_i, u_i, i) + V(F(x_i, u_i), i+1) \big \} \]

动态规划采用分而治之的思路将多步或长期优化问题通过贝尔曼方程转化为单步或短期优化问题， 从而将指数复杂度降低为多项式复杂度。 求解动态规划的最常用的方法是“值迭代”法（Value iteration）。 对于有限时域的最优控制问题，其最后阶段的值函数通常是已知的，即： \[ V(x_N, N) = \Phi(x_N, N) \]

其他阶段的值函数可以根据贝尔曼方程反向迭代得到： \[ V(x_i, i) = \min_{u} \big \{ L(x_i, u, i) + V(F(x_i, u_i), i+1) \big \} \]

在数值计算中，需要对值函数\(V(x_i, i)\) 进行近似，通常采用的方法是表格法（Tabular method）。 表格法对状态空间进行离散化，然后对网格点处的状态运用贝尔曼方程求解值函数。 在迭代的过程中，非网格点处的值函数通过函数差插值得到。

已知值函数，最优状态反馈控制律可以通过下面的方程得到： \[ {u}^*(x,i) = \arg \min_{u}\big \{ L(x, u, i)+ V(F(x, u), i+1)\big \} \]

有了最优状态反馈控制律，轨迹优化问题可以采用得到的最优状态反馈控制律和系统状态方程生成起始于任意指定状态的最优轨迹。

如果状态变量的量化级数为\(R_x\) ，控制变量的量化级数为\(R_u\) ，状态变量的维数为\(d_x\) ，控制变量的维数为\(d_u\) ，那么动态 规划的计算量将正比于\(R_x^{d_x}R_{u}^{d_u}\) ，而存储量正比于\(R_{x}^{d_x}\) 。动态规划方法的计算量和存储量随着状态变量维 数成指数增长的问题称为“维数灾”问题。“维数灾”问题的存在限制了其在实际中的应用。

在动态规划法中我们需要对整个状态空间的值函数进行近似，因此存在“维数灾”问题。 为了克服这个问题，微分动态规划只对当前轨迹的邻域内的值函数进行近似，从而得到当前轨迹临域内的最优反馈控制律。 通过临域最优反馈控制律不断更新轨迹，进而不断逼近最优轨迹。

设当前轨迹上一点的状态和控制分别为\(x_i\) 和\(u_i\) ， 对当轨迹做很小的绕动\(x=x_i + \Delta x_i\) ，\(u = u_i + \Delta u_i\) 。

定义一个中间函数：\(Q(x,u) = L(x,u) + V(F(x,u))\) 。 根据贝尔曼方程，阶段\(i\) 的最优控制和值函数可以通过最小化\(Q\) 函数得到。 \(Q\) 函数的二阶近似可以表示为：

\begin{align*} Q(x, u) &= Q(x_i, u_i) + Q_x(x_i, u_i) \Delta x_i + Q_u(x_i, u_i) \Delta u_i + \\ & \Delta x_{i}^\top Q_{xu}(x_i, u_i) \Delta u_i + \frac{1}{2} \Delta x_{i}^\top Q_{xx}(x_i, u_i) \Delta x_i + \frac{1}{2} \Delta u_{i}^\top Q_{uu}(x_i, u_i) \Delta u_i \end{align*}

使得\(Q\) 函数最小的控制扰动 \(\Delta u\) 为：

\begin{align} \Delta u_i^* &= - Q_{uu}(i)^{-1}Q_{u}(i) - Q_{uu}(i)^{-1}Q_{ux}(i) \Delta x_i \end{align}

阶段\(i\) 的值函数在当前轨迹临域附近可以近似为：

\begin{align} \label{org9a096ad} V(x) &= \min_{\Delta u_i} Q(x_i + \Delta x_i, u_i + \Delta u_i) \nonumber \\ & = Q(x_i + \Delta x_i, u_i + \Delta u_i^*) \nonumber \\ & = Q(x_i, u_i) - \frac{1}{2}Q_u Q_{uu}^{-1} Q_u^{\top} + (Q_x - Q_u Q_{uu}^{-1}Q_{ux}) \Delta x_i + \frac{1}{2}\Delta x_{i}^\top (Q_{xx} - Q_{xu} Q_{uu}^{-1} Q_{ux}) \Delta x_i \end{align}

\(Q\) 函数的各阶导数如下：

\begin{align} \label{eq:value-update-start} Q_{x}(i) &= L_{x}(i)+V_{x}(i+1) F_{x}(i)\\ Q_{u} (i)&= L_{u}(i)+V_{x}(+1)F_{u}(i)\\ Q_{xx}(i) &= L_{xx}(i) + F_x(i)^\top V_{xx}(i+1)F_{x}(i) +V_{x}(i+1)F_{xx}(i) \\ Q_{uu} (i)&= L_{uu}(i) + F_u(i)^\top V_{xx}(i+1)F_{u}(i) +V_{x}(i+1)F_{uu}(i) \\ Q_{ux} (i)&= L_{ux}(i) + F_u(i)^\top V_{xx}(i+1)F_{x}(i) +V_{x}(i+1)F_{ux}(i) \\ \end{align}

其中\(F_x(i) := F_x|_{x_i, u_i}\) , \(F_u(i) := F_u|_{x_i, u_i}\) , \(F_{xx}(i) := F_{xx}|_{x_i, u_i}\) ， \(F_{uu}(i) := F_{uu}|_{x_i, u_i}\) ， \(F_{xu}(i) := F_{xu}|_{x_i, u_i}\) ， \(L_x(i) := L_x|_{x_i, u_i}\) ， \(L_u(i) := L_{u}|_{x_i, u_i}\) ， \(L_{xx}(i) := L_{xx}|_{x_i, u_i}\) ， \(L_{xu}(i) := L_{xu}|_{x_i, u_i}\) ， \(L_{uu}(i) := L_{uu}|_{x_i, u_i}\) ， \(V_x(i) := V_x(x)|x=x_i\) ， \(V_{xx}(i) := V_{xx}(x)|_{x = x_i}\) 。

\begin{align} V_{x}(i) &:= V_x(x)|_{x = x_i} = Q_x(i) - Q_u(i) Q_{uu}(i)^{-1} Q_{ux}(i) \\ V_{xx}(i) &:= V_{xx}(x)|_{x=x_i} = Q_{xx}(i) - Q_{xu}(i) Q_{uu}(i)^{-1} Q_{ux}(i) \end{align}

边界条件为：

\begin{align} V(N) &= \Phi(N) \\ V_{x}(N) & = \Phi_x(N) \\ V_{xx}(N) &= \Phi_{xx}(N) \end{align}

微分动态规化法可以描述为：

1. 根据控制变量的初始值，通过前向积分得到系统的状态轨迹。
2. 反向更新系统的值函数估计并计算临域最优反馈控制律。
3. 利用临域最优控制生成新的轨迹。
4. 计算新轨迹的代价，如果满足退出条件则退出；否则，如果代价的下降满足下降预期，返回步骤2并重复；否则算法报错。

因为上面得到的最优控制律是基于值函数近似得到的， 所以在更新控制的时候需要选取一定的下降步长： \[ \Delta u_i^* = - \alpha Q_{uu}(i)^{-1}Q_{u}(i) - Q_{uu}(i)^{-1}Q_{ux}(i) \Delta x_i \]

在求解的过程中，可以采用置信域（trust region） 方法或者线性回溯搜索（backtracking line search）方法⁷。 线性回溯搜索法是非线性优化中常用的一种方法， 其基本思想是在迭代开始的时候采用一个较大的步长，然后对生成的轨迹进行评估。 如果当前轨迹的代价没有达到下降预期，减小搜索步长和下降预期， 重复上面的过程直到代价的实际下降达到或超过下降预期为止，否则算法报错。 根据方程 \eqref{org9a096ad}，代价函数的最大下降预期为： \[ \Delta Q = \frac{1}{2}Q_u Q_{uu}^{-1} Q_u^{\top} \]

微分动态规划法需要用到代价函数的Hessian 矩阵，虽然在理论上能够提高收敛的速度， 但是在实践中因为Hessian 矩阵的维数通常比较高，计算比较耗时，有时反而会降低求解速度。 如果系统的状态方程非线性不强，代价函数接近二次型，可以对算法做进一步简化，例如采用一阶模型近似动力学方程，采用二次型近似代价函数， 从而得一种新的优化方法，迭代线性二次型方法（iLQR）。iLQR 与Gauss-Newton方法类似，为保证收敛，在应用中需要注意以下两点：

1. 代价函数在最优轨迹附近趋近于零。
2. 代价函数的非线性不能太强。

DDP 要求在迭代的过程中 Hessian 矩阵必须是正定的。如果在求解的过程中 Hessian 出现非正定，可以采用 Trust region 方法或者Levenberg-Marquardt 方法^{8, 9}。Levenberg-Marquardt 方法是一种在梯度下降方 法和牛顿方法之间进行自适应的方法，即如果代价下降，减小阻尼系数使得算法趋向于牛顿方法；否则提高阻尼系数使算法趋向于 梯度方法。如果将函数的优化过程看成是动力学系统的镇定控制过程，Levernberg-Marquardt 方法非常类似于变增益控制，两者之间 存在很多有趣的联系值得我们去探索。

我们首先介绍了轨迹优化的定义，分类及与最优控制之间的关系。然后介绍了最优控制中一种全局方法（动态规划方法）和局部方法 （微分动态规划法）。最后介绍了轨迹优化在实际应用中会遇到的问题和解决方法。在下一章，我们将介绍迹优化的概率解释，从而 建立起轨迹优化与机器学习之间的联系。