Shape-Aware MPPI（SA MPPI）算法：基于RC-ESDF的任意形状机器人实时轨迹优化

Shape-Aware MPPI（SA MPPI）算法：基于RC-ESDF的任意形状机器人实时轨迹优化

作者 : Robot_Nav
机构 : HKD-NEU
日期 : 2026-04-23
关键词: Model Predictive Path Integral (MPPI), Robo-Centric ESDF, 任意形状碰撞检测, 实时轨迹优化, ROS导航

---

目录

1. 项目概述
2. 算法原理
3. 算法公式与数学推导
4. 项目结构与核心代码实现
5. 运行步骤与环境配置
6. 实验结果与性能评估
7. 结论与展望

---

1. 项目概述

1.1 研究背景

在移动机器人导航领域，Model Predictive Path Integral (MPPI) 控制因其优异的实时性和对非凸代价函数的天然适应能力，已成为局部轨迹规划的主流方案之一。MPPI通过大量随机采样轨迹并在概率框架下加权优化，避免了传统优化方法对梯度信息的强依赖，特别适用于障碍物密集、环境动态变化的复杂场景。

然而，传统MPPI存在一个关键局限 ：其障碍物代价计算通常将机器人简化为圆形模型（质点+半径），通过计算机器人中心到障碍物的距离来判定碰撞。这种简化对于圆形或近似圆形的机器人（如扫地机器人、差速小车）尚可接受，但对于具有复杂非圆形轮廓的机器人（如矩形底盘、L形机械结构、带有突出传感器的平台）则会产生显著的安全隐患：

• 过度保守：圆形外包络远大于实际 footprint，导致机器人在本可通过的狭窄通道前停止。
• 碰撞风险：某些凸出部位可能已接触障碍物，而中心点仍在"安全距离"之外。

1.2 研究意义

本项目提出的 Shape-Aware MPPI (SA MPPI) 算法，核心创新在于将 Robo-Centric Euclidean Signed Distance Field (RC-ESDF) 引入MPPI的障碍物代价计算中，实现了对任意多边形 footprint 的精确碰撞检测。其研究意义体现在：

维度	传统MPPI	SA MPPI (本项目)
机器人模型	圆形（质点+半径）	任意凸/凹多边形 footprint
碰撞检测精度	低（外包络近似）	高（精确到轮廓点级别）
狭窄空间通过性	差	优（充分利用实际轮廓间隙）
计算复杂度	O(1) 简单距离查询	O(1) ESDF双线性插值查询
梯度信息	数值近似/无	解析梯度，平滑连续

1.3 应用场景

SA MPPI算法适用于以下典型场景：

• 仓储物流AGV：矩形底盘需在狭窄货架间穿梭，圆形模型导致大量无效停滞。
• 服务机器人：搭载机械臂或非对称传感器布局，需精确保护突出结构。
• 自动驾驶低速场景：园区接驳车、无人配送车具有明确的长方形轮廓。
• 高动态环境：行人、其他机器人频繁穿行，需要实时、精确的碰撞边界判定。

---

2. 算法原理

2.1 整体架构

SA MPPI算法的系统架构由两大核心模块组成：

┌─────────────────────────────────────────────────────────────────┐
│                      SA MPPI 系统架构                             │
├─────────────────────────────────────────────────────────────────┤
│  ┌──────────────┐          ┌──────────────┐                     │
│  │   RC-ESDF    │          │    MPPI      │                     │
│  │   构建模块    │◄────────►│   优化核心    │                     │
│  └──────────────┘          └──────────────┘                     │
│         ▲                         ▲                             │
│         │                         │                             │
│  ┌──────────────┐          ┌──────────────┐                     │
│  │ 机器人Footprint│          │  采样轨迹集   │                     │
│  │ (多边形顶点)   │          │ (Batch×Time) │                     │
│  └──────────────┘          └──────────────┘                     │
│         ▲                         ▲                             │
│         │                         │                             │
│  ┌──────────────────────────────────────────┐                  │
│  │           ROS1 传感器接口层                │                  │
│  │  (LaserScan / Odometry / Global Path)    │                  │
│  └──────────────────────────────────────────┘                  │
└─────────────────────────────────────────────────────────────────┘

2.2 RC-ESDF：机器人中心符号距离场

2.2.1 核心思想

RC-ESDF（Robo-Centric ESDF）由浙江大学FAST-Lab提出，其核心思想是：以机器人自身坐标系（Body Frame）为参考，在机器人 footprint 上预计算一个欧几里得符号距离场。与传统全局ESDF（如FIESTA、Voxblox）不同，RC-ESDF具有以下特点：

1. 机器人中心坐标系：ESDF原点在机器人中心，随机器人移动而"移动"，无需维护全局地图。
2. 离线生成，在线查询：ESDF在程序启动时根据 footprint 多边形一次性生成，运行时仅做 O(1) 查询。
3. 内部为负，外部为正：距离场在机器人轮廓内部存储负距离（越深入内部越负），外部存储正距离（越远离轮廓越正）。
4. 解析梯度：通过双线性插值同时获得距离值和连续梯度，为优化器提供平滑的斥力方向。

2.2.2 距离场生成算法

对于 footprint 多边形的每一个顶点 v i v_i vi，RC-ESDF遍历栅格地图的每个像素中心 p p p，执行：

1. 点到线段距离计算 ：计算 p p p 到多边形每条边 ( v i , v i + 1 ) (v_i, v_{i+1}) (vi,vi+1) 的最小距离 d m i n d_{min} dmin。
2. 内外判定 ：使用射线法（Ray Casting）判断 p p p 是否在多边形内部。
3. 符号赋值 ：
   • 若 p p p 在内部： D ( p ) = − d m i n D(p) = -d_{min} D(p)=−dmin
   • 若 p p p 在外部： D ( p ) = + d m i n D(p) = +d_{min} D(p)=+dmin（或根据安全余量策略调整）

2.2.3 双线性插值查询

在线阶段，给定障碍物点 p b o d y p_{body} pbody（已转换到Body Frame），通过双线性插值获取连续距离和梯度：

设 p b o d y p_{body} pbody 对应栅格坐标 ( u , v ) (u, v) (u,v)，其相邻四个栅格值为 Q 00 , Q 10 , Q 01 , Q 11 Q_{00}, Q_{10}, Q_{01}, Q_{11} Q00,Q10,Q01,Q11，则：

D ( p ) = ( 1 − α ) ( 1 − β ) Q 00 + α ( 1 − β ) Q 10 + ( 1 − α ) β Q 01 + α β Q 11 D(p) = (1-\alpha)(1-\beta)Q_{00} + \alpha(1-\beta)Q_{10} + (1-\alpha)\beta Q_{01} + \alpha\beta Q_{11} D(p)=(1−α)(1−β)Q00+α(1−β)Q10+(1−α)βQ01+αβQ11

梯度通过数值导数计算：

∂ D ∂ x = ( 1 − β ) ( Q 10 − Q 00 ) + β ( Q 11 − Q 01 ) Δ \frac{\partial D}{\partial x} = \frac{(1-\beta)(Q_{10}-Q_{00}) + \beta(Q_{11}-Q_{01})}{\Delta} ∂x∂D=Δ(1−β)(Q10−Q00)+β(Q11−Q01)

∂ D ∂ y = ( 1 − α ) ( Q 01 − Q 00 ) + α ( Q 11 − Q 10 ) Δ \frac{\partial D}{\partial y} = \frac{(1-\alpha)(Q_{01}-Q_{00}) + \alpha(Q_{11}-Q_{10})}{\Delta} ∂y∂D=Δ(1−α)(Q01−Q00)+α(Q11−Q10)

其中 Δ \Delta Δ 为栅格分辨率。该梯度始终指向距离增加的方向（即指向机器人外部），因此 − ∇ D -\nabla D −∇D 指向将机器人推离障碍物的方向。

2.3 MPPI优化核心

2.3.1 标准MPPI算法回顾

MPPI是一种基于信息论的随机最优控制算法。其核心步骤为：

1. 控制序列采样 ：在当前最优控制序列 u ∗ \mathbf{u}^* u∗ 上叠加高斯噪声，生成 K K K 条候选轨迹。
2. 轨迹前向积分：使用运动模型将每条控制序列积分得到状态轨迹。
3. 代价评估：对每条轨迹计算总代价（包含障碍物代价、路径跟踪代价、控制代价等）。
4. 加权更新：使用softmax权重对采样控制序列进行加权平均，更新最优控制序列。

2.3.2 SA MPPI的创新改进

本项目在标准MPPI基础上引入了以下关键改进：

A. 三段式障碍物代价策略

传统MPPI通常使用单一阈值判定碰撞，容易导致在临界距离附近抖动。本项目采用硬碰撞 + 软阻塞 + 安全区的三段式策略：

距离区间	判定	处理方式
d < d h a r d d < d_{hard} d<dhard	硬碰撞	立即赋予极高代价 C c o l l i s i o n C_{collision} Ccollision，终止该轨迹评估
d h a r d ≤ d < d m a r g i n d_{hard} \leq d < d_{margin} dhard≤d<dmargin	Margin高危区	施加衰减软惩罚，鼓励绕障但避免过度反应
d ≥ d m a r g i n d \geq d_{margin} d≥dmargin	安全区	无额外代价

B. 基于RC-ESDF的精确碰撞检测

对于每个轨迹点 ( x , y , θ ) (x, y, \theta) (x,y,θ)，将障碍物点从世界坐标系转换到Body Frame：

p b o d y = R ( θ ) T ( p w o r l d − t ) p_{body} = R(\theta)^T (p_{world} - t) pbody=R(θ)T(pworld−t)

其中 R ( θ ) R(\theta) R(θ) 为旋转矩阵， t = [ x , y ] T t = [x, y]^T t=[x,y]T 为平移向量。然后查询RC-ESDF获取距离和梯度。

C. 梯度引导的代价加权

在Margin高危区内，本项目创新性地利用RC-ESDF的解析梯度调整代价：

若轨迹运动方向与梯度方向（脱离碰撞的方向）一致，则降低惩罚；若方向相反（朝向障碍物运动），则加重惩罚。这一机制使得优化器不仅知道"有多危险"，还知道"往哪个方向走更安全"。

D. 全向/差速模式自动切换

针对具备全向移动能力的机器人，本项目实现了基于障碍物距离和路径偏离程度的自动模式切换：

• 差速模式（DiffDrive）：默认模式，适合开阔空间高效前进。
• 全向模式（Omni）：当障碍物距离小于阈值或路径偏离过大时自动触发，允许横向平移以快速避障。

2.4 技术优势总结

技术优势	具体表现
形状感知精确避障	支持任意多边形footprint，告别圆形近似，狭窄空间通过性提升显著
O(1) 实时查询	RC-ESDF单次查询约2.4μs，满足20-50Hz控制频率需求
解析梯度引导	连续平滑的梯度信息避免数值震荡，优化收敛更稳定
多层级安全策略	硬碰撞/软阻塞/安全区三段式策略，平衡安全性与通过性
多运动模型支持	差速/全向/阿克曼模型一键切换，自适应场景变化
工业级鲁棒性	阻塞检测、fallback恢复、启动辅助、SG滤波等多重安全机制

---

3. 算法公式与数学推导

3.1 符号说明

符号	含义	单位/类型
K K K	采样轨迹数量（batch size）	-
T T T	预测时间步数（time steps）	-
Δ t \Delta t Δt	模型时间步长	s
u t = [ v x , v y , ω z ] T \mathbf{u}_t = [v_x, v_y, \omega_z]^T ut=[vx,vy,ωz]T	时刻 t t t 的控制量	m/s, rad/s
x t = [ x , y , θ ] T \mathbf{x}_t = [x, y, \theta]^T xt=[x,y,θ]T	时刻 t t t 的机器人状态	m, m, rad
u ∗ = { u 0 ∗ , . . . , u T − 1 ∗ } \mathbf{u}^* = \{\mathbf{u}0^*, ..., \mathbf{u}{T-1}^*\} u∗={u0∗,...,uT−1∗}	当前最优控制序列	-
u ~ k \tilde{\mathbf{u}}^k u~k	第 k k k 条采样控制序列	-
ϵ t k \epsilon_t^k ϵtk	时刻 t t t 第 k k k 条轨迹的高斯噪声	-
λ \lambda λ	温度参数（temperature）	-
γ \gamma γ	控制代价权重	-
S ( u ~ k ) S(\tilde{\mathbf{u}}^k) S(u~k)	第 k k k 条轨迹的总代价	-
D ( p ) D(p) D(p)	RC-ESDF在点 p p p 处的距离值	m
∇ D ( p ) \nabla D(p) ∇D(p)	RC-ESDF在点 p p p 处的梯度	-
d h a r d d_{hard} dhard	硬碰撞距离阈值	m
d s o f t d_{soft} dsoft	软阻塞距离阈值	m
d m a r g i n d_{margin} dmargin	碰撞Margin距离	m

3.2 运动学模型

3.2.1 差速驱动模型（DiffDrive）

{ x ˙ = v x cos ⁡ θ y ˙ = v x sin ⁡ θ θ ˙ = ω z \begin{cases} \dot{x} = v_x \cos\theta \\ \dot{y} = v_x \sin\theta \\ \dot{\theta} = \omega_z \end{cases} ⎩ ⎨ ⎧x˙=vxcosθy˙=vxsinθθ˙=ωz

离散形式（前向欧拉）：

{ x t + 1 = x t + v x cos ⁡ θ t ⋅ Δ t y t + 1 = y t + v x sin ⁡ θ t ⋅ Δ t θ t + 1 = θ t + ω z ⋅ Δ t \begin{cases} x_{t+1} = x_t + v_x \cos\theta_t \cdot \Delta t \\ y_{t+1} = y_t + v_x \sin\theta_t \cdot \Delta t \\ \theta_{t+1} = \theta_t + \omega_z \cdot \Delta t \end{cases} ⎩ ⎨ ⎧xt+1=xt+vxcosθt⋅Δtyt+1=yt+vxsinθt⋅Δtθt+1=θt+ωz⋅Δt

3.2.2 全向移动模型（Omni）

{ x ˙ = v x cos ⁡ θ − v y sin ⁡ θ y ˙ = v x sin ⁡ θ + v y cos ⁡ θ θ ˙ = ω z \begin{cases} \dot{x} = v_x \cos\theta - v_y \sin\theta \\ \dot{y} = v_x \sin\theta + v_y \cos\theta \\ \dot{\theta} = \omega_z \end{cases} ⎩ ⎨ ⎧x˙=vxcosθ−vysinθy˙=vxsinθ+vycosθθ˙=ωz

3.2.3 阿克曼转向模型（Ackermann）

在差速模型基础上增加最小转弯半径约束：

R = ∣ v x ∣ ∣ ω z ∣ ≥ R m i n R = \frac{|v_x|}{|\omega_z|} \geq R_{min} R=∣ωz∣∣vx∣≥Rmin

若 R < R m i n R < R_{min} R<Rmin，则调整 ω z \omega_z ωz：

ω z = sign ( ω z ) ⋅ ∣ v x ∣ R m i n \omega_z = \text{sign}(\omega_z) \cdot \frac{|v_x|}{R_{min}} ωz=sign(ωz)⋅Rmin∣vx∣

3.3 控制序列采样

在第 n n n 次迭代中，对当前最优控制序列叠加高斯噪声：

u ~ x , t k = u x , t ∗ + ϵ x , t k , ϵ x , t k ∼ N ( 0 , σ x 2 ) \tilde{u}{x,t}^k = u{x,t}^* + \epsilon_{x,t}^k, \quad \epsilon_{x,t}^k \sim \mathcal{N}(0, \sigma_x^2) u~x,tk=ux,t∗+ϵx,tk,ϵx,tk∼N(0,σx2)

u ~ y , t k = u y , t ∗ + ϵ y , t k , ϵ y , t k ∼ N ( 0 , σ y 2 ) \tilde{u}{y,t}^k = u{y,t}^* + \epsilon_{y,t}^k, \quad \epsilon_{y,t}^k \sim \mathcal{N}(0, \sigma_y^2) u~y,tk=uy,t∗+ϵy,tk,ϵy,tk∼N(0,σy2)

u ~ ω , t k = u ω , t ∗ + ϵ ω , t k , ϵ ω , t k ∼ N ( 0 , σ ω 2 ) \tilde{u}{\omega,t}^k = u{\omega,t}^* + \epsilon_{\omega,t}^k, \quad \epsilon_{\omega,t}^k \sim \mathcal{N}(0, \sigma_\omega^2) u~ω,tk=uω,t∗+ϵω,tk,ϵω,tk∼N(0,σω2)

其中 k = 1 , . . . , K k = 1, ..., K k=1,...,K 为轨迹索引， t = 0 , . . . , T − 1 t = 0, ..., T-1 t=0,...,T−1 为时间步索引。

3.4 轨迹代价函数

总代价由多个critic（评价函数）加权组成：

S ( u ~ k ) = ∑ c ∈ C w c ⋅ S c ( u ~ k ) S(\tilde{\mathbf{u}}^k) = \sum_{c \in \mathcal{C}} w_c \cdot S_c(\tilde{\mathbf{u}}^k) S(u~k)=c∈C∑wc⋅Sc(u~k)

其中 C \mathcal{C} C 为critic集合， w c w_c wc 为权重。

3.4.1 障碍物代价（ObstaclesCritic）------ SA MPPI核心

对于轨迹第 k k k 条、时刻 t t t 的状态 x t k = [ x t k , y t k , θ t k ] T \mathbf{x}_t^k = [x_t^k, y_t^k, \theta_t^k]^T xtk=[xtk,ytk,θtk]T，以及障碍物点集 O \mathcal{O} O：

Step 1: 坐标变换

将障碍物点 o ∈ O o \in \mathcal{O} o∈O 从世界坐标转换到Body Frame：

o b o d y = R ( θ t k ) T ( o − [ x t k , y t k ] T ) o_{body} = R(\theta_t^k)^T (o - [x_t^k, y_t^k]^T) obody=R(θtk)T(o−[xtk,ytk]T)

其中旋转矩阵：

R ( θ ) = [ cos ⁡ θ − sin ⁡ θ sin ⁡ θ cos ⁡ θ ] R(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix} R(θ)=[cosθsinθ−sinθcosθ]

Step 2: RC-ESDF查询

对每个 o b o d y o_{body} obody 查询RC-ESDF：

d e s d f = D ( o b o d y ) , g = ∇ D ( o b o d y ) d_{esdf} = D(o_{body}), \quad \mathbf{g} = \nabla D(o_{body}) desdf=D(obody),g=∇D(obody)

Step 3: 碰撞判定与代价计算

定义轨迹点最小距离：

d m i n k = min ⁡ o ∈ O d e s d f ( o b o d y ) d_{min}^k = \min_{o \in \mathcal{O}} d_{esdf}(o_{body}) dmink=o∈Omindesdf(obody)

• 硬碰撞 （若 d m i n k < d h a r d d_{min}^k < d_{hard} dmink<dhard）：
  S o b s k = C c o l l i s i o n S_{obs}^k = C_{collision} Sobsk=Ccollision

  该轨迹立即终止评估。

• Margin高危区 （若 d h a r d ≤ d m i n k < d m a r g i n d_{hard} \leq d_{min}^k < d_{margin} dhard≤dmink<dmargin）：
  S o b s k = ∑ t = 0 T − 1 w c r i t i c a l ⋅ f d e c a y ( d m i n , t k ) ⋅ η g r a d S_{obs}^k = \sum_{t=0}^{T-1} w_{critical} \cdot f_{decay}(d_{min,t}^k) \cdot \eta_{grad} Sobsk=t=0∑T−1wcritical⋅fdecay(dmin,tk)⋅ηgrad

  其中衰减函数为归一化指数衰减：

  f d e c a y ( d ) = e − k ⋅ d − d h a r d d m a r g i n − d h a r d − e − k 1 − e − k f_{decay}(d) = \frac{e^{-k \cdot \frac{d - d_{hard}}{d_{margin} - d_{hard}}} - e^{-k}}{1 - e^{-k}} fdecay(d)=1−e−ke−k⋅dmargin−dhardd−dhard−e−k

  k k k 为代价缩放因子， η g r a d \eta_{grad} ηgrad 为梯度方向修正因子：

  η g r a d = 1 + 0.3 ⋅ v b o d y ⋅ g ∣ v b o d y ∣ ⋅ ∣ g ∣ \eta_{grad} = 1 + 0.3 \cdot \frac{\mathbf{v}{body} \cdot \mathbf{g}}{|\mathbf{v}{body}| \cdot |\mathbf{g}|} ηgrad=1+0.3⋅∣vbody∣⋅∣g∣vbody⋅g

  其中 v b o d y \mathbf{v}{body} vbody 为轨迹在Body Frame下的运动方向。当运动方向与脱离碰撞方向一致时（点积为正）， η g r a d > 1 \eta{grad} > 1 ηgrad>1，代价降低；反之代价升高。

3.4.2 路径跟随代价（PathFollowCritic）

S f o l l o w k = w f o l l o w ⋅ ∥ x T k − x r e f ∥ 2 S_{follow}^k = w_{follow} \cdot \|\mathbf{x}T^k - \mathbf{x}{ref}\|_2 Sfollowk=wfollow⋅∥xTk−xref∥2

其中 x r e f \mathbf{x}_{ref} xref 为路径上参考点（最远可达点 + offset）。

3.4.3 路径对齐代价（PathAlignCritic）

S a l i g n k = w a l i g n ⋅ 1 N s ∑ j ∈ S ∥ x j k − x p a t h ( s j ) ∥ 2 S_{align}^k = w_{align} \cdot \frac{1}{N_s} \sum_{j \in \mathcal{S}} \|\mathbf{x}j^k - \mathbf{x}{path}(s_j)\|_2 Salignk=walign⋅Ns1j∈S∑∥xjk−xpath(sj)∥2

其中 S \mathcal{S} S 为采样时间步集合， x p a t h ( s j ) \mathbf{x}_{path}(s_j) xpath(sj) 为与轨迹累计距离最接近的路径点。

3.4.4 控制代价（MPPI核心正则项）

为防止优化器过度探索，惩罚偏离当前控制序列的噪声：

S c t r l k = γ σ x 2 ∑ t = 0 T − 1 u x , t ∗ ⋅ ( u ~ x , t k − u x , t ∗ ) + γ σ ω 2 ∑ t = 0 T − 1 u ω , t ∗ ⋅ ( u ~ ω , t k − u ω , t ∗ ) S_{ctrl}^k = \frac{\gamma}{\sigma_x^2} \sum_{t=0}^{T-1} u_{x,t}^* \cdot (\tilde{u}{x,t}^k - u{x,t}^*) + \frac{\gamma}{\sigma_\omega^2} \sum_{t=0}^{T-1} u_{\omega,t}^* \cdot (\tilde{u}{\omega,t}^k - u{\omega,t}^*) Sctrlk=σx2γt=0∑T−1ux,t∗⋅(u~x,tk−ux,t∗)+σω2γt=0∑T−1uω,t∗⋅(u~ω,tk−uω,t∗)

3.5 控制序列更新

计算每条轨迹的指数权重：

w k = exp ⁡ ( − S ( u ~ k ) − S m i n λ ) w^k = \exp\left(-\frac{S(\tilde{\mathbf{u}}^k) - S_{min}}{\lambda}\right) wk=exp(−λS(u~k)−Smin)

其中 S m i n = min ⁡ k S ( u ~ k ) S_{min} = \min_k S(\tilde{\mathbf{u}}^k) Smin=minkS(u~k)， λ \lambda λ 为温度参数。

归一化权重：

w ~ k = w k ∑ j = 1 K w j \tilde{w}^k = \frac{w^k}{\sum_{j=1}^K w^j} w~k=∑j=1Kwjwk

更新控制序列：

u x , t ∗ = ∑ k = 1 K w ~ k ⋅ u ~ x , t k u_{x,t}^* = \sum_{k=1}^K \tilde{w}^k \cdot \tilde{u}_{x,t}^k ux,t∗=k=1∑Kw~k⋅u~x,tk

u y , t ∗ = ∑ k = 1 K w ~ k ⋅ u ~ y , t k u_{y,t}^* = \sum_{k=1}^K \tilde{w}^k \cdot \tilde{u}_{y,t}^k uy,t∗=k=1∑Kw~k⋅u~y,tk

u ω , t ∗ = ∑ k = 1 K w ~ k ⋅ u ~ ω , t k u_{\omega,t}^* = \sum_{k=1}^K \tilde{w}^k \cdot \tilde{u}_{\omega,t}^k uω,t∗=k=1∑Kw~k⋅u~ω,tk

3.6 自适应温度调整

为平衡探索与 exploitation，本项目支持自适应温度：

λ a d a p t = λ ⋅ ( 1 + ln ⁡ ( S m a x − S m i n + 1 ) 5 ) \lambda_{adapt} = \lambda \cdot \left(1 + \frac{\ln(S_{max} - S_{min} + 1)}{5}\right) λadapt=λ⋅(1+5ln(Smax−Smin+1))

当代价分布范围较大时，温度自动升高以增加探索；当分布集中时，温度降低以加速收敛。

3.7 阻塞检测与故障恢复

3.7.1 全轨迹碰撞判定

定义硬碰撞比例：

ρ h a r d = N h a r d K \rho_{hard} = \frac{N_{hard}}{K} ρhard=KNhard

软阻塞比例：

ρ s o f t = N h a r d + N m a r g i n K \rho_{soft} = \frac{N_{hard} + N_{margin}}{K} ρsoft=KNhard+Nmargin

阻塞条件：

Blocked = ( ρ h a r d ≥ θ h a r d ) ∨ ( ρ s o f t ≥ θ s o f t ∧ C n e a r ≥ N c o n f i r m ) \text{Blocked} = (\rho_{hard} \geq \theta_{hard}) \lor (\rho_{soft} \geq \theta_{soft} \land C_{near} \geq N_{confirm}) Blocked=(ρhard≥θhard)∨(ρsoft≥θsoft∧Cnear≥Nconfirm)

其中 C n e a r C_{near} Cnear 为连续软阻塞帧计数， N c o n f i r m N_{confirm} Nconfirm 为确认阈值。

3.7.2 原地打转检测

计算优化后控制序列的平均角速度/线速度比率：

spinning = ω ˉ v ˉ > θ r a t i o \text{spinning} = \frac{\bar{\omega}}{\bar{v}} > \theta_{ratio} spinning=vˉωˉ>θratio

若连续 N s p i n N_{spin} Nspin 帧检测到打转，则判定为阻塞，输出零速度。

---

4. 项目结构与核心代码实现

4.1 项目文件结构

JZJ_SA_MPPI/
├── RC-ESDF/                          # RC-ESDF核心库（独立模块）
│   ├── include/
│   │   └── rc_esdf.h                 # RC-ESDF头文件
│   ├── src/
│   │   └── rc_esdf.cpp               # RC-ESDF实现
│   ├── main.cpp                      # RC-ESDF独立测试程序
│   ├── CMakeLists.txt
│   └── README.md
│
└── src/
    └── mppi_laser_example/           # SA MPPI ROS1节点
        ├── include/
        │   └── mppi_controller.hpp   # 完整MPPI控制器（~3900行）
        ├── src/
        │   └── mppi_ros1_node.cpp    # ROS1节点封装
        ├── scripts/
        │   ├── mppi_path_publisher.py    # 路径发布器（直线/矩形/S形）
        │   ├── JZJ_path.py               # 圆弧-直线路径生成器
        │   └── clean_path.py             # 路径清理工具
        ├── config/
        │   ├── mppi_params.yaml      # 完整参数配置
        │   └── mppi_test.rviz        # RViz配置
        ├── xsimd/                    # xsimd头文件库（SIMD加速）
        ├── xtensor/                  # xtensor头文件库（张量运算）
        ├── xtl/                      # xtl头文件库（基础工具）
        ├── CMakeLists.txt
        └── package.xml

4.2 RC-ESDF核心实现

4.2.1 类定义（rc_esdf.h）

class RcEsdfMap {
public:
    void initialize(double width_m, double height_m, double resolution);
    void generateFromPolygon(const std::vector<Eigen::Vector2d>& polygon);
    bool query(const Eigen::Vector2d& pos_body, double& dist, Eigen::Vector2d& grad) const;
    double lookupDistance(const Eigen::Vector2d& pos_body) const;
    void visualizeEsdf(const std::vector<Eigen::Vector2d>& footprint);

private:
    double resolution_;
    double width_m_, height_m_;
    double origin_x_, origin_y_;
    int grid_size_x_, grid_size_y_;
    std::vector<float> data_;  // 一维数组存储2D栅格
};

4.2.2 距离场生成（rc_esdf.cpp）

void RcEsdfMap::generateFromPolygon(const std::vector<Eigen::Vector2d>& polygon) {
    for (int y = 0; y < grid_size_y_; ++y) {
        for (int x = 0; x < grid_size_x_; ++x) {
            // 计算栅格中心物理坐标
            double px = origin_x_ + (x + 0.5) * resolution_;
            double py = origin_y_ + (y + 0.5) * resolution_;
            Eigen::Vector2d p(px, py);

            // 1. 计算到多边形轮廓的最小距离
            double min_dist_sq = std::numeric_limits<double>::max();
            for (size_t i = 0; i < polygon.size(); ++i) {
                Eigen::Vector2d v1 = polygon[i];
                Eigen::Vector2d v2 = polygon[(i + 1) % polygon.size()];
                double d_sq = pointToSegmentDistSq(p, v1, v2);
                if (d_sq < min_dist_sq) min_dist_sq = d_sq;
            }
            double min_dist = std::sqrt(min_dist_sq);

            // 2. 内外判定 + 符号赋值
            if (isPointInPolygon(p, polygon)) {
                data_[y * grid_size_x_ + x] = -min_dist;  // 内部为负
            } else {
                data_[y * grid_size_x_ + x] = min_dist;   // 外部为正
            }
        }
    }
}

4.2.3 双线性插值查询

bool RcEsdfMap::query(const Eigen::Vector2d& pos_body, double& dist, Eigen::Vector2d& grad) const {
    double gx, gy;
    posToGrid(pos_body, gx, gy);
    double u = gx - 0.5;
    double v = gy - 0.5;

    if (u < 0 || u >= grid_size_x_ - 1 || v < 0 || v >= grid_size_y_ - 1) {
        dist = 100.0;
        grad.setZero();
        return false;
    }

    int x0 = std::floor(u);
    int y0 = std::floor(v);
    double alpha = u - x0;
    double beta  = v - y0;

    float v00 = getRaw(x0, y0);
    float v10 = getRaw(x0 + 1, y0);
    float v01 = getRaw(x0, y0 + 1);
    float v11 = getRaw(x0 + 1, y0 + 1);

    // 双线性插值
    dist = (1 - alpha) * (1 - beta) * v00 +
           alpha       * (1 - beta) * v10 +
           (1 - alpha) * beta       * v01 +
           alpha       * beta       * v11;

    // 数值梯度
    double d_alpha = (1 - beta) * (v10 - v00) + beta * (v11 - v01);
    double d_beta  = (1 - alpha) * (v01 - v00) + alpha * (v11 - v10);
    grad.x() = d_alpha / resolution_;
    grad.y() = d_beta  / resolution_;

    return true;
}

4.3 SA MPPI核心实现

4.3.1 ObstaclesCritic：形状感知碰撞检测

class ObstaclesCritic : public CriticFunction {
public:
    void setParams(float critical_weight, float collision_cost,
                   float collision_margin, float hard_collision_distance,
                   float cost_scaling, float near_goal_distance,
                   float robot_radius, float grid_resolution,
                   int grid_width, int grid_height,
                   bool consider_footprint = false,
                   const std::vector<Point2D>& footprint = {},
                   float soft_block_distance = 0.30f);

    void score(CriticData & data) override {
        // 三种检测模式：
        // 1. RC-ESDF精确模式（consider_footprint=true，rc_esdf_已初始化）
        // 2. 栅格footprint模式（consider_footprint=true，无rc_esdf_）
        // 3. 圆形机器人模式（consider_footprint=false）

        for (size_t i = 0; i < batch_size; ++i) {
            for (size_t j = 0; j < traj_len; ++j) {
                float px = data.trajectories.x(i, j);
                float py = data.trajectories.y(i, j);
                float ptheta = data.trajectories.yaws(i, j);

                if (consider_footprint_ && rc_esdf_) {
                    // ===== RC-ESDF精确模式 =====
                    FootprintCheckResultRcEsdf result =
                        checkFootprintWithDistanceRcEsdf(px, py, ptheta);

                    if (result.is_collision || result.min_distance < hard_collision_distance_) {
                        trajectory_collide = true;
                        break;
                    }

                    if (result.min_distance < collision_margin_distance_) {
                        float base_cost = computeSoftMarginDecay(result.min_distance);

                        // 梯度引导加权：若运动方向与脱离方向一致，降低惩罚
                        if (j > 0) {
                            float traj_body_x = ...;  // 轨迹在Body Frame的位移
                            float traj_body_y = ...;
                            float dot = (traj_body_x * result.grad_body_x +
                                         traj_body_y * result.grad_body_y) /
                                        (traj_len * grad_len);
                            float gradient_factor = 1.0f + 0.3f * clamp(dot, -1.0f, 1.0f);
                            base_cost *= gradient_factor;
                        }
                        cur_raw += base_cost;
                    }
                }
                // ... 其他两种模式
            }
        }
    }

private:
    FootprintCheckResultRcEsdf checkFootprintWithDistanceRcEsdf(float x, float y, float theta) const {
        FootprintCheckResultRcEsdf result;

        // 将障碍物缓存从旧机器人位姿转换到当前轨迹点
        float delta_theta = theta - obstacle_cache_pose_.theta;
        float cos_dt = std::cos(delta_theta);
        float sin_dt = std::sin(delta_theta);
        float cos_pt = std::cos(theta);
        float sin_pt = std::sin(theta);
        float delta_x = x - obstacle_cache_pose_.x;
        float delta_y = y - obstacle_cache_pose_.y;
        float trans_x =  delta_x * cos_pt + delta_y * sin_pt;
        float trans_y = -delta_x * sin_pt + delta_y * cos_pt;

        for (const auto& cached : obstacle_body_cache_) {
            // 障碍物点在Body Frame下的坐标（考虑机器人旋转和平移）
            float new_body_x =  cached.body_x * cos_dt + cached.body_y * sin_dt + trans_x;
            float new_body_y = -cached.body_x * sin_dt + cached.body_y * cos_dt + trans_y;

            if (std::abs(new_body_x) > rc_esdf_check_range_ ||
                std::abs(new_body_y) > rc_esdf_check_range_)
                continue;

            double dist;
            Eigen::Vector2d grad;
            if (!rc_esdf_->query(Eigen::Vector2d(new_body_x, new_body_y), dist, grad))
                continue;

            float surface_dist = static_cast<float>(dist);
            if (surface_dist < 0.0f) result.is_collision = true;
            if (surface_dist < result.min_distance) {
                result.min_distance = surface_dist;
                result.grad_body_x = static_cast<float>(grad.x());
                result.grad_body_y = static_cast<float>(grad.y());
            }
        }
        return result;
    }
};

4.3.2 优化器核心：轨迹生成与加权更新

class Optimizer {
public:
    Twist2D evalControl(const Pose2D & robot_pose, const Twist2D & robot_speed,
                        const Path & path, const std::vector<bool> * path_pts_valid = nullptr) {
        // 1. 准备：路径裁剪、状态更新
        prepare(robot_pose, robot_speed, path, path_pts_valid);

        // 2. 迭代优化
        for (size_t iter = 0; iter < settings_.iteration_count; ++iter) {
            optimize(fail_flag);

            // 3. 全轨迹碰撞检查
            all_trajectories_collide = fail_flag || checkAllTrajectoriesCollide();

            if (all_trajectories_collide) {
                if (!fallback(need_recovery)) {
                    fatal_blocked_latched_ = true;
                    return Twist2D(0.0f, 0.0f, 0.0f);  // 输出零速度
                }
                prepare(robot_pose, robot_speed, path, path_pts_valid_);
            }
        }

        // 4. 阻塞检测（原地打转）
        if (isBlocked()) {
            control_sequence_.reset(settings_.time_steps);
            return Twist2D(0.0f, 0.0f, 0.0f);
        }

        // 5. SG滤波平滑
        if (settings_.use_sg_filter) {
            savitskyGolayFilter(control_sequence_, control_history_, settings_);
        }

        return getControlFromSequence();
    }

private:
    void optimize(bool& fail_flag) {
        // 1. 生成带噪声轨迹
        generateNoisedTrajectories();

        // 2. 找到最远可达路径点（Nav2风格）
        std::optional<size_t> furthest = findFurthestReachedPathPoint();

        // 3. 构建CriticData并评分
        CriticData data{state_, generated_trajectories_, path_, costs_,
                        settings_.model_dt, false, motion_model_, furthest, &dynamic_obstacles_};
        data.path_pts_valid = path_pts_valid_;
        critic_manager_->evalTrajectoriesScores(data);

        fail_flag = data.fail_flag;
        // 缓存障碍物统计
        last_obstacle_hard_collision_count_ = data.obstacle_hard_collision_count;
        last_obstacle_margin_only_count_ = data.obstacle_margin_only_count;
        last_obstacle_safe_count_ = data.obstacle_safe_count;

        // 4. 加权更新控制序列
        updateControlSequence();
    }

    void updateControlSequence() {
        // 1. 添加控制代价（正则项）
        auto bounded_noises_vx = state_.cvx - xt::view(control_sequence_.vx, xt::newaxis(), xt::all());
        costs_ += settings_.gamma / (settings_.sampling_std.vx * settings_.sampling_std.vx) *
                  xt::sum(xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1);

        // 2. 计算softmax权重（min归一化）
        float min_cost = xt::amin(costs_)(0);
        float T = settings_.temperature;

        // 自适应温度
        if (settings_.adaptive_temperature) {
            float cost_range = xt::amax(costs_)(0) - min_cost;
            if (cost_range > EPSILON) {
                float adaptive_factor = std::log(cost_range + 1.0f) / 5.0f;
                T = clamp(settings_.temperature * (1.0f + adaptive_factor),
                         settings_.adaptive_temperature_min,
                         settings_.adaptive_temperature_max);
            }
        }

        xt::xtensor<float, 1> weights = xt::exp(-(costs_ - min_cost) / T);
        weights /= xt::sum(weights)(0);

        // 3. 加权平均更新控制序列
        for (size_t j = 0; j < settings_.time_steps; ++j) {
            float sum_vx = 0.0f, sum_wz = 0.0f;
            for (size_t i = 0; i < settings_.batch_size; ++i) {
                sum_vx += weights(i) * state_.cvx(i, j);
                sum_wz += weights(i) * state_.cwz(i, j);
            }
            control_sequence_.vx(j) = sum_vx;
            control_sequence_.wz(j) = sum_wz;
        }
    }

    // 多线程轨迹积分
    void integrateStateVelocities(Trajectories & trajectories, const State & state) const {
        unsigned int num_threads = settings_.thread_count;
        unsigned int batch_per_thread = (settings_.batch_size + num_threads - 1) / num_threads;

        auto worker = [&](unsigned int start, unsigned int end) {
            for (size_t i = start; i < end; ++i) {
                float yaw = state.pose.theta;
                float x = state.pose.x;
                float y = state.pose.y;

                for (size_t j = 1; j < time_steps; ++j) {
                    yaw += state.wz(i, j-1) * model_dt;
                    float cos_yaw = std::cos(yaw);
                    float sin_yaw = std::sin(yaw);
                    float vx = state.vx(i, j-1);
                    float vy = isHolonomic() ? state.vy(i, j-1) : 0.0f;

                    x += (vx * cos_yaw - vy * sin_yaw) * model_dt;
                    y += (vx * sin_yaw + vy * cos_yaw) * model_dt;

                    trajectories.x(i, j) = x;
                    trajectories.y(i, j) = y;
                    trajectories.yaws(i, j) = yaw;
                }
            }
        };

        std::vector<std::thread> threads;
        for (unsigned int t = 0; t < num_threads; ++t) {
            unsigned int start = t * batch_per_thread;
            unsigned int end = std::min(start + batch_per_thread, settings_.batch_size);
            if (start < end) threads.emplace_back(worker, start, end);
        }
        for (auto & t : threads) t.join();
    }
};

4.3.3 全向/差速模式自动切换

class MPPIController {
private:
    void updateMotionModel(const Pose2D& robot_pose, const std::vector<Point2D>& obstacles) {
        if (!settings_.enable_omni_switching) return;

        // 计算最近障碍物距离和路径偏离
        min_obstacle_distance_ = computeMinObstacleDistance(robot_pose, obstacles);
        current_path_deviation_ = computePathDeviation(robot_pose);

        bool need_omni = (min_obstacle_distance_ < settings_.omni_trigger_obstacle_dist) ||
                         (current_path_deviation_ > settings_.omni_trigger_path_deviation);

        bool can_restore_diff = (min_obstacle_distance_ > settings_.omni_trigger_obstacle_dist * 1.2f) &&
                                (current_path_deviation_ < settings_.diff_restore_path_threshold);

        // 延迟切换防止抖动
        if (need_omni && !omni_mode_active_) {
            if (++omni_switch_counter_ >= settings_.omni_switch_delay_frames) {
                switchToOmniMode();
            }
        } else if (can_restore_diff && omni_mode_active_) {
            if (++omni_switch_counter_ >= settings_.omni_switch_delay_frames) {
                switchToDiffMode();
            }
        } else {
            omni_switch_counter_ = 0;
        }
    }

    void switchToOmniMode() {
        omni_mode_active_ = true;
        optimizer_->softReset();
        auto new_model = std::make_shared<OmniMotionModel>();
        optimizer_->setMotionModel(new_model);
        motion_model_ = new_model;
        // 更新ConstraintCritic为全向模式
        // ...
    }
};

4.4 ROS1节点封装

class MPPIRos1Node {
private:
    void controlLoop(const ros::TimerEvent&) {
        // 1. 将激光点转换到全局坐标系
        std::vector<mppi::Point2D> global_points;
        for (const auto& pt : laser_points_relative_) {
            float global_x = robot_x + pt.x * cos_theta - pt.y * sin_theta;
            float global_y = robot_y + pt.x * sin_theta + pt.y * cos_theta;
            global_points.emplace_back(global_x, global_y);
        }

        // 2. 更新障碍物（触发RC-ESDF缓存重建）
        controller_->updateStaticObstacles(global_points, robot_pose_);

        // 3. 计算控制指令
        mppi::Twist2D cmd = controller_->computeVelocityCommands(robot_pose_, robot_speed_);

        // 4. 启动辅助（防止初始静止 stuck）
        tryApplyStartupAssist(cmd, global_points.empty(), dist_to_goal);

        // 5. 发布速度指令
        geometry_msgs::Twist twist_msg;
        twist_msg.linear.x = cmd.vx;
        twist_msg.linear.y = cmd.vy;
        twist_msg.angular.z = cmd.wz;
        cmd_vel_pub_.publish(twist_msg);

        // 6. 发布调试轨迹（可视化）
        publishDebugTrajectory();
    }
};

---

5. 运行步骤与环境配置

5.1 系统依赖

# Ubuntu 20.04 + ROS Noetic（推荐）
# 或 Ubuntu 18.04 + ROS Melodic

# 1. 安装ROS基础环境
sudo apt update
sudo apt install -y ros-noetic-desktop-full

# 2. 安装编译依赖
sudo apt install -y \
    build-essential cmake git \
    libeigen3-dev \
    libopenblas-dev liblapack-dev \
    libopencv-dev \
    python3-catkin-tools

# 3. 创建工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
git clone <your-repo-url> JZJ_SA_MPPI
cd ~/catkin_ws

5.2 编译项目

cd ~/catkin_ws

# 方式1：使用catkin_make
catkin_make

# 方式2：使用catkin_tools（推荐，支持并行编译）
catkin build mppi_laser_example

#  source环境
source devel/setup.bash

5.3 配置参数

编辑 src/mppi_laser_example/config/mppi_params.yaml：

# 核心参数示例
mppi:
  batch_size: 1500          # 采样轨迹数
  time_steps: 55            # 预测步数
  model_dt: 0.05            # 时间步长（50ms）
  temperature: 0.2          # 温度参数
  gamma: 0.015              # 控制代价权重

constraints:
  vx_max: 1.5               # 最大线速度
  vx_min: -0.2              # 最小线速度（允许倒车）
  wz_max: 4.0               # 最大角速度

obstacles:
  consider_footprint: true  # 启用形状感知（RC-ESDF）
  robot_radius: 0.40
  footprint:                # 机器人footprint多边形
    - [-0.56, -0.30]
    - [ 0.26, -0.30]
    - [ 0.26,  0.30]
    - [-0.56,  0.30]
  critical_weight: 20.0
  collision_margin: 0.55
  hard_collision_distance: 0.20

5.4 运行演示

# 终端1：启动ROS核心
roscore

# 终端2：启动路径发布器（生成测试路径）
rosrun mppi_laser_example mppi_path_publisher.py

# 终端3：启动MPPI控制器
roslaunch mppi_laser_example mppi_controller.launch

# 终端4：启动RViz可视化
rviz -d $(rospack find mppi_laser_example)/config/mppi_test.rviz

5.5 与仿真器/真实机器人对接

话题名	类型	方向	说明
/plan	nav_msgs/Path	Sub	全局路径输入
/odom	nav_msgs/Odometry	Sub	机器人里程计
/scan	sensor_msgs/LaserScan	Sub	激光雷达数据
/cmd_vel	geometry_msgs/Twist	Pub	输出速度指令
/debug_optimal_trajectory	nav_msgs/Path	Pub	最优轨迹可视化

---

6. 实验结果与性能评估

6.1 RC-ESDF查询性能

在普通移动端CPU（Intel i5-8250U）上的测试：

测试项	结果
单次 query(dist, grad)	~2.4 μs
单次 lookupDistance()	~1.8 μs
1000次查询（批量）	~2.2 ms
地图分辨率	0.05 m
地图尺寸	5m × 5m（100×100栅格）

注：上述性能满足20Hz控制频率需求（每帧50ms内可完成数千次查询）。

6.2 MPPI优化性能

参数配置	单次优化耗时	说明
K=1500, T=55, 4线程	~12-18 ms	典型配置
K=1000, T=50, 4线程	~8-12 ms	轻量配置
K=2000, T=80, 8线程	~25-35 ms	高精度配置

6.3 形状感知 vs 圆形模型对比

场景	圆形模型	SA MPPI (RC-ESDF)
0.8m宽通道，矩形机器人(1.1m×0.6m)	无法通过（圆形直径>通道）	顺利通过
L形障碍物，突出传感器	传感器碰撞风险	精确保护凸出结构
狭窄走廊直角转弯	提前停止或碰撞	贴边精确转弯

6.4 可视化效果

通过RViz可观察以下调试信息：

• 最优轨迹 （/debug_optimal_trajectory）：绿色路径，显示MPPI规划的未来轨迹。
• 采样轨迹（需扩展发布）：大量灰色候选轨迹，显示搜索空间。
• 障碍物点云 ：红色点，从 /scan 转换到全局坐标系。
• 路径有效性：被阻挡的路径点显示为红色，有效点为绿色。

---

7. 结论与展望

7.1 项目成果总结

本项目成功实现了 Shape-Aware MPPI (SA MPPI) 算法，主要成果包括：

1. RC-ESDF高效集成：将FAST-Lab的RC-ESDF算法无缝嵌入MPPI框架，实现了对任意多边形 footprint 的 O(1) 精确碰撞检测。
2. 三段式安全策略：硬碰撞/软阻塞/安全区的分层策略，在保证安全的同时最大化通过性。
3. 梯度引导优化：利用RC-ESDF的解析梯度调整代价权重，使优化器具备"方向感知"能力。
4. 工业级鲁棒性：阻塞检测、fallback恢复、模式自动切换、SG滤波等多重机制确保系统稳定运行。
5. 完整ROS1实现：提供从参数配置到可视化调试的完整工具链，可直接部署于真实机器人。

7.2 未来改进方向

方向	描述
3D RC-ESDF扩展	当前为2D实现，可扩展至3D以支持无人机、机械臂等三维避障场景
动态障碍物预测	集成行人/车辆运动预测模型，提升动态环境下的安全性
GPU并行加速	利用CUDA对轨迹积分和代价评估进行GPU加速，支持更大batch size
学习型Critic	使用神经网络学习更复杂的场景代价，如社交约束、地形可通行性
多机器人协同	扩展至多机器人系统，引入互避碰代价项
在线ESDF更新	当前为离线生成，可探索局部ESDF在线更新以适应变形机器人

7.3 参考文献

1. Williams, G., et al. "Information theoretic MPC for model-based reinforcement learning." ICRA 2017.
2. Zhou, W., Gao, F., et al. "Robo-centric ESDF: A Fast and Accurate Whole-body Collision Evaluation Tool for Any-shape Robotic Planning." IEEE RAL.
3. Nav2 MPPI Controller: https://github.com/ros-navigation/navigation2

---

开源声明：本项目基于 Apache 2.0 License 开源，欢迎社区贡献与反馈。