一、引言

在仓储物流机器人、园区巡检机器人、无人配送小车以及室外移动机器人应用中，导航系统不仅需要让机器人按照规划路径前进，更重要的是能够实时感知周围环节变化，并在遇到障碍物时快速做出避障决策。
在ROS2生态里，Nav2已经是移动机器人常用的导航框架，MPPI（Model Predictive Path Integral，模型预测路径积分控制）作为Nav2中局部控制器适合用来提升路径跟踪、动态避障和运动平滑性。

MPPI的做法很直观：每个控制周期先生成一批后选动作，在模型里预测这些动作接下来会走成什么样，再根据路径、障碍物、目标点等评分项，算出当前最合适的控制指令。

二、MPPI主要解决什么问题？

全局规划器负责给机器人画出从起点到目标点的参考路线，局部控制器负责把这条路线落到实时运动上。实际运行时，地图、传感器、底盘控制和周围环境都会带来偏差，机器人需要在很短的时间内不断修正自己的动作。

MPPI会在每个控制周期内生成大量候选控制序列，并预测未来一小段时间内机器人可能走的轨迹。随后，他用代价函数对这些轨迹进行评分，选择更安全、更平滑、更接近目标的控制趋势。

因此，MPPI更像一个带预判能力的局部决策模块：跟住路径、避开障碍、控制速度、稳定姿态，这几件事会在一个评分体系中进行权衡。

三、核心流程：采样、预测、评分、融合

MPPI的流程可以拆成四步：采样、预测、评分、融合。它先试出很多种走法，再通过评分机制降低高风险方案的影响，把更可靠的运动趋势融合到最终指令里。

3.1 控制序列采样

与DWB在速度空间采样候选速度类似，MPPI会围绕当前控制输入生成大量候选控制序列。直观来看，就是机器人在当前位置附近同时尝试多组速度和转向组合。

其数学表达通常为：

$$u_i=u+{\epsilon }_i$$

其中，$u$ 表示当前控制输入，${\epsilon }_i$ 表示随机扰动，$u_i$ 表示第 $i$ 条候选控制序列。通过向当前最优控制输入 $u$ 叠加一系列随机扰动 ${\epsilon }_i$，MPPI 得以在控制输入空间中进行高效探索，生成多样化的候选动作序列以供后续的轨迹预测与评分。

3.2 前向滚动预测

在完成控制序列采样后，MPPI会将每一条候选控制序列输入到机器人运动学模型或动力学模型中，向前预测机器人在未来一段时间内的运动轨迹。采样阶段只负责生成多组可能的速度和转向组合，而前向滚动预测阶段，则用于判断：如果机器人按某一控制序列执行，那么未来的运动轨迹是怎么样的。

对于$i$条候选控制序列：
$$u_{0:T-1}^{(i)}=\left[u_0^{(i)},u_1^{(i)},\cdots ,u_{T-1}^{(i)}\right]$$

MPPI 会从当前机器人状态 $x_0$ 出发，利用系统模型 $f$ 逐步预测未来状态：

$$x_{t+1}^{(i)}=f\left(x_t^{(i)},u_t^{(i)}\right),t=0,1,\cdots ,T-1$$

其中，$x_t^{(i)}$ 表示第 $i$ 条候选轨迹在第 $t$ 个预测时刻的机器人状态，$u_t^{(i)}$ 表示该时刻对应的控制输入，$f(\cdot )$ 表示机器人运动学或动力学模型，$T$ 表示预测时域长度。通过迭代执行上述状态更新，即可得到整条候选轨迹 {x0(i),x1(i),…,xT(i)}\{ x_0^{(i)}, x_1^{(i)}, \dots, x_T^{(i)} \}{x0(i)​,x1(i)​,…,xT(i)​}，为后续的轨迹评分提供基础。

对于差速移动机器人，机器人状态通常可以表示为：
$$x_t=\left[\begin{array}{c}{x}_t\\ y_t\\ {\theta }_t\end{array}\right]$$
控制输入可以表示为：
$$u_t=\left[\begin{array}{c}{v}_t\\ {\omega }_t\end{array}\right]$$
其中，$x_t,y_t$ 表示机器人位置，${\theta }_t$ 表示机器人航向角，$v_t$ 表示线速度，${\omega }_t$ 表示角速度。根据差速运动学模型，其离散状态更新可写为：
$$\begin{array}{rl}{x}_{t+1}& =x_t+v_t\cos ({\theta }_t)\Delta t\\ y_{t+1}& =y_t+v_t\sin ({\theta }_t)\Delta t\\ {\theta }_{t+1}& ={\theta }_t+{\omega }_t\Delta t\end{array}$$
其中，$\Delta t$ 表示预测步长。经过连续 $T$ 次状态递推后，每一条候选控制序列都会生成一条对应的候选轨迹：
$${\tau }^{(i)}=\left[x_0^{(i)},x_1^{(i)},x_2^{(i)},\cdots ,x_T^{(i)}\right]$$
由于 MPPI 会采样 $N$ 条候选控制序列，因此前向滚动预测后也会得到 $N$ 条候选轨迹。直观来看，这相当于机器人在当前状态下提前模拟多种可能的运动结果：有些轨迹可能会靠近障碍物，有些轨迹可能会偏离全局路径，也有些轨迹能够较平滑地绕开障碍物并继续朝目标前进。后续的代价评估阶段，就是对这些预测轨迹进行评分，从而判断哪一类控制序列更适合当前局部避障任务。

---

3.3 轨迹代价评估

在前向滚动预测阶段，MPPI根据采样得到的$N$条候选控制序列生成了$N$条候选轨迹。轨迹代价评估的作用，就是对每一条候选轨迹进行评分，判断其是否适合当前局部避障任务。对于移动机器人而言，一条较优的轨迹应当同时满足：能够朝目标方向前进、尽量贴近全局路径、与障碍物保持安全距离，并且控制输入变化平滑。

对于第 $i$ 条候选轨迹：
$${\tau }^{(i)}=\left[x_0^{(i)},x_1^{(i)},\cdots ,x_T^{(i)}\right]$$
其总代价可以表示为多个子代价函数的加权和：
$$J^{(i)}=w_g{J}_{\text{goal}}^{(i)}+w_p{J}_{\text{path}}^{(i)}+w_o{J}_{\text{obs}}^{(i)}+w_s{J}_{\text{smooth}}^{(i)}+w_v{J}_{\text{vel}}^{(i)}$$
其中，$J^{(i)}$ 表示第 $i$ 条候选轨迹的总代价；$J_{\text{goal}}^{(i)}$ 表示目标跟踪代价；$J_{\text{path}}^{(i)}$ 表示路径偏离代价；$J_{\text{obs}}^{(i)}$ 表示障碍物代价；$J_{\text{smooth}}^{(i)}$ 表示控制平滑代价；$J_{\text{vel}}^{(i)}$ 表示速度约束或速度偏好代价。$w_g,w_p,w_o,w_s,w_v$ 分别为各项代价对应的权重，用于调节不同优化目标的重要程度。

在局部避障任务中，障碍物代价 $J_{\text{obs}}^{(i)}$ 是最关键的一项。它通常根据预测轨迹与障碍物之间的距离，或者局部代价地图中的栅格代价值进行计算。若轨迹距离障碍物越近，则代价越大；若轨迹发生碰撞，则可以直接赋予极大代价：

$$J_{\text{obs}}^{(i)}=\sum _{t=0}^T\max {\left(0,d_{\text{safe}}-d_t^{(i)}\right)}^2$$

其中，$d_t^{(i)}$ 表示第 $i$ 条轨迹在第 $t$ 个预测时刻距离最近障碍物的距离，$d_{\text{safe}}$ 表示设定的安全距离。当 $d_t^{(i)}<d_{\text{safe}}$ 时，说明机器人过于接近障碍物，需要增加惩罚；当预测轨迹进入障碍物区域时，该轨迹会获得很高的代价，从而在后续权重融合中被削弱。

路径偏离代价 $J_{\text{path}}^{(i)}$ 用于约束机器人不要过度偏离全局路径，可表示为：

$$J_{\text{path}}^{(i)}=\sum _{t=0}^T{d}_{\text{path}}{\left(x_t^{(i)}\right)}^2$$

其中，$d_{\text{path}}\left(x_t^{(i)}\right)$ 表示预测状态 $x_t^{(i)}$ 到参考路径的距离。该项代价可以保证机器人在避障的同时，仍然沿着全局规划路径附近运动。

因此，轨迹代价评估阶段本质上是在综合比较每一条候选轨迹的安全性、目标趋近性、路径一致性和控制平滑性。代价越低，说明该轨迹越适合当前环境；代价越高，说明该轨迹可能存在靠近障碍物、偏离路径或控制不稳定等问题。需要注意的是，MPPI 后续通常不是简单选择代价最低的一条轨迹，而是根据这些代价计算权重，对多条候选控制序列进行加权融合，从而得到更加平滑、鲁棒的控制输出。

3.4 权重计算与融合

在完成轨迹代价评估后，MPPI 会根据每条候选轨迹的代价大小计算权重，并对多条候选控制序列进行加权融合。与直接选择代价最低轨迹不同，MPPI 更强调利用多条优质轨迹共同生成控制结果，从而提高控制输出的平滑性和鲁棒性。

对于第 $i$ 条候选轨迹，其代价为 $J^{(i)}$，对应权重可表示为：

$$w_i=\frac{\exp \left(-\frac{1}{\lambda }{J}^{(i)}\right)}{{\sum }_{j=1}^N\exp \left(-\frac{1}{\lambda }{J}^{(j)}\right)}$$

其中，$w_i$ 表示第 $i$ 条候选控制序列的权重，$J^{(i)}$ 表示第 $i$ 条候选轨迹的总代价，$N$ 表示候选轨迹数量，$\lambda$ 表示温度系数，用于调节代价对权重分布的影响。从该公式可以看出，当某条轨迹代价较小时，其指数项较大，对应权重也更高；当某条轨迹代价较大时，其权重会被削弱。因此，安全性更高、路径跟踪效果更好、控制更平滑的轨迹，会在最终控制输出中占据更大比例；而靠近障碍物、偏离路径或运动不稳定的轨迹，则会对最终控制结果产生较小影响。

得到权重后，MPPI 会对所有候选控制序列进行加权融合：

$$u_{0:T-1}^{\ast }=\sum _{i=1}^N{w}_i{u}_{0:T-1}^{(i)}$$

其中，$u_{0:T-1}^{\ast }$ 表示融合后的最优控制序列，$u_{0:T-1}^{(i)}$ 表示第 $i$ 条候选控制序列。由于每条控制序列都包含未来 $T$ 个时刻的控制输入，因此融合得到的是一整段未来控制序列：

$$u_{0:T-1}^{\ast }=\left[u_0^{\ast },u_1^{\ast },\cdots ,u_{T-1}^{\ast }\right]$$

对于差速移动机器人，每个时刻的控制输入通常为：

$$u_t^{\ast }=\left[\begin{array}{c}{v}_t^{\ast }\\ {\omega }_t^{\ast }\end{array}\right]$$

其中，$v_t^{\ast }$ 表示融合后的线速度，${\omega }_t^{\ast }$ 表示融合后的角速度。

在实际执行过程中，MPPI 通常不会一次性执行完整的控制序列，而是只取第一步控制量 $u_0^{\ast }$ 发送给底盘执行：

$$u_{\text{exec}}=u_0^{\ast }$$

执行完成后，机器人会根据新的传感器数据和自身状态重新进行采样、预测、评估和融合。这样，MPPI 就形成了一个不断滚动更新的闭环控制过程，能够根据环境变化实时调整运动方向，从而实现动态局部避障。

总结

综上所述，MPPI 局部避障算法可以理解为一种基于采样优化的滚动时域控制方法。它并不是直接在当前位置选择一个固定速度指令，而是在未来一段预测时域内生成大量候选控制序列，并根据机器人运动模型预测这些控制序列对应的未来轨迹。随后，算法会结合目标距离、路径偏离、障碍物距离、控制平滑性等因素，对每一条候选轨迹进行代价评估。MPPI 的整体流程可以概括为：控制采样→前向预测→代价评估→权重融合→执行控制。

这种方法的优势在于：

1. 滚动优化：每个控制周期都重新采样、预测、评估，能够实时响应环境变化。
2. 多轨迹融合：不是简单地选择代价最低的轨迹，而是通过权重融合多条优质轨迹，使控制输出更平滑、鲁棒。
3. 模型驱动：利用机器人运动学/动力学模型进行前向预测，使决策更符合实际物理约束。
4. 可扩展性：代价函数可以灵活设计，适应不同的导航任务（如动态避障、路径跟踪、速度平滑等）。

在 ROS2 Nav2 框架中，MPPI 控制器通过上述流程，能够在复杂动态环境中实现安全、平滑的局部避障，是提升移动机器人导航性能的有效方案之一。