机器人软件系统
SLAM定位建图、路径规划(A*/RRT*)、运动规划(MoveIt2)、行为树任务编排、LLM任务规划——完整的机器人软件栈。
1. SLAM：同时定位与建图
问题：$$p(x_{1:T}, m | z_{1:T}, u_{1:T})$$——未知环境中同时定位和建图
| 类型 | 传感器 | 代表 |
|---|---|---|
| 激光SLAM | 2D/3D LiDAR | Cartographer |
| 视觉SLAM | RGB-D | ORB-SLAM3 |
图优化SLAM：节点=位姿，边=约束，最小化$$\min_x \sum \|e_{ij}(x_i, x_j)\|^2_{\Sigma_{ij}}$$
2. 路径规划
A*：$$f(n)=g(n)+h(n)$$，保证最优但高维困难
RRT*：采样基+局部重连，适合6DOF+的高维空间
# A* 核心循环
while open_set:
_, _, current = heapq.heappop(open_set)
if current == goal: return reconstruct_path(came_from, current)
for neighbor in neighbors(current):
tentative_g = g_score[current] + 1
if tentative_g < g_score.get(neighbor, inf):
came_from[neighbor] = current
g_score[neighbor] = tentative_g
heapq.heappush(open_set, (tentative_g + h(neighbor), -tentative_g, neighbor))3. 运动规划：MoveIt2
架构：规划请求→规划管道→OMPL运动规划器→轨迹
约束规划：笛卡尔路径+定向约束（工具保持竖直）+关节限位
4. 行为树
| 节点 | 符号 | 作用 |
|---|---|---|
| Sequence | → | 顺序执行，一个失败整体失败 |
| Fallback | ？ | 尝试子节点直到一个成功 |
bt = Sequence("抓取任务", [
Condition("检测物体", obj_detected),
Action("接近", approach),
Action("抓取", grasp),
Fallback("验证重试", [
Sequence("验证-提起", [
Condition("验证成功", verify_grasp),
Action("提起", lift),
]),
Action("重新抓取", grasp),
]),
])5. LLM任务规划
自然语言→LLM→JSON技能序列→行为树执行→运动规划→轨迹执行
6. ROS通信节点/拓扑图网页实验
真实机器人软件栈不是单个脚本，而是一张持续流动的通信网络。感知节点发布图像和点云，融合节点生成目标，规划器输出路径，控制节点发送关节或底盘命令；任何一个节点频率、QoS、CPU负载或桥接延迟异常，都会变成“机器人反应慢、TF不同步、控制发抖”。
实验前置问题
- 相机从30Hz升到60Hz时，哪个节点最可能先变成瓶颈？
- QoS可靠度提高以后，为什么延迟有时会变大？
- 如果控制节点收到旧消息，真实机器人会出现什么症状？
完成标准
- 能解释每条边对应的Topic和消息方向。
- 能把端到端延迟压到80ms以内，并说明你牺牲了什么。
- 能写出一次ROS系统排错顺序：先看Topic频率，再看TF，再看节点负载，最后看控制器状态。
阶段总结
SLAM是机器人的"眼睛+记忆"，运动规划是从可行到最优的桥梁，行为树比状态机更适合复杂任务的反应式执行。
知识地图
| 系统模块 | 输入 | 输出 | 典型工具 |
|---|---|---|---|
| SLAM | LiDAR/RGB-D/IMU/里程计 | 地图+机器人位姿 | Cartographer、ORB-SLAM3、Nav2 |
| 任务规划 | 自然语言目标、场景约束 | 可执行技能序列 | PDDL、LLM planner、行为树 |
| 运动规划 | 起点、目标、障碍物、约束 | 无碰撞轨迹 | MoveIt2、OMPL、TrajOpt |
| 行为执行 | 技能序列、反馈状态 | 成功/失败/重试 | BehaviorTree.CPP、ROS2 action |
| 系统监控 | 日志、TF、topic、诊断信息 | 健康状态和故障定位 | ros2 doctor、rqt、Foxglove |
任务规划到行为树
现代具身智能常用“LLM负责高层语义，行为树负责可控执行”的分层结构。LLM不直接控制电机，而是输出技能调用计划；行为树负责检查前置条件、失败恢复、超时和安全中断。
{
"goal": "把红色积木放入左侧盒子",
"plan": [
{"skill": "detect", "object": "red_block"},
{"skill": "pick", "object": "red_block"},
{"skill": "place", "target": "left_box"},
{"skill": "verify", "condition": "object_in_box"}
]
}MoveIt2工程工作流
- 建模：准备URDF/Xacro、关节限位、惯量、碰撞几何和末端执行器。
- 配置：用MoveIt Setup Assistant生成SRDF、规划组、Kinematics、OMPL参数。
- 规划：设置目标位姿、姿态约束、路径约束和规划时间，比较RRTConnect、RRT*、PRM。
- 执行：把轨迹发送给ros2_control，监控joint trajectory controller状态。
- 诊断：用RViz、TF tree、PlanningScene、ros2 topic echo检查碰撞体和坐标系。
实验任务
- A*导航：在栅格地图上实现A*，比较曼哈顿、欧氏、对角线启发函数的路径质量。
- RRT*机械臂规划：在2D/3D关节空间中规划避障路径，报告采样次数、路径长度和平滑后长度。
- 行为树抓取：实现检测-接近-抓取-验证-重试流程，模拟检测失败和抓取失败。
- MoveIt2 Pick&Place：在仿真中完成一套pick/place demo，并保存RViz截图和轨迹耗时。
- ROS拓扑诊断：完成网页端ROS通信拓扑实验，记录延迟、丢包率、瓶颈节点和你的调参策略。
常见问题排查
- TF错误：优先检查frame_id、静态外参、时间戳和tf buffer长度。
- 规划失败：检查目标是否可达、碰撞几何是否过大、IK是否有解、规划时间是否太短。
- 轨迹抖动：检查速度/加速度限幅、控制频率、joint state延迟和轨迹插值。
- 行为树卡死：为每个Action添加超时、失败码和Recovery分支。
能力清单与下一阶段前置要求
完成本阶段后，应能画出机器人软件栈数据流，独立解释SLAM、任务规划、行为树、MoveIt2之间的接口边界。进入机器人控制前，需要掌握坐标系、状态反馈、轨迹、误差和控制频率这些概念。
教材式自测与学习进度
概念自测
- 解释SLAM中定位和建图为什么互相依赖。
- 说明A*启发函数需要满足什么条件才能保证最优。
- 比较A*、RRT、RRT*、PRM的适用场景。
- 解释MoveIt2中的PlanningScene有什么作用。
- 说明IK失败、碰撞失败、控制失败分别应该在哪里排查。
- 比较行为树和有限状态机在失败恢复上的差异。
- 解释LLM任务规划为什么不能直接绕过行为树和安全检查。
- 说明ROS2 action为什么适合长时间机器人技能。
操作练习
- 画出一个移动机械臂系统的topic、service、action数据流。
- 实现A*并把搜索过程可视化。
- 实现简化RRT并统计不同步长下的成功率。
- 用XML写一个BehaviorTree.CPP风格的抓取行为树。
- 在RViz中检查TF tree和PlanningScene碰撞体。
进度追踪
| 里程碑 | 验收标准 | 完成 |
|---|---|---|
| SLAM理解 | 能解释前端里程计和后端图优化 | □ |
| 路径规划 | 能实现A*和RRT基础版本 | □ |
| MoveIt2 | 能在仿真中完成一次无碰撞规划 | □ |
| 行为树 | 能设计含重试和恢复的任务树 | □ |
| 任务规划 | 能把自然语言目标拆成可执行技能序列 | □ |
扩展挑战
- 把LLM输出计划自动转换为行为树XML。
- 为MoveIt2规划失败设计fallback策略。
- 把SLAM地图用于导航，再把导航结果交给机械臂抓取。
- 用Foxglove录制一次完整任务并做故障复盘。
阶段复盘问题
- 一个机器人任务失败时，如何区分感知失败、规划失败、控制失败和执行器故障？
- 如果行为树频繁进入重试分支，应该优先看日志、传感器还是规划参数？
- 为什么复杂任务最好拆成可复用技能，而不是写一个巨大的端到端脚本？