1. 轨迹规划概述
1.1 什么是轨迹规划？
先问大家一个问题：你让机器人去抓桌上的杯子，它知道杯子在哪，也知道自己胳膊在哪。但怎么从A点到B点？这就是轨迹规划要解决的问题。
说白了，轨迹规划就是给机器人设计一条“时空路径”。它不仅要告诉机器人“往哪走”，还要告诉它“什么时候走到什么位置、速度多快、加速度多大”。
我个人习惯把轨迹规划比作开车去公司：
- 路径规划：导航告诉你走哪条路
- 轨迹规划：导航告诉你每个路口什么时候转弯、油门踩多深、刹车什么时候踩
你看，后者比前者多了“时间”和“运动状态”两个维度。
1.2 为什么轨迹规划对具身智能如此重要？
我在项目中遇到过不少“路径规划完美，但机器人就是动不了”的情况。举个例子：
有一次做机械臂抓取任务，路径规划算出了一条完美的避障路径。结果一运行，机械臂在拐角处剧烈抖动，差点把工件甩飞。为什么？因为轨迹规划没做好——加速度突变太大了。
轨迹规划的重要性体现在三个层面：
- 安全第一：没有平滑的轨迹，机器人可能撞墙、自碰撞、甚至伤人。嗯，这里要注意，工业机器人伤人事件，多半是轨迹规划没做好。
- 任务可行性：比如抓取一个移动的传送带上的零件，你不仅要规划路径，还要算好时间——去早了抓空，去晚了撞到传送带。
- 效率与能耗：好的轨迹规划能节省30%以上的运动时间，同时降低电机发热。我曾经优化过一个码垛机器人的轨迹，单次循环时间从12秒降到8秒，客户直接加钱。
1.3 从路径规划到轨迹规划的演进
你想想看，早期的机器人是怎么动的？
我记得刚入行时，用的还是“点到点”运动。就是告诉机器人：从A点直线走到B点。简单粗暴，但问题很多：
- 遇到障碍物怎么办？——绕不过去
- 关节限位怎么办？——直接报错
- 运动不平滑怎么办？——抖动、冲击
后来有了路径规划，比如A*、RRT这些算法。它们解决了“能不能到”的问题，但没解决“怎么到得舒服”的问题。
再后来，大家发现光有路径不够。你想想，一条路径上，如果速度忽快忽慢，加速度突变，电机受不了，工件也会晃动。于是轨迹规划应运而生。
这张图展示了从路径到轨迹的演进逻辑：
1.4 轨迹规划的核心要素
搞清楚了演进过程，我们来看看轨迹规划到底要处理哪些东西。我总结为“三要素”：
| 要素 | 含义 | 实际例子 |
|---|---|---|
| 空间路径 | 机器人末端或关节在空间中的几何曲线 | 从(0,0,0)到(1,1,1)的直线或圆弧 |
| 运动律 | 沿路径运动的速度、加速度随时间的变化 | 梯形速度曲线、S形速度曲线 |
| 约束条件 | 关节限位、速度上限、加速度上限、力矩上限 | 关节1最大速度 30°/s，最大加速度 60°/s² |
1.5 一个简单的轨迹规划示例
光说不练假把式。我们来看一个最简单的轨迹规划——梯形速度曲线。
假设要让机器人关节从0°转到90°，最大速度30°/s，最大加速度60°/s²。代码实现如下：
import numpy as np
def trapezoidal_trajectory(q_start, q_end, v_max, a_max, dt=0.01):
"""
梯形速度曲线轨迹规划
参数：
q_start: 起始位置
q_end: 终止位置
v_max: 最大速度
a_max: 最大加速度
dt: 时间步长
返回：
t, q, v, a: 时间、位置、速度、加速度序列
"""
# 计算总位移
delta_q = q_end - q_start
# 计算加速段和减速段所需时间
t_acc = v_max / a_max
# 加速段位移
d_acc = 0.5 * a_max * t_acc**2
# 判断是否能达到最大速度
if 2 * d_acc >= abs(delta_q):
# 三角形速度曲线（达不到最大速度）
t_acc = np.sqrt(abs(delta_q) / a_max)
v_peak = a_max * t_acc
t_const = 0
else:
# 梯形速度曲线
v_peak = v_max
d_const = abs(delta_q) - 2 * d_acc
t_const = d_const / v_peak
# 生成轨迹
t_total = 2 * t_acc + t_const
t = np.arange(0, t_total, dt)
q = np.zeros_like(t)
v = np.zeros_like(t)
a = np.zeros_like(t)
for i, ti in enumerate(t):
if ti < t_acc:
# 加速段
a[i] = a_max * np.sign(delta_q)
v[i] = a_max * ti * np.sign(delta_q)
q[i] = q_start + 0.5 * a_max * ti**2 * np.sign(delta_q)
elif ti < t_acc + t_const:
# 匀速段
a[i] = 0
v[i] = v_peak * np.sign(delta_q)
q[i] = q_start + d_acc * np.sign(delta_q) + v_peak * (ti - t_acc) * np.sign(delta_q)
else:
# 减速段
t_dec = ti - t_acc - t_const
a[i] = -a_max * np.sign(delta_q)
v[i] = (v_peak - a_max * t_dec) * np.sign(delta_q)
q[i] = q_end - 0.5 * a_max * (t_total - ti)**2 * np.sign(delta_q)
return t, q, v, a
# 使用示例
t, q, v, a = trapezoidal_trajectory(0, 90, 30, 60)
print(f"轨迹总时长: {t[-1]:.2f} 秒")
print(f"最大速度: {np.max(np.abs(v)):.2f} °/s")
print(f"最大加速度: {np.max(np.abs(a)):.2f} °/s²")1.6 轨迹规划在具身智能中的定位
最后，我想说说轨迹规划在整个具身智能系统中的位置。你想想看，一个完整的具身智能系统通常包含：
- 感知层：摄像头、激光雷达、触觉传感器 → 获取环境信息
- 决策层：任务规划、行为选择 → 决定“做什么”
- 规划层：路径规划 + 轨迹规划 → 决定“怎么做”
- 控制层：PID控制、模型预测控制 → 执行“动起来”
轨迹规划正好卡在“规划”和“控制”之间。它把决策层的抽象指令，翻译成控制层能执行的“运动指令”。没有这层翻译，控制层根本不知道该怎么动。
我个人觉得，轨迹规划是具身智能里最“工程”的部分。它不像感知那样依赖深度学习，也不像控制那样依赖经典理论。它需要你同时理解几何、运动学、动力学，还要会写高效的代码。嗯，这也是为什么我喜欢做轨迹规划——既有理论深度，又有工程挑战。