轨迹优化与直接配点法

本文主要讨论轨迹优化问题中的直接配点法，希望通过此教程帮助读者理解轨迹优化问题的基本概念与直接配点法的相关理论基础，最后我们还会讨论一些具体实现问题。直接配点法最终是通过非线性规划求解器来得到最终结果，因此在阅读本文之间可以先了解NLP问题相关基础知识，当然这并不影响对本文的理解，本文仅需要一些简单的高等数学与微分方程的知识。

轨迹优化问题

问题简介

轨迹用来描述一个物体的运动过程，通常是关于时间的变量。轨迹优化一种用于寻找最佳轨迹选择的方法，通常是通过选择合适的系统输入或控制量，是系统完成期望的运动过程。在控制领域，轨迹优化近似于最优控制，但从更广的概念上来讲，轨迹优化更具一般性。

下面我们用一个例子来进一步说明轨迹优化问题。

如图所示，一个滑块放置在光滑地面上，并受到一个水平方向的作用力。我们想要滑块在里的作用下，在1s的时间内从开始位置运动到指定位置，并刚好停在该处。这个问题存在着无数种可能的运动轨迹，进一步，我们希望从中确定一条最优的轨迹，如图所示。

我们可以把这个问题用数学语言形式化。系统满足如下的动力学方程：
$$\dot{x}=\nu ,\dot{\nu }=u$$
并要求满足如下的边界约束：
$$\begin{array}{ll}x(0)=0,& x(1)=1\\ \nu (0)=0,& \nu (1)=0\end{array}$$
轨迹优化关心在一定指标下的最优轨迹，这样的指标可以用如下的目标函数来描述：
$$\underset{u(t),x(t),\nu (t)}{\min }{\int }_0^1{u}^2(\tau )d\tau$$

形式化描述

一般来说，轨迹优化问题通过在一定的约束条件下优化如下的目标函数来得到最优轨迹：
$$\underset{t_0,t_F,\mathbf{x}(t),\mathbf{u}(t)}{\min }\underset{\text{Mayer Term }}{\underbrace{J\left(t_0,t_F,\mathbf{x}\left(t_0\right),\mathbf{x}\left(t_F\right)\right)}}+\underset{\text{Lagrange Term }}{\underbrace{{\int }_{t_0}^{t_F}w(\tau ,\mathbf{x}(\tau ),\mathbf{u}(\tau ))d\tau }}$$
其中的约束条件包含如下形式：
系统的动力学约束：
$$\dot{\mathbf{x}}(t)=\mathbf{f}(t,\mathbf{x}(t),\mathbf{u}(t))$$
路径约束：
$$\mathbf{h}(t,\mathbf{x}(t),\mathbf{u}(t))\le 0$$
边界约束：
$$\mathbf{g}\left(t_0,t_F,\mathbf{x}\left(t_0\right),\mathbf{x}\left(t_F\right)\right)\le 0$$
和状态量与控制量边界：
$$\begin{array}{l}{\mathbf{x}}_{\mathrm{l}\mathrm{o}\mathrm{w}}\le \mathbf{x}(t)\le {\mathbf{x}}_{\mathrm{u}\mathrm{p}\mathrm{p}}\\ {\mathbf{u}}_{\mathrm{l}\mathrm{o}\mathrm{w}}\le \mathbf{u}(t)\le {\mathbf{u}}_{\mathrm{u}\mathrm{p}\mathrm{p}}\end{array}$$

由于最终我们想要求得的是控制量关于时间的具体函数形式，而非一个值或一个参数，因此轨迹优化问题也可以理解为在一定约束或大量约束下的泛函问题。

变分法求解

（对变分法不熟悉的可以跳过本部分）
针对泛函问题，可以采用变分法进行求解。以滑块移动的问题为例，由于其目标函数可以表示为如下形式：
J=∫01u2(τ)dτ=∫01x¨2(τ)dτJ=\int_{0}^{1} u^{2}(\tau) d \tau=\int_{0}^{1} \ddot{x}^{2}(\tau) d \tauJ=∫01​u2(τ)dτ=∫01​x¨