引言
机械臂的软件管线有四段：规划器给出无碰撞的几何路径，逆解与轨迹生成把它变成关节空间的带时间轨迹，控制器把它变成伺服指令，硬件接口把指令变成电流。四段中任何一段配置错误都会表现为「机械臂不动」或「动得不对」，而错误信息往往指向错误的位置。
工程上最常见的问题是「规划成功但执行失败」。规划器在关节空间里认为路径可行，但执行时因为速度限制、控制器增益、或者轨迹点的密度不足而抖动或报警。这类问题的根源是规划与执行对「可行性」的定义不一致：规划只检查位置层面的碰撞，执行还要求速度、加速度、力矩都在限内。
第二个高频问题是逆解插件的选择。MoveIt2 默认用 KDL，它的逆解成功率在复杂位形下只有 60%~80%，TRAC-IK 能提到 95% 以上，IKFast 在支持的构型上接近 100% 但需要离线生成。选错插件会让「明明可达的目标规划失败」成为常态。
本文按「MoveIt2 管线 → 配置 → 逆解 → ros2_control → 轨迹执行 → 笛卡尔运动 → 抓取 → 装配 → 调参」的顺序展开，给出可直接照抄的配置与参数取值。示例以 Franka Panda 与 UR5 为主，两者的配置差异会在关键处指出。
目录
- MoveIt2 管线总览
- 规划组、SRDF 与规划场景
- 逆解插件：KDL、TRAC-IK 与 IKFast
- ros2_control 与硬件接口
- 轨迹执行与 joint_trajectory_controller
- 笛卡尔空间运动：LIN、CIRC 与 PTP
- 夹爪控制与抓取规划
- 装配：轴孔装配与力控
- 调参、监控与故障排查
1. MoveIt2 管线总览
MoveIt2 把「从目标到轨迹」拆成一条明确的管线，每一段都可以替换。
一次 move_group 请求的完整链路：
1. 目标解析
- 关节空间目标：直接给定关节角
- 位姿目标：调用 IK 插件求解
- 命名目标：从 SRDF 的 group_state 读取
2. 规划（Planning Pipeline）
- 规划请求适配器（Plan Request Adapters）：
修正起始状态、检查工作空间边界、检查目标是否在限位内
- 规划器插件（Planner Plugin）：OMPL / Pilz / CHOMP / STOMP
- 规划响应适配器：缩短路径、时间参数化（TOTG）
3. 轨迹处理
- 时间参数化：AddTimeParameterization（旧）或 TOTG（新，推荐）
- 速度/加速度缩放：max_velocity_scaling_factor
- 检查：是否满足速度、加速度限
4. 执行（Execute Trajectory Action）
- 发给 joint_trajectory_controller
- 等待执行结果（成功/失败/超时，动作的写法见 ROS2 实战）
5. 状态更新
- 从 /joint_states 更新当前状态
- 更新规划场景中的机器人位姿
关键认知：max_velocity_scaling_factor 只缩放速度，不缩放加速度（动作与话题的语义差异见 ROS2 实战
）。设为 0.1 时轨迹会慢十倍，但加速度限制仍然按原值约束，这在某些情况下会因时间拉长导致加速度需求反而增加。需要同时设置 max_acceleration_scaling_factor。
2. 规划组、SRDF 与规划场景
SRDF 定义了 MoveIt 如何理解机器人，规划组是最核心的概念。
<!-- SRDF：定义规划组、虚拟关节、禁用碰撞对、命名位姿 -->
<robot name="panda">
<!-- 规划组：手臂 -->
<group name="panda_arm">
<chain base_link="panda_link0" tip_link="panda_link8"/>
</group>
<!-- 规划组：手爪 -->
<group name="hand">
<link name="panda_hand"/>
<joint name="panda_finger_joint1"/>
<joint name="panda_finger_joint2"/>
</group>
<!-- 虚拟关节：把机器人固定到世界 -->
<virtual_joint name="fixed_base" type="fixed"
parent_frame="world" child_link="panda_link0"/>
<!-- 禁用碰撞对：相邻连杆与已知永不碰撞的对，能大幅加速规划 -->
<disable_collisions link1="panda_link0" link2="panda_link1" reason="Adjacent"/>
<disable_collisions link1="panda_link1" link2="panda_link2" reason="Adjacent"/>
<disable_collisions link1="panda_link0" link2="panda_link2" reason="Never"/>
<!-- ... 通常有几十到上百条 -->
<!-- 命名位姿：常用构型 -->
<group_state name="ready" group="panda_arm">
<joint name="panda_joint1" value="0"/>
<joint name="panda_joint2" value="-0.785"/>
<joint name="panda_joint3" value="0"/>
<joint name="panda_joint4" value="-2.356"/>
<joint name="panda_joint5" value="0"/>
<joint name="panda_joint6" value="1.571"/>
<joint name="panda_joint7" value="0.785"/>
</group_state>
</robot>
disable_collisions 是最容易被忽略但收益最大的配置。一个七轴臂若不配置，规划器会检查所有连杆两两之间的碰撞（21 对），配置后通常只剩 35 对需要检查，规划速度提升 24 倍。MoveIt Setup Assistant 能自动生成，但生成后应人工核对，特别是「Never」类的条目。
3. 逆解插件：KDL、TRAC-IK 与 IKFast
逆解插件的选择直接决定「可达目标是否规划成功」。
KDL（MoveIt2 默认）：
算法：牛顿-拉夫逊迭代 + 关节限位处理
成功率：复杂位形下 60%~80%
速度：平均 1~5 ms，失败时耗满超时
问题：容易陷入局部极小、对初值敏感
TRAC-IK：
算法：并行跑 KDL 与 SQP 两种求解器，谁先收敛用谁
成功率：95% 以上，比 KDL 显著提升
速度：平均略慢，但失败快速返回
配置：solve_type 可选 Speed / Distance / Manip1 / Manip2
IKFast：
算法：离线生成解析解代码（OpenRAVE 工具链）
成功率：接近 100%（在支持的构型上）
速度：微秒级
限制：需满足特定构型（球形腕等），需离线生成，换机器人要重新生成
插件配置：
# move_group 的 kinematics.yaml
panda_arm:
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
kinematics_solver_search_resolution: 0.005 # 搜索分辨率
kinematics_solver_timeout: 0.05 # 50 ms 超时
kinematics_solver_attempts: 3 # 重试次数
solve_type: Distance # 优先返回与种子位形最近的解
position_only_ik: false
solve_type 的取舍：Speed 最快返回任一解；Distance 返回与种子位形（通常是当前位形）最接近的解，能避免大幅绕路，是机械臂的推荐值；Manip1/Manip2 优先返回可操作度高的解，能远离奇异，但计算更慢。
4. ros2_control 与硬件接口
ros2_control 把「控制器」与「硬件」解耦，是 ROS2 的标准控制框架。
三层结构：
1. 硬件接口（Hardware Interface）
实现 read() 与 write()，周期由 controller_manager 驱动
类型：System（多关节）、Actuator（单执行器）、Sensor（只读）
插件：实际硬件 / Gazebo 仿真 / mock（测试用）
2. 控制器管理器（Controller Manager）
以固定周期（典型 500 Hz~1 kHz）调用 update()
顺序：读硬件 → 更新控制器 → 写硬件
管理控制器的加载、配置、激活、切换
3. 控制器（Controller）
joint_state_broadcaster：把关节状态发布到 /joint_states
joint_trajectory_controller：执行轨迹（最常用）
forward_command_controller：直接转发位置/速度/力矩指令
admittance_controller：导纳控制（力控场景）
# ros2_control 的硬件与控制器配置
controller_manager:
ros__parameters:
update_rate: 500 # 控制周期 2 ms
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
gripper_controller:
type: position_controllers/GripperActionController
arm_controller:
ros__parameters:
joints: [panda_joint1, panda_joint2, panda_joint3, panda_joint4,
panda_joint5, panda_joint6, panda_joint7]
command_interfaces: [position, velocity] # 位置 + 速度前馈
state_interfaces: [position, velocity, effort]
state_publish_rate: 100
action_monitor_rate: 20
allow_partial_joints_goal: false
constraints:
stopped_velocity_tolerance: 0.01
goal_time: 0.5
panda_joint1: { trajectory: 0.1, goal: 0.01 }
panda_joint4: { trajectory: 0.1, goal: 0.01 }
command_interfaces 的选择影响精度：只给 position 时控制器内部做插值并输出位置；给 position, velocity 时速度作为前馈，跟踪精度更高。对高动态轨迹，速度前馈能把跟踪误差降低 50% 以上。
5. 轨迹执行与 joint_trajectory_controller
轨迹执行的问题几乎总是「轨迹本身不满足执行约束」。
轨迹被拒绝或执行失败的常见原因：
1. 轨迹的起始点与当前状态不匹配
joint_trajectory_controller 会检查起始误差是否在容差内
容差由 constraints.<joint>.goal 与 stopped_velocity_tolerance 决定
现象：报 "start point not within tolerance"
2. 轨迹点密度不足
两点之间跨度大时，控制器内部插值会产生超速
现象：执行时超速报警或轨迹抖动
对策：规划器的时间参数化分辨率调小（TOTG 的 resample_dt 默认 0.1s）
3. 速度/加速度超限
规划时未考虑控制器的实际限值
现象：执行中途报 velocity limit exceeded
4. 时间戳错误
轨迹点的时间戳必须单调递增，且从当前时间开始
现象：控制器直接拒绝整条轨迹
// 用 MoveIt2 的 C++ 接口执行轨迹（生产代码推荐，比 Python 稳）
#include <moveit/move_group_interface/move_group_interface.h>
auto arm = moveit::planning_interface::MoveGroupInterface(node, "panda_arm");
arm.setMaxVelocityScalingFactor(0.3); // 速度缩放到 30%
arm.setMaxAccelerationScalingFactor(0.2); // 加速度缩放到 20%
arm.setPlanningTime(1.0); // 规划时间上限 1 秒
arm.setNumPlanningAttempts(5); // 最多尝试 5 次
arm.setNamedTarget("ready");
moveit::planning_interface::MoveGroupInterface::Plan plan;
if (arm.plan(plan) != moveit::core::MoveItErrorCode::SUCCESS) {
RCLCPP_ERROR(node->get_logger(), "planning failed");
return;
}
// 执行前检查轨迹的时间参数化是否合理
const auto & traj = plan.trajectory_;
RCLCPP_INFO(node->get_logger(), "trajectory has %zu points, duration %.2fs",
traj.joint_trajectory.points.size(),
rclcpp::Duration(traj.joint_trajectory.points.back()
.time_from_start).seconds());
auto result = arm.execute(plan);
缩放因子的取值策略：调试期用 0.10.2，验证期 0.30.5，生产期 0.5~0.8。不要直接给 1.0，除非已验证过极限工况。同时缩放速度与加速度时，两者比例会影响轨迹形状，通常加速度缩放应小于等于速度缩放。
6. 笛卡尔空间运动：LIN、CIRC 与 PTP
工业场景常要求末端走直线或圆弧，而不是关节空间的任意路径。Pilz 的工业运动规划器提供这三种原语。
PTP（Point-to-Point）：
关节空间同步运动，各关节同时到达
优点：快、无奇异问题
缺点：末端轨迹不可控（可能走弧线）
适用：快速定位、无路径要求
LIN（Linear）：
末端走直线，姿态线性插值
优点：路径可控、适合接近与离开
缺点：经过奇异位形会失败、需要逆解可行
适用：直线接近、插拔、涂胶
CIRC（Circular）：
末端走圆弧，需要中间点与终点
优点：圆弧路径
缺点：约束更多、失败率更高
适用：圆弧焊接、绕行避障
# 用 MoveIt2 的 Python 接口做笛卡尔直线运动
from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
from geometry_msgs.msg import Pose
def make_pose_constraint(pose, frame, link, tol_pos=0.001, tol_ori=0.01):
pc = PositionConstraint()
pc.header.frame_id = frame
pc.link_name = link
pc.constraint_region.primitives.append(
SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[tol_pos]))
pc.constraint_region.primitive_poses.append(pose)
pc.weight = 1.0
oc = OrientationConstraint()
oc.header.frame_id = frame
oc.link_name = link
oc.orientation = pose.orientation
oc.absolute_x_axis_tolerance = tol_ori
oc.absolute_y_axis_tolerance = tol_ori
oc.absolute_z_axis_tolerance = tol_ori
oc.weight = 1.0
c = Constraints()
c.position_constraints.append(pc)
c.orientation_constraints.append(oc)
return c
# 笛卡尔路径规划（走直线，逐点求逆解）
waypoints = [pose_start, pose_mid, pose_end]
(plan, fraction) = arm.compute_cartesian_path(
waypoints,
eef_step=0.01, # 末端步长 1cm，越小越平滑越慢
jump_threshold=0.0, # 0 表示禁用关节跳变检测（不推荐）
avoid_collisions=True)
print(f"完成度 {fraction*100:.1f}%") # 必须接近 100%，否则有段不可达
compute_cartesian_path 返回的 fraction 是关键指标。小于 1.0 表示部分路径无法走直线（通常是遇到奇异或不可达），必须检查而不是直接执行，否则会得到一条不完整的轨迹。jump_threshold 设为 0 会禁用关节空间跳变检测，在奇异附近可能产生关节的瞬间大幅跳变，生产环境应设为 0.0 以外的合理值（如 1.5 倍最大关节步长）。
7. 夹爪控制与抓取规划
夹爪在 MoveIt2 里通常是一个独立的规划组，通过 GripperActionController 控制。
# GripperActionController 配置
gripper_controller:
ros__parameters:
joint: panda_finger_joint1 # 主动关节（另一指为 mimic）
goal_tolerance: 0.005 # 5mm 容差
max_effort: 20.0 # 最大力 20N
allow_stalling: true # 允许堵转（抓取时必需）
stall_velocity_threshold: 0.001 # 堵转判定速度阈值
stall_timeout: 1.0 # 堵转超时
allow_stalling 是抓取场景的关键配置。默认情况下控制器会因「目标未到达」而报失败，但抓取时夹爪本来就会因接触物体而停在中间，必须允许堵转并把「堵转且有力」判定为成功。
抓取规划涉及三个动作的编排，顺序不能错：
抓取序列（每一步都要检查结果）：
1. 移动到预抓取位姿（沿抓取方向后退 5~10 cm）
- 用位姿目标或笛卡尔直线
2. 直线接近到抓取位姿
- 必须用 LIN 或笛卡尔直线，不能用 PTP（否则可能撞到物体侧面）
3. 闭合夹爪（GripperActionController 的 grasp action）
- 检查返回的 success 与达到的开度
- 开度明显大于预期 → 抓空
- 开度接近 0 → 物体太薄或没抓到
4. 抬起（沿抓取反方向直线后退，再移到安全高度）
5. 移动到放置点，张开夹爪
第 2 步和第 4 步必须用直线运动是工程铁律。用 PTP 接近时，末端可能沿一条弧线扫过物体，造成碰撞或推走物体。
8. 装配：轴孔装配与力控
轴孔装配（peg-in-hole）是机械臂最难的任务之一，间隙小于 0.1 mm 时纯位置控制几乎不可能成功。
装配的三个阶段与策略：
1. 接近阶段（间隙 > 5 mm）
位置控制，快速接近到孔上方 5 mm
用视觉或示教确定孔的大致位置
2. 搜索阶段（间隙 0.1~5 mm）
位置控制在精度极限，需要柔顺或搜索
策略 A：螺旋搜索（spiral search）
在孔附近画螺旋线，靠接触力变化判断是否对准
策略 B：力控搜索（力引导）
施加恒定的下压力与横向柔顺，靠倒角自动对中
3. 插入阶段（间隙 < 0.1 mm）
必须力控
策略：导纳控制，横向刚度设小（100~500 N/m），
轴向设中等刚度，让轴在孔内自动找正
关键参数：横向刚度、阻尼、下压力（5~20 N）
// 轴孔装配的导纳控制核心：横向柔顺 + 轴向恒力
// 力传感器读数 F，期望下压力 F_d
const Eigen::Vector3d F = ft_sensor.readForce();
const Eigen::Vector3d F_err = F - F_d; // F_d = (0, 0, -10) N 向下
// 导纳方程：M·ẍ + D·ẋ + K·x = F_err
// 横向刚度小（柔顺），轴向刚度中等
Eigen::Vector3d K(200.0, 200.0, 1000.0); // N/m
Eigen::Vector3d D(20.0, 20.0, 60.0); // N·s/m
Eigen::Vector3d M(2.0, 2.0, 2.0); // kg
// 离散积分求位置修正量
x_acc_ = (F_err - D.cwiseProduct(x_vel_) - K.cwiseProduct(x_)) / M;
x_vel_ += x_acc_ * dt;
x_ += x_vel_ * dt;
// x_ 作为位置修正叠加到名义轨迹上，下发给位置控制器
装配成功的三个关键：倒角（孔口有倒角时成功率提升数倍，是机械设计而非软件能解决的问题）、柔顺方向正确（横向柔顺、轴向刚性，反过来会卡死）、力控带宽足够（至少 100 Hz，低于 50 Hz 时接触振荡）。
9. 调参、监控与故障排查
故障排查应按固定顺序，避免盲目试错。
排查顺序（从底层到上层）：
1. /joint_states 是否有数据、频率是否正常
ros2 topic hz /joint_states
2. TF 树是否完整、末端位姿是否正确
ros2 run tf2_tools view_frames
3. 逆解是否成功（用 move_group 的 IK 服务单独测试）
ros2 service call /compute_ik moveit_msgs/srv/GetPositionIK "{...}"
4. 规划是否成功（单独测规划，不执行）
在 RViz 里用 Plan 按钮，看规划耗时与失败原因
5. 轨迹是否合理（点数、时长、起止点）
打印 trajectory 的元信息
6. 控制器是否激活、约束是否满足
ros2 control list_controllers
7. 执行是否超时或被拒绝（看 controller 的日志）
关键监控指标：规划耗时（p50/p99）、规划失败率、逆解失败率、轨迹执行成功率、执行跟踪误差。前三项反映软件配置质量，后两项反映机械与控制质量。规划失败率超过 5% 就应该排查配置，正常配置良好的系统应在 1% 以下。
# 常用诊断命令
ros2 control list_controllers # 控制器状态
ros2 control list_hardware_interfaces # 硬件接口状态
ros2 topic echo /panda_arm_controller/state --once # 控制器状态与误差
ros2 param get /move_group robot_description_kinematics.panda_arm.kinematics_solver
权衡取舍
| 决策 | 选 A | 选 B |
|---|---|---|
| 逆解插件 | KDL：默认、零配置 | TRAC-IK：成功率高、推荐 |
| 逆解速度 | IKFast：微秒级、需生成 | TRAC-IK：毫秒级、通用 |
| 规划器 | OMPL：通用、随机 | Pilz：工业原语、可预测 |
| 接近方式 | LIN：路径可控、可能失败 | PTP：快、路径不可控 |
| 控制接口 | 仅 position：简单 | position + velocity：精度高 |
| 装配 | 纯位置：间隙大时可行 | 力控：小间隙必需 |
核心原则：接近与离开必须走直线，中间移动可以走关节空间。这条规则能避免绝大多数「机械臂撞到东西」的事故，代价只是稍微慢一点。
常见坑清单
- 用 PTP 接近物体，末端弧线扫过物体造成碰撞——接近与离开必须用 LIN 或笛卡尔直线。
max_velocity_scaling_factor只缩放速度不缩放加速度，时间拉长后加速度需求反增——同时设置加速度缩放。- SRDF 未配置
disable_collisions，规划慢 2~4 倍——按相邻与不可达关系配置并核对。 - KDL 逆解成功率低，可达目标频繁规划失败——换 TRAC-IK 并把
solve_type设为Distance。 compute_cartesian_path的fraction小于 1.0 仍直接执行，轨迹不完整——必须检查 fraction 接近 100%。jump_threshold设为 0，奇异附近关节瞬间大幅跳变——设为合理值而非 0。- 夹爪控制器未开
allow_stalling，抓取时因堵转报失败——抓取场景必须允许堵转。 - 轨迹起始点与当前状态不匹配，控制器拒绝执行——检查起始容差并确认状态已同步。
- 装配时轴向柔顺横向刚性，导致卡死无法自动找正——横向柔顺、轴向刚性，方向不能反。
- 力控带宽低于 50 Hz，接触时振荡——力控循环至少 100 Hz，并注意滤波引入的延迟。
小结
机械臂管线的问题分布很有规律：规划失败多半是配置（逆解插件、SRDF、超时），执行失败多半是约束（速度、加速度、起始点），抓取失败多半是接近方式（用了 PTP），装配失败多半是柔顺方向或力控带宽。按这个映射去排查，效率远高于随机调参。
下一步建议阅读运动规划与轨迹优化 ，理解规划器内部的搜索与时间参数化；机器人视觉与抓取 会讲清抓取位姿如何从感知得到并经过手眼变换进入本文的管线；动力学与控制基础 提供了力控与阻抗控制的数学细节，是装配任务的必要前置。
最后一句：先在 RViz 里把规划和逆解调通，再上真机。MoveIt2 的 RViz 插件能可视化规划耗时、失败原因与碰撞体，把这些问题在仿真环境里解决，能省下大量真机调试时间。
继续阅读
探索更多技术文章
浏览归档，发现更多关于系统设计、工具链和工程实践的内容。