05 规划、导航与操作

本章抓手

• 规划把目标转成可执行意图，但几何可行、动力学可行和控制可跟踪是三件事。
• 导航主线是 localization + map + costmap + planner + controller。
• 操作主线是 object state + grasp / task plan + IK / motion planning + contact control。

它解决什么问题

规划把“我要完成什么任务”转化成“下一步怎么运动”。在 robotics 中，规划不只是找一条路线，还要满足碰撞、运动学、动力学、接触、安全和实时性约束。

规划可以分成三层：

• Task planning：做什么，顺序是什么。
• Motion planning：几何上怎么动。
• Trajectory generation / optimization：随时间如何执行。

Configuration Space

Configuration space 是 motion planning 的核心概念。机器人每一种可能姿态或关节配置都是空间中的一个点。

例子：

• 2D 小车：x, y, theta。
• 机械臂：每个关节角组成 q。
• 无人机：位置、姿态、速度组成状态。

关键概念：

• Free space：不碰撞的配置集合。
• Obstacle space：会碰撞的配置集合。
• Constraint manifold：满足约束的低维集合，例如末端保持水平。
• Kinodynamic planning：同时考虑运动学和动力学约束。

学习关键词：configuration space, collision checking, constraint manifold, kinodynamic planning。

路径规划 Path Planning

路径规划关注从起点到终点的几何路径，通常不直接考虑时间参数。

代表算法：

• Dijkstra：最短路基础算法。
• A*：用 heuristic 加速搜索。
• D* / D* Lite：适合环境变化后的重规划。
• Hybrid A*：考虑车辆非完整约束 nonholonomic constraints。
• PRM：先采样路网，再查询路径。
• RRT / RRT*：在高维空间中快速探索。

适用判断：

• 栅格地图和低维空间：A* 系列常用。
• 高维机械臂：RRT-Connect 常用。
• 需要渐近最优：RRT* / PRM*。
• 车辆转弯半径约束：Hybrid A*。

学习关键词：A*, D*, PRM, RRT, RRT-Connect, Hybrid A*。

轨迹优化 Trajectory Optimization

轨迹优化把路径变成带时间的、可执行的轨迹，并优化平滑度、能耗、时间、安全距离等目标。

常见目标：

• 最短时间。
• 最小加速度或 jerk。
• 最大安全距离。
• 最小能耗。
• 最小控制输入。

代表方法：

• CHOMP：用梯度优化平滑和避障代价。
• STOMP：随机轨迹优化。
• TrajOpt：把碰撞和运动约束写进优化。
• Direct collocation：把连续控制问题离散成非线性规划。
• Minimum snap trajectory：无人机常用。
• MPC：在线滚动优化。

学习关键词：trajectory optimization, CHOMP, STOMP, TrajOpt, direct collocation, minimum snap。

导航 Navigation

导航把定位、地图、规划和控制组合成移动机器人闭环。

典型 pipeline：

localization -> global costmap -> global planner
-> local costmap -> local planner / controller
-> velocity command

关键组件：

• Global planner：长距离路径。
• Local planner：短时避障和轨迹跟踪。
• Costmap：把障碍、安全距离和语义约束转成代价。
• Recovery behavior：卡住时恢复。
• Dynamic obstacle handling：处理移动人和车。

代表方法：

• NavFn / A* global planning。
• Dynamic Window Approach。
• Timed Elastic Band。
• MPC local planner。
• Social navigation。

学习关键词：robot navigation, costmap, local planner, DWA, TEB, social navigation。

机械臂运动规划 Manipulator Motion Planning

机械臂规划在高维关节空间中找一条无碰撞路径，并满足末端任务约束。

难点：

• 高维 configuration space。
• 碰撞检测昂贵。
• Inverse kinematics 可能多解或无解。
• 奇异点 singularity。
• 窄通道 narrow passage。

常见方法：

• IK + sampling-based planner。
• RRT-Connect。
• Constraint-based planning。
• Trajectory optimization。
• Planning scene + collision checking。

学习关键词：manipulator planning, inverse kinematics, collision checking, MoveIt, RRT-Connect。

抓取 Grasping

抓取是 manipulation 的基础问题：如何选择接触点、夹爪姿态和执行策略，让物体稳定被拿起。

核心概念：

• Force closure：接触力能抵抗任意外部扰动。
• Form closure：几何约束锁住物体。
• Grasp quality：抓取稳定性的评分。
• Antipodal grasp：常见二指夹爪抓取结构。
• Pregrasp pose：抓取前的靠近位姿。

代表方法：

• Analytic grasp synthesis。
• Grasp pose detection from point cloud。
• Learning-based grasp scoring。
• Dex-Net style grasp dataset。
• Diffusion / transformer based grasp policy。

学习关键词：robot grasping, force closure, grasp quality, antipodal grasp, 6D grasp pose。

长时程任务 Task and Motion Planning

很多真实任务不是一次移动，而是一连串符号动作和几何动作。

例子：

打开柜门 -> 找到杯子 -> 抓杯子 -> 移到水龙头 -> 接水 -> 放到桌上

Task and Motion Planning 同时处理：

• Symbolic preconditions：动作前提是否满足。
• Geometric feasibility：动作几何上是否可行。
• Object state：物体位置、开关状态、容器内容。
• Replanning：失败后如何恢复。

代表方法：

• PDDL + motion planner。
• Hierarchical planning。
• Behavior tree。
• LLM as high-level planner + robot controller。

学习关键词：task and motion planning, PDDL, behavior tree, hierarchical planning, long-horizon manipulation。

与感知、估计和控制的关系

• 感知提供障碍、物体和可操作区域。
• 状态估计提供机器人和目标的当前状态。
• 规划生成期望路径或轨迹。
• 控制负责跟踪轨迹并处理扰动。
• 学习方法可以提供 cost function、heuristic、grasp proposal 或 high-level plan。

常见失败模式

• 规划器找到几何路径，但动力学上无法执行。
• 轨迹可执行，但控制器跟踪误差导致碰撞。
• 地图过时，动态障碍没有被及时反映。
• IK 解跳变导致机械臂动作不连续。
• 长时程任务中一个子动作失败后没有恢复策略。

学习关键词

motion planning, path planning, trajectory optimization, navigation, manipulation, grasping, task and motion planning, collision checking。