400-123-4657

基于最优控制的轨迹优化方法通常可以分为三类：

该方法将轨迹优化问题转化为一个非线性规划问题（NLP）或者一个多项式优化问题，这个过程常称为transcription。通过对目标函数和约束条件进行适当的转化，然后使用标准的数值优化算法来求解问题。直接方法的优点是求解速度快，可以适用于高维问题，同时也能够处理非线性约束。缺点是当系统动力学比较复杂时，直接方法可能会受到局部最优解的影响，从而导致收敛困难。

直接方法常见的有以下几种：

1. 伪谱方法（Pseudospectral Method）：将状态和控制输入离散化为一组节点，在节点处近似表示动态方程和边界条件，并将最优控制问题转化为非线性规划问题。伪谱方法具有高精度和高效率的优点，被广泛应用于飞行器、机器人等领域。
2. 直接单步法（Direct Single Shooting Method）：将整个时间段划分为若干个小时间段，将控制输入视为自变量，状态变量视为因变量，通过求解微分方程来获得状态轨迹，最终将最优控制问题转化为一个非线性规划问题。直接单步法计算效率高，但是精度较低。
3. 直接多步法（Direct Multiple Shooting Method）：将整个时间段划分为若干个小时间段，在每个时间段内都将状态方程视为一组非线性方程，通过求解微分方程来获得状态轨迹，最终将最优控制问题转化为一个非线性规划问题。直接多步法计算效率和精度均比直接单步法高。

直接法中，常用的是shooting法（single shooting，multiple shooting）和collocation这种伪谱方法，其中，single shooting只对控制进行离散化，multiple shooting将轨迹分成多个段，并在段之间的连接点处施加等式约束以确保连续性。在collocation法中，控制和状态均表示为分段多项式(piecewise polynomials)。

通过对整个时间区间进行离散化，将系统状态的演化过程分为若干个子区间，在每个子区间内确定控制函数的值和状态的演化，最终求解出整个时间区间内的最优控制函数和状态演化轨迹。其数学表达式如下：

其中， 为系统状态， 为控制函数， 为性能指标， 为状态和控制的代价函数， 为终端代价函数。

将时间区间分割成若干个子区间，对每个子区间分别进行状态和控制的离散，通过建立各子区间之间的连续性条件来求解最优控制函数和状态演化轨迹。multiple shooting相比single shooting的优点在于，可以减少状态演化(evolution)过程的误差积累，提高求解的精度。

假设机器人的状态向量为 ，控制向量为 ，状态方程为

将轨迹分成多个段如:N，每个段中的状态和控制变量表示为 和 ，则代价函数为

并在相邻时间段的节点上添加等式约束来确保轨迹的连续性：

这种方法是一种将控制和状态表示为分段多项式函数的方法，通过在每个区间内选择一些离散点（称为collocation点）来近似表示这些函数，并将微分方程约束转换为代数约束。

以自动驾驶为例，假设我们希望将车辆从起点移动到终点，并使其满足一些约束条件，例如最大速度、最小转弯半径、最大加速度和最大刹车力等。我们可以将该问题转换为一个最优控制问题，其中我们需要确定车辆在每个时间点上的最佳控制输入（例如，油门、刹车和转向角度），以最小化到达终点所需的总时间和燃料消耗。

为了将这个问题转化为NLP问题，我们可以采用Collocation方法，将车辆状态和控制输入表示为分段多项式函数，例如五次多项式。然后，在每个区间内选择一些collocation点，用这些点来近似表示函数。此外，我们还需要将微分方程约束转换为代数约束，以确保解在每个区间内满足微分方程。这些代数约束通常采用拉格朗日乘数法或者伪谱方法求解。

在优化过程中，我们需要施加约束来确保多项式满足系统动态并避免障碍物。这些约束包括：

• 初始状态和终止状态：车辆的起点和终点必须满足给定的约束条件；
• 多项式的连续性：在相邻的时间段内，车辆的位置、速度和加速度必须连续；
• 动态约束：车辆的速度和加速度必须满足动力学模型的限制；
• 障碍物约束：车辆的轨迹必须避开障碍物。

通过这种方法，我们可以将最优控制问题转换为一个非线性规划问题，其中控制和状态变量都是变量。我们可以采用一些优化算法，例如SQP算法或IPOPT算法，来解决这个问题，并得到车辆在每个时间点上的最佳控制输入，以及车辆行驶的最佳轨迹。

假设我们有以下微分方程：

其中 是状态向量， 是控制向量， 是系统动态函数。我们希望将该问题转化为一个最优控制问题，以最小化某个性能指标 ，如总消耗的燃料或到达终点所需的时间。

为了在NLP中求解该问题，我们将状态向量 和控制向量 表示为分段多项式函数，例如五次多项式。然后，我们在每个区间内选择一些collocation点，用这些点来近似表示函数。

假设我们将时间区间 划分为 个子区间 ，每个子区间内选择 个collocation点。我们可以表示状态和控制函数为：

其中， 是五次Lagrange插值多项式的第 项。

现在，我们需要将微分方程约束转换为代数约束。我们可以使用Gauss-Lobatto collocation点，将微分方程在每个collocation点上近似表示为：

其中， 是Gauss-Lobatto collocation点，满足 ，且 。这样，我们可以得到以下 个代数约束：

其中， ， 。这些代数约束要求函数在collocation点上满足微分方程。

此外，我们还需要一些边界条件来约束状态和控制函数的值。常见的边界条件包括：

• 初始状态
• 终端状态
• 初始控制
• 终端控制

最终，我们可以将最优控制问题表示为以下NLP形式：

其中， 分别是给定的初始和终端状态/控制。求解该NLP，可以得到满足微分方程和边界条件的最优状态和控制函数，以最小化性能指标 。

该方法则将轨迹优化问题转化为一个边值问题（两个端点状态已知），然后通过使用变分法将该问题转化为一个微分方程组，最后使用数值方法求解微分方程组得到最优轨迹。与直接方法相比，间接方法的优点是能够保证全局最优解，同时对于特定的系统动力学模型，该方法能够得到系统的解析解。缺点是该方法求解速度比直接方法慢，同时需要较高的数学功底和计算能力。

常见的间接方法有以下几种：

1. 庞特里亚金极小值原理（Pontryagin's Minimum Principle）：将最优控制问题转化为极小值问题，求解得到极小值控制量和状态轨迹。此定理一开始的名称是庞特里亚金最大化原理（Pontryagin's maximum principle），其证明也是以控制哈密顿量最大化为基础。但是，由于在实际应用中，通常需要最小化性能指标，因此人们也将其称为"Pontryagin's minimum principle"。在庞特里亚金的著作中，他解决了许多最小化性能指标的问题，这些问题涵盖了广泛的应用领域，如火箭制导、经济控制等。但当系统的状态空间很大或者非线性时，使用庞特里亚金极小值原理的效果可能会变得不那么好。此外，该方法的求解速度相对较慢，需要高水平的数学和计算能力，因为它需要解决一组微分方程以及运用最优性条件等。
2. 变分法：它用于求解泛函问题。泛函是一个函数集合，其中每个函数映射一个函数到实数。变分法基于寻找泛函的极值，从而导出微分方程的解。它可以将最优控制问题转化为求解变分问题，从而可以简化问题的求解过程，并且可以处理连续和离散的最优控制问题。变分法还具有比较高的准确性和精度。 然而，变分法求解过程中需要解决一组微分方程或者偏微分方程，这个过程可能比较复杂，需要高水平的数学和计算能力。另外，变分法通常需要采用数值方法来求解，求解时间可能会比较长。
3. 欧拉-拉格朗日微分方程：一种基于变分法的优化方法，它将系统的能量泛函通过欧拉-拉格朗日方程变换成等效的微分方程组，然后通过求解这个方程组来得到系统的控制量和状态轨迹。它可以处理非线性问题，并且可以通过求解一组常微分方程来得到系统的控制量和状态轨迹。此外，欧拉-拉格朗日方法还可以处理多目标优化问题。但是，欧拉-拉格朗日方法的缺点是它需要计算高阶导数，因此在处理高阶微分方程和奇异点时会变得复杂和困难。

PMP 的核心思想是，在任意时刻 ，存在一种最优控制策略 和相应的状态轨迹 ，使得在这种控制策略下，性能指标（通常是一个积分函数）达到最小值，同时满足系统动力学方程和控制输入的限制条件。

更具体地说，PMP 可以表示为以下条件：

(1) 存在一个 co-state 变量 ，使得在最优控制策略下，系统的 Hamiltonian 函数 取得最小值：

(2) 通过计算 Hamiltonian 函数 对于控制变量 的偏导数 ，可以得到最优控制输入 ：

(3) 在最优控制策略下，状态和 co-state 变量满足动态系统方程和边界条件：

其中， 和是起点和终点状态。

动态规划（Dynamic Programming）：将最优控制问题转化为一个动态规划问题，将问题分解为若干个子问题，通过反向递推得到最优控制量和状态轨迹。动态规划方法的核心是Bellman最优性原理，即最优策略的子策略也是最优的。通过对状态空间和控制空间进行离散化，将问题转化为一个离散的动态规划问题。在求解过程中，需要反向递推求解值函数或者控制策略，以及状态轨迹。动态规划方法适用于状态空间离散化的问题，但是在高维状态空间和复杂约束条件的问题上，由于状态空间的维度增加，所需存储空间和计算量也增加，因此求解难度增大。

ILQR (Iterative Linear Quadratic Regulator) 是其中一种代表方法，它是一种基于LQR的迭代优化方法，通过近似非线性系统的动力学模型，将原始优化问题转化为线性二次型问题，然后进行迭代求解。ILQR通过反复线性化系统动力学方程，并重新求解控制输入和状态变量，最终收敛于最优轨迹。近年来，ILQR也被广泛应用于自动驾驶领域的轨迹优化。

具体数学原理可以参考CMU:10703 Deep Reinforcement Learning and Control课程中关于ILQR轨迹优化的章节：

ILQR_trajectoryoptimization

进一步考虑和环境障碍物交互的Tomizuka 老师组里的经典工作 ： UCB的Constrained Iterative LQR

基于最优控制的轨迹优化方法是一个广泛的领域，目前有许多有价值的学术研究方向。以下是简单列举其中几个当前热门且有价值的方向：

1. 非线性和高维系统优化：传统的间接法被认为只适用于线性问题和低维系统，但近年来已有许多工作探索如何将这些方法推广到非线性和高维系统中，例如使用数值方法求解欧拉-拉格朗日方程或采用高阶近似方法。
2. 基于深度学习的轨迹优化：深度学习在机器学习领域的广泛应用激发了许多研究人员探索如何将其应用于最优控制问题中。一些研究尝试使用深度神经网络学习控制策略或值函数，从而提高轨迹优化的效果。
3. 多智能体系统的轨迹优化：随着多智能体系统的不断发展，如何对多个智能体的轨迹进行优化成为一个热门研究方向。这需要将最优控制问题扩展到多个智能体的情况下，并考虑智能体之间的相互作用和约束条件。
4. 实时轨迹优化：在许多实际应用中，轨迹优化需要在实时环境中进行。因此，如何实现快速、准确的实时轨迹优化成为一个重要的研究方向。一些研究者提出了基于模型预测控制的方法，以及使用快速数值优化技术（如L-BFGS和ADMM）等方法。

例如：23年IEEE TITS发表的一篇基于Dual Consensus ADMM的多智能体轨迹协同优化的论文：

以上只是目前热门的几个方向，还有许多其他的研究方向，例如轨迹优化中的鲁棒性、非凸优化问题、嵌入式系统中的轨迹优化等。直接法和间接法各有优劣，需要根据具体问题选择合适的方法。

另一个当前在学界很热门的研究方向：Safe learning in robotics: From learning-based control(安全学习控制) to safe reinforcement learning- 小先生的文章 - 知乎

参考文献：

[1]J. T. Betts, Practical Methods for Optimal Control and Estimation Using Nonlinear Programming, 2nd ed. Philadelpia, PA: SIAM, 2010.

[2]F. Borrelli, A. Bemporad, and M. Morari, Predictive Control for Linear and Hybrid Systems. New York, NY, USA: Cambridge University Press, 2017.

[3]Xu W, Wang Q, Dolan J M. Autonomous vehicle motion planning via recurrent spline optimization[C]//2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021: 7730-7736.

[4]Fuller, A. T. Bibliography of Pontryagin's maximum principle. J. Electronics & Control. 1963, 15 (5): 513–517.

[5]L.S. Pontryagin, V.G. Boltyanski, R.V. Gamkrelidze, and E.F. Miscenko. The Mathematical Theory of Optimal Processes. Wiley, Chichester, 1962.

[6]Z. Huang, S. Shen, and J. Ma, “Decentralized iLQR for cooperative trajectory planning of connected autonomous vehicles via dual consensus ADMM,” IEEE Transactions on Intelligent Transportation Systems, 2023