规划：从目标状态到一条能执行的行动方案
规划是在状态、障碍、动力学和任务规则的约束下，搜索一条把机器人从当前带到目标的可执行方案。
路径、轨迹与任务是三个层次
路径只描述经过哪些几何状态，不含时间；轨迹给路径加上时间、速度和加速度；任务规划决定先做什么、后做什么，以及条件不满足时怎样分支。例如“拿杯子”可以被任务规划展开为观察、选择抓取、移动到预抓取位、闭合夹爪、确认抓稳；其中机械臂从预抓取位到抓取位的无碰撞运动由运动规划处理，具体每个毫秒的关节参考值由轨迹生成器给出。
运动规划通常在构型空间 \(\mathcal C\) 中进行。机械臂的一个点 \(\mathbf q\) 代表整台机械臂的一种姿态；造成碰撞的构型形成障碍区域 \(\mathcal C_{obs}\)，其余为自由空间 \(\mathcal C_{free}\)。规划问题是寻找连续路径
\(\mathbf q_s\) 是起点，\(\mathcal Q_g\) 是目标构型集合。目标集合保留了选择空间：末端到达某个容差区间可能有多组关节姿态，让规划器选择更安全的一组；固定单点会提前丢掉这些候选。
图搜索：把连续世界离散成节点和边
栅格导航把空间划成格子，边表示相邻格可通行。Dijkstra 算法按从起点的累计代价 \(g(n)\) 扩展节点，能得到非负边权下的最短路。A* 再加入到目标的启发式估计 \(h(n)\)：
若 \(h(n)\) 从不高估真实剩余代价，A* 仍可保证最优，同时少扩展许多无关节点。欧氏距离和曼哈顿距离是常见启发式，但实际机器人代价还会包含离障碍距离、转弯、坡度和禁行区。权重一旦改变，“最短”就变成相对于该代价函数最优。
分辨率决定搜索规模和安全性。格子太大，窄通道会消失；太小，内存与搜索时间增加。占据地图通常先按机器人外形膨胀，规划中心点路径，这等价于把有体积的机器人近似成点。若机器人会旋转且轮廓非圆，单一二维膨胀仍可能过于保守或漏碰撞，需要将朝向纳入状态空间。
碰撞检测是规划时间的主要消耗之一。离散检查只验证采样姿态，两个安全采样点之间的连续运动仍可能穿过薄障碍；连续碰撞检测或自适应插值能覆盖这段风险。机器人模型中的视觉网格适合显示，碰撞几何常用简化凸体以提高速度。简化过粗会封死通道，过细会拖慢查询，必须用真实任务中的最小间隙验证。
采样规划：在高维空间中寻找通路
机械臂的构型空间可能有 6、7 甚至更多维，均匀栅格数量随维数指数增长。PRM 在自由空间采样节点，连接相近且局部无碰撞的节点，形成可供多次查询的路线图；RRT 从起点树反复朝随机样本延伸，具有向未探索大区域扩展的倾向，适合单次查询和动力学约束。
RRT 在满足条件时具有概率完备性：若可行路径存在，采样趋于无限时找到路径的概率趋近 1；它不保证有限时间一定找到，也不保证路径短。RRT* 和 PRM* 在相应假设下具渐近最优性，但有限时间的工程结果仍受采样、碰撞检测和参数影响。
窄通道是采样法的典型难点，因为随机点落入狭小可行区域的概率低。可使用目标偏置、桥采样、任务空间引导、双向树或经验分布。任何偏置都可能加速某类任务，也可能让另一类任务更难，因此应以代表性场景做成功率与时间分布评估。
从路径到可执行轨迹
几何路径可能要求关节瞬间停下或转向。时间参数化在不改变或小幅调整路径的情况下，满足关节速度、加速度、加加速度和力矩限制。简单的多项式轨迹能保证端点连续；S 曲线限制加加速度，减小机械冲击；时间最优参数化沿路径尽量提高速度，同时尊重约束。
轨迹优化直接把离散状态 \(\mathbf x_{0:N}\) 和控制 \(\mathbf u_{0:N-1}\) 作为变量：
\(\ell\) 可以惩罚时间、能耗、离障碍过近或不平滑，\(f\) 是动力学。优化法能产生平滑、高质量轨迹，也可能陷入局部最优并依赖初值。常见组合是用采样法找到可行路径，再用优化器平滑和加入动力学。
动态环境与重规划
真实环境中的人和车辆会移动。把某一帧障碍位置永久写入地图会过度保守，只看当前帧又可能撞上即将到达的行人。动态规划需要预测障碍在时间上的占据，或在滚动时域内频繁重算局部轨迹。预测必须带不确定性，距离越远通常越不确定。
规划器还要避免“每帧换主意”。相近代价的左右绕行方案若交替胜出，会让机器人摇摆。可在代价中加入轨迹一致性、设置决策滞后，或由行为层锁定通行策略直到明确失败。重规划也需停止条件：计算超时、局部无解、进展不足时，应减速、停车或恢复；过期轨迹需要明确作废。
任务规划、状态机与行为树
符号任务规划把世界描述成谓词，例如 cup_on_table、gripper_empty，动作带前置条件和效果。规划器可搜索出满足目标的动作序列，但符号状态必须由感知落地；“夹爪已闭合”不能自动推出“杯子已抓住”。
有限状态机适合状态少、转移清晰的流程；层级状态机把子流程封装；行为树通过顺序、选择、并行和条件节点组合行为，便于复用和恢复。复杂系统常让任务规划生成高层方案，行为树负责在线执行与异常分支，运动规划器负责几何可行性，控制器负责实时跟踪。
工程例子：机械臂能规划到目标，却总在货架口超时
窄货架任务中，采样规划器偶尔几十毫秒找到路径，偶尔数秒无解。原因可能是有效通道在构型空间中很窄，默认采样分布不容易进入；末端目标被固定为唯一姿态，进一步压缩可行集；碰撞模型把软线缆当成刚性大体积，也会封死通路。
改进应依次验证几何模型、目标容差和可行性，再选择双向规划、任务空间约束采样或经验引导。设置明确的规划时间预算和候选策略；超过预算后换抓取姿态或请求重新摆放物体。单纯把超时时间调得更长会拖慢节拍，仍无法解释最坏情况。