MPPI 局部规划器实验设计讲解

MPPI 作为局部规划器的优势、对比实验设计与 RC-ESDF 融合分析

---

摘要：

1、可以结合多目标检测与跟踪，对不同目标设定不同的MPPI参数等；

---

目录

1. [MPPI 作为局部规划器的优势分析](#MPPI 作为局部规划器的优势分析)
2. 纯全局路径跟踪实验如何设计
3. [非理想全局路径下的 MPPI 优势验证实验](#非理想全局路径下的 MPPI 优势验证实验)
4. [RC-ESDF 融合后能提升哪些能力](#RC-ESDF 融合后能提升哪些能力)
5. [MPPI + RC-ESDF 经过复杂空间的实现思路](#MPPI + RC-ESDF 经过复杂空间的实现思路)
6. 推荐最终实验体系

---

1. MPPI 作为局部规划器的优势分析

1.1 基本问题

问题是：

MPPI 作为局部规划器，去纯跟踪全局路径，然后遇到障碍物进行避障，对比 DWA、TEB、MPC 有什么优势？

可以这样理解：MPPI 作为局部规划器的优势，不在于"单纯跟踪路径"一定比所有算法都强，而在于它可以在跟踪全局路径的同时，把避障、平滑性、动力学约束、速度偏好、动态障碍物预测等多个目标统一写进代价函数里，然后通过采样优化选择当前最优控制。

如果只是普通小车、低速、障碍物简单，DWA、TEB、MPC 都可以完成任务。但如果希望在复杂障碍物环境下做更灵活的局部规划，尤其是结合 RC-ESDF 这种非圆形机器人精确碰撞代价，MPPI 的优势会更明显。

---

1.2 MPPI 做局部规划的基本逻辑

MPPI 每次控制周期都会从当前状态出发，随机采样很多组控制序列：

U i = { u i , 0 , u i , 1 , . . . , u i , T − 1 } U_i = \{u_{i,0}, u_{i,1}, ..., u_{i,T-1}\} Ui={ui,0,ui,1,...,ui,T−1}

然后用车辆或机器人运动模型前向 rollout：

x t + 1 = f ( x t , u t ) x_{t+1} = f(x_t, u_t) xt+1=f(xt,ut)

得到大量候选轨迹：

τ i = { x i , 0 , x i , 1 , . . . , x i , T } \tau_i = \{x_{i,0}, x_{i,1}, ..., x_{i,T}\} τi={xi,0,xi,1,...,xi,T}

再对每条轨迹计算总代价：

J i = J p a t h + J g o a l + J o b s + J s m o o t h + J d y n a m i c + J c o n s t r a i n t J_i = J_{path} + J_{goal} + J_{obs} + J_{smooth} + J_{dynamic} + J_{constraint} Ji=Jpath+Jgoal+Jobs+Jsmooth+Jdynamic+Jconstraint

最后用 softmax 权重更新控制：

w i = exp ⁡ ( − 1 λ J i ) ∑ j exp ⁡ ( − 1 λ J j ) w_i = \frac{ \exp\left(-\frac{1}{\lambda}J_i\right) }{ \sum_j \exp\left(-\frac{1}{\lambda}J_j\right) } wi=∑jexp(−λ1Jj)exp(−λ1Ji)

u ∗ = ∑ i w i u i u^* = \sum_i w_i u_i u∗=i∑wiui

所以 MPPI 不是像 DWA 那样简单搜索一个速度，也不是像传统 MPC 那样强依赖可微优化器，而是：

采样大量可能控制 → 前向预测轨迹 → 多 critic 打分 → 选出综合代价最低的控制。

---

1.3 MPPI 与 DWA 的对比

DWA 的核心是：在当前速度附近采样若干个 ((v, \omega))，然后短时间模拟，选一个评分最高的速度。

DWA 的优点是：

• 简单；
• 实时性好；
• 工程成熟；
• ROS 中应用广泛。

但 DWA 的问题也比较明显：

1. 速度空间采样比较局部，容易短视；
2. 对复杂动力学支持弱；
3. 轨迹形态比较单一；
4. 遇到狭窄通道、动态障碍物、非圆形机器人时容易抖动；
5. 多目标代价很难设计得特别自然。

MPPI 相比 DWA 的优势如下：

对比点	DWA	MPPI
优化对象	当前速度 ((v, \omega))	一整段控制序列
预测能力	较短、较局部	可预测未来多个时间步
动力学模型	通常较简单	可接入非线性模型
轨迹多样性	较少	采样大量复杂轨迹
避障能力	依赖局部评分	可加入复杂 obstacle critic
平滑性	需要额外调速度惩罚	可直接惩罚控制序列变化
复杂场景	容易陷入局部最优	更容易从多种绕行方向中选择

例如：车前方有障碍物，左边和右边都可以绕。DWA 可能只在当前速度附近试探，容易因为短视而停住或者左右抖。MPPI 会同时采样很多未来控制序列，有些往左绕，有些往右绕，有些减速，有些轻微转向，它可以比较整段轨迹的总代价，所以更容易选出一条连续的绕障轨迹。

---

1.4 MPPI 与 TEB 的对比

TEB，即 Timed Elastic Band，本质是对一条带时间信息的局部轨迹进行优化。它会把轨迹看成一串状态点，然后优化这些状态点的位置、时间间隔、速度、加速度、障碍物距离等。

TEB 的优点是：

1. 轨迹优化思路清晰；
2. 输出路径相对平滑；
3. 对非完整约束支持较好；
4. 在 ROS 导航里应用很多。

TEB 的问题是：

1. 对初始轨迹依赖较强；
2. 容易卡在局部最优；
3. 多障碍物、动态障碍物、窄通道场景下容易变形失败；
4. 优化问题复杂时容易求解不稳定；
5. 对复杂非凸代价不如采样方法自然。

MPPI 相比 TEB 的优势如下：

对比点	TEB	MPPI
优化方式	轨迹点连续优化	控制序列采样优化
初值依赖	比较强	相对弱
非凸场景	容易陷局部最优	采样多模态，抗局部最优更好
动态障碍物	可支持，但复杂	可直接把预测轨迹写进 critic
复杂代价	需要可优化形式	critic 可以很自由
模型扩展	有一定限制	可接入任意 rollout 模型
避障策略	轨迹形变避障	采样控制自然绕障

TEB 更像是：

"我已经有一条初始路径了，我把它像橡皮筋一样拉开，尽量避开障碍物。"

MPPI 更像是：

"我从当前状态开始尝试很多种未来控制动作，看哪一种综合效果最好。"

所以在障碍物复杂、绕行方向不确定、动态障碍物较多时，MPPI 往往更自然。

但需要注意：TEB 在低速差速小车、静态环境、普通 ROS 导航任务里非常实用，调好了并不差。MPPI 的优势更多体现在复杂代价和复杂动力学场景。

---

1.5 MPPI 与传统 MPC 的对比

传统 MPC 一般会把问题写成优化形式：

min ⁡ u 0 : T − 1 ∑ t = 0 T l ( x t , u t ) \min_{u_{0:T-1}} \sum_{t=0}^{T} l(x_t, u_t) u0:T−1mint=0∑Tl(xt,ut)

约束为：

x t + 1 = f ( x t , u t ) x_{t+1} = f(x_t, u_t) xt+1=f(xt,ut)

u m i n ≤ u t ≤ u m a x u_{min} \le u_t \le u_{max} umin≤ut≤umax

g ( x t ) ≤ 0 g(x_t) \le 0 g(xt)≤0

如果模型线性、代价二次、约束简单，可以做成 QP，求解很快；如果是非线性模型，就是 NMPC，通常要用 CasADi、IPOPT、ACADOS 等求解器。

传统 MPC 的优点是：

1. 理论性强；
2. 约束处理严谨；
3. 控制精度高；
4. 对车辆轨迹跟踪非常成熟；
5. 适合结构化道路、车辆控制、机器人运动控制。

传统 MPC 的问题是：

1. 对模型准确性要求较高；
2. 非线性、非凸障碍物约束会让求解变难；
3. 需要可微代价和约束；
4. 局部最优明显；
5. 动态障碍物和复杂地图代价接入比较麻烦。

MPPI 相比传统 MPC 的优势如下：

对比点	传统 MPC / NMPC	MPPI
优化方式	数值优化器求解	采样优化
是否需要可微	通常需要	不需要
非凸障碍物代价	难处理	critic 可直接写
动态障碍物	约束建模复杂	预测后直接加代价
局部最优	比较明显	采样可缓解
工程实现	依赖求解器	并行 rollout + 打分
任意形状 footprint	建约束复杂	可以用 RC-ESDF critic
GPU/多线程加速	不一定容易	很适合并行

所以 MPPI 特别适合这种情况：

不想把所有障碍物约束都写成严格可微的数学约束，只想判断这条候选轨迹好不好，然后用 critic 给它打分。

这也是 RC-ESDF 和 MPPI 比较适合结合的原因。RC-ESDF 可以直接查询距离：

d = S D F ( p ) d = SDF(p) d=SDF(p)

然后构造：

J o b s = { C c o l l i s i o n , d < d h a r d w ( d s a f e − d ) 2 , d h a r d ≤ d < d s a f e 0 , d ≥ d s a f e J_{obs} = \begin{cases} C_{collision}, & d < d_{hard} \\ w(d_{safe} - d)^2, & d_{hard} \le d < d_{safe} \\ 0, & d \ge d_{safe} \end{cases} Jobs=⎩ ⎨ ⎧Ccollision,w(dsafe−d)2,0,d<dharddhard≤d<dsafed≥dsafe

这个代价不一定要可微，也不一定要写成严格优化约束，MPPI 只要能算出每条轨迹的代价即可。

---

1.6 MPPI 最突出的优势：critic 设计非常自由

这是 MPPI 做局部规划最重要的优势。

一条候选轨迹可以叠加很多 critic：

1. 路径跟踪代价；
2. 目标靠近代价；
3. 障碍物距离代价；
4. RC-ESDF footprint 碰撞代价；
5. 速度平滑代价；
6. 加速度平滑代价；
7. 角速度惩罚；
8. 动态障碍物预测代价；
9. 低速惩罚；
10. 逆行惩罚；
11. 靠近全局路径方向一致性代价；
12. 不希望贴墙的安全裕度代价。

局部规划总代价可以写成：

J = w p J p a t h + w g J g o a l + w o J o b s + w e J e s d f + w s J s m o o t h + w c J c o n s t r a i n t J = w_p J_{path} + w_g J_{goal} + w_o J_{obs} + w_e J_{esdf} + w_s J_{smooth} + w_c J_{constraint} J=wpJpath+wgJgoal+woJobs+weJesdf+wsJsmooth+wcJconstraint

对于 DWA、TEB、MPC 来说，也能加这些项，但没有 MPPI 这么自然。MPPI 的 critic 不要求特别规整，只要能对轨迹打分就行。

这对 RC-ESDF critic 很有利，因为 RC-ESDF 查询本身可以直接作为轨迹评价函数：

J r c − e s d f = ∑ t = 0 T ϕ ( min ⁡ o k S D F r o b o t ( R ( θ t ) T ( o k − p t ) ) ) J_{rc-esdf}= \sum_{t=0}^{T} \phi \left( \min_{o_k} SDF_{robot} \left( R(\theta_t)^T(o_k - p_t) \right) \right) Jrc−esdf=t=0∑Tϕ(okminSDFrobot(R(θt)T(ok−pt)))

它不需要像传统 MPC 那样把每个障碍物都写成复杂约束。

---

1.7 MPPI 对复杂动力学更友好

DWA 通常更适合简单差速模型：

x t + 1 = x t + v cos ⁡ θ Δ t x_{t+1} = x_t + v\cos\theta \Delta t xt+1=xt+vcosθΔt

y t + 1 = y t + v sin ⁡ θ Δ t y_{t+1} = y_t + v\sin\theta \Delta t yt+1=yt+vsinθΔt

θ t + 1 = θ t + ω Δ t \theta_{t+1} = \theta_t + \omega \Delta t θt+1=θt+ωΔt

但 MPPI 的 rollout 模型可以换成任意形式，比如：

差速模型

u = v , ω u = v, \\omega u=v,ω

阿克曼模型

u = v , δ u = v, \\delta u=v,δ

θ ˙ = v L tan ⁡ δ \dot{\theta} = \frac{v}{L}\tan\delta θ˙=Lvtanδ

全向底盘模型

u = v x , v y , ω u = v_x, v_y, \\omega u=vx,vy,ω

带动力学约束的无人车模型

u = a , δ u = a, \\delta u=a,δ

v ˙ = a \dot{v} = a v˙=a

θ ˙ = v L tan ⁡ δ \dot{\theta} = \frac{v}{L}\tan\delta θ˙=Lvtanδ

这意味着 MPPI 不只是一个"路径跟踪器"，它可以根据真实机器人运动学/动力学模型来预测未来运动。

---

1.8 MPPI 更适合动态障碍物

如果有动态障碍物，可以预测障碍物未来位置：

o k ( t ) o_k(t) ok(t)

然后 MPPI 在每个未来时刻 (t) 检查候选轨迹状态 (x_t) 和障碍物 (o_k(t)) 的距离：

d k , t = ∥ p t − o k ( t ) ∥ d_{k,t} = \|p_t - o_k(t)\| dk,t=∥pt−ok(t)∥

构造动态避障代价：

J d y n = ∑ t ∑ k max ⁡ ( 0 , d s a f e − d k , t ) 2 J_{dyn} = \sum_t \sum_k \max(0, d_{safe} - d_{k,t})^2 Jdyn=t∑k∑max(0,dsafe−dk,t)2

也可以考虑时间到碰撞 TTC：

T T C = d v r e l TTC = \frac{d}{v_{rel}} TTC=vreld

加入：

J t t c = w t t c max ⁡ ( 0 , T s a f e − T T C ) 2 J_{ttc} = w_{ttc} \max(0, T_{safe} - TTC)^2 Jttc=wttcmax(0,Tsafe−TTC)2

MPPI 的好处是：它本来就在预测未来一段轨迹，所以把动态障碍物未来预测一起放进去非常自然。

---

1.9 MPPI 更适合非圆形机器人和复杂 footprint

很多传统局部规划器为了简单，会把机器人看成圆：

d = ∥ p − o ∥ − r d = \|p - o\| - r d=∥p−o∥−r

这对圆形机器人还可以，但对于长方形车体、履带机器人、带机械臂的移动底盘就不够准确。

MPPI 可以很自然地接入 footprint critic：

每个候选轨迹点：
    得到机器人位姿 x, y, theta
    把障碍物点变换到 body frame
    查询 RC-ESDF
    得到 signed distance
    计算代价

即：

p b = R ( θ ) T ( o − p ) p_b = R(\theta)^T(o - p) pb=R(θ)T(o−p)

d = S D F r o b o t ( p b ) d = SDF_{robot}(p_b) d=SDFrobot(pb)

这对复杂 footprint 很有优势。

---

1.10 MPPI 的劣势

MPPI 不是完美的，缺点也明显。

1）算力要求更高

MPPI 要采样很多轨迹，例如：

batch_size = 1000 ~ 5000
time_steps = 30 ~ 80

计算量约为：

N b a t c h × T h o r i z o n × N c r i t i c N_{batch} \times T_{horizon} \times N_{critic} Nbatch×Thorizon×Ncritic

所以需要：

• 障碍物降采样；
• 空间索引；
• 距离场查询；
• 并行计算；
• 提前终止；
• 降低不必要的 critic。

2）参数敏感

MPPI 效果依赖：

• batch size；
• time steps；
• model dt；
• temperature；
• noise std；
• critic weight；
• collision cost；
• 控制约束；
• 速度上下限。

其中 temperature 和 critic weight 很关键。

如果代价太大，softmax 权重会塌缩，只有极少数轨迹有权重，控制会抖。

如果代价太小，避障不明显，机器人会贴障甚至撞障。

3）不能保证严格最优

MPPI 是采样优化，不是严格求解最优控制问题。

如果采样数量不够，或者噪声分布不合理，可能根本采不到好的轨迹。

4）安全约束不是天然硬约束

MPPI 通常通过代价惩罚障碍物，而不是像约束 MPC 那样严格保证：

g ( x ) ≤ 0 g(x) \le 0 g(x)≤0

所以对于安全要求高的场景，需要额外加：

• 紧急制动；
• 碰撞检测兜底；
• 安全距离硬阈值；
• 速度限制；
• 控制屏蔽 safety shield。

---

1.11 什么时候 MPPI 比 DWA / TEB / MPC 更适合？

场景	更推荐
简单差速小车，低速，静态障碍物	DWA 足够
ROS 传统导航，路径平滑要求高	TEB 很实用
结构化道路车辆控制，模型明确	MPC / NMPC
动态障碍物多，局部决策复杂	MPPI 更合适
需要接入复杂非凸代价	MPPI 更合适
非圆形机器人精确避障	MPPI + RC-ESDF 更合适
需要严格安全约束证明	MPC / CBF / Safety Shield 更合适
算力有限、平台简单	DWA / TEB 更稳
复杂动力学、采样并行能力强	MPPI 更合适

---

1.12 对当前研究方向的定位

你现在想做的是：

MPPI 作为局部规划器，主要跟踪全局路径；遇到障碍物时，通过 RC-ESDF critic 提供精确 footprint 避障代价，让 MPPI 选出更平滑、更安全的局部轨迹。

这个方向是合理的。

但不建议把 MPPI 设计成：

只靠 RC-ESDF 避障 + 纯路径跟踪

更合理的结构是：

全局路径
   ↓
MPPI 局部采样
   ↓
Path Follow Critic：保证不偏离全局路径太多
Goal Critic：保证朝目标前进
Progress Critic：防止原地抖动
Env Obstacle Critic：提供远距离障碍物趋势
RC-ESDF Critic：提供近距离精确 footprint 碰撞检测
Smooth Critic：保证速度、角速度、加速度平滑
Constraint Critic：保证速度、加速度、角速度不超限
   ↓
输出 cmd_vel / 控制量

也就是说，RC-ESDF 不应该单独承担全部避障任务。它最适合做：

• 近距离精确碰撞检测；
• 复杂机器人轮廓避障；
• 窄通道安全裕度判断；
• 贴边时的真实距离评估。

---

1.13 小结

MPPI 相比 DWA、TEB、MPC 的最大优势是：它可以用采样方式在未来一段时间内同时考虑路径跟踪、避障、动态障碍物、复杂 footprint、安全距离和平滑控制，不要求代价函数可微，也不强依赖凸优化形式，因此特别适合和 RC-ESDF 这种复杂碰撞距离 critic 结合。

但是它的代价是：

• 算力更高；
• 参数更敏感；
• 安全性需要额外兜底；
• 工程实现比 DWA / TEB 更容易出问题。

---

2. 纯全局路径跟踪实验如何设计

2.1 实验目的

如果要突出 MPPI 在"纯跟踪全局路径"上的优势，实验不能只比"能不能走到终点"，而要重点比较：

同一条全局路径、同一机器人模型、同一速度约束下，谁跟踪得更准、更平滑、更少抖动、更能处理高曲率和扰动。

需要注意：如果只是低速、无障碍、简单直线或大圆弧路径，MPPI 不一定比 MPC、TEB、Pure Pursuit 明显更强。

MPPI 的优势更容易体现在：

1. 高曲率路径跟踪；
2. 较高速度下的路径跟踪；
3. 初始偏差较大时的收敛能力；
4. 控制量平滑性；
5. 存在模型误差、噪声、延迟时的鲁棒性；
6. 同时考虑路径误差、朝向误差、速度约束、控制平滑等多目标。

实验目的可以写为：

为验证 MPPI 局部规划器在全局路径跟踪任务中的性能，本文选取 DWA、TEB、MPC 等典型局部规划/控制算法作为对比方法，在无障碍静态环境下构造多种不同曲率、不同速度、不同初始偏差和不同扰动条件的路径跟踪实验。通过横向误差、航向误差、控制平滑性、速度稳定性、完成时间、约束违背次数和计算耗时等指标，系统评估各算法在纯路径跟踪场景下的精度、稳定性和鲁棒性。

重点是：无障碍环境下先比较纯跟踪能力，不要一开始就加障碍物，否则比较的是避障能力，不是路径跟踪能力。

---

2.2 对比算法选择

建议至少选：

算法	定位	为什么对比
Pure Pursuit	经典几何跟踪算法	简单、常用，作为基础 baseline
Stanley	经典横向控制算法	自动驾驶路径跟踪常用
DWA	传统局部规划器	ROS 中常见，适合对比局部速度采样方法
TEB	优化型局部规划器	ROS 中常见，路径跟踪与平滑性较好
MPC	优化控制方法	理论性强，是 MPPI 最直接对手
MPPI	当前方法	采样式非线性预测控制，支持复杂 critic

如果篇幅有限，建议选择：

DWA + TEB + MPC + MPPI

如果想更像论文实验，可以再加：

Pure Pursuit / Stanley

---

2.3 实验场景设计

实验 1：直线路径跟踪

用于验证基本跟踪能力。

路径类型：直线
路径长度：10 m / 20 m
目标速度：0.3 m/s、0.6 m/s、1.0 m/s
初始偏差：0 m、0.3 m、0.5 m

参考路径：

y = 0 y = 0 y=0

初始状态可以设置为：

x 0 = 0 , y 0 = 0.3 , θ 0 = 10 ∘ x_0 = 0,\quad y_0 = 0.3,\quad \theta_0 = 10^\circ x0=0,y0=0.3,θ0=10∘

这个实验中，MPPI 不一定最有优势，但可以证明：

• MPPI 不比传统方法差；
• MPPI 在较大初始偏差下收敛更平滑；
• MPPI 控制量变化更连续。

---

实验 2：圆弧路径跟踪

用于测试算法对曲率的适应能力。

路径类型：圆弧
半径：2 m、3 m、5 m
速度：0.3 m/s、0.6 m/s、1.0 m/s

参考路径：

x = R cos ⁡ s x = R\cos s x=Rcoss

y = R sin ⁡ s y = R\sin s y=Rsins

曲率为：

κ = 1 R \kappa = \frac{1}{R} κ=R1

半径越小，曲率越大，跟踪难度越高。

该实验可以突出：

• MPPI 可以通过预测未来一段轨迹提前调整速度和角速度；
• DWA 容易出现局部转向不足或抖动；
• TEB 在高曲率和速度较高时可能出现轨迹拉扯；
• MPC 如果模型和权重合适，效果会很好，是强 baseline。

---

实验 3：S 型路径跟踪

这个实验最适合突出 MPPI 的优势。

参考路径可以设为：

y = A sin ⁡ ( ω x ) y = A\sin(\omega x) y=Asin(ωx)

例如：

A = 1.0 m
ω = 0.5
路径长度 = 20 m

S 型路径特点是：

1. 曲率连续变化；
2. 左右转向频繁；
3. 对角速度平滑性要求高；
4. 对前瞻能力要求高。

这类路径下，DWA 容易出现局部跟踪抖动；Pure Pursuit 容易在弯道内外摆；TEB 有时会拉直路径；MPC 效果较好，但对模型和参数敏感；MPPI 如果 critic 设计合理，通常可以表现出较好的平滑性和鲁棒性。

---

实验 4：急弯路径 / U 型路径跟踪

用于测试高曲率极限。

路径类型：U 型弯 / 直角弯 / 发卡弯
最小转弯半径：接近机器人运动约束极限
速度：0.3 m/s、0.6 m/s

可以设计一个路径：

直线 5 m
左转 90°
直线 5 m
再左转 90°
直线 5 m

这个实验能突出：

• MPPI 能在未来预测时域内提前减速和转向；
• DWA 容易靠近弯点才反应；
• TEB 可能在角点附近局部震荡；
• MPC 如果没有良好的速度规划，容易超调。

---

实验 5：大初始偏差恢复实验

设置机器人初始状态偏离全局路径：

横向偏差：0.3 m、0.5 m、1.0 m
航向偏差：10°、30°、60°

比较机器人回到全局路径的过程。

重点观察：

• 是否能稳定回到路径；
• 是否发生大幅摆动；
• 是否出现控制饱和；
• 是否有明显过冲；
• 恢复时间是多少。

MPPI 的优势可以体现在：

• 不是只看当前误差，而是评估未来一整段控制序列；
• 可以选择更平滑的回归路径；
• 在大偏差恢复时不容易出现猛烈转向。

---

实验 6：速度提升实验

同一条路径，逐渐提高期望速度。

例如 S 型路径：

v_ref = 0.2 m/s
v_ref = 0.4 m/s
v_ref = 0.6 m/s
v_ref = 0.8 m/s
v_ref = 1.0 m/s

对比不同算法在速度升高后的性能退化。

统计：

• 最大可稳定跟踪速度；
• 横向误差随速度变化曲线；
• 控制抖动随速度变化曲线；
• 失败率随速度变化曲线。

这个实验非常关键，因为 MPPI 的预测控制优势在高速度下更明显。

---

实验 7：扰动鲁棒性实验

为了突出 MPPI 的鲁棒性，可以加入扰动，但仍然不加障碍物。

扰动包括：

1. 位姿观测噪声；
2. 速度执行误差；
3. 控制延迟；
4. 轮速打滑；
5. 模型参数误差。

例如：

v r e a l = v c m d ( 1 + ϵ v ) v_{real} = v_{cmd}(1 + \epsilon_v) vreal=vcmd(1+ϵv)

ω r e a l = ω c m d ( 1 + ϵ ω ) \omega_{real} = \omega_{cmd}(1 + \epsilon_\omega) ωreal=ωcmd(1+ϵω)

其中：

ϵ v , ϵ ω ∼ N ( 0 , σ 2 ) \epsilon_v, \epsilon_\omega \sim \mathcal{N}(0, \sigma^2) ϵv,ϵω∼N(0,σ2)

可以设置：

无噪声
小噪声：σ = 0.03
中噪声：σ = 0.05
大噪声：σ = 0.10

这个实验可以体现：

• MPPI 通过在线滚动优化不断修正控制序列；
• 对模型误差和扰动具有一定鲁棒性；
• 相比固定几何跟踪方法更稳定。

---

2.4 评价指标设计

实验一定不要只看轨迹图，需要量化指标。

1）横向跟踪误差

机器人当前位置到全局路径最近点的距离：

e y ( t ) = min ⁡ p i ∈ P r e f ∥ p ( t ) − p i ∥ e_y(t) = \min_{p_i \in P_{ref}} \|p(t) - p_i\| ey(t)=pi∈Prefmin∥p(t)−pi∥

统计：

R M S E y = 1 N ∑ t = 1 N e y ( t ) 2 RMSE_y = \sqrt{ \frac{1}{N} \sum_{t=1}^{N} e_y(t)^2 } RMSEy=N1t=1∑Ney(t)2

还可以统计：

• 平均横向误差；
• 最大横向误差；
• 95% 横向误差。

示例表格：

算法	平均横向误差/m	RMSE/m	最大横向误差/m
DWA
TEB
MPC
MPPI

---

2）航向误差

参考路径最近点处的切线方向为：

θ r e f ( t ) \theta_{ref}(t) θref(t)

机器人当前航向为：

θ ( t ) \theta(t) θ(t)

航向误差：

e θ ( t ) = w r a p T o P i ( θ ( t ) − θ r e f ( t ) ) e_\theta(t) = wrapToPi(\theta(t) - \theta_{ref}(t)) eθ(t)=wrapToPi(θ(t)−θref(t))

统计：

R M S E θ = 1 N ∑ t = 1 N e θ ( t ) 2 RMSE_\theta = \sqrt{ \frac{1}{N} \sum_{t=1}^{N} e_\theta(t)^2 } RMSEθ=N1t=1∑Neθ(t)2

航向误差可以体现机器人是否"顺着路径方向走"，而不是只是位置接近路径。

---

3）控制平滑性

线速度变化：

Δ v t = v t − v t − 1 \Delta v_t = v_t - v_{t-1} Δvt=vt−vt−1

角速度变化：

Δ ω t = ω t − ω t − 1 \Delta \omega_t = \omega_t - \omega_{t-1} Δωt=ωt−ωt−1

控制变化代价：

J s m o o t h = ∑ t = 1 N ( v t − v t − 1 ) 2 + λ ω ( ω t − ω t − 1 ) 2 J_{smooth} = \sum_{t=1}^{N} \left (v_t - v_{t-1})\^2 + \\lambda_\\omega (\\omega_t - \\omega_{t-1})\^2 \\right Jsmooth=t=1∑N(vt−vt−1)2+λω(ωt−ωt−1)2

也可以统计 jerk：

j v ( t ) = a t − a t − 1 Δ t j_v(t) = \frac{a_t - a_{t-1}}{\Delta t} jv(t)=Δtat−at−1

j ω ( t ) = α t − α t − 1 Δ t j_\omega(t) = \frac{\alpha_t - \alpha_{t-1}}{\Delta t} jω(t)=Δtαt−αt−1

如果 MPPI 的 smooth critic 设计得好，它应该在这个指标上明显优于 DWA 和部分 TEB。

---

4）速度跟踪误差

如果给了期望速度 (v_{ref})，可以统计：

e v ( t ) = v ( t ) − v r e f e_v(t) = v(t) - v_{ref} ev(t)=v(t)−vref

R M S E v = 1 N ∑ t = 1 N e v ( t ) 2 RMSE_v = \sqrt{ \frac{1}{N} \sum_{t=1}^{N} e_v(t)^2 } RMSEv=N1t=1∑Nev(t)2

用于说明：

• MPPI 不仅能跟路径，还能保持稳定速度；
• 不会因为频繁修正航向导致速度忽快忽慢。

---

5）完成时间

统计从起点到终点所需时间：

T f i n i s h T_{finish} Tfinish

但完成时间不能单独使用，因为有些算法跑得快但误差大，有些算法慢但稳。完成时间要和误差一起看。

---

6）约束违背次数

例如：

• 线速度是否超过限制；
• 角速度是否超过限制；
• 加速度是否超过限制；
• 是否出现倒车；
• 是否出现过大角速度；
• 是否距离路径过远。

可以定义：

N v i o l = ∑ t I ( ∣ v t ∣ > v m a x or ∣ ω t ∣ > ω m a x ) N_{viol} = \sum_t \mathbb{I} \left( |v_t| > v_{max} \ \text{or}\ |\omega_t| > \omega_{max} \right) Nviol=t∑I(∣vt∣>vmax or ∣ωt∣>ωmax)

对于 MPPI 来说，如果在 critic 和采样中加入控制约束，它应该表现得比较稳定。

---

7）计算耗时

MPPI 算力更高，所以必须报告计算时间，否则实验不完整。

统计每个控制周期耗时：

• 平均耗时；
• 最大耗时；
• 95% 耗时；
• 控制频率。

表格：

算法	平均耗时/ms	最大耗时/ms	是否满足实时性
DWA			是
TEB			是
MPC			是/否
MPPI			是/否

MPPI 的表达重点不是"算得最快"，而是：

在满足实时性的前提下，获得更好的跟踪精度和平滑性。

---

2.5 推荐实验表格设计

总表可以这样设计：

场景	算法	平均横向误差/m	最大横向误差/m	航向 RMSE/deg	控制平滑代价	完成时间/s	平均耗时/ms
直线	DWA
直线	TEB
直线	MPC
直线	MPPI
S 型	DWA
S 型	TEB
S 型	MPC
S 型	MPPI
U 型	DWA
U 型	TEB
U 型	MPC
U 型	MPPI

鲁棒性表格：

噪声等级	算法	成功率/%	平均误差/m	最大误差/m	控制抖动	是否失稳
无噪声	DWA
无噪声	MPPI
中噪声	DWA
中噪声	MPPI
大噪声	DWA
大噪声	MPPI

---

2.6 结果图建议

至少画以下几类。

1）轨迹对比图

同一张图中画：

• 全局参考路径；
• DWA 实际轨迹；
• TEB 实际轨迹；
• MPC 实际轨迹；
• MPPI 实际轨迹。

重点看：

• 谁偏离路径更少；
• 谁在弯道处过冲更小；
• 谁在 S 型路径中更平滑；
• 谁是否出现明显震荡。

2）横向误差随时间变化图

横轴是时间，纵轴是横向误差：

e y ( t ) e_y(t) ey(t)

希望看到：

• MPPI 的误差峰值更低；
• MPPI 的误差波动更小；
• MPPI 在大初始偏差时收敛更快；
• MPPI 在弯道处误差增长更小。

3）航向误差随时间变化图

横轴时间，纵轴：

e θ ( t ) e_\theta(t) eθ(t)

可以体现算法是否提前调整航向。

4）控制量曲线

画：

• (v(t))；
• (\omega(t))；
• (a(t))；
• 角加速度 (\alpha(t))。

重点看：

• DWA 是否速度跳变明显；
• TEB 是否在局部调整时角速度波动；
• MPC 是否控制更规整；
• MPPI 是否能保持较平滑控制。

如果想突出 MPPI，控制量平滑性一定要画。

5）箱线图 / 柱状图

多个随机初始偏差、多次实验后，用箱线图画：

• 横向 RMSE；
• 最大误差；
• 控制平滑代价；
• 完成时间。

这比单次实验更有说服力。

---

2.7 如何公平设置实验

1）统一机器人模型

所有算法使用同一个运动模型。

差速模型：

x ˙ = v cos ⁡ θ \dot{x} = v\cos\theta x˙=vcosθ

y ˙ = v sin ⁡ θ \dot{y} = v\sin\theta y˙=vsinθ

θ ˙ = ω \dot{\theta} = \omega θ˙=ω

阿克曼模型：

x ˙ = v cos ⁡ θ \dot{x} = v\cos\theta x˙=vcosθ

y ˙ = v sin ⁡ θ \dot{y} = v\sin\theta y˙=vsinθ

θ ˙ = v L tan ⁡ δ \dot{\theta} = \frac{v}{L}\tan\delta θ˙=Lvtanδ

2）统一控制限制

例如：

v_max = 0.8 m/s
v_min = 0.0 m/s
ω_max = 1.2 rad/s
a_max = 1.0 m/s²
α_max = 2.0 rad/s²

3）统一控制频率

例如：

控制频率：20 Hz
仿真步长：0.05 s

4）统一路径点密度

全局路径离散点间距建议：

0.05 m 或 0.1 m

5）统一目标速度

所有算法都跟踪同一个目标速度：

v_ref = 0.5 m/s

也可以做多速度实验。

6）每组实验重复多次

对于含随机扰动的实验，每组至少重复：

10 次或 20 次

然后统计均值和标准差：

μ ± σ \mu \pm \sigma μ±σ

示例表格：

算法	横向 RMSE/m	航向 RMSE/deg	平滑代价
DWA	0.142 ± 0.035	6.8 ± 1.4	2.51 ± 0.42
TEB	0.105 ± 0.028	5.2 ± 1.1	1.84 ± 0.36
MPC	0.083 ± 0.019	4.1 ± 0.8	1.31 ± 0.22
MPPI	0.071 ± 0.016	3.6 ± 0.7	1.08 ± 0.19

---

2.8 如何突出 MPPI 的优势

如果只做一条直线，MPPI 很难突出。建议重点设计下面几组实验。

重点实验 1：高曲率 S 型路径

原因：

• MPPI 预测未来一段控制序列；
• 可以提前看到后续路径曲率变化；
• 通过轨迹 rollout 选择更平滑的速度和角速度；
• 相比 DWA 的短视速度采样更有优势。

实验结论可以写：

在 S 型高曲率路径中，DWA 由于预测时域较短，容易在连续转弯处产生横向误差峰值；TEB 在路径曲率快速变化处存在局部轨迹调整抖动；MPC 跟踪精度较好，但对模型参数和代价权重敏感。MPPI 通过对未来控制序列进行采样预测，并综合路径误差、航向误差和控制平滑代价，在保持较低横向误差的同时获得更平滑的角速度输出。

---

重点实验 2：大初始偏差恢复

原因：

• MPPI 不是只根据当前误差修正；
• 它会预测未来一段轨迹；
• 因此可以选择更自然的回归路径。

实验结论可以写：

在初始横向偏差较大的情况下，MPPI 能够通过预测控制序列在未来时域内逐步减小横向误差，避免了传统几何跟踪方法中常见的急转和过冲现象，表现出更好的误差收敛平滑性。

---

重点实验 3：速度提升实验

原因：

• 速度越高，短视方法越容易滞后；
• MPPI 的预测优势越明显。

实验结论可以写：

随着期望速度升高，各算法横向误差均有所增加，但 MPPI 的误差增长幅度较小，说明其前向预测和采样优化机制能够在较高速度下提前调整控制输入，提升高速路径跟踪稳定性。

---

重点实验 4：扰动鲁棒性实验

原因：

• MPPI 是在线滚动优化；
• 每个周期根据当前状态重新采样和修正；
• 对噪声、延迟、模型误差有一定适应能力。

实验结论可以写：

在加入位姿噪声和控制执行误差后，MPPI 仍能保持较低的平均横向误差和较小的控制抖动，说明其滚动采样优化机制对不确定性具有较好的鲁棒性。

---

2.9 MPPI 的 path tracking critic 设计

纯跟踪实验里，MPPI 的 critic 可以这样设计：

J = w y J y + w θ J θ + w v J v + w u J u + w Δ u J Δ u + w g o a l J g o a l J = w_y J_y + w_\theta J_\theta + w_v J_v + w_u J_u + w_{\Delta u} J_{\Delta u} + w_{goal} J_{goal} J=wyJy+wθJθ+wvJv+wuJu+wΔuJΔu+wgoalJgoal

横向误差代价

J y = ∑ t = 0 T e y ( t ) 2 J_y = \sum_{t=0}^{T} e_y(t)^2 Jy=t=0∑Tey(t)2

航向误差代价

J θ = ∑ t = 0 T e θ ( t ) 2 J_\theta = \sum_{t=0}^{T} e_\theta(t)^2 Jθ=t=0∑Teθ(t)2

速度误差代价

J v = ∑ t = 0 T ( v t − v r e f ) 2 J_v = \sum_{t=0}^{T} (v_t - v_{ref})^2 Jv=t=0∑T(vt−vref)2

控制量代价

J u = ∑ t = 0 T − 1 ( v t 2 + λ ω ω t 2 ) J_u = \sum_{t=0}^{T-1} \left( v_t^2 + \lambda_\omega \omega_t^2 \right) Ju=t=0∑T−1(vt2+λωωt2)

控制变化平滑代价

J Δ u = ∑ t = 1 T − 1 ( v t − v t − 1 ) 2 + λ Δ ω ( ω t − ω t − 1 ) 2 J_{\Delta u}= \sum_{t=1}^{T-1} \left (v_t - v_{t-1})\^2 + \\lambda_{\\Delta\\omega} (\\omega_t - \\omega_{t-1})\^2 \\right JΔu=t=1∑T−1(vt−vt−1)2+λΔω(ωt−ωt−1)2

终点接近代价

J g o a l = ∥ p T − p g o a l ∥ 2 J_{goal} = \|p_T - p_{goal}\|^2 Jgoal=∥pT−pgoal∥2

其中最关键的是：

• 横向误差；
• 航向误差；
• 速度误差；
• 控制平滑。

如果要突出 MPPI 的跟踪效果，不能只靠 path distance，必须加控制平滑和速度保持，否则 MPPI 可能为了贴路径产生很抖的控制。

---

2.10 建议的 MPPI 纯跟踪参数

可以从这个开始：

batch_size: 1500
time_steps: 40
model_dt: 0.05
prediction_horizon: 2.0

temperature: 0.3 ~ 0.7
gamma: 0.01 ~ 0.05

vx_std: 0.2
wz_std: 0.4

vx_max: 0.8
vx_min: 0.0
wz_max: 1.5

ax_max: 1.0
az_max: 2.0

path_weight: 8.0
heading_weight: 4.0
velocity_weight: 2.0
control_weight: 0.2
control_smooth_weight: 1.0
goal_weight: 3.0

调参原则：

现象	调整方向
横向误差大	提高 path_weight
转向滞后	提高 heading_weight 或增加预测时域
速度波动大	提高 velocity_weight
控制抖动大	提高 control_smooth_weight
反应太慢	降低 smooth_weight 或提高采样噪声
局部摆动	降低 path_weight，增加 heading_weight 和 smooth_weight

---

2.11 DWA、TEB、MPC 怎么设置才公平

DWA

让 DWA 在空地图中只做路径跟踪：

• 关闭障碍物层或保证无障碍物；
• 保留 path distance cost；
• 保留 goal distance cost；
• 设置相同速度限制；
• 设置相同控制频率。

但要注意：DWA 本身不是严格路径跟踪控制器，它更像速度空间局部搜索方法，所以在复杂曲率下可能弱一些。

TEB

TEB 需要设置：

• 无障碍环境；
• 相同 max_vel_x；
• 相同 max_vel_theta；
• 相同 acc_lim_x；
• 相同 acc_lim_theta；
• 相同 global path；
• 相同控制频率。

TEB 的路径会被优化变形，所以要看它是否偏离全局路径。

如果要突出"严格跟踪全局路径"，就要提高 TEB 的 global path following 权重。否则 TEB 可能为了优化局部轨迹把路径拉直，横向误差会变大。

MPC

MPC 是最强 baseline。若 MPPI 能比 MPC 好，说服力最强；但现实中 MPC 在纯跟踪任务上通常很强。

公平设置：

• MPC 使用同样的运动模型；
• 同样预测时域；
• 同样控制约束；
• 同样速度参考；
• 同样路径误差代价。

MPC 可能在简单路径上比 MPPI 更准。因此要突出 MPPI 的优势，最好放在：

• 非线性强；
• 路径曲率变化快；
• 初始偏差大；
• 扰动多；
• 代价函数复杂；
• 不希望依赖精确模型。

---

2.12 实验结论

可以写成：

在无障碍全局路径跟踪实验中，本文从直线、圆弧、S 型曲线、U 型急弯以及大初始偏差恢复等多个场景对 DWA、TEB、MPC 和 MPPI 进行了对比测试。实验结果表明，在低速直线场景下，各算法均能完成路径跟踪任务，性能差异较小；随着路径曲率增大和目标速度提高，DWA 由于预测时域较短，横向误差和角速度波动明显增大；TEB 能够生成较平滑轨迹，但在高曲率路径和大初始偏差下容易出现局部轨迹拉扯和收敛滞后；MPC 在模型准确时具有较高跟踪精度，但对模型参数和代价权重较敏感。相比之下，MPPI 通过对未来控制序列进行采样预测，并综合考虑路径误差、航向误差、速度保持和控制平滑代价，在复杂曲率路径和扰动条件下保持了较低的横向误差和更平滑的控制输出，体现出较好的非线性路径跟踪能力和鲁棒性。

---

2.13 最终推荐的纯跟踪实验组合

如果时间有限，建议做 5 组：

实验编号	场景	目的
Exp 1	直线路径	验证基础跟踪能力
Exp 2	圆弧路径	验证曲率跟踪能力
Exp 3	S 型路径	突出连续高曲率下的优势
Exp 4	大初始偏差恢复	突出预测控制和平滑收敛
Exp 5	加噪声/延迟扰动	突出鲁棒性

最终展示：

1. 一张轨迹对比图；
2. 一张横向误差曲线；
3. 一张航向误差曲线；
4. 一张控制量曲线；
5. 一张总指标表；
6. 一张鲁棒性统计表。

---

3. 非理想全局路径下的 MPPI 优势验证实验

3.1 实验思路

你提出的思路是：

对比"不正常的路径"，比如不满足运动学约束的全局路径，然后比较不同局部规划器走完整条路径后的 odom 轨迹平滑性以及是否满足动力学约束。

这个思路很好，而且比单纯"跟踪一条正常全局路径"更容易突出 MPPI 的优势。

因为如果全局路径本身已经很平滑、曲率合理、满足机器人运动学约束，那么 Pure Pursuit、Stanley、MPC、TEB 都可能表现不错，MPPI 的优势不明显。

但如果全局路径是"不完美的"，例如：

• 折线；
• 急转；
• 锯齿；
• 局部跳变；
• 曲率过大；
• 路径点稀疏；
• 不满足机器人最小转弯半径；

那么局部规划器的能力就不只是"跟踪"，而是：

在尽量贴近全局路径的同时，生成一条机器人实际可执行、平滑、满足速度/加速度/角速度约束的 odom 轨迹。

这个角度非常适合突出 MPPI。

---

3.2 实验定位

不要叫"纯路径跟踪实验"，更建议叫：

不规则全局路径下的局部轨迹生成与可执行性验证实验

或者：

全局路径质量退化条件下的局部规划鲁棒性实验

核心问题从：

谁能更准确跟踪全局路径？

变成：

当全局路径不满足机器人运动学/动力学约束时，
哪个局部规划器能生成更平滑、更可执行、更安全的 odom 轨迹？

这个问题对 MPPI 更有利。

因为 MPPI 可以通过 critic 权衡：

1. 不要偏离全局路径太远；
2. 但也不要强行贴着不可行路径；
3. 控制量要平滑；
4. 速度、角速度、加速度不能超限；
5. 未来一段轨迹要整体代价最小。

---

3.3 为什么"不正常路径"能突出 MPPI

普通路径跟踪算法通常有一个隐含前提：

参考路径本身是合理的、可跟踪的。

但实际全局规划器输出的路径经常并不理想，比如 A*、RRT、栅格规划、采样规划得到的路径可能是：

1. 折线很多；
2. 曲率不连续；
3. 局部存在急转弯；
4. 路径点稀疏；
5. 路径贴墙；
6. 路径经过狭窄区域；
7. 局部路径方向突变；
8. 不满足机器人最小转弯半径；
9. 不满足速度和加速度连续性要求。

如果局部规划器只是"死跟踪路径"，实际 odom 轨迹会出现：

1. 角速度突变；
2. 原地抖动；
3. 左右摆动；
4. 路径过冲；
5. 曲率过大；
6. 频繁减速停车；
7. 甚至跟踪失败。

而 MPPI 可以把全局路径当作一个软约束，不是硬约束：

J = w p J p a t h + w h J h e a d i n g + w s J s m o o t h + w c J c o n s t r a i n t J = w_p J_{path} + w_h J_{heading} + w_s J_{smooth} + w_c J_{constraint} J=wpJpath+whJheading+wsJsmooth+wcJconstraint

也就是说：

全局路径只是"引导方向"，
真正输出的 odom 轨迹由预测模型和综合代价决定。

这就是 MPPI 的优势。

---

3.4 可以专门设计的"不正常全局路径"

实验 1：锯齿形路径

设计一条类似这样的路径：

起点 → 左前 → 右前 → 左前 → 右前 → 终点

或者数学形式：

y = { A , x ∈ 0 , 1 − A , x ∈ 1 , 2 A , x ∈ 2 , 3 y = \begin{cases} A, & x \in 0, 1 \\ -A, & x \in 1, 2 \\ A, & x \in 2, 3 \end{cases} y=⎩ ⎨ ⎧A,−A,A,x∈0,1x∈1,2x∈2,3

这种路径特点是：

1. 路径方向频繁突变；
2. 局部曲率不连续；
3. 不适合机器人真实跟踪；
4. 很容易引起控制抖动。

对比预期：

• DWA：可能左右摆动明显；
• TEB：可能会尝试把路径拉平，但可能局部优化不稳定；
• MPC：如果强跟踪参考点，容易产生较大角速度；
• MPPI：可以在路径跟踪和平滑控制之间折中，生成更自然的 odom 轨迹。

重点指标：

• odom 轨迹曲率；
• 角速度变化；
• 控制平滑性；
• 最大横向偏差；
• 是否完成路径。

---

实验 2：直角折线路径

设计 90° 直角弯：

(0,0) → (5,0) → (5,5)

这个路径本身在转角处曲率无限大，机器人不可能严格跟踪。

理想情况不是贴着直角走，而是提前转弯，形成一条平滑圆弧。

这个实验很容易突出：

• MPPI 是否能提前通过预测时域进行转向；
• 是否能避免到拐角处突然大角速度；
• 是否能生成平滑过渡轨迹。

可以统计转弯区域的曲率：

κ ( t ) = x ˙ y ¨ − y ˙ x ¨ ( x ˙ 2 + y ˙ 2 ) 3 / 2 \kappa(t) = \frac{ \dot{x}\ddot{y} - \dot{y}\ddot{x} }{ (\dot{x}^2 + \dot{y}^2)^{3/2} } κ(t)=(x˙2+y˙2)3/2x˙y¨−y˙x¨

以及曲率变化率：

κ ˙ ( t ) \dot{\kappa}(t) κ˙(t)

曲率和曲率变化率越小，说明 odom 轨迹越平滑。

---

实验 3：曲率超过机器人极限的路径

这个更专业，也更适合写论文。

对于差速机器人，如果最大线速度为 (v_{max})，最大角速度为 (\omega_{max})，则路径可跟踪曲率大致满足：

κ m a x = ω m a x v \kappa_{max} = \frac{\omega_{max}}{v} κmax=vωmax

如果参考路径曲率：

κ r e f > κ m a x \kappa_{ref} > \kappa_{max} κref>κmax

说明这条路径在当前速度下不可跟踪。

对于阿克曼车，最小转弯半径为：

R m i n = L tan ⁡ δ m a x R_{min} = \frac{L}{\tan \delta_{max}} Rmin=tanδmaxL

最大曲率为：

κ m a x = 1 R m i n = tan ⁡ δ m a x L \kappa_{max} = \frac{1}{R_{min}}= \frac{\tan \delta_{max}}{L} κmax=Rmin1=Ltanδmax

可以故意设计一条：

κ r e f > κ m a x \kappa_{ref} > \kappa_{max} κref>κmax

的路径，比如很小半径的圆弧或 U 型弯。

实验重点不是谁最贴路径，而是谁能够：

1. 不强行跟踪不可行路径；
2. 自动生成满足曲率约束的 odom 轨迹；
3. 不出现大角速度和控制突变；
4. 最终仍能沿总体方向完成任务。

这个实验非常能突出 MPPI 的"可执行轨迹生成能力"。

---

实验 4：路径点稀疏导致的方向突变

很多全局路径是一串稀疏 waypoints，例如：

(0,0), (3,0), (3,3), (6,3), (6,6)

如果局部规划器直接跟踪这些点，方向会不断突变。

可以设置两种输入：

1. 稠密平滑路径；
2. 稀疏折线路径。

然后比较不同局部规划器的 odom 轨迹变化。

这个实验可以说明：

MPPI 对全局路径离散质量不那么敏感，可以通过局部 rollout 和 smooth critic 生成更连续的控制。

---

实验 5：带噪声的全局路径

把一条正常路径加入随机扰动：

y n o i s y ( x ) = y r e f ( x ) + ϵ y_{noisy}(x) = y_{ref}(x) + \epsilon ynoisy(x)=yref(x)+ϵ

其中：

ϵ ∼ N ( 0 , σ 2 ) \epsilon \sim \mathcal{N}(0, \sigma^2) ϵ∼N(0,σ2)

设置不同噪声强度：

σ = 0.05 m
σ = 0.10 m
σ = 0.20 m

这种路径模拟的是：

1. 全局规划路径不平滑；
2. 地图栅格分辨率较低；
3. 路径后处理不足；
4. 传感器/定位噪声导致路径局部抖动。

如果局部规划器过度依赖路径点，会导致 odom 轨迹也抖。MPPI 如果 path critic 和 smooth critic 设计合理，则应该不会完全照抖动路径走，而是生成更平滑的实际轨迹。

---

实验 6：路径贴障环境

这个很适合和 RC-ESDF 结合。

让全局路径贴着障碍物边缘，例如：

全局路径离墙只有 0.05 m，
但机器人实际安全距离要求 0.25 m。

此时全局路径本身是"不安全的"。

局部规划器要做的不是严格跟踪路径，而是：

在不偏离路径太多的情况下，
主动远离障碍物，
生成安全可执行轨迹。

这个场景可以突出：

MPPI + RC-ESDF 能够利用 footprint 精确距离代价，自动在路径跟踪和安全距离之间做权衡。

DWA 可能贴墙抖动；TEB 可能局部变形；MPC 如果障碍物约束处理不好会比较麻烦；MPPI 可以直接把 RC-ESDF 距离写成 critic。

---

实验 7：全局路径局部断裂或跳变

模拟全局规划器在重规划时路径突然变化：

前 5 秒全局路径 A；
第 5 秒切换成全局路径 B；
路径 B 和路径 A 有明显偏移。

比较算法对路径跳变的响应：

1. 是否突然大角速度；
2. 是否急刹；
3. 是否产生 odom 轨迹折角；
4. 是否能平滑过渡到新路径。

MPPI 由于有控制平滑代价和滚动优化，理论上可以比强跟踪类方法更平滑地过渡。

---

3.5 评价指标要从"跟踪精度"变成"可执行性"

在"不正常路径"实验里，不能只比较横向误差，因为有时候偏离不合理全局路径反而是正确的。

指标应该分成四类。

---

1）路径接近性指标

仍然保留横向误差：

e y ( t ) = min ⁡ p i ∈ P g l o b a l ∥ p ( t ) − p i ∥ e_y(t) = \min_{p_i \in P_{global}} \|p(t) - p_i\| ey(t)=pi∈Pglobalmin∥p(t)−pi∥

统计：

• 平均横向误差；
• 最大横向误差；
• RMSE 横向误差。

但它不能作为唯一指标。

---

2）轨迹平滑性指标

这是重点。

odom 轨迹曲率

κ ( t ) = x ˙ y ¨ − y ˙ x ¨ ( x ˙ 2 + y ˙ 2 ) 3 / 2 \kappa(t) = \frac{ \dot{x}\ddot{y} - \dot{y}\ddot{x} }{ (\dot{x}^2 + \dot{y}^2)^{3/2} } κ(t)=(x˙2+y˙2)3/2x˙y¨−y˙x¨

统计：

• 平均曲率；
• 最大曲率；
• 曲率标准差；
• 曲率变化率。

曲率变化率

J κ = ∑ t ( κ t − κ t − 1 ) 2 J_{\kappa} = \sum_t (\kappa_t - \kappa_{t-1})^2 Jκ=t∑(κt−κt−1)2

这个指标很有说服力。锯齿路径或直角路径下，如果某个算法 odom 曲率变化很大，说明它实际走得不平滑。

---

3）控制平滑性指标

线速度变化：

Δ v t = v t − v t − 1 \Delta v_t = v_t - v_{t-1} Δvt=vt−vt−1

角速度变化：

Δ ω t = ω t − ω t − 1 \Delta \omega_t = \omega_t - \omega_{t-1} Δωt=ωt−ωt−1

控制平滑代价：

J Δ u = ∑ t ( v t − v t − 1 ) 2 + λ ( ω t − ω t − 1 ) 2 J_{\Delta u}= \sum_t \left (v_t - v_{t-1})\^2 + \\lambda (\\omega_t - \\omega_{t-1})\^2 \\right JΔu=t∑(vt−vt−1)2+λ(ωt−ωt−1)2

加速度：

a t = v t − v t − 1 Δ t a_t = \frac{v_t - v_{t-1}}{\Delta t} at=Δtvt−vt−1

角加速度：

α t = ω t − ω t − 1 Δ t \alpha_t = \frac{\omega_t - \omega_{t-1}}{\Delta t} αt=Δtωt−ωt−1

jerk：

j t = a t − a t − 1 Δ t j_t = \frac{a_t - a_{t-1}}{\Delta t} jt=Δtat−at−1

这些指标越小，说明局部规划器输出越平滑，越适合真实机器人执行。

---

4）运动学/动力学约束满足指标

比如差速机器人：

∣ v t ∣ ≤ v m a x |v_t| \le v_{max} ∣vt∣≤vmax

∣ ω t ∣ ≤ ω m a x |\omega_t| \le \omega_{max} ∣ωt∣≤ωmax

∣ a t ∣ ≤ a m a x |a_t| \le a_{max} ∣at∣≤amax

∣ α t ∣ ≤ α m a x |\alpha_t| \le \alpha_{max} ∣αt∣≤αmax

统计约束违背次数：

N v i o l = ∑ t I ( ∣ a t ∣ > a m a x or ∣ ω t ∣ > ω m a x or ∣ α t ∣ > α m a x ) N_{viol}= \sum_t \mathbb{I} \left( |a_t| > a_{max} \ \text{or}\ |\omega_t| > \omega_{max} \ \text{or}\ |\alpha_t| > \alpha_{max} \right) Nviol=t∑I(∣at∣>amax or ∣ωt∣>ωmax or ∣αt∣>αmax)

如果是阿克曼车，还可以统计曲率是否超过最大可行曲率：

∣ κ t ∣ ≤ κ m a x |\kappa_t| \le \kappa_{max} ∣κt∣≤κmax

其中：

κ m a x = tan ⁡ δ m a x L \kappa_{max} = \frac{\tan \delta_{max}}{L} κmax=Ltanδmax

这个指标非常适合当前思路，因为要证明的是：

MPPI 不是最死板地跟踪路径，而是能生成更满足运动学/动力学约束的实际 odom 轨迹。

---

3.6 对比实验组合

建议最终做 4 组实验：

实验	全局路径特点	主要验证
Exp 1	锯齿/噪声路径	抗路径抖动能力、odom 平滑性
Exp 2	90° 直角折线	高曲率不可行路径下的平滑过渡
Exp 3	小半径 U 型路径	超出运动学曲率约束时的可执行轨迹生成
Exp 4	贴障路径	路径跟踪与安全距离之间的权衡能力

这四个实验加起来，逻辑很完整：

• Exp 1：路径本身不平滑；
• Exp 2：路径方向不连续；
• Exp 3：路径曲率不可行；
• Exp 4：路径虽然可走但不安全。

这正好对应真实全局路径常见问题。

---

3.7 对比方法

可以这样设置：

方法	说明
DWA	传统速度空间采样局部规划
TEB	轨迹优化型局部规划
MPC	约束优化型路径跟踪
MPPI	采样预测控制局部规划
MPPI + RC-ESDF	最终方法

如果要突出自己的方法，最好不要只写 MPPI，可以拆成两个版本：

MPPI：只包含 path / goal / smooth / constraint critic
MPPI + RC-ESDF：额外加入 footprint 障碍物 critic

实验结论更清楚：

1. MPPI 本身在不规则路径下平滑性更好；
2. 加入 RC-ESDF 后，在贴障和复杂障碍环境下安全性更好。

---

3.8 可以这样描述 MPPI 的优势

针对"不正常路径"，MPPI 的优势可以这样写：

与 DWA、TEB 和传统 MPC 相比，MPPI 将全局路径跟踪项作为软代价，而非强制约束，通过对未来控制序列进行采样预测，在路径接近性、控制平滑性、运动约束和避障安全性之间进行综合权衡。当全局路径存在折线、噪声、曲率不连续或局部不可行时，MPPI 能够避免对参考路径的过度跟踪，生成更符合机器人运动学和动力学约束的实际 odom 轨迹。

---

3.9 实验假设

可以写为：

在理想全局路径条件下，传统路径跟踪算法能够取得较好的跟踪效果，MPPI 的优势并不明显。然而在实际导航任务中，全局路径通常由栅格搜索、采样规划或拓扑规划得到，路径可能存在折线、曲率不连续、贴近障碍物或不满足机器人运动约束等问题。因此，局部规划器不仅需要跟踪全局路径，还需要根据机器人运动模型和局部环境约束对路径进行可执行化修正。为验证 MPPI 在该类场景下的优势，本文构造多组非理想全局路径，并比较不同局部规划方法生成的实际 odom 轨迹在平滑性、约束满足性和任务完成率方面的差异。

---

3.10 实验结果

每个实验建议给这几类结果。

1）轨迹图

同一张图画：

• 原始全局路径；
• DWA odom；
• TEB odom；
• MPC odom；
• MPPI odom。

直观看出谁走得更平滑。

2）速度和角速度曲线

画：

• (v(t))；
• (\omega(t))。

如果某个算法抖动，角速度曲线会非常明显。

3）曲率曲线

画：

κ ( t ) \kappa(t) κ(t)

这个比单纯看轨迹更专业。

4）指标表

建议这样设计：

场景	算法	横向 RMSE/m	最大曲率	曲率变化代价	控制平滑代价	约束违背次数	完成率
锯齿路径	DWA
锯齿路径	TEB
锯齿路径	MPC
锯齿路径	MPPI
直角路径	DWA
直角路径	TEB
直角路径	MPC
直角路径	MPPI

注意：横向误差不是越小越好到绝对。如果全局路径本身不可行，适当偏离路径是合理的。所以要同时看：

• 横向误差；
• 轨迹平滑性；
• 约束违背次数；
• 任务完成率。

---

3.11 如何避免别人质疑实验"不公平"

因为故意设计"不正常路径"，别人可能会说：

这不是路径跟踪问题，是你给了不合理路径。

所以要提前说明：

• 真实全局规划输出并不总是满足局部运动约束；
• 局部规划器本来就应该具备路径可执行化能力；
• 实验目的是验证局部规划器面对非理想全局路径时的鲁棒性。

同时保证公平：

1. 所有算法输入同一条全局路径；
2. 所有算法使用相同速度和加速度限制；
3. 所有算法使用相同控制频率；
4. 所有算法在同一初始位姿和终点下测试；
5. 每组实验重复多次；
6. 不只看路径误差，也看平滑性和约束满足。

---

3.12 MPPI 的环境

除了"不正常路径"，还可以做下面几类环境。

环境 1：动态障碍物横穿

机器人跟踪全局路径时，一个动态障碍物从侧面横穿。

MPPI 优势：

• 可以在未来预测时域内评估动态障碍物位置；
• 可以提前减速或绕行；
• 相比 DWA 更不短视。

指标：

• 最小安全距离；
• 速度变化；
• 是否急刹；
• 是否完成任务。

---

环境 2：窄通道 + 非圆形机器人

这个非常适合 RC-ESDF。

例如长方形机器人通过狭窄门洞。

MPPI + RC-ESDF 优势：

• 可以利用真实 footprint 距离；
• 不再简单使用圆形膨胀；
• 可以通过采样找到合适姿态穿过窄通道。

指标：

• 最小障碍物距离；
• 碰撞次数；
• 通过率；
• 轨迹平滑性；
• 角速度变化。

---

环境 3：全局路径贴墙

全局路径离墙很近，但实际机器人需要保持安全距离。

MPPI + RC-ESDF 优势：

• 全局路径跟踪是软约束；
• 障碍物安全距离也是 critic；
• 可以自动偏离贴墙路径，保持安全。

这个场景很适合证明 RC-ESDF critic 的意义。

---

环境 4：低摩擦/控制执行误差

给机器人执行控制时加误差：

v r e a l = v c m d ( 1 + ϵ ) v_{real} = v_{cmd}(1 + \epsilon) vreal=vcmd(1+ϵ)

ω r e a l = ω c m d ( 1 + ϵ ) \omega_{real} = \omega_{cmd}(1 + \epsilon) ωreal=ωcmd(1+ϵ)

MPPI 优势：

• 在线滚动优化；
• 每个控制周期重新采样；
• 对扰动有一定恢复能力。

---

环境 5：局部目标突然变化

例如全局路径重规划后，局部目标点突然移动。

MPPI 优势：

• 控制平滑项可以避免剧烈转向；
• 预测时域内可以平滑过渡。

---

3.13 推荐实验设计

建议最终做成两大类。

第一类：非理想全局路径跟踪实验

目的：证明 MPPI 能把不合理全局路径转化为平滑可执行 odom。

场景	重点
锯齿路径	抗路径抖动
直角折线路径	平滑转弯
超曲率 U 型路径	满足运动学曲率约束
噪声路径	不过度跟随路径噪声

第二类：复杂环境局部规划实验

目的：证明 MPPI + RC-ESDF 的避障和局部规划优势。

场景	重点
贴墙路径	路径跟踪与安全距离权衡
窄通道	非圆形 footprint 精确避障
动态障碍物横穿	预测避障
多障碍物 S 型绕行	多 critic 综合规划能力

实验逻辑：

• 第一类证明 MPPI 的局部轨迹生成和平滑控制能力；
• 第二类证明 MPPI + RC-ESDF 的复杂环境避障能力。

---

3.14 核心

可以把方法定位为：

面向非理想全局路径和复杂局部环境的 MPPI 局部规划方法。

而不是简单说：

我用 MPPI 跟踪路径。

更具体地说：

1. 将全局路径跟踪作为软代价，而非强制约束；
2. 通过控制平滑和约束 critic 提升 odom 轨迹可执行性；
3. 通过 RC-ESDF critic 引入任意形状机器人 footprint 的精确避障代价；
4. 在非平滑、超曲率、贴障等非理想全局路径下，生成更平滑、更安全、更符合运动约束的局部轨迹。

---

3.15 最值得优先做的三个实验

如果时间有限，最值得做的是：

优先级	实验	为什么
1	直角折线路径 / 锯齿路径	最容易看出 odom 平滑性优势
2	超曲率 U 型路径	最能体现运动学约束满足能力
3	贴墙路径 + RC-ESDF	最能体现 RC-ESDF critic 价值

这三个实验做出来，MPPI 的优势会比单纯跟踪正常路径明显很多。

---

4. RC-ESDF 融合后能提升哪些能力

4.1 基本结论

RC-ESDF + MPPI 最值得做的地方是：

MPPI 负责"采样很多种可能动作"，RC-ESDF 负责"精确判断每种动作下机器人真实轮廓离障碍物还有多远"。

普通障碍物代价很多时候把机器人近似成圆，或者只查中心点、外接圆、footprint 顶点；而 RC-ESDF 是在机器人本体坐标系 Body Frame 下构建任意形状 footprint 的 signed distance field，可以 O(1) 查询距离，并提供梯度信息。

---

4.2 更精确的非圆形机器人避障

如果机器人是长方形、履带车、无人车底盘、带突出结构的移动机器人，用圆形膨胀会很保守。

例如一个长方形机器人：

真实尺寸：长 0.8 m，宽 0.4 m
外接圆半径：约 0.45 m

如果用外接圆避障，机器人会认为自己四周都是 0.45 m 半径，很多窄通道明明能过，却会被认为过不去。

RC-ESDF 则是按真实 footprint 判断：

• 车头；
• 车尾；
• 车身侧边；
• 角点。

都按真实形状计算距离。

因此它能做到：

圆形模型认为过不去；
RC-ESDF 判断只要姿态合适就能过。

这就是它对复杂空间最直接的优势。

---

4.3 不只是判断碰撞，还能判断安全余量

普通碰撞检测一般是：

撞了 / 没撞

但 RC-ESDF 可以给出 signed distance：

d < 0：障碍物进入机器人轮廓，碰撞
d = 0：刚好接触机器人边界
d > 0：距离机器人边界还有 d 米

因此 MPPI 不只是知道"这条轨迹能不能走"，还能知道：

• 这条轨迹离障碍物还有多少安全距离；
• 哪条轨迹更贴边；
• 哪条轨迹更居中；
• 哪条轨迹安全余量更连续。

这对狭窄通道、门洞、墙角、复杂障碍物特别重要。

---

4.4 减少过度膨胀导致的"假不可行"

传统 costmap inflation 或圆形 footprint 经常会把窄空间膨胀没了。

RC-ESDF 的优势是可以把"保守膨胀"变成"精确几何距离"：

不是简单把障碍物周围全部膨胀一圈；
而是根据机器人当前姿态和真实轮廓计算最近距离。

这样在窄门、货架通道、走廊拐角处，就不容易因为外接圆或过大 inflation 导致规划失败。

---

4.5 让 MPPI 自动选择更合适的姿态

复杂空间里，很多时候不是"中心点能不能过"，而是"机器人用什么姿态过"。

例如长方形机器人过窄门：

姿态 1：斜着进门，角点容易撞；
姿态 2：车身对齐门洞方向，可以安全通过。

RC-ESDF 对每个候选轨迹点都根据当前 ((x,y,\theta)) 查询真实 footprint 距离，因此 MPPI 可以自然地发现：

• 某些姿态虽然中心点接近路径，但角点会撞；
• 某些姿态稍微偏离路径，但整体 footprint 更安全。

这就是 RC-ESDF 对 MPPI 的关键提升：

它把"位置避障"升级成了"位姿避障"。

---

5. MPPI + RC-ESDF 经过复杂空间的实现思路

5.1 RC-ESDF 如何参与 MPPI 打分

对于 MPPI 的一条候选轨迹：

τ i = { x i , 0 , x i , 1 , . . . , x i , T } \tau_i = \{x_{i,0}, x_{i,1}, ..., x_{i,T}\} τi={xi,0,xi,1,...,xi,T}

其中每个状态为：

x i , t = ( p x , p y , θ ) x_{i,t} = (p_x, p_y, \theta) xi,t=(px,py,θ)

对于某个障碍物点：

o k = ( o x , o y ) o_k = (o_x, o_y) ok=(ox,oy)

先把障碍物点变换到当前候选位姿的机器人本体坐标系：

p b = R ( θ ) T ( o k − p ) p_b = R(\theta)^T(o_k - p) pb=R(θ)T(ok−p)

然后查询机器人本体 RC-ESDF：

d k , t = S D F r o b o t ( p b ) d_{k,t} = SDF_{robot}(p_b) dk,t=SDFrobot(pb)

对该轨迹点取最小距离：

d t = min ⁡ k d k , t d_t = \min_k d_{k,t} dt=kmindk,t

对整条轨迹取最小距离：

d m i n ( τ i ) = min ⁡ t d t d_{min}(\tau_i) = \min_t d_t dmin(τi)=tmindt

然后 MPPI 根据这个距离给轨迹打分：

J r c = { C c o l l i s i o n , d t < d h a r d w r c ( d s a f e − d t ) 2 , d h a r d ≤ d t < d s a f e 0 , d t ≥ d s a f e J_{rc} = \begin{cases} C_{collision}, & d_t < d_{hard} \\ w_{rc}(d_{safe} - d_t)^2, & d_{hard} \le d_t < d_{safe} \\ 0, & d_t \ge d_{safe} \end{cases} Jrc=⎩ ⎨ ⎧Ccollision,wrc(dsafe−dt)2,0,dt<dharddhard≤dt<dsafedt≥dsafe

即：

碰撞轨迹：直接高代价淘汰；
太贴障碍物的轨迹：给中等惩罚；
安全距离足够的轨迹：不惩罚；
在狭窄空间中，只要真实 footprint 能过，就允许通过。

---

5.2 复杂空间不能简单"距离越大越好"

这一点非常关键。

如果代价函数只写成：

距离障碍物越远越好

那机器人在窄通道里会不愿意进去，因为通道内部左右都很近，所有轨迹都会被判高代价。

所以复杂空间下，RC-ESDF 代价应该分成两层。

---

第一层：硬安全约束

这个必须严格：

d t > d h a r d d_t > d_{hard} dt>dhard

例如：

d_hard = 0.00 m 或 0.02 m

只要小于这个值，就认为碰撞或者近似碰撞，直接淘汰。

---

第二层：软安全距离

这个不能太大，尤其是窄通道里。

例如：

普通开放空间：d_safe = 0.25 ~ 0.40 m
狭窄空间：d_safe = 0.05 ~ 0.15 m

如果在窄门场景里还设置：

d_safe = 0.50 m

那机器人根本不可能进门。

正确做法是：

开放区域：鼓励离障碍物远一点；
窄通道区域：只保证不碰撞，允许小安全距离通过。

这可以叫：

adaptive clearance margin，自适应安全距离。

---

5.3 让机器人"钻过复杂空间"的实际策略

策略 1：RC-ESDF 负责判断真实 footprint 是否可通过

对每条采样轨迹，检查所有关键时刻的 footprint 距离：

如果 d_min < 0：碰撞，淘汰；
如果 0 < d_min < d_safe：可以走，但代价较高；
如果 d_min > d_safe：安全，代价低。

这样 MPPI 会自然选择：

• 既能沿全局路径前进；
• 又不会让真实车身撞障碍物；
• 同时控制变化较平滑的轨迹。

---

策略 2：加入"通道居中"代价

如果机器人要经过窄通道，只靠最小距离可能会贴一边墙。

可以设计一个左右平衡代价。

假设能估计机器人左侧和右侧最近距离：

d L , d R d_L, d_R dL,dR

则加入：

J c e n t e r = w c ( d L − d R ) 2 J_{center} = w_c (d_L - d_R)^2 Jcenter=wc(dL−dR)2

含义是：

• 左边太近、右边太远：代价变大；
• 右边太近、左边太远：代价变大；
• 左右距离接近：说明机器人在通道中间，代价较小。

这样机器人会更倾向于从复杂空间中间穿过，而不是贴墙擦边。

---

策略 3：加入"姿态对齐通道方向"代价

复杂空间里，姿态比位置更重要。

例如长方形机器人过门洞，应该让车身方向和门洞方向对齐：

J θ − c o r r i d o r = w θ ( θ − θ c o r r i d o r ) 2 J_{\theta-corridor} =w_\theta (\theta - \theta_{corridor})^2 Jθ−corridor=wθ(θ−θcorridor)2

其中：

• θ \theta θ：机器人当前航向；
• θ c o r r i d o r \theta_{corridor} θcorridor：窄通道/门洞方向。

这可以让 MPPI 采样时更偏向：

• 提前摆正车身；
• 再进入狭窄空间；
• 通过后再恢复跟踪全局路径。

如果没有这个项，MPPI 可能会斜着试探，导致角点频繁接近障碍物，产生抖动。

---

策略 4：加入"距离变化率"代价，让机器人不要越走越危险

RC-ESDF 可以在轨迹序列上连续查询距离：

d 0 , d 1 , . . . , d T d_0, d_1, ..., d_T d0,d1,...,dT

可以惩罚距离持续变小的轨迹：

J d ˙ = w d ˙ ∑ t max ⁡ ( 0 , d t − d t + 1 ) 2 J_{\dot d}= w_{\dot d} \sum_t \max(0, d_t - d_{t+1})^2 Jd˙=wd˙t∑max(0,dt−dt+1)2

含义是：

• 如果下一步更接近障碍物，增加代价；
• 如果下一步远离障碍物，不惩罚。

这个比直接用梯度调制更适合 MPPI，因为 MPPI 本来就是对一整条候选轨迹打分。

---

策略 5：复杂空间内自动降速

越接近障碍物，速度应该越低。

可以设计：

v m a x l o c a l = v m a x ⋅ clip ( d t − d h a r d d s a f e − d h a r d , v m i n _ s c a l e , 1 ) v_{max}^{local}= v_{max} \cdot \text{clip} \left( \frac{d_t - d_{hard}}{d_{safe} - d_{hard}}, v_{min\_scale}, 1 \right) vmaxlocal=vmax⋅clip(dsafe−dharddt−dhard,vmin_scale,1)

也就是：

• 离障碍物远：允许正常速度；
• 离障碍物近：限制最大速度；
• 非常贴近障碍物：低速通过。

这样可以解决复杂空间中 MPPI 抖动、过冲、撞角的问题。

---

5.4 推荐完整代价函数

将 RC-ESDF 融合到 MPPI 后，总代价建议写成：

J = w p J p a t h + w g J g o a l + w p r o g J p r o g r e s s + w r c J r c − e s d f + w c J c e n t e r + w θ J θ − c o r r i d o r + w s J s m o o t h + w c o n J c o n s t r a i n t J = w_p J_{path} + w_g J_{goal} + w_{prog}J_{progress} + w_{rc}J_{rc-esdf} + w_c J_{center} + w_\theta J_{\theta-corridor} + w_s J_{smooth} + w_{con}J_{constraint} J=wpJpath+wgJgoal+wprogJprogress+wrcJrc−esdf+wcJcenter+wθJθ−corridor+wsJsmooth+wconJconstraint

各项作用如下：

代价项	作用
J p a t h J_{path} Jpath	不要离全局路径太远
J g o a l J_{goal} Jgoal	朝局部目标前进
J p r o g r e s s J_{progress} Jprogress	防止原地抖动
J r c − e s d f J_{rc-esdf} Jrc−esdf	真实 footprint 避障
J c e n t e r J_{center} Jcenter	窄通道中尽量居中
J θ − c o r r i d o r J_{\theta-corridor} Jθ−corridor	通过门洞/窄通道时姿态对齐
J s m o o t h J_{smooth} Jsmooth	控制平滑
J c o n s t r a i n t J_{constraint} Jconstraint	满足速度、角速度、加速度约束

重点是：

RC-ESDF 不只是一个"碰撞检测模块"，而是给 MPPI 提供真实 footprint 的连续安全距离，让 MPPI 在路径、避障、姿态、平滑性之间做权衡。

---

5.5 复杂空间下的通过流程

可以把机器人经过复杂空间的流程设计成：

1. 全局路径给出大致通行方向
        ↓
2. 感知模块提供局部障碍物点云 / 激光点
        ↓
3. MPPI 采样多组控制序列
        ↓
4. 对每条候选轨迹 rollout 得到未来位姿序列
        ↓
5. 对每个候选位姿：
      把障碍物点转换到机器人 body frame
      查询 RC-ESDF 得到真实 footprint 距离
        ↓
6. 判断轨迹是否碰撞
        ↓
7. 对非碰撞轨迹计算：
      路径跟踪代价
      安全距离代价
      通道居中代价
      姿态对齐代价
      控制平滑代价
        ↓
8. softmax 加权选择控制序列
        ↓
9. 输出第一帧控制
        ↓
10. 下一周期重新感知、重新采样、重新优化

它能穿过复杂空间的原因是：

不是提前算一条固定路径死跟踪，而是每个控制周期都用真实 footprint 距离重新评估大量未来动作。

---

5.6 典型场景下 RC-ESDF 的作用

场景 1：长方形机器人过窄门

普通圆形近似：

外接圆太大，认为门过不去。

RC-ESDF：

只要车身方向对齐门洞，真实 footprint 不碰撞，就允许通过。

MPPI 会选择：

• 提前减速；
• 调整航向；
• 让车身对齐门洞；
• 低速穿过；
• 出门后恢复路径跟踪。

---

场景 2：货架通道 / 狭窄走廊

普通避障：

左右障碍物都很近，代价很高，机器人容易停住。

RC-ESDF + 自适应 margin：

只要 d_min > d_hard，就认为可通过；
通过 J_center 保持左右距离平衡；
通过 J_smooth 降低摆动。

机器人会沿通道中间平滑通过。

---

场景 3：墙角转弯

普通中心点避障：

中心点没撞，但车尾或角点可能扫到墙角。

RC-ESDF：

每个候选姿态都检查真实 footprint；
如果转弯时角点会扫到墙，代价升高。

MPPI 会选择：

• 提前扩大转弯半径；
• 降低角速度；
• 必要时先向外侧偏一点；
• 再完成转弯。

---

场景 4：全局路径贴墙

普通路径跟踪：

强行贴着全局路径走，容易擦墙。

RC-ESDF critic：

检测真实 footprint 到墙距离过小；
让 MPPI 主动偏离全局路径一点；
保持安全距离后再回到路径。

这正好适合前面提到的"不正常全局路径"实验。

---

5.7 实现细节

1）不要把 RC-ESDF 代价权重设得过大

否则 MPPI 会过度怕障碍物，复杂空间反而过不去。

建议初始值：

rc_esdf_weight: 2.0 ~ 5.0
d_hard: 0.00 ~ 0.03
d_safe_open: 0.25 ~ 0.40
d_safe_narrow: 0.05 ~ 0.15
collision_cost: 1e5

---

2）窄通道里要降低 soft margin

可以根据局部空间宽度自适应：

如果左右障碍物距离都很近，但前方存在通行方向：
    使用 d_safe_narrow
否则：
    使用 d_safe_open

简单写法：

d s a f e = min ⁡ ( d s a f e _ o p e n , α W l o c a l ) d_{safe}= \min(d_{safe\open}, \alpha W{local}) dsafe=min(dsafe_open,αWlocal)

其中 W l o c a l W_{local} Wlocal 是估计的局部通道宽度。

---

3）MPPI 需要足够的横向/角速度采样

如果采样范围太小，它根本采不到"摆正车身穿过门"的动作。

差速机器人：

vx_std: 0.15 ~ 0.30
wz_std: 0.4 ~ 0.8

全向机器人还要加：

vy_std: 0.10 ~ 0.25

阿克曼车则要保证：

steer_std 足够大
预测时域足够长

---

4）预测时域不能太短

复杂空间里，机器人需要提前调整姿态。

如果预测时域只有：

0.5 s

那它看到门洞时已经太晚了。

建议：

普通路径跟踪：1.0 ~ 1.5 s
窄通道 / 门洞：2.0 ~ 3.0 s

但预测时域越长，计算量越大，需要配合降采样和并行。

---

5.8 表述

可以这样写：

本文将 RC-ESDF 引入 MPPI 局部规划框架，构建面向任意形状机器人 footprint 的精确避障 critic。与传统基于圆形膨胀或中心点距离的障碍物代价不同，RC-ESDF 在机器人本体坐标系下预构建 signed distance field，在线阶段将局部障碍物点转换到候选轨迹对应的机器人坐标系中，快速查询障碍物相对于真实机器人轮廓的 signed distance。基于该距离构建硬碰撞惩罚、软安全距离惩罚、通道居中代价和姿态对齐代价，使 MPPI 能够在复杂狭窄空间中综合考虑路径跟踪、真实 footprint 安全距离、控制平滑性和运动约束，从而生成更安全、更平滑且更具可执行性的局部轨迹。

---

5.9 核心结论

RC-ESDF 能提高的不是 MPPI 的全局搜索能力，而是提高 MPPI 对机器人真实几何形状和复杂空间可通行性的判断能力。

它让 MPPI 从：

中心点 / 圆形机器人避障

升级为：

真实 footprint / 位姿级避障

因此可以帮助机器人通过：

• 窄门；
• 窄走廊；
• 贴墙路径；
• 货架通道；
• 墙角转弯；
• 非规则障碍物间隙；
• 长方形机器人需要调整姿态才能通过的空间。

但要注意：

RC-ESDF 本身不会自动"找到路"，它只是告诉 MPPI 某个候选位姿是否安全、离障碍物多远；真正让机器人穿过复杂空间的是 MPPI 的采样轨迹、全局路径引导、姿态对齐、通道居中、速度约束和平滑代价共同作用。

---

6. 实验体系

6.1 实验体系总览

为了完整突出 MPPI 及 MPPI + RC-ESDF 的优势，建议最终实验分为两大类。

---

6.2 第一类：非理想全局路径下的轨迹可执行性实验

目的：证明 MPPI 不只是"跟踪路径"，而是能够把不合理全局路径转化为平滑、可执行、满足运动学约束的 odom 轨迹。

实验	场景	主要指标	预期优势
Exp 1	锯齿路径	曲率变化、角速度抖动、完成率	MPPI 不会死跟踪锯齿，轨迹更平滑
Exp 2	直角折线	转弯曲率、角速度峰值	MPPI 能提前转弯，避免急转
Exp 3	超曲率 U 型路径	最大曲率、约束违背次数	MPPI 能生成满足约束的可执行轨迹
Exp 4	噪声路径	odom 平滑性、横向误差	MPPI 不会过度跟随路径噪声

---

6.3 第二类：复杂空间下的避障与通行能力实验

目的：证明 MPPI + RC-ESDF 的真实 footprint 避障能力和复杂空间通过能力。

实验	场景	主要指标	预期优势
Exp 5	贴墙路径	最小安全距离、路径偏离量	MPPI + RC-ESDF 能主动偏离不安全路径
Exp 6	窄门 / 窄通道	通过率、碰撞次数、姿态平滑性	RC-ESDF 能判断真实 footprint 是否可过
Exp 7	墙角转弯	角点最小距离、角速度峰值	RC-ESDF 能避免车尾/角点扫墙
Exp 8	动态障碍物横穿	最小 TTC、最小距离、完成率	MPPI 能提前预测并选择绕行或减速

---

6.4 对比方法设置

建议对比：

方法	作用
DWA	传统局部速度空间采样 baseline
TEB	传统轨迹优化局部规划 baseline
MPC	优化控制 baseline
MPPI	仅路径、目标、平滑、约束 critic
MPPI + RC-ESDF	加入真实 footprint 避障 critic 的完整方法

这样可以区分：

1. MPPI 作为采样预测控制，本身在非理想路径下的平滑性优势；
2. RC-ESDF 加入后，在复杂障碍物环境中的精确避障优势。

---

6.5 重点评价指标

路径接近性

• 平均横向误差；
• 最大横向误差；
• 横向 RMSE。

轨迹平滑性

• 最大曲率；
• 曲率变化代价；
• odom 轨迹曲率标准差。

控制平滑性

• 线速度变化；
• 角速度变化；
• 加速度；
• 角加速度；
• jerk。

约束满足性

• 速度约束违背次数；
• 角速度约束违背次数；
• 加速度约束违背次数；
• 曲率约束违背次数。

避障安全性

• 最小障碍物距离；
• 最小 RC-ESDF signed distance；
• 碰撞次数；
• 通过率；
• 最小 TTC。

实时性

• 平均计算耗时；
• 最大计算耗时；
• 95% 计算耗时；
• 是否满足控制频率。

---

6.6 最终总结表述

可以写为：

本文针对非理想全局路径和复杂局部障碍环境下的机器人局部规划问题，构建基于 MPPI 的采样预测局部规划框架。与 DWA、TEB 和传统 MPC 相比，MPPI 将路径跟踪、目标趋近、控制平滑、运动约束和避障安全性统一建模为多 critic 代价函数，通过对未来控制序列进行采样预测和滚动优化，在全局路径存在折线、噪声、曲率不连续或局部不可行时，能够生成更平滑、更符合运动学约束的实际 odom 轨迹。进一步地，本文引入 RC-ESDF 构建面向任意形状机器人 footprint 的精确避障 critic，通过查询候选位姿下障碍物相对于真实机器人轮廓的 signed distance，实现复杂狭窄空间中的位姿级安全评估。实验可从非理想路径跟踪、窄通道通行、贴墙路径修正和动态障碍物避障等场景展开，系统验证 MPPI + RC-ESDF 在轨迹平滑性、约束满足性和复杂空间通过能力方面的优势。

---

参考链接

• RC-ESDF-2D GitHub：https://github.com/JackJu-HIT/RC-ESDF-2D
• SA-MPPI GitHub：https://github.com/Robot-Nav/SA-MPPI
• Nav2 MPPI Controller 文档：https://docs.nav2.org/configuration/packages/configuring-mppic.html