4.1 运动规划与轨迹生成

4.1 运动规划与轨迹生成

本节摘要：机器人要从一个位置移动到另一个位置，中间有障碍，关节有角度限制，速度有上限——怎么算出一条能走、安全、平滑的路径？这就是运动规划解决的问题。本节讲运动规划和轨迹生成的流程，介绍采样规划（如 RRT）和优化规划两类主流方法的思路差别，以及怎么把路径变成可执行的时间轨迹。运动规划是机器人从“想去哪”到“怎么去”的关键一步。

学习目标

阅读完本节，你应当能够：

1. 区分路径（无时间）和轨迹（含时间）
2. 说清运动规划要满足的约束
3. 理解 RRT 等采样规划方法的思路
4. 理解优化规划方法的思路
5. 说清轨迹平滑化的作用

一、问题与直觉

机器人要伸手去抓桌上的杯子，手现在在身侧，杯子在正前方，中间还挡着一把椅子。机器人怎么动？

最直觉的想法：画条直线从手到杯子。但这条路可能撞上椅子，或者超出关节能弯曲的范围。所以要考虑障碍（不能撞椅子）、关节限位（关节转不到那个角度）、动力学（速度和加速度不能超电机能力）。在这些约束下，找到一条能走、安全、还得尽量短或快的路径，这就是运动规划。

运动规划在机器人里是个经典且难的问题。难点在于约束多（障碍、关节、速度）、空间维度高（一个七轴机械臂的运动空间是七维的）、还要求实时（不能算太久）。几十年来发展出两大类方法：采样法和优化法，各有优劣。理解它们的思路，你才知道不同场景该用哪种。

二、核心原理

2.1 路径与轨迹

先把两个概念区分清楚。

路径（Path）：纯几何的一条线，只描述“经过哪些点”，没有时间信息。比如“从 A 点经过 B 点到 C 点”。

轨迹（Trajectory）：加了时间信息的路径，描述“什么时候到哪个点”。轨迹 = 路径 + 时间律。比如“0 秒在 A，1 秒到 B，2 秒到 C，且速度连续变化”。

机器人执行需要的是轨迹，因为控制器要按时间发出指令。但规划常常先算路径（几何上可行），再给路径加时间律（变成轨迹）。这个分离让问题简化。

2.2 运动规划的约束

运动规划要满足好几类约束。

碰撞约束：机器人任何部位不能撞上环境障碍，也不能撞自己（手臂不能绕到打结）。这是最基本的安全约束。

关节限位：每个关节的转动范围有限（肘关节不能反向弯），规划出的关节角度必须在限位内。

动力学约束：速度和加速度有上限（电机功率有限），轨迹的速度加速度不能超标。

任务约束：有些任务对姿态有要求（端水杯要保持杯子竖直，不能翻转），这限制了末端的姿态自由度。

满足所有约束的路径叫可行路径。运动规划的目标是在可行路径里找一条最优的（最短、最快、最平滑）。

2.3 采样规划：RRT

第一类主流方法是采样法，代表是快速探索随机树（RRT）。

RRT 的思路很巧妙：在高维运动空间里随机撒点，逐步长出一棵树探索空间，直到树碰到目标。具体做法：在空间里随机取一个点，从树里找离它最近的节点，往随机点方向扩展一小步，如果这步不撞障碍就加入树，重复直到树扩展到目标。

RRT 的好处是适合高维空间（因为它随机探索，不被维度困住）、能处理复杂障碍。短板是规划出的路径不最优（随机的，绕来绕去）、不平滑（折线）、不保证找到解（虽然实际中大概率能找到）。

因为 RRT 的路径不优不平滑，实际中常加后处理：用路径平滑（把折线弄顺）、路径优化（把路径往更短更平滑推）。

2.4 优化规划

第二类方法是优化法。思路是：定义一个目标函数（路径长度、平滑度、离障碍距离），在约束下优化这个目标，得到最优路径。

优化法的好处是规划出的路径质量高（最优、平滑），短板是对初始猜测敏感（容易陷局部最优）、在复杂障碍环境可能找不到解（被障碍困住）。它适合障碍较简单、对路径质量要求高的场景。

实际中常把两者结合：先用采样法（RRT）找到一条可行路径（保证能找到解），再用优化法把它优化得平滑最优（保证质量）。

2.5 轨迹生成

路径算出来后，要变成轨迹——加上时间律。这步要保证速度和加速度连续（不连续会让机器人抖动），且不超动力学限制。

常用做法是用多项式或样条函数对路径做时间参数化，让位置随时间平滑变化。平滑化的价值是让运动连续柔顺——机器人不会急停急起，减少机械冲击、提高精度。

三、工程实践要点

3.1 选规划方法看场景

采样法和优化法各有优劣，选哪个看场景。障碍复杂、维度高、要求实时找到解，用采样法（RRT 及其变种）；障碍简单、要求路径质量高、可以慢一点，用优化法。多数实际系统两者结合——采样保底找解，优化提升质量。

3.2 规划要留安全余量

规划算出的路径紧贴障碍边缘很危险——模型有偏差、控制有误差，紧贴的路径实际执行可能撞上。所以规划时要给障碍留安全余量（把障碍“膨胀”一点，让路径离真实障碍有距离），用这点距离换安全。

3.3 实时规划与重规划

环境是动态的（人走过来、东西倒了），规划不能一次算完就死板执行。要实时感知环境变化，发现原路径要撞上新障碍，立即重规划。这种实时重规划能力对在动态环境里作业的机器人至关重要。

⚠️ 常见坑：规划算完就死板执行，中间环境变了（人挡过来了）也不调整，结果撞上。规划必须是动态的——持续感知，环境变化就重规划，别指望一次规划管到底。

💡 关键直觉：运动规划的本质是在多维约束空间里搜索。约束越多（障碍多、关节限位严、任务要求高），搜索空间越窄、越难找解。理解这点，你才知道为什么复杂场景的规划要花时间、为什么有时找不到解——不是算法差，是约束空间真的窄。

本节速览

• 路径vs轨迹：路径纯几何无时间，轨迹加时间律，执行需要轨迹。
• 四类约束：碰撞、关节限位、动力学、任务约束，都要满足。
• RRT 采样法：随机撒点长树探索，适合高维复杂障碍，但路径不优不平滑。
• 优化法：定义目标函数优化，路径质量高但易陷局部最优、复杂障碍难找解。
• 两者结合：采样找可行解，优化提质量，主流做法。
• 轨迹平滑：多项式或样条参数化，保证速度加速度连续，减少冲击。
• 安全余量：膨胀障碍，路径离真实障碍有距离，换安全。
• 实时重规划：环境变化立即重规划，别死板执行一次规划的结果。

下一节讲运动控制的另一半——力控，为什么光控位置不够、接触操作必须控力。