一、轨迹规划概述
1.1 到底什么是轨迹规划？
先说说我的理解。轨迹规划，说白了就是回答三个问题：去哪儿、怎么去、路上别撞到啥。
你想想看，一个机械臂要抓杯子，光知道目标位置不够。你还得告诉它：肘关节抬多高、手腕怎么转、中间经过哪些点、速度怎么变化。这一整套动作的时空描述，就是轨迹规划。
我习惯把轨迹规划拆成两层：
- 路径层——只关心空间几何，不关心时间。比如从A点到B点，中间绕过桌子腿，这是一条路径。
- 轨迹层——在路径上加上时间信息。什么时候加速、什么时候减速、在哪个位置停留0.5秒。
核心定义：轨迹规划 = 路径（空间几何） + 时间律（速度/加速度/加加速度）
我在做第一个抓取项目时，就踩过这个坑。路径规划跑通了，但机械臂一到目标点就猛冲过去，杯子直接被撞飞。后来才意识到——我根本没做轨迹规划，只做了路径规划。
1.2 轨迹规划在具身智能里的角色
具身智能强调什么？感知→决策→执行的闭环。轨迹规划正好卡在决策和执行之间。
我画了张图，帮你理清这个位置：
你看，轨迹规划是承上启下的枢纽。上面接任务目标，下面连电机控制。少了它，感知再准、决策再聪明，机器人也动不起来。
我的经验：在真实机器人上，轨迹规划往往是整个系统里最容易被低估的模块。很多人花大量精力优化感知模型，结果最后卡在轨迹平滑性上。我建议你从一开始就给轨迹规划留足设计空间。
1.3 轨迹规划和路径规划到底有啥区别？
这个问题我几乎每次讲课都会被问到。直接上对比表：
| 对比维度 | 路径规划 | 轨迹规划 |
|---|---|---|
| 核心问题 | 空间上能不能到？ | 时间上怎么到？ |
| 输出 | 点序列（无时间戳） | 时间-位置-速度-加速度 |
| 约束类型 | 障碍物、关节限位 | 速度限、加速度限、力矩限、平滑性 |
| 典型算法 | A*、RRT、PRM | 多项式插值、样条、优化方法 |
| 评价指标 | 路径长度、无碰撞 | 时间最优、能量最优、平滑度 |
举个例子你就明白了。路径规划告诉你：绕过桌子，从椅子右边过去，到达桌面。轨迹规划告诉你：前2秒加速到0.3m/s，中间1秒匀速，最后1秒减速到0，并且在终点前0.2秒开始调整手腕姿态。
我曾经在一个码垛项目里只做了路径规划，结果机器人每到拐角就急停急启，电机嗡嗡响。后来加上梯形速度规划，整个动作就丝滑了。嗯，这就是轨迹规划的威力。
1.4 应用场景：抓取、导航、操作
轨迹规划在具身智能里无处不在。我挑三个典型场景说说：
1.4.1 抓取（Grasping）
抓取是轨迹规划最经典的应用。机械臂要从初始位置移动到目标物体，然后抓起来。
这里有个难点：末端执行器的姿态约束。你不仅要让手到位置，还要让手掌朝向、手指开合角度都对。
我记得做过一个抓取螺丝刀的项目。螺丝刀躺在桌上，手柄朝左。路径规划告诉我能到，但轨迹规划必须保证：最后5cm时手腕旋转30度，手指在接触前0.3秒开始闭合。这些时间上的精细控制，全靠轨迹规划。
抓取轨迹规划的关键约束：
- 接近方向（通常沿物体表面法线）
- 末端速度（接触瞬间速度要低）
- 力控切换时机（位置控制→力控制）
1.4.2 导航（Navigation）
移动机器人的导航轨迹规划，和机械臂不太一样。它更关注：
- 运动学约束——差速轮不能侧移，阿克曼转向有最小转弯半径
- 动态障碍物——行人、其他机器人
- 地形变化——斜坡、门槛、地毯
我做过一个仓储AGV项目。一开始我们用纯路径规划，结果机器人每次转弯都打滑。后来加了速度约束和曲率约束的轨迹规划，转弯时自动降速，问题就解决了。
你想想看，导航轨迹规划本质上是在回答：以什么速度通过哪个弯道最安全？
1.4.3 操作（Manipulation）
操作比抓取更复杂。它涉及多步骤、多物体、多接触。比如：
- 开抽屉（先拉把手，再平移）
- 拧瓶盖（先夹紧，再旋转）
- 叠衣服（多次抓取、折叠、放置）
操作的轨迹规划有个特点：任务阶段切换。每个阶段的轨迹约束完全不同。比如拧瓶盖，前半段是直线接近，后半段是旋转运动。轨迹规划要保证这两个阶段平滑衔接。
避坑提醒：操作任务里最容易出问题的是接触瞬间。我曾经在装配任务里没处理好接触过渡，结果零件被怼飞了。后来我在轨迹规划里加了阻抗控制接口，接触前切换到柔顺模式，才搞定。
我把三个场景的核心差异整理了一下：
| 场景 | 主要约束 | 典型轨迹类型 | 我遇到的坑 |
|---|---|---|---|
| 抓取 | 姿态、接触速度 | 笛卡尔直线+关节插值 | 末端速度没控制好，撞飞物体 |
| 导航 | 运动学、动态避障 | DWA、TEB | 转弯打滑，没考虑地面摩擦 |
| 操作 | 接触力、阶段切换 | 分段多项式、优化轨迹 | 接触瞬间怼飞零件 |
1.5 本章小结
这一章我们聊了：
- 轨迹规划是路径+时间律的组合
- 它在具身智能里扮演决策到执行的桥梁
- 和路径规划的区别在于时间维度和动态约束
- 抓取、导航、操作各有各的轨迹规划难点