mpc local planner 模型预测控制的主要思想-CSDN博客

本文探讨了时间最优的MPC算法，包括非均匀和均匀网格版本，以及超图在表示系统耦合关系中的应用。文章详细解释了边界值问题、初始值问题和数值分析的概念，以及打靶法和稀疏矩阵在MPC中的作用。此外，还介绍了正则化在优化中的运用，以及MPC中的闭环控制和超图在优化问题中的表示方法。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

参考

control_box_rst 文档

Uniform Grid Time-Optimal MPC 论文

Non-Uniform Grid Time-Optimal MPC 论文

Stand MPC and Hypergraph 论文

Time-Optimal MPC and Hypergraph 论文

数学概念来自 wiki

Mpc local planner 相关算法

其实在 README.md 文件中已经写了的：

Standard MPC and Hypergraph，标准 MPC 和超图

C. Rösmann, M. Krämer, A. Makarow, F. Hoffmann und T. Bertram: Exploiting Sparse Structures in Nonlinear Model Predictive Control with Hypergraphs, IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM), New Zealand, July 2018.

Time-Optimal MPC and Hypergraph，时间最优 MPC 和超图

C. Rösmann: Time-optimal nonlinear model predictive control, Direct transcription methods with variable discretization and structural sparsity exploitation. Dissertation, Technische Universität Dortmund, Oct. 2019.

Uniform Grid Time-Optimal MPC，统一网格时间最优 MPC

C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, European Control Conference (ECC), Austria, July 2015.

C. Rösmann, F. Hoffman und T. Bertram: Convergence Analysis of Time-Optimal Model Predictive Control under Limited Computational Resources, European Control Conference (ECC), Denmark, June 2016.

Non-Uniform Grid Time-Optimal MPC，非均匀网格时间最优 MPC

C. Rösmann, A. Makarow, F. Hoffmann und T. Bertram: Sparse Shooting at Adaptive Temporal Resolution for Time-Optimal Model Predictive Control, IEEE Conference on Decision and Control (CDC), Australia, December 2017.

C. Rösmann, A. Makarow, F. Hoffmann und T. Bertram: Time-Optimal Nonlinear Model Predictive Control with Minimal Control Interventions, IEEE Conference on Control Technology and Applications (CCTA), Hawai'i, August 2017.

数学概念

超图 Hypergrapher

图就是顶点(元素) vertex 和边(元素间的关系) edge 组成。

超图是图的概括，其中边可以连接任意数量的顶点。

在普通图中，边恰好连接两个顶点，一般用连线表示，例如图 fig.1。

在超图中，由于边可以连接多个顶点，可以表示耦合的关系，一般用曲线把顶点圈起来，例如图 fig.2。

边界值问题 Boundary value problem 和初值问题 Initial value problem

边界值问题的条件指定在方程中自变量的极值（“边界”），边界值是系统指定的最小或最大输入情况下、系统内部或者系统输出相对应的数据值。

e.g.

在“时间 - 位移”系统 s=f(t) ，t ∈[0, 5] 中，有系统属性例如固定的加速度 a = 1 和初始速度 v = 0，系统输出位移 s。

边界值问题就是在 t=0 和 t=5 条件下的系统属性 a，v，s 数值问题。

而初始值问题的所有条件指定在自变量值不变的情况下，并且该值位于下边界，因此称为“初始”值。初值问题经常在微分中出现。

e.g.

在“时间 - 位移”系统 s=f(t) ，t ∈[0, 5] 中，有系统属性例如变化的加速度 a 和初始速度 v = 0，系统输出位移 s。

初值问题是系统在 t=0 时刻，系统属性 a，v，s 的数值问题。

由于加速度 a 是变化的，对系统的观察一般是在极端时间区间 [t0, t0+ε] 内，认为在 ε 时间内，系统状态完全处于 t0 时刻的状态。

数值分析 Numerical analysis

研究连续问题算法的科学。目的是将连续无穷维的问题离散化, 得到一个离散的有限维的可解问题, 进而得到近似解。

例如求一个函数的积分是一个连续模型的问题，若将其离散化变成数值积分，就变成将上述面积用许多较简单的形状（如长方形、梯形）近似，因此只要求出这些形状的面积再相加即可。

打靶法 Shooting method

在数值分析中，打靶法是一种通过将边值问题简化为初值问题来求解的方法。它涉及在不同的初始条件下寻找初值问题的解，直到找到满足边值问题的边界条件的解。通俗地说，就是从一个边界向不同方向“射出”轨迹，直到找到“击中”另一边界条件的轨迹。

e.g.

直线运动中，系统初始位移为 0 （边界其一），通过假设速度 v = [1, 2, 3, ..., 100] 等不同的速度（不同条件/不同方向打靶），使得系统在第 5 秒位移为 56 （边界其二）。这个系统中，加速度是不确定的，为了简化问题，把系统直线运动的这 5 秒中的全部条件认为同初始条件一致（边界问题转为初值问题），求解出初始条件在不同速度，不同加速度情况下，系统在 5 秒内位移 56 的解。

控制优化问题 OCP (optimal control problem)

非线性规划 NLP (Nonlinear Programming)

非线性规划，求解具有非线性约束的优化问题

最优化问题

损失函数或成本函数是指一种将一个事件（在一个样本空间中的一个元素）映射到一个表达与其事件相关的经济成本或机会成本的实数上的一种函数，借此直观表示的一些"成本"与事件的关联。一个最佳化问题的目标是将损失函数最小化。一个目标函数通常为一个损失函数的本身或者为其负值。当一个目标函数为损失函数的负值时，目标函数的值寻求最大化。

有限差分 Finite difference

有限差分是 f (x + b) − f (x + a) 形式的数学表达式。如果有限差分除以 b − a，就会得到差商，也就是在区域 [a, b] 内的导数。

稀疏矩阵 Sparse matrix 和稠密矩阵 Dense matrix

在数值分析和科学计算中，稀疏矩阵或稀疏数组是大多数元素为零的矩阵。对于稀疏矩阵的零值元素比例没有严格的定义，但一个常见的标准是非零元素的数量大致等于行数或列数。相反，如果大多数元素非零，则矩阵被认为是稠密的。

从概念上讲，稀疏矩阵对应于几乎没有成对相互作用的系统。而稠密矩阵则反映了系统内复杂的相互作用。

在计算机上存储和操作稀疏矩阵时，如果使用标准稠密矩阵结构和算法的运算速度缓慢且效率低下，因为处理和内存都浪费在零上。稀疏数据本质上更容易压缩，因此需要的存储量要少得多。

e.g.

以球 A，球 B 为一个系统，AB 互不相关。球 A 以恒加速度 aA，初始速度 vA 运动，球 B 以恒加速度 aB，初始速度 vB 运动，它们的位移是 sA，sB。那么这个系统有

球 A 位移 $s_A=\frac{1}{2}t^2a_A+tv_A$

球 A 速度 $v_A=ta_A+v_A$

球 B 位移 $s_B=\frac{1}{2}t^2a_B+tv_B$

球 B 速度 $v_B=ta_B+v_B$

将这个系统写为矩阵形式是

$\begin{bmatrix} s_A\\v_A\\s_B\\v_B \end{bmatrix}= \begin{bmatrix} \frac{1}{2}t^2 & t & 0 & 0 \\ t & 1 & 0 & 0 \\ 0 & 0 & \frac{1}{2}t^2 & t \\ 0 & 0 & t & 1 \end{bmatrix} \begin{bmatrix} a_A\\v_A\\a_B\\v_B \end{bmatrix}= \begin{bmatrix} k_A & 0 \\ 0 & k_B \end{bmatrix} \begin{bmatrix} a_A\\v_A\\a_B\\v_B \end{bmatrix}= K*\begin{bmatrix} a_A\\v_A\\a_B\\v_B \end{bmatrix}$

因为系统的输出 sA vA 和 aB vB 无关，sB vB 和 aA vA 无关，在矩阵 K 中表现为右上角矩阵块和左下角矩阵块是零（稀疏）矩阵。

因为系统的输出 sA vA 和 aA vA 相关，sB vB 和 aB vB 相关，在矩阵 K 中表现为左上角矩阵块和右下角矩阵块是稠密矩阵。

一个稠密矩阵块反映了这部分输入与系统输出强相关，可以重点关注，一个稀疏矩阵块反映了这部分输入与系统输出弱相关，可以降低优先级甚至忽略。

硬约束和软约束

硬约束是绝对不能协商的约束。如果条件不满足约束，就是失败了。硬约束消耗更多计算量。

软约束是可以偶尔协商的约束。一般情况下系统遵守每个软约束，但在必须牺牲某些条件的极少数情况下会对违背软约束的要求。

正则化 Regulation

正则化是指为解决适定性问题或过拟合而加入额外信息的过程。在机器学习和逆问题的优化过程中，正则项往往被加在目标函数当中。

这些额外信息其实就是限制条件，防止优化发散。

Uniform Grid Time-Optimal MPC 简述

比较抽象，没有公式。Sparse Shooting at Adaptive Temporal Resolution for Time-Optimal Model Predictive Control 论文中提供了该思想的公式。

略

Non-Uniform Grid Time-Optimal MPC 简述

统一时间网格优化控制

$\dot{x}(t)=f(x(t),u(t)),\quad x(t=0)=x_s \qquad (1)$

系统状态 $x(t)\in\mathbb{R}^p$ ，系统输入 $u(t)\in\mathbb{R}^q$ ，系统初始状态 $x_s$

公式 (1) 表示根据系统状态和输入得到下个时刻的状态增量 $\dot{x}(t)$

优化就是将系统在时间轴上离散为 n 个边界问题

$\begin{aligned} 0&=t_0\leq t_0+\Delta T_0 = t_1, \\ &=t_1\leq t_1+\Delta T_1 = t_2, \\ & \qquad \vdots \qquad \qquad \qquad \qquad \qquad \qquad \qquad(2) \\ &=t_{n-1}\leq t_{n-1}+\Delta T_{n-1} = t_n = T \end{aligned}$

$\Delta T$ 通常是均匀的，控制(系统输入)就在这些离散的时刻输入

$u(t):=u_k for t \in [t_k,t_k+\Delta T_k] \quad k \in [0,1,...,n] \qquad (3)$

同步打靶法就是在离散的时间区间分别“打靶”解决区间的初值问题，把区间记为射击节点 $s_k:=x(t_k)$ (:= 是定义符号)，区间初值问题表示为

$x(t_k+\Delta T_k,s_k,u_k)= \int_{t_k}^{t_k+\Delta T_k}{f(x(t),u_k)dt} \qquad (4)$

时间成本函数为

$\tilde{V}^*=\underset{s_k,u_k,T}{min} \int_{t=0}^{T}{1dt}= \underset{s_k,u_k,T}{min}{T} \qquad (5)$

把完成优化的离散时间区间全部加起来，使得总时间最小，就是要优化的成本函数

约束条件为：

$s_0=x_s$ ， $s_n=x_f$ ， $s_{k+1}=x(t_{k+1},s_k,u_k)$ ， $g(s_k,u_k)\geq0$ ，(k = 0,1,...n-1)

不等式约束 $g:\mathbb{R}^p\times\mathbb{R}^q\rightarrow\mathbb{R}^r$ ， $\mathbb{R}^p$ 是系统状态空间， $\mathbb{R}^q$ 是控制空间

统一时间网格把离散的时间间隔固定 $\Delta T_k:=\Delta T$ ，时间网格上的时间表示为 $T=n\Delta T$

动态时间网格优化控制

$V^*=\underset{s_k,u_k,\Delta T_k}{min} \sum_{k=0}^{n-1}{[\Delta T_k+\lambda r(\Delta T_k)]} \qquad (6)$

对比公式 (5)，系统总时间的计算由积分符号变为累加符号，因为 (5) 中的时间间隔是固定的，而 (6) 中的时间间隔是动态变化的。

约束条件为：

$s_0=x_s$ ， $s_n=x_f$ ， $0\leq \Delta T_k$ ， $s_{k+1}=x(t_{k}+\Delta T_k,s_k,u_k)$ ， $g(s_k,u_k,\Delta T_k)\geq0$ ，(k = 0,1,...n-1)

对比公式 (5)，离散区间射击节点 $s_{k+1}$ 的公式输入中时间从 $t_{k+1}$ 变为 $t_k+\Delta T_k$ ，因为动态时间网格中时间间隔不固定了。不等式约束 g 增加了时间间隔 $\Delta T_k$ 的约束。

r() 是一个正则化函数，用来调整 $\Delta T_k$ ，一般 r 可以采样平方，即 $r(\Delta T_k)=(\Delta T_k)^2$

平方具有自加权特性，即 $\Delta T_k$ 越大， $(\Delta T_k)^2$ 越大， $\Delta T_k$ 越小， $(\Delta T_k)^2$ 越小。

系数 λ 是个正数。

最小控制干涉的实时正则化

时间网格的优化算法控制序列（时间网格离散程度/射击节点数量） n 中的数量来适应网格大小和分辨率。目标是寻求控制干预数最少的网格，以实现底层时间最优和时间连续的控制信号。

在优化中，需要测试增加或减少时间网格离散程度 n，尝试 n+1 或 n-1 的时间网格是否也达到总时间最小。如果增加或减少时间网格离散数量后的总优化时间<=之前的优化时间，那就需要插入或删除离散网格（射击节点 / 控制）。

时间网格优化算法

$1.procedure ADAPT AND SOLVE NLP(b,n^*_b) \\ 2.\quad for all Iterations i=1,2,...,I do \\ 3.\qquad if i > 1 or b is a warm-start then \\ 4.\qquad \quad {n_b,\kappa}\leftarrow Count validity of |u_{k+1}-u_k|\leq \epsilon \forall k in b\\ 5.\qquad \quad if n_b < n^*_b then \\ 6.\qquad \qquad b\leftarrow INSERT VARIABLES(b,n^*_b-n_b)\\ 7.\qquad \quad else if n_b > n^*_b then \\ 8.\qquad \qquad b\leftarrow erase u_{k+1},s_{k+1} and \Delta T_{k+1} in b \forall k \in \kappa \\ 9.\qquad {V^*,b^*}\leftarrow SOLVENLP(b)\\ 10.\qquad if V^* is non-feasible then \\ 11.\qquad \quad b\leftarrow INSERT VARIABLES(b^*,1) \\ 12.\qquad \quad Goto 9 and resolve \\ 13.retrun b^*$

其中

$b=[s^T_0,u^T_0,\Delta T_0,s^T_1,u^T_1,\Delta T_1,...,s^T_{n-1},u^T_{n-1},\Delta T_{n-1},s^T_n]^T$ 表示离散的射击节点序列，每个节点包含自身的初值，控制，和时间网格大小（控制时间长度）

$n^*_b\in \mathbb{N}\geq0$ 表示控制的期望数量。初始 b 是由起动和最终状态之间的线性插值 $s_k=x_s+k\frac{(x_f-x_s)}{n}$ 得到。

在离散的射击节点序列 b 中，一个冗余控制表现为 $|u_{k+1}-u_k|\leq \epsilon$ ，k+1 是冗余节点，会被记录到 κ 中。

当 $n_b<n^*_b$ ，说明优化后的控制比预期要少，可以在 b 中寻找时间网格最大的节点作线性插值，增加一个控制节点。

当 $n_b>n^*_b$ ，那么删除冗余控制节点。

翻译一下：

NLP 优化，输入控制序列 b 和期望的节点数 $n^*_b$ （最开始控制序列为空）
循环迭代控制序列 b 中所有节点
如果 i >1 （之前迭代过）或者 b 是热启动（加载过参数了？）
遍历 b 中所有控制序列，检查控制增量几乎可以忽略的节点，冗余节点下标记为 k，返回控制节点数量 $n_b$ 和冗余控制的下标集 κ
如果 $n_b$ 小于期望的节点数 $n^*_b$ ，那么
那么在最大的时间控制序列中线性插入 $n^*_b-n_b$ 个新的节点，返回新的控制序列
如果 $n_b$ 大于期望的节点数 $n^*_b$ ，那么
那么遍历冗余控制的下标集 κ ，把下标对应的序列的控制，状态和时间间隔数据一并删除
优化函数处理非线性规划问题，获得新的成本函数 $V^*$ 和新的控制序列 $b^*$
如果不满足成本函数，那么
给控制序列插入一个新节点
返回到 9 继续
返回优化后的控制序列 $b^*$

闭环控制

$1.procedure FEEDBACK CONTROL STEP(b,x_s,x_f) \\ 2.\quad Initialize or update trajectory \\ 3.\quad b^*\leftarrow ADAPT AND SOLVE NLP(b,n^*_b) \\ 4.\quad return {u^*_0,b^*}$

闭环控制(控制序列，系统初始状态，系统终点状态)
初始化，更新轨迹（控制序列）
求解 NLP （非线性规划），调整控制序列
返回控制序列第一个控制

这里的闭环控制说的是系统状态 $x_s$ 和 $x_f$ 实时由外部测量输入，也可以是估算值输入。

Standard MPC and Hypergraph 简述

在连续时间动态模型的背景下，间接方法利用变分法，而直接方法将连续时间问题转化为具有有限数量优化变量的非线性程序。直接方法离散化潜在的边值问题，即遵守连续时间动态，并且进一步分为顺序(单次打靶)或同步策略（多次打靶、匹配）。

顺序方法仅优化控制顺序。

同步策略还考虑状态轨迹，这会导致稀疏但通常会提高收敛性。在多次打靶中，状态轨迹被划分为多个离散区间，并解决孤立的初始值问题。打靶间隔连续性的约束被纳入非线性方程中。

配置方法通过有限差分或求积规则（即隐式数值积分或样条插值）对状态和控制轨迹（动力学方程）进行插值。与多重射击类似，配置方程被纳入作为额外的等式约束进行优化。

为无约束最小二乘问题提出了一个有效的通用图优化框架。问题是根据超图来表述的，其中边对应于成本函数项。边反映了由顶点表示的优化参数的连通性，因此超图保留了优化问题的结构。

非线性模型预测控制

单次打靶法

时间连续的控制

$\dot{x}=f(x(t), u(t))$

t 时刻的系统状态 $x(t)$

t 时刻的控制输入，表示为控制输入序列 $u(t)$

系统输出状态 $\dot{x}$

单次打靶方法沿着具有 N 分区的固定时间网格离散控制输入，状态轨迹作为初值问题的解出现。

$x(t)=\int^{t_f}_{t_0}{f(x(t),u(t))dt}$

系统初始状态为 $x_s$

上式表示 t 时刻系统状态 x(t)，在控制输入 u(t) 作用下运行 dt 时间得到下个系统状态 x(t+1)，累积系统状态 x(t) 得到状态轨迹。

单次打靶的 OCP 优化控制问题描述：

$\underset{u_0,u_1,...,u_{N-1}}{min}[V_f(x(t_f))+\int^{t_f}_{t_0}\ell(x(t),u(t))dt] \qquad (7)$

$x(t_0)=x_s, x(t_k)\in{\mathbb{X}}, u_k\in{\mathbb{U}}, x(t_f)\in{\mathbb{X}_f}$

t 表示连续的时间

dt 是离散后的时间间隔

k 是离散后的时间时刻

$x(t_k)$ 表示在离散后的网格时间点 k 时刻的状态的集合

集合 U 和 X 定义了控制和状态的可行区域

$V_f(x(t_f))$ 表示最终 tf 时刻状态的成本

$\ell$ 感觉是符号写错了，得写成 $\mathcal{L}$ 表示损失函数，表示成本（损失）

翻译一下，上面的公式就是说控制优化问题 OCP 是求终点成本+轨迹控制成本最低的优化问题。

同步打靶法

同步打靶法，在 [t0, tf] 时间上分割出多个 IVP （初值问题）分别优化求解。 IVP 之间的连接性和合规性通过额外的方程来约束。

$\underset{u_0,...,u_{N-1} s_0,...s_{M}}{min}[V_f(s_N)+\sum^{M-1}_{i=0}{\int^{t_{i+1}}_{t_i}{\mathcal{L}(x(t),u(t))dt}}]$

约束条件是

初始射击节点 $s_0=x_s$ ，射击节点 $s_i\in \mathbb{X}$ ，控制 $u_k\in \mathbb{U}$ ，终点（目标）节点 $s_M\in \mathbb{X}_f$ ，系统的推导方程 $s_{i+1}=\psi(u(t),s_i)$

需要注意的是，在同步打靶中，由于分割为多个区间优化，所以除了系统初始状态 x0=s0，后续区间内的系统启动状态 xi 是未知的，也是需要优化的参数。而在单次打靶，也就是顺序打靶中，分区的系统启动状态 xi 就是由上一个分区的系统输出状态确定的。

这样子的话，就需要有附加的方程来约束上一个区间的终点边界值和下一个区间的初值，以保持输出的连续性。

直接匹配法

直接匹配法也是离散化状态和控制输入轨迹。配置方法是在两个连续状态和具有预定义基函数的控制之间插值动力学方程和成本函数。例如 b 样条插值。

完全离散法

没懂怎么个完全法。找不到“完全离散”这个数学概念。

超图公式

一般情况下，导数是通过有限差分计算得来。通过计算系统在一个极小范围 h 内的变化量，认为得到该范围中心处的导数。

中心差分法： $\frac{d\psi(x)}{dx}=\frac{\psi(x+h)-\psi(x-h)}{2h}$

顶点：顶点是指优化参数向量，即状态向量和控制输入向量。特别是速度约束，其他不需要优化的参数也会出现在成本和约束条件中。

边：这里是成本项 V，等式 g 和不等式约束 h，称为三兄贵。

图 fig.2 中，顶点 (s0 s1 u0 u1) 由边 (成本函数 V0 和等式方程 ψ ) 约束，终点 sn 还会受到边终点成本函数 Vf 的约束。

边迭代的导数算法

$1.procedure COMPUTEDERIVATIVESEB(\upsilon,\epsilon,\nabla{V},J_g,J_h)\\ 2.\quad for all edges l = 0,1,...,L do\\ 3.\qquad for each connected unfixed vertex v do\\ 4.\qquad \quad Compute gradient of V_l and dense block Jacobians of g_l and h_l w.r.t v\\ 5.\qquad \quad Write values to proper positions in \nabla{V},J_g,J_h\\ 6.return \nabla{V},J_g,J_h$

翻译一下：

计算边导数，输入顶点集合 υ，边集合 ε，成本函数 V，等式 g（动力学方程），不等式 h（约束等）
遍历每条边
遍历这条边连接的非固定顶点（待优化顶点）
计算这条边的成本函数梯度，和 g 和 h 对 υ 的稠密块矩阵的雅克比矩阵（稠密块矩阵即系统有相互作用部分，w.r.t 是相对于的意思）。
更新成本函数 V，g 雅克比矩阵，h 雅克比矩阵
返回成本函数 V，g 雅克比矩阵，h 雅克比矩阵

顶点迭代的导数算法

$1.procedure COMPUTEDERIVATIVESVB(\upsilon,\epsilon,\nabla{V},J_g,J_h)\\ 2.\quad for all unfixed vertices j=0,1,..,\upsilon do\\ 3.\qquad for each element e in vertices[j] do\\ 4.\qquad \quad e\leftarrow{e+h}\\ 5.\qquad \quad Evaluate and cache V_l,g_l,h_l of connected edges\\ 6.\qquad \quad e\leftarrow{e-2h}\\ 7.\qquad \quad Evaluate V_l,g_l,h_l of connected edges\\ 8.\qquad \quad Apply central differences formula and write values to proper positions in \nabla{V},J_g,J_h\\ 9.\qquad \quad e\leftarrow{e+h}\\ 10.return \nabla{V},J_g,J_h$