局部路径规划器 DWB：速度采样与评分权重的取值分析
移动机器人导航里有个很常见的问题：能到达目标，但过程很难看------站在目标附近原地打转、走两步退一步、直线速度始终提不起来。
这类问题通常被归因为"参数没调好"。但真正的原因往往是评分函数的结构性失衡：每一项评分都有权重，权重决定了规划器在各种取舍里偏向哪一边。当"转向"的权重总和逼近甚至超过"前进"，规划器就会做出人类看来荒诞的选择------明明目标在正前方，它却优先调整朝向。
这篇文章讲三件事：速度采样的离散本质、评分权重如何决定速度选择、以及为什么"改了参数变快"和"真正变好"是两件不同的事。
左图：纵向速度的候选值是离散网格上的点，不是连续区间。区间 -0.45, 0.30 等分成 20 份，每份 0.0375 米每秒。右图：每一档速度对应的最小总代价。蓝线是朝向权重较高的配置，代价随速度单调下降，最快档最优；橙线是降低朝向权重后的配置，最优解移到了中间某档。这张图说明：规划器选哪个速度，完全由代价曲线的形状决定，而这个形状由权重决定。
一、速度是离散采样的
局部规划器的核心循环是：在每个控制周期，枚举一批候选速度，用评分函数给每条候选轨迹打分，选分最低的一条执行。
候选速度不是连续区间，是从采样网格里取的：
vx ∈ linspace(vx_min, vx_max, vx_samples)
vy ∈ linspace(vy_min, vy_max, vy_samples)
wz ∈ linspace(-max_vel_theta, max_vel_theta, vtheta_samples)以一组典型配置为例：vx_min=-0.45、vx_max=0.30、vx_samples=20，步长是
(0.30 - (-0.45)) / 20 = 0.0375采样点就是 -0.4500、-0.4125、-0.3750......一直到 0.3000，共 21 个边界点。
这一点很重要 ：如果你在日志里看到规划器输出的前进速度永远是一个固定值，且这个值恰好等于某个采样点，那不是它"只想走这么快"，而是它每次都在同一个格子上打转。
实际排查中遇到过这样的情况：某段行程里所有前进帧输出的纵向速度是同一个值，均值、中位数、90 分位、最大值完全相同。查采样网格，0.1026 正好是第 13 个格子。
规划器不是"算出来 0.1026"，它是"从 20 个候选里选了第 13 个"。
二、权重决定了取舍方向
评分函数是若干项加权求和。每项衡量一个维度，权重决定这个维度有多重要。实测配置的权重结构：
七项评分项中，两项管距离（路径距离 32、目标距离 24，合计 56），三项管朝向（路径对齐 16、目标对齐 12、终点转向 12，合计 40），其余是障碍代价与前进偏好。这张图说明：朝向类的总权重已经接近距离类，在某些几何条件下足以反超。
距离类 关心"离目标/路径有多近"，朝向类关心"朝向对不对"。
两者在数学上是竞争关系：
- 想更快逼近目标 → 倾向选高前进速度
- 想把朝向调正 → 倾向选高转向角速度，而合速度有上限，前进就得让位
实测的合速度约束是 max_speed_xy = 0.45。这意味着
sqrt(vx² + vy²) ≤ 0.45横移打满 0.25 时，前进最大只能到 sqrt(0.45² - 0.25²) = 0.374；但实际规划器会更保守，因为它还要给转向留余地。
于是出现一个典型结果：规划器输出的最优解是前进 0.142 加转向 0.35（转向打满），而不是前进 0.30。
从数据能看得很清楚：
| 纵向速度 | 最小总代价 |
|---|---|
| 0.000 | 74.00 |
| 0.103 | 73.03 |
| 0.142 | 72.50 ← 最优 |
| 0.182 | 72.50 |
| 0.221 | 74.50 |
| 0.300 | 75.00 |
最优解在中间，不在最快档。 代价曲线是 U 形，不是单调下降。
三、验证"权重是不是原因"
要确认权重失衡是原因，不能只看现象，得看改权重后代价曲线是否变形。
把朝向类的两项降下来（RotateToGoal 12→4、PathAlign 16→8），重新采集同样的候选轨迹数据，对比最优解落在哪个速度档：
| 配置 | 最优纵向速度 | 对应总代价 |
|---|---|---|
| 朝向权重高 | 0.142 | 72.5 |
| 朝向权重低 | 0.142 | 74.0 |
最优纵向速度没变，但代价结构变了------看其他分量才看得出效果：
| 指标 | 改前 | 改后 |
|---|---|---|
| 全局最优纵向速度 | 0.142 | 0.142 |
| 全局最优横移速度 | 0.250（打满） | 0.083 |
| 全局最优转向速度 | 0.350（打满） | 0.203 |
| 总耗时（5 个目标） | 661 秒 | 294 秒 |
| 前进时间占比 | 59% | 67% |
| 平均转向角速度 | 0.079 | 0.067 |
横移和转向从"打满"降到了中位值，这是权重起作用的直接证据。 耗时也降了一半以上。
但这里有个陷阱。
四、"变快"不等于"变好"
同一批目标点，改权重前后各跑一遍，用导航自身的判定位姿源评估到达精度：
| 目标序号 | 改前最近距离 | 改后最近距离 |
|---|---|---|
| 1 | 0.030 米 | 0.157 米 |
| 2 | 0.063 米 | 0.061 米 |
| 3 | 0.013 米 | 0.169 米 |
| 4 | 0.023 米 | 0.217 米 |
| 5 | 0.023 米 | 0.026 米 |
| 平均 | 0.030 米 | 0.126 米 |
速度快了 2.2 倍，精度差了 4 倍。
原因不难理解：朝向权重降低后，规划器不再那么执着于"把朝向对准"，于是它更愿意以斜角冲向目标。斜角切入在长距离上节省时间，但在最后几厘米的对齐上就不够细致。
所以"降权重让导航变快"是一个正确但可能有害的优化。 是否采纳取决于应用更在乎哪个指标：如果任务对到达位置精度要求高（比如要对准某个检测点），就不该降；如果只是"到那一带就行"，降权重的收益很可观。
这次最终选择回滚，维持原有精度。
五、一个容易踩的坑：位姿源搞错
分析导航性能时，最容易犯的错误不是算错公式，而是用错了输入。
移动机器人通常有两个位姿源：
- 里程计 ：连续的、平滑的、会缓慢漂移，坐标系是
odom - 全局定位 ：由建图匹配或回环校正给出，会跳变，坐标系是
map
两者之间通过一个变换连接：
T_map_to_odom = T_map_to_base × T_odom_to_base⁻¹
两个位姿源随时间的差异。灰色区域是偏差，实测可达 0.4 至 9.6 米------取决于全局定位校正了多少漂移。
导航的到达判定用的是全局定位位姿，不是里程计位姿。 如果用里程计去算"离目标多远"，会得出完全错误的结论。
实测踩过这个坑：用里程计评估，5 个目标只有 1 个到达、平均误差 0.96 米；换用全局定位评估，5 个目标全部到达、平均误差 0.030 米。
结论差了 30 倍，原因只是读了另一个话题。
这个错误特别隐蔽，因为两个位姿源在短时间尺度上是同步的------都反映同样的运动，只是绝对坐标不同。只有当你做"绝对位置判定"时，差异才暴露。
排查方法很简单：同时订阅两个话题，看它们各自的坐标系，以及 T_map_to_odom 的平移量。如果这个平移量不是零，两个位姿源就不能混用。
六、建图侧的同类问题：初始化姿态
顺带提一个建图侧的结构性缺陷，机理和上面的"权重失衡"很像------一个本该精确的量被近似掉了，误差沿距离放大。
激光惯性里程计的世界系通常直接用 IMU 的初始姿态定义。如果初始化时没做重力对齐，而 IMU 有安装倾角，那么整个世界系从一开始就是歪的。
左图：激光束以掠射角打向地面时，垂直方向的等效测距误差随夹角放大。右图：世界系初始倾角如何变成远处的高度偏差------同一个 0.596 度的倾角，20 米处的地面会被放低 0.21 米。
某次实测中这个倾角是 0.596 度。听上去很小，但它造成的后果是：
| 指标 | 修复前 | 修复后 |
|---|---|---|
| 地面拟合倾角 | 1.062 度 | 0.440 度 |
| 地面高度跨度 | 0.431 米 | 0.152 米 |
| 地面下深鬼点 | 1826 个 | 109 个 |
| 平面内点率 | 75.7% | 83.6% |
修法是用静止时的重力方向构造初始旋转：
cpp
V3D g_dir = -mean_acc / mean_acc.norm(); // 重力方向
V3D g_target(0.0, 0.0, -1.0); // 世界系 Z 向下
V3D axis = g_dir.cross(g_target);
double angle = acos(g_dir.dot(g_target));
init_state.rot = SO3(M3D(Eigen::AngleAxisd(angle, axis.normalized())));注意这不依赖"地面是平的"这个假设------它只用 IMU 重力，所以对坑洼地形同样有效。这一点很重要：很多建图后处理方案靠拟合地面平面来纠偏，遇到真实起伏就会把地形起伏也一起抹平。
七、几条可操作的经验
1. 看到速度恒定不变，先去查采样网格
如果规划器输出长期锁定在一个值，算一下它是不是某个采样点。如果是，说明问题在评分函数，不在速度上限。
2. 评估导航性能前，先确认位姿源
odom 和 map 的位姿不要混用。做绝对位置判定时，必须用导航自己判定的那个源。先看 T_map_to_odom 是否为单位变换。
3. 改权重必须同时看两个指标
速度和精度通常是此消彼长。只看速度会做出错误决策，只看精度会误以为改动无效。
4. 横移链路往往是最弱的一环
全向底盘的横移执行率通常低于前进和转向------实测前进 83%、转向 87%、横移只有 56%，方向一致率 61%。横移在规划里很好用（能斜着走），但执行端跟不上，规划器的假设就会失真。
5. 初始化姿态的误差会沿距离放大
一个 0.6 度的倾角在 20 米处就是 0.2 米的高度偏差。这类误差不会因为"多走几圈"而收敛，必须在初始化时解决。
结语
这三个问题（权重失衡导致只转不走、位姿源混用导致评估失真、初始化倾角导致地图倾斜）看起来毫不相干，但内在逻辑是一样的：
系统里有些量的误差不会自己暴露，只会安静地累积，直到某天以完全无关的形态表现出来。
权重失衡不报错，只是走得难看；位姿源混用不报错，只是统计数字骗人；初始化倾角不报错，只是地图慢慢歪掉。
工程上真正难的不是修这些缺陷，而是在它们还没造成事故时发现它们 。这需要的不是更强的算力或更好的算法，而是对每一层输入输出的物理含义保持怀疑------多问一句"这个数字是从哪来的、代表什么坐标系、边界在哪里"。