第二章 动作规划原理：轨迹规划基础、直线插补与圆弧插补、速度规划与加速度控制、奇异点规避策略
2.1 轨迹规划基础——让机器人“走对路”
各位工程师朋友，咱们直接切入正题。动作规划，说白了就是告诉机器人“你该怎么动”。
我个人习惯把轨迹规划分成两个层面：路径规划和轨迹规划。路径规划只关心“走哪条路”，不关心“走多快”。而轨迹规划，则是在路径上加上时间信息——什么时刻到哪个点，速度是多少。
举个例子。你让机器人从A点抓个零件放到B点。路径规划告诉你：先往上抬50mm，再平移300mm，最后下降50mm。轨迹规划则告诉你：前0.5秒加速到200mm/s，中间1秒匀速，最后0.5秒减速到0。
核心概念：轨迹 = 路径 + 时间律
路径是几何描述，轨迹是运动描述。两者缺一不可。
我在项目中遇到过不少新手，把路径规划和轨迹规划混为一谈。结果呢？路径画得漂漂亮亮，一跑起来要么抖动，要么过冲。嗯，这里要注意：轨迹规划的核心是“平滑”——位置连续、速度连续、加速度连续，最好加加速度也连续。
2.2 直线插补与圆弧插补——最常用的两种“走法”
2.2.1 直线插补
直线插补，说白了就是让机器人末端走直线。听起来简单？实现起来有讲究。
假设起点P0(x0,y0,z0)，终点P1(x1,y1,z1)。我们要在两点之间插入N个中间点。最简单的做法是线性插值：
// 直线插补示例（伪代码）
for i = 0 to N:
t = i / N
P.x = P0.x + t * (P1.x - P0.x)
P.y = P0.y + t * (P1.y - P0.y)
P.z = P0.z + t * (P1.z - P0.z)
// 发送位置指令
MoveTo(P)
但这里有个坑。如果你直接用等时间间隔插值，机器人末端速度会不均匀——因为关节空间和笛卡尔空间不是线性映射。我曾经调试一个六轴机器人，直线插补时末端速度忽快忽慢，查了半天才发现是没做速度补偿。
我的经验：直线插补时，建议先规划好末端速度曲线，再反算每个插补点的位置。这样能保证末端匀速运动。
2.2.2 圆弧插补
圆弧插补比直线复杂一些。三点确定一个圆弧——起点、中间点、终点。或者用圆心+半径+起始角+终止角。
我常用的方法是：
- 根据三点计算圆心和半径
- 计算起始角和终止角
- 按角度增量插值
- 将角度转换为笛卡尔坐标
// 圆弧插补示例（三点法）
// 输入：P1(起点), P2(中间点), P3(终点)
// 输出：圆弧上的插补点
// 1. 计算圆心
center = ComputeCircleCenter(P1, P2, P3)
radius = Distance(center, P1)
// 2. 计算角度
startAngle = Atan2(P1.y - center.y, P1.x - center.x)
endAngle = Atan2(P3.y - center.y, P3.x - center.x)
// 3. 插值角度
for i = 0 to N:
theta = startAngle + i/N * (endAngle - startAngle)
P.x = center.x + radius * Cos(theta)
P.y = center.y + radius * Sin(theta)
P.z = center.z + (P3.z - P1.z) * i/N // 如果不在同一平面
你想想看，如果圆弧不在XY平面内，还要做旋转变换。嗯，这里我建议直接用四元数或旋转矩阵来处理，避免欧拉角带来的万向锁问题。
2.3 速度规划与加速度控制——别让机器人“急刹车”
速度规划，说白了就是决定“什么时候加速、什么时候减速”。
最常见的速度曲线是梯形速度曲线：匀加速→匀速→匀减速。但梯形曲线有个问题——加速度突变，会导致机器人抖动。
我更喜欢用S形速度曲线。它的加速度是连续的，启动和停止都很平滑。
| 速度曲线类型 | 加速度连续性 | 适用场景 | 我的评价 |
|---|---|---|---|
| 梯形曲线 | 不连续（有突变） | 低速、精度要求不高的场合 | 简单但粗暴 |
| S形曲线 | 连续 | 高速、高精度场合 | 推荐使用 |
| 多项式曲线 | 高阶连续 | 超精密加工 | 计算量大，但效果最好 |
避坑指南：我曾经调试一个码垛机器人，用了梯形速度曲线，结果每次启停时工件都晃动。后来换成S形曲线，问题解决了。记住：加速度突变是抖动的根源。
加速度控制还有个重要参数——加加速度（Jerk）。它是加速度的变化率。控制好加加速度，能让运动更平滑。我一般把加加速度限制在最大加速度的10倍以内。
2.4 奇异点规避策略——机器人的“死穴”
奇异点，是机器人运动学中的“数学奇点”。在奇异点附近，关节速度会趋于无穷大，机器人会失控。
常见的奇异点有三种：
- 腕部奇异点：关节4和关节6的轴线共线
- 肩部奇异点：关节1和关节2的轴线共线
- 肘部奇异点：关节3达到极限位置
为什么会这样？说白了就是雅可比矩阵的行列式为零，逆运动学无解。
我处理奇异点的策略有几种：
- 路径规划阶段规避：在规划路径时，避开奇异点区域。这需要提前计算工作空间中的奇异区域。
- 运行时检测：实时计算雅可比矩阵的条件数，当接近奇异点时发出警告。
- 奇异点附近降速：在奇异点附近降低速度，避免关节速度超限。
- 奇异点穿越：如果必须经过奇异点，采用“奇异点穿越”算法——在奇异点附近切换为关节空间运动。
我的经验：奇异点规避，最好的办法是“防患于未然”。在离线编程阶段就检查路径是否经过奇异区域。如果实在避不开，我建议在奇异点附近用“阻尼最小二乘法”来求解逆运动学，虽然会牺牲一点精度，但至少不会失控。
知识体系总览
下面这张图，是我自己总结的动作规划知识体系。你可以把它当作一个“地图”，随时回来查阅。
这张图把动作规划的四个核心模块串起来了。从轨迹规划基础出发，到具体的插补方法，再到速度规划，最后是奇异点规避。每个模块都不是孤立的——比如你选择了S形速度曲线，那插补点的密度就要相应调整，否则平滑效果出不来。
最后说一句：动作规划这东西，理论是一回事，实际调试又是另一回事。我建议你在仿真环境里多跑几遍，把速度、加速度、加加速度这些参数调顺了，再上真机。不然，真机一抖，心就慌了。
公众号:蓝海资料掘金营，微信deep3321