移动底盘与机械臂联合规划：开源算法与工程实现

问题背景

在移动操作机器人中，底盘负责移动和导航，机械臂负责抓取、放置、检测、操作设备等任务。

传统方案通常将任务拆成两步：

底盘导航到目标附近

↓

机械臂进行抓取或操作

这种方式实现简单，但存在明显问题：

底盘停的位置不一定是机械臂最佳工作位姿；
机械臂可能够不到目标，或者接近奇异位姿；
只规划机械臂时，无法利用底盘移动来绕开障碍物；
整机避障时，底盘、机械臂和末端执行器之间缺少协同。

因此，移动操作机器人通常需要采用底盘-机械臂联合规划，也称为：

Whole-Body Planning，全身规划；
Mobile Manipulator Planning，移动操作机器人规划；
Whole-Body IK，全身逆运动学；
Base-Arm Coordinated Motion Planning，底盘-机械臂协同规划。

──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────

将底盘和机械臂放到同一个优化空间

假设机器人包含一个平面移动底盘和一个 (n) 自由度机械臂。

机器人整体构型可以表示为：

\\mathbf{q} = \[x_b,\\ y_b,\\ \\theta_b,\\ q_1,\\ q_2,\\ldots,q_n\]

其中：

(x_b,y_b)：底盘在地图坐标系中的位置；
(\theta_b)：底盘航向角；
(q_1 \sim q_n)：机械臂各关节角。

因此，整个机器人不是单独在"机械臂关节空间"中规划，而是在：

3+n

维的联合构型空间中规划。

例如，六轴机械臂加差速底盘的变量为：

[x_base, y_base, yaw_base,

joint_1, joint_2, joint_3,

joint_4, joint_5, joint_6]

联合优化问题的数学形式

设机器人需要移动到目标附近，并使机械臂末端到达抓取位姿，则可构造如下优化问题：

\\min_{\\mathbf{q}(t)} w_1 J_{path} - w_2 J_{smooth} - w_3 J_{collision} - w_4 J_{ee} - w_5 J_{base}

其中：

(J_{path})：整机轨迹长度；
(J_{smooth})：底盘和机械臂运动平滑性；
(J_{collision})：碰撞和安全距离惩罚；
(J_{ee})：机械臂末端与目标位姿的误差；
(J_{base})：底盘停车位姿或移动距离代价；
(w_1 \sim w_5)：不同目标的权重。

需要满足的约束包括：

3.1 末端位姿约束

FK(\\mathbf{q}) = \\mathbf{T}_{ee}\^{goal}

即机械臂末端必须到达指定目标位置和姿态。

3.2 关节限位约束

\\mathbf{q}{min} \\leq \\mathbf{q} \\leq \\mathbf{q}{max}

机械臂关节、升降机构和底盘运动范围不能超过机械限制。

3.3 碰撞约束

d(\\text{robot}(\\mathbf{q}), \\text{obstacle}) \\geq d_{safe}

机器人本体、机械臂、末端工具都需要与障碍物保持安全距离。

3.4 差速底盘非完整约束

对于差速底盘，不能直接横向移动，需要满足：

\\dot{x}_b\\sin\\theta_b-\\dot{y}_b\\cos\\theta_b=0

这也是移动底盘和普通机械臂联合规划中最容易被忽略的问题。

相关开源算法与项目

4.1 REMANI-Planner

项目地址：

https://github.com/Robotics-STAR-Lab/REMANI-Planner

REMANI-Planner 是面向移动操作机器人的实时全身运动规划算法。它不是简单地让底盘先导航、再让机械臂规划，而是在联合空间中同时考虑：

移动底盘轨迹；
机械臂关节轨迹；
机器人整体碰撞；
障碍物距离；
时间可行性；
速度与加速度约束。

整体思路可以概括为：

联合状态空间搜索

↓

找到底盘和机械臂的可行通道

↓

进行时空轨迹优化

↓

输出整机连续轨迹

输出状态通常为：

q(t) =

[x_base(t), y_base(t), yaw_base(t),

q1(t), q2(t), ..., qn(t)]

适合场景：

移动过程中机械臂也需要协同避障；
抓取目标位置不确定；
狭窄环境下需要底盘和机械臂共同调整；
研究全身规划和实时轨迹优化算法。

4.2 GCDF：移动操作机器人轨迹优化

项目地址：

https://github.com/YulinLi0/GCDF

GCDF 的核心思路是使用广义构型空间距离场，为移动底盘和机械臂组成的高维构型空间提供更快的碰撞距离估计。

传统碰撞检测在高自由度系统中计算量较大。GCDF 尝试直接学习或近似构型空间中的距离信息，用于优化：

底盘位置

底盘朝向
机械臂关节角
整段时间轨迹

主要特点：

面向移动操作机器人；
强调快速、安全的轨迹优化；
可用于复杂障碍环境中的联合避障；
偏研究型实现，需要一定算法基础。

适合场景：

复杂障碍环境

高自由度机械臂
对实时性要求较高

4.3 Tesseract + TrajOpt

项目地址：

https://github.com/tesseract-robotics/trajopt

Tesseract 是一个面向机器人运动规划的开源框架，TrajOpt 是其轨迹优化模块。

TrajOpt 的核心思想是：将连续轨迹离散为多个时刻的状态，然后在这些离散状态上进行优化。

例如：

t0: $base_x0, base_y0, yaw0, q1_0, ..., qn_0$

t1: $base_x1, base_y1, yaw1, q1_1, ..., qn_1$

t2: $base_x2, base_y2, yaw2, q1_2, ..., qn_2$

...

优化目标可以包括：

末端到达目标位姿；
底盘移动距离尽量短；
机械臂关节运动尽量小；
轨迹速度和加速度平滑；
整机远离障碍物；
满足底盘运动学约束。

TrajOpt 常用于：

抓取轨迹优化；
避障轨迹优化；
多关节机械臂规划；
移动底盘与机械臂协同规划；
已有初始轨迹后的局部优化。

适合工程实现的原因：

可以定义自定义代价函数；
可以加入自定义约束；
支持碰撞检测；
可以结合 ROS 2、URDF、SRDF 和 MoveIt 使用。

4.4 MoveIt 2

项目地址：

https://github.com/moveit/moveit2

MoveIt 2 是 ROS 2 中最常用的机械臂运动规划框架，集成了：

正运动学与逆运动学；
OMPL 采样规划；
碰撞检测；
轨迹执行；
Planning Scene 环境模型；
ros2_control 控制接口；
抓取和任务级规划支持。

MoveIt 2 也可以用于移动操作机器人，但需要将底盘自由度加入规划组。

例如，在机器人模型中定义底盘的平面虚拟关节：

<virtual_joint

name="base_planar_joint"

type="planar"

parent_frame="map"

child_link="base_link" />

然后在 SRDF 中创建一个包含底盘和机械臂的规划组：

mobile_manipulator_group =

base_planar_joint

arm_joint_1
arm_joint_2
...
arm_joint_n

这样 MoveIt 规划的状态就是：

x_base, y_base, yaw_base, q1, q2, ..., qn

│ 注意：MoveIt 的平面虚拟关节本身并不自动保证差速底盘不能横移。

│ 如果底盘是差速、阿克曼或履带底盘，需要额外加入运动学约束，或者让 Nav2 / Hybrid-A* 提供可行底盘初始轨迹。

4.5 youBot MoveIt 联合规划示例

项目地址：

https://github.com/seanbai2008/youbot_moveit

该项目基于 KUKA youBot，包含：

全向移动底盘

5 自由度机械臂

项目中展示了移动底盘与机械臂的联合规划。其描述中明确包含全自由度规划，并考虑了底盘运动限制。

虽然该项目偏 ROS 1，代码和依赖可能较旧，但对于理解联合构型空间和移动操作机器人 MoveIt 配置仍有参考价值。

4.6 Stretch ROS 2

项目地址：

https://github.com/hello-robot/stretch_ros2

https://github.com/hello-robot/stretch_tutorials

Stretch 是一类典型的移动操作机器人，其结构包括：

移动底盘

升降机构
伸缩机械臂
夹爪
相机与深度传感器

其 ROS 2 教程中包含 MoveIt 2 的全身规划思路，可用于参考：

底盘和机械臂的 TF 坐标系设计；
URDF/SRDF 建模；
MoveIt 2 配置；
抓取任务；
视觉和机械臂协同。

4.7 Drake

项目地址：

https://github.com/RobotLocomotion/drake

Drake 是一个更加偏底层数学和优化的机器人软件框架，支持：

非线性优化；
直接配点；
最优控制；
轨迹优化；
多刚体动力学；
碰撞检测；
接触约束；
MPC；
全身控制。

如果需要严格处理以下问题，Drake 很适合：

差速底盘/阿克曼底盘运动学

机械臂动力学
速度与加速度限制
轨迹优化
模型预测控制 MPC

缺点是 ROS 2 工程集成成本通常高于 MoveIt 2 和 Tesseract。

不同方案的选择建议

┌────────────────────────────┬──────────────────────┐

│ 目标 │ 推荐方案 │

├────────────────────────────┼──────────────────────┤

│ 快速搭建 ROS 2 原型 │ Nav2 + MoveIt 2 │

├────────────────────────────┼──────────────────────┤

│ 底盘和机械臂统一优化 │ Tesseract + TrajOpt │

├────────────────────────────┼──────────────────────┤

│ 研究实时全身移动操作规划 │ REMANI-Planner │

├────────────────────────────┼──────────────────────┤

│ 研究高维构型空间碰撞优化 │ GCDF │

├────────────────────────────┼──────────────────────┤

│ 需要严格动力学和 MPC │ Drake + CasADi/IPOPT │

├────────────────────────────┼──────────────────────┤

│ 学习完整移动操作机器人案例 │ Stretch ROS 2 │

├────────────────────────────┼──────────────────────┤

│ 参考早期联合规划实现 │ youBot MoveIt │

└────────────────────────────┴──────────────────────┘

推荐的工程架构

对于实际机器人项目，可以采用下面的分层架构：

GNSS/RTK、IMU、轮速里程计、激光雷达、RGB-D 相机

↓

状态估计与坐标变换 TF

↓

SLAM / AMCL / GNSS 融合定位 / 地图构建

↓

Nav2 全局导航

↓

联合优化层：底盘位姿 + 机械臂关节轨迹

↓

MoveIt 2 / Tesseract / TrajOpt

↓

ros2_control 底盘与机械臂执行

典型任务流程如下：

接收抓取目标

↓

视觉识别目标物体或接收目标坐标

↓

联合优化底盘停靠位姿与机械臂抓取姿态

↓

Nav2 导航至操作区域

↓

底盘局部微调

↓

机械臂执行抓取轨迹

↓

撤臂、返回或继续导航

室外 RTK 场景说明

如果系统使用 RTK/组合导航设备，例如 GNSS RTK + IMU，则可以将其用于：

室外全局定位

初始航向估计
大范围路径导航
地图坐标系定位

但 RTK 不能直接保证机械臂抓取精度。

机械臂抓取阶段仍需要依赖：

RGB-D 相机或双目相机；
激光雷达；
AprilTag / ArUco 标记；
视觉伺服；
机械臂与相机外参标定；
底盘与机械臂的 TF 标定。

建议分工如下：

RTK/IMU：解决"机器人在大地图中的位置"

激光雷达：解决"机器人在局部环境中的避障与定位"

RGB-D 相机：解决"目标物体在哪里"

联合规划器：解决"底盘和机械臂如何一起运动"

总结

移动底盘与机械臂联合规划的核心，是将：

底盘位置 x、y、yaw

机械臂所有关节角

放到统一构型空间中进行求解。

对于实际项目，推荐优先采用：

ROS 2

Nav2
MoveIt 2
Tesseract / TrajOpt

对于算法研究，可以重点关注：

REMANI-Planner

GCDF

Drake

其中，REMANI-Planner 更接近完整的移动操作机器人实时全身规划问题；Tesseract + TrajOpt 更适合工程落地时构建自定义优化目标和约束。