系列： 2026 ROS 2 Lyrical 踩坑实录
环境： Ubuntu 26.04 / WSL2、ROS 2 Lyrical、Gazebo Sim、Nav2、RViz2
关键词： ROS 2、Lyrical、Gazebo、移动机械臂、差速底盘、关节控制、重力补偿、LiDAR、自遮挡

前言

在 Gazebo 中搭建移动机械臂时，经常会遇到一些“不报错，但就是不正常”的问题：

• Nav2 已经输出速度，底盘却慢得像没有移动；
• 机械臂的旋转关节能够工作，垂直升降轴却无法到位，甚至持续下坠；
• 周围明明没有障碍物，机器人身边却始终存在一团跟着它移动的激光点云；
• TF、Topic 和控制器看起来都正常，继续调整 Nav2 参数却没有明显改善。

这些现象分别发生在速度约束、关节动力学和传感器几何三个层面。只盯着 /cmd_vel、PID 或 Costmap 中的某一项参数，很容易在错误的方向上反复调试。

本文整理最近开发移动机械臂仿真时遇到的三个实际问题，并给出一条可以复用的诊断思路。

说明：本文记录的是开发过程中已经定位清楚的独立问题，不代表整个移动机械臂工程已经完成最终验收。文中的质量、尺寸和控制参数来自当前仿真模型，用于解释问题，不应直接复制到其他机器人。

---

一、先把问题放回完整链路

移动机械臂不是“Nav2 底盘加一个机械臂模型”这么简单。它至少包含三条相互影响的链路：

导航链：Nav2 → 速度指令 → 差速驱动 → 轮轴 → 底盘运动

控制链：目标关节位置 → 控制器 → 力/速度输出 → 关节反馈

感知链：Gazebo LiDAR → LaserScan → 本体过滤 → Costmap → Nav2

三个典型症状与故障位置的关系如下：

症状	真正的故障位置	容易被误判为
/cmd_vel 正常，但底盘移动极慢	轮轴速度上限	Nav2 参数或 Gazebo 卡顿
升降轴无法保持目标高度	重力与控制输出	逆运动学或 Topic 错误
Costmap 中存在跟随机器人的障碍	LiDAR 扫描到机器人自身	地图错误或定位漂移

排查时应该先确认问题发生在哪一层，再修改对应配置。

---

二、坑一：底盘不报错，但移动速度只有 0.072 m/s

1. 问题现象

Nav2 能够规划路径，控制器也持续发布速度：

ros2 topic hz /cmd_vel
ros2 topic echo /cmd_vel

但是 Gazebo 中的机器人移动非常缓慢。由于 WSL2 和 Gazebo 本身也可能受到图形性能影响，第一反应往往是：

• Gazebo 实时因子太低；
• Nav2 的 max_vel_x 设置太小；
• DDS 或 Bridge 存在延迟；
• 机器人质量或轮胎摩擦力不合理。

这些方向都值得检查，但本次问题的根因不在 Nav2，而在轮轴模型的速度上限。

2. 根因：轮轴继承了机械臂的低速限制

为了让机械臂运动更平稳，模型中的操作关节统一设置了较低的速度上限：

<limit effort="1000" velocity="0.6"/>

生成差速轮关节时错误地复用了这个默认值，导致左右轮最多只能达到：

0.6 rad/s

轮子的线速度由下面的公式决定：

v = ω × r

当前模型的轮子半径为 0.12 m，因此理论最大线速度只有：

v = 0.6 × 0.12 = 0.072 m/s

这意味着机器人走完一条几米长的路线就需要很长时间。Nav2 可以正常工作，也不会因此直接报错，但整个系统看起来像是“卡住了”。

3. 修复：把底盘与机械臂的速度边界分开

轮轴不应该复用机械臂关节的默认速度限制。可以在模型生成函数或 URDF/SDF 中分别配置：

<!-- 差速轮 -->
<joint name="left_wheel_joint" type="revolute">
  <axis>
    <xyz>0 1 0</xyz>
    <limit>
      <effort>1000</effort>
      <velocity>8.0</velocity>
    </limit>
  </axis>
</joint>

<!-- 机械臂旋转关节 -->
<joint name="shoulder" type="revolute">
  <axis>
    <xyz>0 0 1</xyz>
    <limit>
      <lower>-3.14</lower>
      <upper>3.14</upper>
      <effort>1000</effort>
      <velocity>0.6</velocity>
    </limit>
  </axis>
</joint>

轮轴上限设置为 8.0 rad/s 后，按照相同轮径计算，模型能力边界约为：

8.0 × 0.12 = 0.96 m/s

这里的 0.96 m/s 只是关节模型允许的理论上限，不表示 Nav2 应该以这个速度运行。实际导航速度仍然应由 Nav2 控制器、加速度限制和使用场景决定。例如当前任务可以继续将期望线速度限制在 0.25 m/s 左右。

4. 推荐的排查顺序

遇到“有速度但走得慢”时，可以按照下面的顺序检查：

# 1. Nav2 是否真的在发布速度
ros2 topic hz /cmd_vel
ros2 topic echo /cmd_vel

# 2. 里程计是否持续更新
ros2 topic hz /odom
ros2 topic echo --once /odom

# 3. 轮轴是否有实际状态反馈
ros2 topic echo --once /joint_states

然后检查：

• Nav2 的最大线速度；
• 速度平滑器或安全联锁是否进行了二次限速；
• 差速驱动插件是否订阅了正确的 Topic；
• 轮子半径与轮距是否正确；
• SDF/URDF 中轮轴的 velocity 上限；
• Gazebo 的实时因子是否明显低于 1。

最值得记住的是：

/cmd_vel 中的目标速度不等于轮子一定能够达到的速度。关节上限、驱动插件和物理模型都可能在后面继续限制它。

---

三、坑二：升降轴收到位置指令后仍然下坠

1. 问题现象

移动机械臂采用 SCARA 结构，肩关节、肘关节和腕关节都能够响应位置指令，但是负责垂直运动的升降轴出现异常：

• 目标位置已经发布，关节却无法上升；
• 升到一定高度后又缓慢下坠；
• 提高位置控制器的比例增益后出现抖动；
• 程序按照等待时间继续执行，但机械臂实际上没有到位。

如果只检查 Topic，很容易得出“命令已经发出，控制器应该没有问题”的错误结论。

2. 根因：垂直关节必须持续抵消重力

水平旋转关节和垂直移动关节面对的动力学条件不同。

升降轴承受的不只是滑轨自身质量，还包括随它一起移动的结构，例如：

• 滑台；
• 上臂和前臂；
• 腕部与夹爪；
• 被抓取的物体。

如果升降部分的等效质量为 m，仅保持静止就需要大约：

F = m × g

当前仿真模型中，升降轴上方结构的名义质量约为 3.2 kg，对应的静态重力约为：

F = 3.2 × 9.81 ≈ 31.392 N

如果控制器只根据位置误差临时产生输出，没有合适的力控制或前馈补偿，就可能出现到不了目标、持续下坠或者增益过大后振荡的问题。

3. 修复：力 PID 加名义重力补偿

当前模型为垂直升降轴单独配置了力控制，并使用 cmd_offset 提供名义重力补偿：

<plugin filename="gz-sim-joint-position-controller-system"
        name="gz::sim::systems::JointPositionController">
  <joint_name>lift</joint_name>
  <topic>/mm/joint/lift</topic>

  <p_gain>500</p_gain>
  <i_gain>30</i_gain>
  <d_gain>70</d_gain>
  <i_max>10</i_max>
  <i_min>-10</i_min>

  <cmd_offset>31.392</cmd_offset>
  <cmd_max>100</cmd_max>
  <cmd_min>-100</cmd_min>
</plugin>

这里有两个关键点。

第一，31.392 不是适用于所有机器人的固定值。修改连杆质量、工具或负载后，重力补偿也需要重新计算。真实系统还要考虑摩擦、传动效率、弹性和负载变化。

第二，重力补偿只是提供维持高度所需的基础输出，PID 仍然负责修正目标位置与实际位置之间的误差。不能只依赖一个固定补偿值代替闭环控制。

4. 不要用“等待几秒”假装关节已经到位

下面这种写法很脆弱：

joint_command.publish(target)
time.sleep(2.0)
# 默认认为关节已经到位

仿真速度、控制增益和负载稍有变化，固定等待时间就可能失效。更可靠的方式是订阅 /joint_states，根据实际反馈判断：

error = abs(actual_position - target_position)
reached = error < tolerance

同时应该设置超时：

持续发送或保持目标
        ↓
读取实际 JointState
        ↓
误差小于阈值 → 到位
        ↓
超过截止时间 → 停止任务并报告失败

如果控制接口或 Bridge 在启动阶段可能丢失首条消息，任务侧还可以在等待期间周期性刷新目标指令，但刷新频率应与控制器设计相匹配。

5. 升降轴的排查清单

# 是否存在目标指令
ros2 topic info /mm/joint/lift
ros2 topic echo /mm/joint/lift

# 是否存在实际关节反馈
ros2 topic hz /joint_states
ros2 topic echo --once /joint_states

还要继续检查：

• Prismatic Joint 的轴方向是否正确；
• 上下限是否包含目标位置；
• 力矩或推力上限是否足够；
• 控制插件工作在力模式还是速度模式；
• 移动部分的质量是否设置合理；
• 是否需要重力前馈；
• PID 是否存在稳态误差、振荡或饱和；
• 程序是否真的使用 JointState 判断到位。

---

四、坑三：LiDAR 把相机立柱和夹爪当成障碍物

1. 问题现象

机器人周围没有动态障碍物，但在 RViz2 中可以看到固定的激光回波。机器人移动时，这些回波也会跟着机器人一起移动，并不断进入 Local Costmap。

可能出现的后果包括：

• 机器人被自己的 Costmap 判定为处于障碍物中；
• Local Planner 频繁绕行、旋转或停止；
• 规划器报告附近没有有效轨迹；
• 机械臂改变姿态后，Costmap 中的异常点也发生变化。

这种现象很像 TF 错误，但它也可能是完全正确的物理仿真结果：LiDAR 确实看到了机器人自身结构。

2. 根因：模型结构穿过了扫描平面

当前模型中，LiDAR 的扫描高度约为 0.60 m。相机立柱从底盘向上延伸，升降柱以及某些姿态下的夹爪也可能穿过这一水平面。

Gazebo 会按照碰撞几何计算激光射线。只要模型的碰撞体出现在扫描平面内，LiDAR 就会产生真实回波。TF 正确并不会自动消除这些回波。

判断是否为本体回波，可以观察：

1. 异常点在 base_link 下的位置是否基本固定；
2. 机器人移动后，异常点是否一起移动；
3. 改变机械臂姿态后，部分异常点是否随之变化；
4. 回波方向是否对应立柱、线缆架或夹爪的实际安装位置。

3. 方案选择

最直接的方案是重新设计传感器位置，让机器人结构不穿过扫描平面。无法修改结构时，可以增加本体过滤：

Gazebo LiDAR
      ↓
  /scan_raw
      ↓
根据机器人几何与关节状态过滤本体回波
      ↓
    /scan
      ↓
AMCL / Nav2 Costmap

保留 /scan_raw 很重要。出现问题时，可以同时观察原始数据和过滤结果，避免把传感器问题误判为 Nav2 问题。

4. 一个简化的几何过滤示例

下面的代码展示了基本思路。它把 LaserScan 中的极坐标测量转换到机器人平面坐标，再判断击中点是否位于已知的本体区域：

import math


def filter_self_returns(ranges, angle_min, angle_increment):
    output = list(ranges)

    for index, distance in enumerate(ranges):
        if not math.isfinite(distance):
            continue

        angle = angle_min + index * angle_increment

        # 示例中 LiDAR 相对 base_link 的 X 偏移为 0.14 m
        x = 0.14 + distance * math.cos(angle)
        y = distance * math.sin(angle)

        # 示例：相机立柱在 base_link 下的平面包围区域
        hits_camera_mast = -0.39 < x < -0.29 and -0.27 < y < -0.17

        # 示例：中央升降柱区域
        hits_lift_column = abs(x) < 0.08 and abs(y) < 0.08

        if hits_camera_mast or hits_lift_column:
            output[index] = float("nan")

    return output

实际工程中不应长期写死坐标。更通用的实现应该根据 TF、URDF 碰撞几何和当前关节状态动态计算本体区域。

机械臂尤其需要动态处理：夹爪是否处于扫描高度，取决于升降轴和其他关节的实时位置。只按照初始姿态建立静态屏蔽区，可能会漏掉移动部件，也可能误删真实障碍物。

5. 为什么不能简单增大 range_min

一种常见做法是增大 LiDAR 的最小测距：

<range>
  <min>0.50</min>
</range>

这可能顺便隐藏机器人本体，但也会删除所有真实的近距离障碍物。对于需要贴近工作台作业的移动机械臂，这会制造新的安全盲区。

更合理的做法是只过滤已知本体几何范围，并保留其他方向的近距离测量。

6. 为什么过滤结果使用 NaN

如果某条射线先撞到相机立柱，那么立柱后方区域实际上没有被观测到。把这条射线直接改成 range_max，可能让下游错误地认为整条射线方向都是空闲空间，进而清除本应保持未知的区域。

将其标记为 NaN，表达的是“这条测量无效”，而不是“这个方向直到最大距离都没有障碍物”。

不过，不同下游组件对无效测量的处理可能存在差异。正式使用前应验证 AMCL、Costmap 和其他订阅者是否按照预期忽略 NaN。

7. 过滤器也需要测试

至少应覆盖下面三种情况：

相机立柱回波                → 应过滤
夹爪处于扫描高度产生的回波   → 应过滤
本体旁边的真实近距离障碍物   → 必须保留

最后一项最重要。一个“删除了所有异常点”的过滤器不一定正确，它也可能把真实障碍物一起删掉。

---

五、三个问题为什么容易互相干扰

移动机械臂的调试比普通差速机器人更复杂，因为一个问题经常会触发另一个问题。

例如：

轮轴速度被限制
    ↓
导航时间明显增长
    ↓
任务等待超时
    ↓
被误判为 Nav2 规划失败

又例如：

升降轴受重力下坠
    ↓
夹爪进入 LiDAR 扫描平面
    ↓
机器人出现新的本体回波
    ↓
Local Costmap 认为前方有障碍物

所以调试时不要一次修改大量 Nav2、控制器和传感器参数。应该先建立能够单独验证每一层的最小测试。

---

六、推荐的分层调试方法

第一步：不启动 Nav2，只检查底盘

直接向底盘控制链发送小速度，观察实际里程计变化，并确认轮子半径、轮距和速度上限。

测试时确保环境安全，并使用较小速度：

ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/Twist \
"{linear: {x: 0.1}, angular: {z: 0.0}}"

停止测试后按 Ctrl+C，并确认底盘收到零速度。真实机器人上还需要独立急停和安全控制，不能只依赖终端命令。

第二步：固定底盘，只测试机械臂

分别发送每个关节目标，持续观察 /joint_states：

ros2 topic hz /joint_states
ros2 topic echo /joint_states

重点确认：

• 关节方向是否正确；
• 目标位置是否可达；
• 垂直轴能否保持高度；
• 停止发送命令后是否发生漂移；
• 控制输出是否达到上限。

第三步：机械臂保持多个姿态，检查 LiDAR

在 RViz2 中同时显示 /scan_raw 和 /scan，让机械臂依次处于：

• 完全收拢；
• 升降轴最低位置；
• 夹爪接近 LiDAR 扫描高度；
• 工作空间边缘位置。

这样可以验证过滤器是否既能删除本体回波，又能保留真实障碍物。

第四步：最后再接入 AMCL、Costmap 和 Nav2

只有底盘、关节和传感器单独可靠后，再启动完整导航链。否则 Nav2 日志里表现出来的“规划失败”可能只是底层问题的二次症状。

---

七、快速定位表

症状	首先检查	不要急着修改
/cmd_vel 有数据但底盘龟速	轮轴速度上限、轮径、实时因子	AMCL 粒子数
轮速正常但里程计比例错误	wheel_radius 与 wheel_separation	全局规划器
升降轴无法上升	力上限、轴方向、重力补偿	逆运动学
升降轴到位后下坠	控制模式、重力前馈、PID	TF
关节命令发出但程序提前进入下一步	/joint_states 闭环与超时	增加固定等待时间
激光异常点跟着底盘移动	传感器扫描平面与本体几何	地图分辨率
机械臂移动后异常点变化	动态本体过滤与关节状态	AMCL 初始位姿
过滤后近距离障碍物消失	过滤区域过大或 range_min 过高	Inflation Layer

---

八、总结

本次三个问题分别给出了三条值得保留的工程经验。

1. 控制命令正常，不代表执行机构没有被二次限制

排查底盘速度时，不能只看 /cmd_vel。轮轴速度上限、驱动插件、加速度约束和物理实时因子都会影响最终运动。

2. 垂直关节不能直接照搬水平关节的控制方式

升降轴需要持续抵消重力。应根据实际移动质量设计力控制、重力前馈和 PID，并使用真实关节反馈判断是否到位。

3. LiDAR 看见机器人自身，不一定是传感器或 TF 出错

只要机器人结构穿过扫描平面，Gazebo 就会产生符合物理模型的本体回波。优先改进安装几何；确实无法避免时，再使用经过测试的本体过滤器。

最终可以把移动机械臂的排错原则压缩成一句话：

先验证执行机构是否真的按命令运动，再验证传感器看到的是否真是外部世界，最后才调整 Nav2。

---