最优控制理论七、关于数值求解算法的总结及软件分享

对很多最优控制问题而言，最难的步骤是数值求解，而不是推导必要条件。所以尽管自从17世纪起新理论层出不穷，然而它的应用却没达到应有的效果，直到上世纪80年代以后计算机流行起来后，理论和应用才焕发了活力。由于我处理的都是轨迹优化（trajectory optimization）问题，而它基本代表了最典型的一类最优控制问题的需要，所以就从我的视角回答一下问题。

文章目录

轨迹优化问题描述
各种求解思路概述
最优控制求解包分享
reference

轨迹优化问题描述

求解从x0x_0x0到xfx_fxf的转移轨迹x(t)x(t)x(t)，使其服从动力学约束x˙=f[(x(t),u(t),t]\dot x=f[(x(t),u(t),t]x˙=f[(x(t),u(t),t]，其中u(t)u(t)u(t)为所要求的控制曲线（control profile）。在满足路径约束S(x(t),t)≤0S(x(t),t)\leq0S(x(t),t)≤0，控制约束C(u(t),t)≤0C(u(t),t)\leq0C(u(t),t)≤0，端点约束ψ(x(t0),x(tf),tf)=0\psi(x(t_0),x(t_f),t_f)=0ψ(x(t0),x(tf),tf)=0的情况下，使性能指标J=φ[x(tf),tf]+∫totfL[x(t),u(t),t]dtJ=\varphi\left[x\left(t_{f}\right), t_{f}\right]+\int_{t_{o}}^{t_{f}} L[x(t), u(t), t] d tJ=φ[x(tf),tf]+∫totfL[x(t),u(t),t]dt最小. 问题描述如下（取自某OCP求解软件的user guide）：
min⁡u,p,TJ(u,p,T;x0)=V(x(T),p,T)+∫0Tl(x(t),u(t),p,t)dts.t.Mx˙(t)=f(x(t),u(t),p,t0+t),x(t0)=x0g(x(t),u(t),p,t)=0,gT(x(T),p,T)=0h(x(t),u(t),p,t)≤0,hT(x(T),p,T)≤0u(t)∈[umin⁡,umax⁡]p∈[pmin⁡,pmax⁡],T∈[Tmin⁡,Tmax⁡]\begin{equation} \begin{array}{cl} \min _{u, \boldsymbol{p}, T} & J\left(\boldsymbol{u}, \boldsymbol{p}, T ; \boldsymbol{x}_0\right)=V(\boldsymbol{x}(T), \boldsymbol{p}, T)+\int_0^T l(\boldsymbol{x}(t), \boldsymbol{u}(t), \boldsymbol{p}, t) \mathrm{d} t \\ \mathrm{s.t.} & \boldsymbol{M} \dot{\boldsymbol{x}}(t)=\boldsymbol{f}\left(\boldsymbol{x}(t), \boldsymbol{u}(t), \boldsymbol{p}, t_0+t\right), \quad \boldsymbol{x}\left(t_0\right)=\boldsymbol{x}_0 \\ & \boldsymbol{g}(\boldsymbol{x}(t), \boldsymbol{u}(t), \boldsymbol{p}, t)=\mathbf{0}, \quad \boldsymbol{g}_T(\boldsymbol{x}(T), \boldsymbol{p}, T)=\mathbf{0} \\ & \boldsymbol{h}(\boldsymbol{x}(t), \boldsymbol{u}(t), \boldsymbol{p}, t) \leq \mathbf{0}, \quad \boldsymbol{h}_T(\boldsymbol{x}(T), \boldsymbol{p}, T) \leq \mathbf{0} \\ & \boldsymbol{u}(t) \in\left[\boldsymbol{u}_{\min }, \boldsymbol{u}_{\max }\right] \\ & \boldsymbol{p} \in\left[\boldsymbol{p}_{\min }, \boldsymbol{p}_{\max }\right], \quad T \in\left[T_{\min }, T_{\max }\right] \end{array} \end{equation} minu,p,Ts.t.J(u,p,T;x0)=V(x(T),p,T)+∫0Tl(x(t),u(t),p,t)dtMx˙(t)=f(x(t),u(t),p,t0+t),x(t0)=x0g(x(t),u(t),p,t)=0,gT(x(T),p,T)=0h(x(t),u(t),p,t)≤0,hT(x(T),p,T)≤0u(t)∈[umin,umax]p∈[pmin,pmax],T∈[Tmin,Tmax]
或

最典型的问题是最速下降曲线问题：

定义状态变量为x=[x,y,v]T\mathbf x=[x,y,v]^\mathrm Tx=[x,y,v]T，已知
x(t0)=[0,0,0]T,x(tf)=[2,2,?]T\mathbf x(t_0)=[0,0,0]^\mathrm T,\ \mathbf x(t_f)=[2,2,?]^\mathrm T x(t0)=[0,0,0]T, x(tf)=[2,2,?]T

服从动力学约束{x˙=vcos⁡θy˙=vsin⁡θv˙=gsin⁡θ\left\{\begin{array}{l} \dot{x}=v\cos\theta\\ \dot{y}=v \sin \theta \\ \dot{v}=g \sin \theta \end{array}\right.⎩⎨⎧x˙=vcosθy˙=vsinθv˙=gsinθ

求最优θ(t)\theta(t)θ(t)使转移时间J=∫t0tf1dtJ=\int_{t_{0}}^{t_{f}}1 d tJ=∫t0tf1dt最小。它只有终端约束ψ(x(tf))=[xf−2,yf−2]=0\psi(\mathbf x(t_f))=[x_f-2,y_f-2]=0ψ(x(tf))=[xf−2,yf−2]=0.

各种求解思路概述

第一种思路，最经典的求解方法，会用到较多的推导，以及或多或少的数值计算。

间接法参数化法，

间接法通过解析推导极大值原理的必要条件将问题转化为边值问题，一般而言，无约束时转化为Hamilton系统的两点边值问题，有约束时为多点边值问题。特殊情况下，可以进一步转化成求根问题。当然大多数情况下，需要求解边值问题，而它无法解析求解。
我在这里把求解HJB方程的动态规划法也归为这一类，因为要用到大量的解析推导。

第二种思路是比较通用的方法，打靶法[1]或转化法（transcription），大部分难点在于数值计算。分为

2.直接单次打靶法
3.间接打靶法

由于单次打靶法收敛性较差，在离散化的每个区间进行打靶，就是：

4.多重打靶法

是把一组ODE初值问题转化成多组ODE初值问题，ODE的连续性要求转化（transcription）成多段离散状态的等式约束。

这3个方法的核心思想是在离散点上逐步求解ODE方程初值问题，把无穷维连续时间系统控制转化为有限维度的非线性规划问题，再去优化有限个参数点上的决策变量。如果只离散控制变量，直接法和间接法的决策变量都是分段常值函数 ui,i=0,1,⋯Nu_i,i=0,1,\cdots Nui,i=0,1,⋯N ；如果离散控制和状态，直接法的决策变量是 xi,ui,i=0,1,⋯Nx_i,u_i,i=0,1,\cdots Nxi,ui,i=0,1,⋯N，间接离散化的决策变量是 xi,λi,ui,i=0,1,⋯Nx_i,\lambda_i,u_i,i=0,1,\cdots Nxi,λi,ui,i=0,1,⋯N. 这两种方法的约束处理程度不同，计算复杂程度也不同。

利用打靶法的思想，只在有限时域内考虑离散系统的参数优化，称为：

5.模型预测控制

上面4个求解方法都是要求解非线性规划问题，直接法可处理路径约束和控制约束，所以它仍然是目前用的很多的方法。求解离散决策变量的方法有很多，如梯度法、智能优化算法、凸优化等，有的已经和前面的形式发生了较大的变化，但是我仍然把它们归为这种思路。

第三种思路不同于离散化 x(t),u(t)x(t),u(t)x(t),u(t)，而是用有限个点上对ODE方程的解做函数逼近，然后要求导数等于动力学方程的右端项，称为配点法（collocation）。采用连续函数的形式不同，有：

5.正交多项式配点（orthogonal collocation）
6.伪谱法（pseudospectral），也就是全局正交多项式配点

这种思路可无限逼近最优解，计算量比转化法效率更高，是目前研究和应用比较多的新方法。

在连续时间最优控制问题转化（transcription）为离散时间非线性规划问题（NLP）前后，前者的协态变量和后者优化问题计算当中所用到的乘子有着紧密的相关性，这个联系称为covectormappingprinciple.\textit{covector mapping principle.}covector mapping principle.

上面这些方法都是很经典的，下面是一些我也不太理解的新方法。

第四种思路是融合了间接法和直接法的方法，当然它的求解仍然是用的前面的方法。有兴趣的可以自己去看Ross[2]的Carathéodory–π\piπ最优反馈解。

第五种思路是微分动态规划（Differential dynamic programming），在初始轨迹附近做二次型最优控制，然后迭代，最终会收敛到局部最优解。

第六种思路是机器学习。

最后再指出，我上面讲述的内容大多数都是解决的开环最优控制问题，由于所处理的系统动力学特性更复杂，所以这些方法应当可以适用于小车、机器人的运动规划。如果对后者，也就是实时在线的路径规划感兴趣，可以参考：

知乎文章 - 路径规划算法总结

最优控制求解包分享

目前最优控制已经发展得很成熟了，有一大批可用的软件包，用于深空探测轨道优化、航天器大气进入弹道优化、航空器航线规划、工业机器人运动规划。这些问题是最优控制最经典的且最常见的，即包含复杂的非线性动力学方程、端点集合约束、路径不等式约束、路径等式约束等，目前几乎都被解决了。我们不用进行很多编程，也不需要自己造轮子，就可以求解很复杂的问题，下面列举近一段时间我收集到的最优控制求解包。需要的同学可以自己去官网下载，当然我也不是都用过，实际体验大家自己检验。加粗的是我看描述，比较推荐的。

PSOPT：基于C++开发，采用伪谱法的开源软件包。应用于巴西小行星探测任务Aster 的最优轨迹规划。伪谱方法通过使用全局多项式（例如勒让德或切比雪夫函数）逼近时间相关变量来解决最优控制问题。局部离散化方法使用局部样条逼近时间相关函数，并且可以看作是隐式龙格-库塔积分器的实现。使用全局和局部方法，与问题相关的微分方程、连续约束和积分都在节点网格上离散化。然后使用稀疏非线性规划来寻找局部最优解。 GitHub/psopt
GPOPS-II: Next-Generation Optimal Control Software：基于MATLAB的成熟的软件包，非开源，应用广泛。解决一般非线性最优控制问题（即需要优化由微分代数方程定义的系统的问题）。 GPOPS-II 实现了一类新的可变阶高斯正交方法（伪谱法），其中连续时间最优控制问题，自适应网格划分近似为稀疏非线性规划问题 (NLP)，然后使用 NLP 求解器 IPOPT 或 NLP 求解器 SNOPT 求解该 NLP。多阶段最优控制问题的定义很方便，求解也很形式化。
ICLOCS-Your One-Stop-Shop Solution for Optimization Based Control in Matlab/Simulink ：伦敦帝国理工大学，基于MATLAB的开源的求解软件。将最优控制问题转化为非线性规划 (NLP) 问题。当前版本的 ICLOCS2 支持使用 IPOPT、fmincon 和 WORHP 来解决 NLP 问题。可选采用SUNDIALS进行ODE/DAE求解，支持采用Adigator进行自动微分以求解Jacobian矩阵和Hessian矩阵，可以与simulink配合完成闭环系统仿真。GitHub/ICLOCS
OpenOCL：基于MATLAB的轻量化最优控制求解包。OpenOCL 可以解决非线性、连续时间、多阶段和约束最优控制问题，以及非线性模型预测控制问题。GitHub/OpenOCL
Yop：基于MATLAB类函数、轻量化的最优控制求解包，基于CasADi的自动微分加速优化。几个学生开发的。
DIDO - the first Optimal control software：MATLAB工具箱，NASA两位大佬Ross 和 Fahroo 在2001年开发的，是第一个通用的面向对象的优化控制软件、第一个伪谱法最优控制软件、第一个经过飞行验证的通用优化控制软件等。它目前仍然广泛使用，优势是无需初始猜测并且可以避免陷于局部最优解、可自动输出哈密顿函数值、协态、路径约束以及端点约束的拉格朗日乘子。DIDO 已被用于解决航空航天、机器人、无人系统、生物技术等领域的工业问题。
ACADO : Toolkit for Automatic Control and Dynamic Optimization：开源，功能强大。基于C++ 代码实现，并带有用户友好的 MATLAB 接口。用于自动控制和动态优化的软件环境和算法集合。它为使用多种算法进行直接优化控制提供了一个通用框架，包括标准最优控制问题、多目标优化、模型预测控制、状态和参数估计以及鲁棒优化。用户手册及其实例，GitHub/acado。Moritz Diehl 后续开发了一个多平台Python，MATLAB版的包，可以解决DAE描述的多种复杂约束的最优控制与估计问题。
PROPT - Matlab Optimal Control Software (DAE, ODE)：基于MATLAB的早期的最优控制求解软件，需要申购。ESA用来进行轨道优化并在GTOC4获得排名。离散方法是伪谱法，处理方式是DAE而非ODE，可支持多种多项式基函数，如 Gauss or Chebyshev-point collocation 。采用解析方法计算偏导，计算较快。由于开发时间较长，用例完善，包括飞行力学、化工、机械、不可微分系统、动态系统的参数估计、奇异最优控制等实例。
[Sparse Optimal Control Software (SOCS)]：波音公司Betts, J.T开发，轨迹优化领域的泰斗。利用稀疏线性代数技术来解决非常大的优化问题，可以在台式计算机上有效地解决具有超过 100,000 个变量和约束的应用程序。
ASTOS (Aerospace Trajectory Optimization Software)：一款拥有独立界面的航天任务分析设计软件，最初版本是用来进行轨迹优化的，现今版本包括：内置轨迹和多学科设计优化、刚柔耦合的多体动力学分析、任务分析功能、性能和系统概念分析功能、发射和再入安全分析。
ETHz-adrl / control-toolbox：开源 C++ 库，工具箱，面向机器人系统、偏向模型预测控制，求解实时性很强。现在在GitHub上搜索最优控制得到的就是它，未来最优控制的发展方向就是它了。支持非线性模型预测控制、多刚体动力学建模与机器人轨迹规划，求解器的特点是逆运动学求解器，采用自动微分求导，支持代码生成。已用于飞行、行走和地面机器人的硬件和模拟控制任务。 ETH的ANYmal机器人已经在超过了波士顿动力的SpotMini，他们的代码应该功不可没。
……

reference

[1]. Betts J T. Survey of numerical methods for trajectory optimization[J]. Journal of Guidance, Control, and Dynamics, 1998, 21(2): 193-207.
[2] Ross I M, Sekhavat P, Fleming A, et al. Optimal feedback control: Foundations, examples, and experimental results for a new approach[J]. Journal of Guidance, Control, and Dynamics, 2008, 31(2): 307-321.
[3]. https://en.wikipedia.org/wiki/Trajectory_optimization
[4] 图示来自： Shirazi A, Ceberio J, Lozano J A. Spacecraft trajectory optimization: A review of models, objectives, approaches and solutions[J]. Progress in Aerospace Sciences, 2018, 102: 76-98.
[5] 罗亚中,孙振江,乔栋.航天动力学软件发展评述[J].力学与实践,2017,39(06):549-560.