项目名称：DC-PMPPI — Disturbance-Compensated Model Predictive Path Integral Control
仓库：Robot-Nav/SA-MPPI
分支：DC-PMPPI
项目地址：https://github.com/Robot-Nav/SA-MPPI/tree/DC-PMPPI
ROS：ROS1 Noetic
语言：C++17 / Python
许可证：Apache-2.0
主要依赖：catkin、Eigen3、OpenMP、xtensor、xsimd、xtl

DC-PMPPI 是一个面向轮式移动机器人的局部路径跟踪控制器。它不是重新设计一种与 MPPI 完全不同的控制框架，而是在标准 MPPI 的“采样—预测—评分—加权更新”闭环中加入在线模型失配估计：利用里程计和实际执行命令估计侧向扰动、横摆扰动以及纵向/横摆控制有效性，并把这些估计结果重新注入预测模型和参考控制生成过程，使控制器在湿滑路面、侧滑、执行器响应衰减、移动甲板或其他外部扰动条件下，仍尽可能保持“预测轨迹”和“真实车辆运动”一致。

---

目录

• 一、项目要解决什么问题
• 二、项目整体结构
• 三、标准 MPPI 基础
• 四、DC-PMPPI 的核心思想
• 五、在线扰动与控制有效性估计
• 六、含扰动的 Rollout 预测模型
• 七、MPCC 式路径跟踪代价
• 八、参考速度与参考控制生成
• 九、采样策略
• 十、自适应温度、ESS 与精英截断
• 十一、三种运动模型与运动约束
• 十二、终点处理与控制平滑
• 十三、核心源码模块说明
• 十四、ROS 输入输出接口
• 十五、关键参数说明
• 十六、项目运行步骤
• 十七、项目依赖库
• 十八、如何做标准 MPPI / DC-PMPPI A-B 对比
• 十九、项目特点与工程注意事项
• 二十、总结

---

一、项目要解决什么问题

标准 MPPI 的预测过程建立在一个很重要的前提上：

控制器内部使用的运动模型，应当能够比较准确地描述机器人真实运动。

例如，对于非完整约束轮式机器人，最简单的二维模型通常写成：

$$\dot{x}=v\cos \theta ,$$

$$\dot{y}=v\sin \theta ,$$

$$\dot{\theta }=\omega .$$

离散化后：

$$x_{k+1}=x_k+v_k\cos {\theta }_k\Delta t,$$

$$y_{k+1}=y_k+v_k\sin {\theta }_k\Delta t,$$

$${\theta }_{k+1}={\theta }_k+{\omega }_k\Delta t.$$

在理想路面上，这种模型可以近似认为“控制器给出什么速度，车辆就产生什么速度”。

但在实际系统中并不总是如此。

典型情况包括：

1. 湿滑、冰雪、低附着路面导致轮胎纵向打滑；
2. 转弯时发生侧滑，车辆真实运动方向与车头朝向不再一致；
3. 横摆响应不足，命令角速度与实际角速度存在比例衰减；
4. 车体受到外部横向运动，例如船舶、甲板晃动；
5. 执行器存在一阶惯性和响应滞后；
6. 定位系统或底盘反馈存在持续性偏置。

如果 MPPI 仍然按照名义模型 Rollout，那么采样得到的“未来轨迹”就会和机器人真正执行出来的轨迹产生系统性差异。

DC-PMPPI 的核心做法不是给每一种物理扰动分别建立复杂动力学模型，而是把它们统一看成等价模型失配，在线估计：

$$d=\left[\begin{array}{c}{v}_y^{dist}\\ {\omega }^{dist}\end{array}\right],$$

以及两个控制有效性系数：

$${\eta }_v,{\eta }_{\omega }.$$

其中：

• $v_y^{dist}$：车体系下的等价侧向扰动速度；
• ${\omega }^{dist}$：等价横摆角速度扰动；
• ${\eta }_v$：纵向控制有效性；
• ${\eta }_{\omega }$：横摆控制有效性。

因此，DC-PMPPI 实际解决的是：

当“控制器内部预测模型”和“真实车辆响应”不一致时，如何在线辨识这部分失配，并让 MPPI 在预测时显式考虑这种失配。

---

二、项目整体结构

当前 DC-PMPPI 分支的代码主体位于：

SA-MPPI/
├── README.md
├── README_CN.md
├── LICENSE
└── rsc/
    ├── CMakeLists.txt
    ├── package.xml
    ├── config/
    │   ├── dc_pmppi_params.yaml
    │   └── dc_pmppi_test.rviz
    ├── include/
    │   ├── controller.hpp
    │   ├── optimizer.hpp
    │   ├── motion_models.hpp
    │   ├── critics/
    │   │   ├── critic_function.hpp
    │   │   ├── critic_manager.hpp
    │   │   ├── critic_data.hpp
    │   │   ├── mpcc_tracking_critic.hpp
    │   │   ├── control_rate_critic.hpp
    │   │   ├── constraint_critic.hpp
    │   │   ├── stability_critic.hpp
    │   │   ├── lateral_error_critic.hpp
    │   │   ├── path_align_critic.hpp
    │   │   ├── path_angle_critic.hpp
    │   │   ├── path_follow_critic.hpp
    │   │   ├── prefer_forward_critic.hpp
    │   │   ├── twirling_critic.hpp
    │   │   └── velocity_deadband_critic.hpp
    │   ├── models/
    │   │   ├── types.hpp
    │   │   ├── constraints.hpp
    │   │   ├── optimizer_settings.hpp
    │   │   ├── path.hpp
    │   │   ├── state.hpp
    │   │   ├── trajectories.hpp
    │   │   └── control_sequence.hpp
    │   └── tools/
    │       ├── disturbance_estimator.hpp
    │       ├── noise_generator.hpp
    │       ├── path_projection.hpp
    │       ├── path_reference.hpp
    │       ├── goal_docking.hpp
    │       └── math_utils.hpp
    ├── src/
    │   └── dc_pmppi_node.cpp
    ├── scripts/
    │   ├── JZJ_path.py
    │   └── twist_to_ackermann.py
    ├── test/
    │   └── disturbance_estimator_test.cpp
    ├── xtensor/
    ├── xsimd/
    └── xtl/

系统数据流如下：

从 ROS 节点角度看，控制器主要接收：

• /plan：参考路径；
• /odom1：机器人当前位姿和速度；
• /constrained_cmd_vel_stamped：真正经过约束器后下发到底盘的执行命令，可选但在 DC 模式下很重要。

控制器输出：

• /cmd_vel：最终速度命令；
• /local_path：当前优化后的预测轨迹；
• /controller_solve_time_ms：单周期计算耗时。

---

三、标准 MPPI 基础

3.1 最优控制问题

设机器人离散系统为：

$$x_{t+1}=F(x_t,u_t),$$

控制序列为：

U = { u 0 , u 1 , … , u T − 1 } . U=\{u_0,u_1,\ldots,u_{T-1}\}. U={u0​,u1​,…,uT−1​}.

总成本可以写成：

$$S(U)=\varphi (x_T)+\sum _{t=0}^{T-1}q(x_t,u_t).$$

MPPI 不直接通过梯度优化整个控制序列，而是在当前名义控制序列附近采样。

对第 $i$ 条样本：

$$u_{i,t}=u_t+{ϵ}_{i,t},$$

其中：

$${ϵ}_{i,t}\sim \mathcal{N}(0,\Sigma ).$$

每一组采样控制序列经过运动模型 Rollout 得到一条未来轨迹，然后计算累计代价 $S_i$。

3.2 MPPI 权重

项目使用指数形式构造轨迹权重：

$$w_i=\frac{\exp \left[-(S_i-S_{\mathrm{ref}})/\lambda \right]}{{\sum }_j\exp \left[-(S_j-S_{\mathrm{ref}})/\lambda \right]}.$$

其中：

• $\lambda$ 或代码中的 temperature：温度参数；
• $S_{\mathrm{ref}}$ 默认使用最小代价；
• 开启 use_mean_normalization 时可使用平均代价。

温度越小，权重越集中在少数低成本样本上；温度越大，更多样本会参与更新。

3.3 控制更新

标准形式可以理解为：

$$U^{new}=U+\sum _i{w}_i{ϵ}_i.$$

在当前实现中，由于采样控制经过边界、Ackermann 曲率等约束，因此代码实际使用“约束后的采样控制”做加权均值，再向该均值更新：

$$u_t^{new}=u_t+\alpha \left(\sum _i{w}_i{u}_{i,t}-u_t\right),$$

其中 $\alpha$ 对应 elite_update_rate；未启用 elite 截断时更新率为 1。

3.4 为什么最终只执行第一项

虽然 MPPI 每次求得的是整个未来控制序列：

U ∗ = { u 0 ∗ , u 1 ∗ , … , u T − 1 ∗ } , U^*= \{u_0^*,u_1^*,\ldots,u_{T-1}^*\}, U∗={u0∗​,u1∗​,…,uT−1∗​},

但实际只执行：

$$u_0^{\ast }.$$

之后进入下一控制周期，重新读取当前真实状态，再优化新的未来序列。

后面的控制量并不是“没用”，因为 $u_0^{\ast }$ 是否合理，正是通过后续整个预测时域的累计结果判断出来的；同时项目还会将序列左移作为下一周期 warm start：

U k + 1 i n i t = { u 1 ∗ , u 2 ∗ , … , u T − 1 ∗ , u T − 1 ∗ } . U_{k+1}^{init} =\{u_1^*,u_2^*,\ldots,u_{T-1}^*,u_{T-1}^*\}. Uk+1init​={u1∗​,u2∗​,…,uT−1∗​,uT−1∗​}.

---

四、DC-PMPPI 的核心思想

DC-PMPPI 可以概括为四层补偿。

4.1 控制有效性补偿

对于理想模型：

$$v_{real}=v_{cmd},$$

$${\omega }_{real}={\omega }_{cmd}.$$

低附着时改写为：

$$v_{real}={\eta }_v{v}_{cmd},$$

$${\omega }_{real}={\eta }_{\omega }{\omega }_{cmd},$$

其中：

$$0.3\le {\eta }_v,{\eta }_{\omega }\le 1.$$

η=1 表示控制基本完全生效；数值越低表示执行响应衰减越明显。

4.2 加性扰动补偿

除“控制增益下降”之外，模型还包含不能由控制有效性解释的加性扰动：

$$v_y^{dist},$$

$${\omega }^{dist}.$$

前者用于描述侧向漂移，后者用于描述额外横摆运动。

4.3 未来扰动衰减

控制器不直接假定当前估计到的扰动会在整个预测时域永久不变，而使用：

$$d(t)=d_0{e}^{-t/{\tau }_d}.$$

其中 ${\tau }_d$ 为：

disturbance_decay_time_constant

默认配置为：

0.8 s

因此，近未来更相信当前扰动估计，预测越远则逐渐回归名义模型。

4.4 置信度渐进启用

扰动估计器不会在刚获得一两个观测时立刻全量接管模型，而维护：

$$c\in [0,1].$$

只有置信度达到最低阈值后，DisturbanceEstimate.valid 才变为真。

这使得启动瞬态、定位跳变、时间戳异常等情况不容易直接污染 MPPI。

---

五、在线扰动与控制有效性估计

源码中最关键的 DC 模块是：

include/tools/disturbance_estimator.hpp

5.1 通过位姿差分获得车体系速度

设两个时刻的世界坐标位姿差为：

$$\Delta x=x_k-x_{k-m},$$

$$\Delta y=y_k-y_{k-m},$$

$$\Delta \theta =wrap({\theta }_k-{\theta }_{k-m}).$$

窗口时间：

$$\Delta t=t_k-t_{k-m}.$$

先得到世界坐标速度：

$$v_x^w=\frac{\Delta x}{\Delta t},$$

$$v_y^w=\frac{\Delta y}{\Delta t}.$$

再根据窗口中点航向 $\bar{\theta }$ 转换到车体系：

$$v_x^{meas}=\cos \bar{\theta }{v}_x^w+\sin \bar{\theta }{v}_y^w,$$

$$v_y^{meas}=-\sin \bar{\theta }{v}_x^w+\cos \bar{\theta }{v}_y^w.$$

横摆角速度：

$${\omega }^{meas}=\frac{\Delta \theta }{\Delta t}.$$

配置中也允许直接使用 odometry twist：

dc_pmppi_use_odom_twist_lateral_velocity: true
dc_pmppi_use_odom_twist_yaw_rate: true
dc_pmppi_use_odom_twist_longitudinal_velocity: true

因此，在仿真或底盘速度反馈可靠时，不必完全依赖位姿差分。

---

5.2 为什么要建执行器一阶模型

如果直接计算：

$${\eta }_v=\frac{v^{meas}}{v^{cmd}},$$

那么车辆正常加速时，由于电机和底盘存在惯性，实际速度暂时低于命令速度，控制器可能错误判断为“打滑”。

项目专门建了一个一阶执行器模型：

$${\dot{u}}_m=\frac{u_{cmd}-u_m}{{\tau }_a}.$$

离散实现：

$$u_m^{k+1}=u_m^k+\left(1-e^{-\Delta t/{\tau }_a}\right)\left(u_{cmd}-u_m^k\right).$$

纵向默认时间常数：

0.10 s

横摆默认：

0.0 s

即当前配置认为纵向通道需要显式处理一阶滞后，而横摆通道不额外建立滞后。

---

5.3 控制有效性估计

项目支持两种方式。

方法一：稳健比值法

当命令激励足够大：

$$|v_{cmd}|>v_{deadzone},$$

则：

$${\eta }_v^{obs}=clip\left(\left|\frac{v_x^{meas}}{v_x^{cmd}}\right|,0.3,1.0\right).$$

同理：

$${\eta }_{\omega }^{obs}=clip\left(\left|\frac{{\omega }^{meas}}{{\omega }^{cmd}}\right|,0.3,1.0\right).$$

默认配置中使用该方式。

方法二：指数加权中心化回归

可选方法为：

$$\eta =\frac{Cov(u,y)}{Var(u)}.$$

中心化回归的意义是：如果测量中存在一个慢变化的加性扰动，

$$y=\eta u+d,$$

那么中心化后常值或慢变的 $d$ 对斜率估计影响较小。

不过当前配置：

dc_pmppi_use_centered_effectiveness_regression: false

注释明确说明，在当前闭环 OU 扰动实验中该方法没有优于稳健比值法，所以默认关闭。

---

5.4 横摆扰动估计

首先根据当前 ${\eta }_{\omega }$ 预测正常情况下应该得到的横摆响应：

$${\omega }^{exp}={\eta }_{\omega }{\omega }_{cmd}.$$

实际横摆残差：

$${\omega }_{obs}^{dist}={\omega }^{meas}-{\omega }^{exp}.$$

这部分残差被解释为无法通过正常控制响应解释的“等价横摆扰动”。

---

5.5 侧向扰动估计

对于非全向 Ackermann / DiffDrive 车辆，正常情况下主动侧向速度应接近 0，因此可把车体系测得的横向速度作为：

$$v_{y,obs}^{dist}=v_y^{meas}.$$

这也是该模型特别适合表达侧滑的原因。

---

5.6 死区、创新限幅与低通滤波

为了防止定位噪声直接进入控制模型，首先设置死区。

例如：

$$|v_y^{meas}|<v_{dead}\Rightarrow v_{y,target}^{dist}=0.$$

然后计算创新：

$$\Delta v_y=clip\left(v_{y,target}^{dist}-{\hat{v}}_y^{dist},-{\Delta }_{max},{\Delta }_{max}\right).$$

低通更新：

$${\hat{v}}_y^{dist}\leftarrow {\hat{v}}_y^{dist}+(1-\beta )\Delta v_y,$$

其中：

$$\beta =e^{-\Delta t/{\tau }_f}.$$

横摆扰动采用相同处理。

默认：

dc_pmppi_filter_time_constant: 0.20
dc_pmppi_max_vy_innovation: 0.40
dc_pmppi_max_wz_innovation: 0.80

---

5.7 扰动方差

项目同步维护：

$${\sigma }_{v_y}^2\leftarrow \beta {\sigma }_{v_y}^2+(1-\beta )(\Delta v_y{)}^2,$$

以及：

$${\sigma }_{\omega }^2.$$

当：

sample_disturbance_uncertainty: true

时，可在 Rollout 中进一步做扰动 sigma-point 采样。

当前 Gazebo 配置默认关闭：

sample_disturbance_uncertainty: false

避免有限样本下把估计噪声进一步放大。

---

5.8 traction_scale

代码还计算一个工程化的附着能力指标：

$$severity=k_y|{\hat{v}}_y^{dist}|+k_{\omega }|{\hat{\omega }}^{dist}|+0.2({\sigma }_{v_y}+{\sigma }_{\omega }),$$

$$traction\_scale=clip\left(1-severity,tractio{n}_{min},1\right).$$

需要注意：

traction_scale 不是摩擦系数 $\mu$ 的在线物理辨识结果。

它只是一个 0～1 的保守能力指标。

而且当前参数：

traction_speed_exponent: 0.0
traction_acceleration_exponent: 0.0

意味着正式配置中不再用 traction_scale 二次降低速度和加速度约束，因为 ${\eta }_v/{\eta }_{\omega }$ 已经显式进入模型，再继续缩放容易形成重复补偿。

---

六、含扰动的 Rollout 预测模型

这是 DC-PMPPI 与普通 MPPI 最直接的差异。

6.1 控制有效性进入预测速度

对于每条样本：

$$v_{i,t}^{target}={\eta }_v{u}_{i,t}^v,$$

$${\omega }_{i,t}^{target}={\eta }_{\omega }{u}_{i,t}^{\omega }.$$

随后再施加加速度约束：

$$v_{i,t}=cli{p}_{acc}\left(v_{i,t}^{target},v_{i,t-1},a_{max}\Delta t\right).$$

注意源码有一个非常合理的细节：

第一预测时刻使用当前测量速度，不会再次乘 $\eta$。

否则就会把已经发生过的控制衰减重复计算一次。

---

6.2 扰动衰减

在第 $j$ 个预测点：

$${\rho }_j=\exp \left(-\frac{t_j}{{\tau }_d}\right).$$

于是：

$$v_{y,j}^{dist}={\hat{v}}_y^{dist}{\rho }_j,$$

$${\omega }_j^{dist}={\hat{\omega }}^{dist}{\rho }_j.$$

---

6.3 航向积分

$${\theta }_{j+1}={\theta }_j+\left({\omega }_j+{\omega }_j^{dist}\right)\Delta t.$$

---

6.4 位置积分

对于非全向模型，主动控制侧向速度为 0，但可以存在外部侧向扰动，因此：

$$v_y=v_y^{dist}.$$

位置更新为：

$$x_{j+1}=x_j+\left(v_x\cos \theta -v_y^{dist}\sin \theta \right)\Delta t,$$

$$y_{j+1}=y_j+\left(v_x\sin \theta +v_y^{dist}\cos \theta \right)\Delta t.$$

这意味着控制器在采样阶段看到的不再是“车辆一定沿车头方向运动”，而是能够预测“车头朝这个方向，但由于侧滑，真实质心还会横向漂”。

---

七、MPCC 式路径跟踪代价

项目的核心跟踪 Critic：

mpcc_tracking_critic.hpp

采用 Model Predictive Contouring Control 的思路，不仅计算机器人与离散路径点的欧氏距离，而是把误差拆分。

7.1 轮廓误差 Contouring Error

轨迹点投影到参考路径后，横向距离记作：

$$e_c.$$

它直接反映几何路径跟踪误差。

7.2 滞后误差 Lag Error

设期望弧长进度为：

$$s_{exp},$$

轨迹投影进度为：

$$s_{proj},$$

则：

$$e_l=s_{exp}-s_{proj}.$$

如果只最小化横向误差，机器人可能通过“少往前走”来换取较小的几何误差；Lag Error 用来防止这种现象。

7.3 航向误差

考虑侧滑角：

$${\beta }_{slip}=atan2\left(v_y^{dist},\max (v_{ref},0.1)\right).$$

目标车体航向需要对运动方向做补偿，因此：

$$e_h=wrap\left[{\theta }_{traj}-({\theta }_{path}-{\beta }_{slip})\right].$$

7.4 速度误差

$$e_v=v_{traj}-v_{ref}(s).$$

7.5 横摆角速度误差

曲率前馈关系为：

$${\omega }_{ref}=v_{ref}\kappa .$$

因此：

$$e_{\omega }=({\omega }_{traj}+{\omega }^{dist}\rho )-v_{ref}\kappa .$$

7.6 综合代价

可以整理为：

$$J_i=\sum _t{w}_t\left[q_c{e}_c^2+q_l{e}_l^2+q_h{e}_h^2+q_v{e}_v^2+q_{\omega }{e}_{\omega }^2\right]+J_{terminal}.$$

项目的主要权重默认值：

mpcc_weight: 8.0
mpcc_contour_weight: 2.2
mpcc_lag_weight: 0.35
mpcc_heading_weight: 1.10
mpcc_velocity_weight: 0.25
mpcc_yaw_rate_weight: 0.20
mpcc_terminal_weight: 2.0

从这些权重可以看出，该配置首先强调几何轮廓跟踪，其次考虑航向，再在进度和速度之间做折中。

---

八、参考速度与参考控制生成

项目并不是始终用固定：

0.6 m/s

向前跟踪，而是对路径进行速度预览。

8.1 曲率限速

横向加速度近似：

$$a_y=v^2|\kappa |.$$

因此：

$$v_{\kappa }=\sqrt{\frac{a_{y,max}}{|\kappa |}}.$$

曲率越大，允许速度越低。

8.2 终点制动

根据制动距离关系：

$$v^2=2ad,$$

得到：

$$v_{goal}=\sqrt{2a_{goal}{d}_{goal}}.$$

机器人越接近终点，参考速度会连续降低。

8.3 弯道前视减速

项目还在：

path_corner_preview_distance

范围内预览未来曲率，并根据允许减速度提前降低速度，而不是等真正进入急弯才突然减速。

8.4 smooth minimum

最终参考速度可以概念性写成：

$$v_{ref}=smooth\_min\left(v_{cruise},v_{\kappa },v_{corner},v_{goal}\right).$$

使用 smooth minimum 而不是简单 min()，可以减轻多个限速条件切换时的控制突变。

---

九、采样策略

noise_generator.hpp 不仅实现普通独立高斯噪声，还保留了多个 MPPI 变体常用的采样策略。

9.1 AR(1) 时间相关噪声

普通白噪声：

$${ϵ}_t\sim \mathcal{N}(0,{\sigma }^2)$$

在时间上完全独立，容易产生锯齿控制序列。

项目默认：

noise_correlation: 0.85

可以理解为：

$$
\epsilon_t

\beta\epsilon_{t-1}
+
\sqrt{1-\beta^2}\xi_t.
$$

因此采样出来的未来控制天然更加平滑。

---

9.2 反对称采样

若生成一个噪声：

$$ϵ,$$

同时生成：

$$-ϵ.$$

这种 antithetic sampling 能提高有限样本下采样空间的对称性，降低蒙特卡洛估计方差。

默认：

antithetic_sampling: true

---

9.3 名义样本

nominal_sample_count: 1

至少保留一条零噪声样本，即当前 warm-start 控制序列本身。

这样可以避免所有样本都随机偏离当前已经不错的解。

---

9.4 Guided Sampling

默认：

guided_sampling_ratio: 0.25
guided_sampling_blend: 0.80

即约 25% 的 Rollout 会向基于路径曲率、横向误差和航向误差生成的参考控制序列靠拢。

因此采样分布不是完全盲目的。

对复杂弯道而言，这可以把更多计算预算放在“可能有用”的控制区域。

---

9.5 KMPPI 风格支持点采样

配置：

sampling_support_points: 0

默认关闭。

启用后，不再对 horizon 中每一个控制时刻独立优化噪声，而是在较少的支持点上采样，再通过 RBF 核插值到整个时域。

优点是降低优化维度。

---

9.6 Smooth-MPPI 风格控制导数采样

配置：

sample_control_derivatives: false

默认关闭。

开启后噪声不直接作用于绝对控制：

$$u_t+{ϵ}_t,$$

而是作用于控制增量：

$$\Delta u_t+{ϵ}_t.$$

随后积分得到控制序列，因此可以得到更平滑的控制。

项目注释说明，该方法实测虽然可能提高精度，但会增加角加速度 RMS，所以正式配置没有默认启用。

---

9.7 跨周期复用噪声

reuse_noise_sequence: true
noise_cycle_correlation: 0.98

上一控制周期的低频采样结构会被左移保留，并只注入少量新探索。

这不仅提高时间连续性，也可以降低相邻控制周期因随机采样产生的指令跳变。

---

十、自适应温度、ESS 与精英截断

10.1 有效样本量 ESS

若归一化权重为：

$$\sum _i{w}_i=1,$$

则有效样本量：

$$ESS=\frac{1}{{\sum }_i{w}_i^2}.$$

一般形式也可写为：

$$ESS=\frac{({\sum }_i{w}_i{)}^2}{{\sum }_i{w}_i^2}.$$

如果所有权重相近：

$$ESS\approx N.$$

如果几乎只有一个样本有权重：

$$ESS\approx 1.$$

---

10.2 为什么固定 temperature 有问题

假设两次实验的路径跟踪代价整体尺度不同。

即使轨迹相对优劣完全一样，只要：

$$S_i$$

整体放大，指数项：

$$e^{-S_i/\lambda }$$

就可能突然变得非常尖锐。

因此同一个 $\lambda$ 在不同路径、速度、Critic 权重下表现会变化。

---

10.3 ESS 目标温度

当前配置：

adaptive_temperature: true
target_ess_ratio: 0.15
adaptive_temperature_min: 0.10
adaptive_temperature_max: 3.00

算法通过二分搜索温度 $T$，使：

$$ESS(T)\approx 0.15N.$$

例如：

batch_size = 400

则目标：

$$ESS\approx 60.$$

这样 temperature 不再只是一个固定调参值，而是围绕“希望多少条轨迹真正参与优化”自动变化。

---

10.4 CEM-style Elite Truncation

设置：

elite_sample_count > 0

后，只保留成本最低的 Top-K 轨迹。

非 elite 样本：

$$w_i=0.$$

elite 内仍保持 MPPI 的指数权重：

$$w_i\propto e^{-(S_i-S_{min})/T}.$$

所以它不是标准 CEM 的“所有 elite 等权求均值”，而是：

CEM 的截断思想 + MPPI 的相对成本权重。

当前正式配置：

elite_sample_count: 0

默认关闭。

配置注释给出的原因很直接：实测它可以提高精度，但也会增加角加速度 RMS。

---

十一、三种运动模型与运动约束

项目支持：

DiffDrive
Omni
Ackermann

当前默认：

motion_model: "Ackermann"

11.1 DiffDrive

非全向：

$$v_y^{cmd}=0.$$

控制变量主要是：

$$u=[v_x,\omega {]}^T.$$

---

11.2 Omni

全向模型额外允许：

$$v_y^{cmd}\ne 0.$$

因此控制变量为：

$$u=[v_x,v_y,\omega {]}^T.$$

---

11.3 Ackermann

项目没有直接把前轮转角 $\delta$ 作为 MPPI 控制量，而仍通过 Twist：

$$(v,\omega )$$

表达。

几何曲率：

$$\kappa =\frac{\omega }{v}.$$

最小转弯半径要求：

$$|\kappa |\le \frac{1}{R_{min}}.$$

因此：

$$|\omega |\le \frac{|v|}{R_{min}}.$$

同时横向加速度：

$$a_y=v\omega .$$

要求：

$$|v\omega |\le a_{y,max},$$

于是：

$$|\omega |\le \frac{a_{y,max}}{|v|}.$$

项目最终取：

$$|\omega |\le \min \left(\frac{|v|}{R_{min}},\frac{a_{y,max}}{|v|}\right).$$

非常重要的一点是：

约束不仅作用于采样控制，还会在加速度传播后的预测状态上重新投影。

因为如果只分别限制：

$$\Delta v,\Delta \omega ,$$

车辆在启动和制动阶段仍可能临时产生不合理的极大曲率。

---

十二、终点处理与控制平滑

12.1 GoalDocking

距离终点较近时，单纯 MPPI 容易出现：

• 速度太小提前停住；
• 在目标附近来回震荡；
• 扰动下始终差几厘米到不了；
• 最后阶段轨迹优化不稳定。

因此项目增加：

tools/goal_docking.hpp

在一定距离内逐渐混合终端位姿反馈。

典型距离—速度关系：

$$v=\min \left(v_{max},k_{d}d,\sqrt{2a_{goal}d}\right).$$

当前：

goal_docking_distance: 0.20
goal_docking_full_control_distance: 0.12
goal_docking_max_speed: 0.18

---

12.2 Reacquisition

如果机器人还没到目标，但因为优化或扰动导致速度已经非常低，则启动重捕获：

goal_recovery_distance: 1.20
goal_recovery_speed_threshold: 0.08
goal_recovery_max_speed: 0.35

目的是避免“离目标还有距离，但控制器自己停住”。

---

12.3 Savitzky-Golay 控制序列平滑

配置：

use_sg_filter: true

优化后使用历史命令和未来控制序列做 Savitzky-Golay 保形平滑。

源码中还有一个关键处理：

SG 滤波结束后必须重新施加运动约束。

否则滤波本身可能让：

• 速度越界；
• 角速度越界；
• Ackermann 曲率约束被破坏。

---

十三、核心源码模块说明

13.1 controller.hpp

控制器对外总接口。

主要职责：

Controller
├── MotionModel
├── Optimizer
└── CriticManager

上层 ROS 节点不需要直接操作 MPPI 内部数据结构，只需要：

1. 设置路径；
2. 设置扰动估计；
3. 输入当前 pose / speed；
4. 调用 computeVelocityCommands()。

---

13.2 optimizer.hpp

整个项目最核心的文件。

主要实现：

prepare
    ↓
generateNoisedTrajectories
    ↓
motion model prediction
    ↓
integrateStateVelocities
    ↓
CriticManager::score
    ↓
updateControlSequence
    ↓
applyControlSequenceConstraints
    ↓
SG filter
    ↓
getControlFromSequence
    ↓
shiftControlSequence

此外还包含：

• 路径裁剪；
• guided sampling；
• 参考控制生成；
• ESS 温度搜索；
• elite 截断；
• Ackermann 后处理；
• ancillary feedback；
• 终点制动；
• 预测轨迹输出。

---

13.3 disturbance_estimator.hpp

DC-PMPPI 的主要新增模块。

负责：

里程计/位姿
     +
执行器实际命令
     ↓
车体系速度
     ↓
执行器滞后补偿
     ↓
η_v / η_ω
     ↓
横摆残差 / 侧向残差
     ↓
低通 + 创新限幅
     ↓
方差 + confidence
     ↓
DisturbanceEstimate

---

13.4 noise_generator.hpp

集中处理各种 sampling policy：

• Gaussian；
• AR(1)；
• antithetic；
• nominal sample；
• guided sample 前的随机基础；
• KMPPI 支持点；
• Smooth-MPPI 控制导数采样；
• noise sequence reuse。

---

13.5 path_projection.hpp

负责将任意二维点投影到分段线性路径，得到：

• 最近投影点；
• 所在线段；
• 路径弧长；
• 有符号横向误差；
• 路径切向航向。

MPCCTrackingCritic、参考控制生成、最近路径点搜索都依赖这部分路径几何信息。

---

13.6 path_reference.hpp

负责：

• 路径累计弧长；
• 根据弧长插值参考状态；
• 曲率计算；
• 曲率限速；
• 弯道预览减速；
• 终点制动；
• smoothMinimum()。

---

13.7 critics/

Critic 使用统一接口：

score(CriticData & data)

每个 Critic 将自己的代价累加到总 costs。

默认核心组合主要是：

MPCCTrackingCritic
+
ControlRateCritic

其余 Critic 更适合作为可选实验模块。

---

十四、ROS 输入输出接口

14.1 /plan

类型：

nav_msgs/Path

ROS 节点收到后会：

1. 删除距离过近的重复路径点；
2. 按固定间距重新采样；
3. 重新计算路径切线航向；
4. 可选保留原路径终点 orientation。

默认：

path_resample_spacing: 0.10
path_duplicate_epsilon: 0.005

---

14.2 /odom1

类型：

nav_msgs/Odometry

用于：

• 当前机器人 pose；
• 当前速度；
• 扰动估计；
• odom timeout 判断。

默认：

odom_timeout: 0.30
odom_topic: "/odom1"
planning_frame: "map"
strict_frame_check: true

---

14.3 /constrained_cmd_vel_stamped

类型：

geometry_msgs/TwistStamped

它的价值是告诉估计器：

“真正到底盘边界的命令到底是多少”。

如果只使用优化器原始 /cmd_vel，而中间还有安全限幅、速度约束器或底盘转换，则估计器会把“中间层改了命令”错误识别为“车辆模型失配”。

当前：

dc_pmppi_use_executed_command: true
dc_pmppi_executed_command_timeout: 0.20

---

14.4 /cmd_vel

类型：

geometry_msgs/Twist

最终控制命令。

对于 Ackermann 底盘，项目还提供：

scripts/twist_to_ackermann.py

用于将 Twist 形式的：

v
ω

转换为 Ackermann 控制。

---

14.5 /local_path

类型：

nav_msgs/Path

用于 RViz 可视化当前优化控制序列重新积分得到的名义预测轨迹。

---

14.6 /controller_solve_time_ms

类型：

std_msgs/Float64

用于实时监测单周期优化耗时。

正式配置：

control_period_ms: 50

因此控制频率约为：

$$f=\frac{1}{0.05}=20Hz.$$

求解时间应尽可能稳定低于：

50 ms

否则无法严格维持 20 Hz 控制周期。

---

十五、关键参数说明

15.1 MPPI 核心

参数	默认值	含义
batch_size	400	每周期 Rollout 数
time_steps	40	预测步数
model_dt	0.05 s	模型时间步
iteration_count	1	单周期优化迭代数
control_period_ms	50 ms	控制周期
temperature	0.60	MPPI 温度初值
gamma	0.015	控制扰动正则系数
target_ess_ratio	0.15	自适应温度的目标 ESS 比例

预测时域：

$$T_h=40\times 0.05=2.0s.$$

---

15.2 采样

参数	默认值
vx_std	0.20 m/s
wz_std	0.35 rad/s
noise_correlation	0.85
antithetic_sampling	true
guided_sampling_ratio	0.25
reuse_noise_sequence	true
noise_cycle_correlation	0.98
nominal_sample_count	1

---

15.3 车辆约束

参数	默认值
vx_max	0.70 m/s
vx_min	0
wz_max	1.0 rad/s
ax_max	1.2 m/s²
az_max	2.5 rad/s²
ackermann_min_turning_radius	0.50 m
path_max_lateral_acceleration	0.60 m/s²

---

15.4 路径

参数	默认值
prune_distance	3.5 m
path_resample_spacing	0.10 m
path_reference_speed	0.60 m/s
path_corner_preview_distance	1.20 m
path_corner_deceleration	0.90 m/s²
goal_deceleration	0.50 m/s²

---

15.5 DC-PMPPI

参数	默认值	说明
dc_pmppi_enabled	false	DC 总开关
disturbance_decay_time_constant	0.8 s	预测扰动衰减
dc_pmppi_filter_time_constant	0.20 s	扰动低通
dc_pmppi_effectiveness_time_constant	1.50 s	η 低通/回归时间尺度
dc_pmppi_confidence_time_constant	0.50 s	confidence 建立速度
dc_pmppi_minimum_confidence	0.25	最低可信阈值
dc_pmppi_compensation_gain	0.85	总补偿增益
dc_pmppi_pose_velocity_window	0.10 s	位姿差分速度窗口
dc_pmppi_max_vy_innovation	0.40 m/s	侧向创新限幅
dc_pmppi_max_wz_innovation	0.80 rad/s	横摆创新限幅

注意：

dc_pmppi_enabled: false

意味着仓库当前正式默认启动的是标准 MPPI 基线。

要验证 DC 效果必须显式开启。

---

十六、项目运行步骤

这里需要区分“README 预期方式”和“当前分支实际提交内容”。

16.1 下载分支

cd ~/catkin_ws/src

git clone -b DC-PMPPI \
  https://github.com/Robot-Nav/SA-MPPI.git

由于 ROS 包本身位于 rsc/，建议将其作为 dc_pmppi 包放入工作空间：

cd ~/catkin_ws/src

mv SA-MPPI/rsc dc_pmppi

也可以保留仓库结构后自行软链接。

---

16.2 安装 ROS 依赖

sudo apt update

sudo apt install \
  ros-noetic-ackermann-msgs \
  libeigen3-dev \
  libomp-dev

然后：

cd ~/catkin_ws

rosdep install \
  --from-paths src \
  --ignore-src \
  -r -y

---

16.3 编译

cd ~/catkin_ws

catkin_make --pkg dc_pmppi

source devel/setup.bash

CMake 会自动检测 OpenMP。

如果成功启用，会打印类似：

dc_pmppi: OpenMP acceleration enabled

否则退化为串行计算。

---

16.4 当前分支关于 launch 文件的注意事项

CMakeLists.txt 中存在：

install(DIRECTORY launch/
    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)

README 也给出了若干：

roslaunch dc_pmppi ...

运行方式。

但当前 DC-PMPPI 分支的 rsc/ 目录树中没有实际提交 launch/ 目录。

因此，如果按当前仓库快照直接复现，不能假设这些 launch 文件一定存在。

方式 A：补齐项目原本的 launch 文件

如果作者后续分支或本地工程中有：

j15_sim.launch
j15_dc_pmppi.launch
j15_path.launch

将其放入：

dc_pmppi/launch/

再按 README 启动。

方式 B：直接运行节点

先加载参数：

rosparam load \
  ~/catkin_ws/src/dc_pmppi/config/dc_pmppi_params.yaml

注意 YAML 顶层命名空间是：

dc_pmppi_node:

然后运行：

rosrun dc_pmppi dc_pmppi_node \
  __name:=dc_pmppi_node

之后确保：

rostopic echo /odom1
rostopic echo /plan

都有有效输入。

---

16.5 发布路径

项目提供：

scripts/JZJ_path.py

用于生成/发布测试路径。

安装后的脚本可尝试：

rosrun dc_pmppi JZJ_path.py

具体使用时仍应根据脚本中的参数和路径形式调整。

---

16.6 标准 MPPI

配置：

dc_pmppi_enabled: false

用于建立 baseline。

---

16.7 DC-PMPPI

修改：

dc_pmppi_enabled: true

并重新加载参数或通过 launch override。

---

16.8 运行时重点检查

rostopic hz /cmd_vel

预期约：

20 Hz

检查求解耗时：

rostopic echo /controller_solve_time_ms

原则上应满足：

$$t_{solve}<50ms.$$

查看预测轨迹：

/local_path

并在 RViz 中与：

/plan

叠加观察。

---

十七、项目依赖库

17.1 ROS

项目面向：

ROS1 Noetic

主要依赖：

roscpp
rospy
nav_msgs
geometry_msgs
std_msgs
sensor_msgs
tf
ackermann_msgs

package.xml 还声明：

onboard_detector

为运行依赖。

不过 DC-PMPPI 的纯路径跟踪主节点本身的核心输入仍然是路径、里程计和执行命令。

---

17.2 Eigen3

CMake：

find_package(Eigen3 REQUIRED)

用于数学与几何相关计算。

---

17.3 OpenMP

项目将计算密集的批量轨迹处理和 Critic 评分并行化。

CMake 使用：

find_package(OpenMP QUIET)

因此 OpenMP 不存在时仍能编译，只是退化为串行。

---

17.4 xtensor

xtensor 是项目中非常重要的批量数值计算基础。

例如：

batch_size × time_steps

维度的：

• 采样速度；
• 状态；
• 轨迹；
• costs；
• weights；

都适合使用张量形式处理。

---

17.5 xsimd

xsimd 为 xtensor 提供 SIMD 向量化能力。

与 OpenMP 的分工可以粗略理解为：

OpenMP：多线程并行
xsimd：单线程内部向量指令并行

---

17.6 xtl

xtensor 生态中的基础模板依赖。

这三个库在仓库中以内嵌源码形式存在，因此通常不需要单独通过 apt 安装。

---

十八、如何做标准 MPPI / DC-PMPPI A-B 对比

这个项目特别适合做控制算法实验，因为 DC 功能有总开关。

18.1 Baseline

dc_pmppi_enabled: false

记录：

• 横向误差 RMSE；
• 最大横向误差；
• 航向误差 RMSE；
• 终点误差；
• 线加速度 RMS；
• 角加速度 RMS；
• solve time；
• 成功率。

---

18.2 DC-PMPPI

保持：

• 相同路径；
• 相同车辆；
• 相同 batch size；
• 相同 random seed；
• 相同扰动序列；

只修改：

dc_pmppi_enabled: true

再比较相同指标。

---

18.3 推荐扰动场景

纵向有效性衰减

例如：

$$v_{actual}=0.65v_{cmd}.$$

检查 ${\eta }_v$ 是否收敛到接近 0.65。

横摆有效性衰减

$${\omega }_{actual}=0.7{\omega }_{cmd}.$$

检查 ${\eta }_{\omega }$。

恒定横向漂移

$$v_y^{dist}=0.1m/s.$$

观察标准 MPPI 是否出现稳定横向偏差，以及 DC 是否通过航向补偿降低误差。

时间变化 OU 扰动

可使用：

$$d_{k+1}=d_k-\frac{\Delta t}{\tau }{d}_k+\sigma \sqrt{\Delta t}{\xi }_k.$$

这种扰动比恒定 bias 更接近低频随机环境扰动。

---

十九、项目特点与工程注意事项

19.1 它更像“鲁棒模型修正 MPPI”，而不是重新发明 MPPI

DC-PMPPI 的核心价值在于：

真实反馈
  ↓
模型失配估计
  ↓
修正 Rollout
  ↓
重新做 MPPI

因此它保持了 MPPI 原有的采样优化框架。

---

19.2 η 和加性扰动拆开是合理的

如果只估计一个总扰动：

$$d=v_{meas}-v_{cmd},$$

则无法区分：

控制增益下降

和：

额外外部运动

项目分别使用：

$$\eta u$$

与：

$$+d$$

表达，可以得到：

$$y=\eta u+d.$$

这种模型比纯残差补偿更有解释性。

---

19.3 executed command 很重要

如果底盘前还有速度约束器：

MPPI cmd
   ↓
限速器
   ↓
底盘真正执行 cmd

估计器应该比较：

真实响应
vs
底盘真正收到的命令

而不是比较：

真实响应
vs
MPPI 原始命令

否则会把约束器行为当成地面滑移。

---

19.4 当前 traction_scale 主要是诊断量

虽然代码支持通过 traction_scale 动态降低速度和加速度，但正式配置指数为 0。

这是一个值得注意的设计选择：

同一种物理效果最好不要在多个模块中重复补偿。

既然 $\eta$ 已经缩放了运动模型，再用 traction_scale 大幅压低可用速度可能造成过度保守。

---

19.5 Ackermann 不能直接“原地旋转”

源码参数中：

heading_alignment_start: 3.00
heading_alignment_stop: 3.14

基本等价于关闭传统“航向差一大就把速度降到 0”的策略。

原因是 Ackermann 车辆本身不能像差速车一样原地旋转。

因此大航向调整主要依靠有限转弯半径圆弧完成。

---

19.6 路径裁剪不是简单最近点

optimizer.hpp 使用：

局部搜索窗口
+
物理距离
+
航向误差

联合寻找最近进度点。

这对：

• 回头弯；
• 发卡弯；
• 相邻两段空间距离很近但方向相反；

特别重要。

否则只按欧氏距离可能跳到错误路径段。

---

19.7 当前项目主要是“路径跟踪控制器”，不是完整导航栈

它本身解决的是：

给定参考路径
       ↓
实时优化速度控制

并不等价于：

建图
+
定位
+
全局规划
+
动态避障
+
局部规划
+
底盘控制

因此实际导航系统中，仍然需要上游模块提供 /plan。

---

二十、总结

DC-PMPPI 的完整逻辑可以压缩成一句话：

在标准 MPPI 采样优化的基础上，通过里程计与真实执行命令在线估计控制有效性和等价外部扰动，再把估计结果写回 Rollout 预测模型，使优化器看到的未来运动更接近真实车辆。

其控制链路为：

参考路径 + 当前状态
        ↓
路径裁剪与速度预览
        ↓
扰动估计
v_y^dist / ω^dist / η_v / η_ω
        ↓
生成参考控制
        ↓
批量采样
        ↓
含扰动 Rollout
        ↓
MPCC + ControlRate 等 Critic
        ↓
ESS 自适应 temperature
        ↓
MPPI 加权更新
        ↓
控制约束 + SG 平滑
        ↓
GoalDocking
        ↓
执行第一项 /cmd_vel
        ↓
控制序列左移
        ↓
下一周期重新优化

从工程实现看，这个项目的重点并不只在一条新的 MPPI 更新公式，而在于把以下模块组合到了一起：

1. MPPI 路径积分采样控制；
2. MPCC 风格路径跟踪目标；
3. 在线等价扰动估计；
4. 控制有效性在线辨识；
5. 执行器一阶滞后补偿；
6. 扰动衰减 Rollout；
7. ESS 自适应 temperature；
8. AR(1)、反对称、引导、可选 KMPPI / Smooth-MPPI 采样；
9. Ackermann 运动可行域；
10. Savitzky-Golay 控制平滑；
11. GoalDocking 终端稳定；
12. OpenMP + xtensor/xsimd 的批量并行实现。

如果用于湿滑路面、滑移底盘、移动平台或模型参数变化明显的机器人，DC-PMPPI 相比完全依赖名义运动模型的 MPPI，最主要的优势是能够通过实时反馈持续修正自己的预测模型，而不是等轨迹已经偏离之后才单纯依靠更大的跟踪误差做被动纠偏。

---

附：建议阅读源码顺序

如果准备继续深入修改代码，建议按下面的顺序阅读：

1. config/dc_pmppi_params.yaml
        ↓
2. src/dc_pmppi_node.cpp
        ↓
3. include/controller.hpp
        ↓
4. include/optimizer.hpp
        ↓
5. include/tools/disturbance_estimator.hpp
        ↓
6. include/motion_models.hpp
        ↓
7. include/critics/mpcc_tracking_critic.hpp
        ↓
8. include/tools/path_projection.hpp
        ↓
9. include/tools/path_reference.hpp
        ↓
10. include/tools/noise_generator.hpp
        ↓
11. include/tools/goal_docking.hpp

其中真正决定 DC-PMPPI 与标准 MPPI 差异的两处核心代码是：

disturbance_estimator.hpp

和：

optimizer.hpp 中的
updateStateVelocities()
integrateStateVelocities()
buildReferenceControlSequence()

前者回答“扰动怎么估”，后者回答“估出来以后怎么进入 MPPI”。