第三章 轨迹规划入门：点到点运动、直线插补、圆弧插补、S型速度曲线
各位工程师朋友，今天我们来聊聊轨迹规划的基础。说实话，我刚入行那会儿，觉得轨迹规划不就是让电机从A点转到B点嘛，有什么难的？直到第一次调试六轴机器人，看着末端执行器在空间里划出一道诡异的弧线，差点撞上夹具，我才意识到——这玩意儿，真没那么简单。
轨迹规划，说白了就是解决两个问题：走什么路径，以及怎么走。路径是空间几何问题，速度是时间问题。两者一结合，才是完整的运动控制。
核心要点：轨迹规划 = 路径规划 + 速度规划。路径决定去哪，速度决定怎么去。
3.1 点到点运动：最基础的运动模式
点到点运动，也叫PTP运动。你想想看，工业机器人从抓取位移动到放置位，中间路径不重要，只要起点和终点准确就行。这就是典型的PTP。
我在项目中遇到过这样一个场景：一个码垛工作站，机器人只需要从传送带抓取箱子，放到托盘上。中间有没有障碍物？没有。那用PTP就足够了，简单高效。
PTP运动的特点：
- 各轴独立运动，互不干涉
- 每个轴以自己的最大速度、加速度运行
- 各轴同时到达终点（同步）
- 路径不可预测——这是最大的坑
注意：PTP运动的路径是空间中的一条曲线，你无法预知它具体怎么走。如果工作空间内有障碍物，千万别用PTP！我曾经见过一个新手，用PTP让机器人穿过一个狭窄的窗口，结果手臂直接撞上了窗框……嗯，那次维修费可不便宜。
3.2 直线插补：让运动沿着直线走
什么时候需要直线插补？比如焊接、涂胶、切割——这些工艺要求工具末端沿着直线运动，不能偏。
直线插补的原理其实不复杂：在起点和终点之间，等间距地插入一系列中间点。控制器计算每个插补点的位置，然后驱动各轴协同运动，让末端走出一条直线。
我习惯把直线插补分成三步：
- 计算总位移：ΔX = Xe - Xs，ΔY = Ye - Ys，ΔZ = Ze - Zs
- 确定插补步数：根据插补周期和运动速度，算出需要多少个中间点
- 逐点计算：第i个点的位置 = 起点 + (i/N) × 总位移
举个例子，假设起点(0,0)，终点(100,100)，插补步数N=100：
// 直线插补伪代码
for i = 0 to N:
t = i / N
x = 0 + t * 100
y = 0 + t * 100
// 输出位置指令到伺服驱动器
这里有个细节要注意：插补步数N不是随便选的。N太小，运动会一顿一顿的；N太大，控制器计算负担重。一般根据插补周期（比如1ms）和运动速度来算。
实战技巧：我一般把插补周期设为1ms，然后根据目标速度算出每毫秒的位移增量。比如速度100mm/s，那每毫秒就走0.1mm。这样计算简单，运动也平滑。
3.3 圆弧插补：曲线运动的基石
圆弧插补比直线复杂一些。为什么？因为圆弧需要三个点才能确定——起点、终点、中间点（或者圆心）。
圆弧插补的核心思路：
- 先根据三点计算出圆心和半径
- 然后计算起始角和终止角
- 最后在角度上等间距插补
我记得有一次调试一个弧焊工作站，客户要求焊缝是圆弧形的。我一开始用直线插补逼近圆弧，结果焊出来的焊缝有棱角，客户不满意。后来换成真正的圆弧插补，焊缝光滑多了。
圆弧插补的代码逻辑大致如下：
// 圆弧插补 - 已知起点P1、中间点P2、终点P3
// 1. 计算圆心和半径
圆心 = 计算三点外接圆圆心(P1, P2, P3)
半径 = 距离(圆心, P1)
// 2. 计算起始角和终止角
起始角 = atan2(P1.y - 圆心.y, P1.x - 圆心.x)
终止角 = atan2(P3.y - 圆心.y, P3.x - 圆心.x)
// 3. 判断圆弧方向（顺时针/逆时针）
方向 = 判断方向(P1, P2, P3)
// 4. 角度插补
for i = 0 to N:
θ = 起始角 + 方向 * (i/N) * (终止角 - 起始角)
x = 圆心.x + 半径 * cos(θ)
y = 圆心.y + 半径 * sin(θ)
避坑指南：我曾经遇到过一个问题——三点共线时，圆弧插补会崩溃。因为共线的三点无法确定一个唯一的圆。所以代码里一定要加判断：如果三点共线，就退化成直线插补。
3.4 S型速度曲线：让运动更丝滑
梯形速度曲线大家应该都熟悉：加速-匀速-减速。简单粗暴，但有个问题——加速度在切换点会突变，导致冲击和振动。
S型速度曲线就是为了解决这个问题。它把加速度的变化也规划成连续的曲线，让加加速度（Jerk）可控。
说白了，S型曲线就是：
- 加加速度阶段：加速度从0逐渐增加到最大值
- 匀加速阶段：以最大加速度加速
- 减加速度阶段：加速度从最大值逐渐减到0
- 匀速阶段：速度保持不变
- 加减速阶段：加速度从0逐渐减到负最大值
- 匀减速阶段：以最大减速度减速
- 减减速阶段：加速度从负最大值逐渐回到0
一共7段，所以也叫七段式S型曲线。
我个人的习惯是，在精密定位场合（比如贴片机、晶圆搬运）一定用S型曲线。虽然计算量比梯形大，但运动结束时的残余振动明显减少。
S型曲线的关键参数：
| 参数 | 符号 | 说明 |
|---|---|---|
| 最大速度 | Vmax | 运动允许的最高速度 |
| 最大加速度 | Amax | 加速度的峰值 |
| 最大加加速度 | Jmax | 加速度的变化率，决定曲线的平滑程度 |
| 总位移 | S | 起点到终点的距离 |
这里有个经验值：Jmax一般取Amax的10~20倍。太小了，加减速时间太长；太大了，又失去了S型曲线的意义。
核心公式：S型曲线的速度表达式是分段函数，每段都是时间的二次或三次函数。实际工程中，我们一般用查表法或者预计算法，不会在实时中断里现场算——太慢了。
3.5 三种轨迹的对比与选型
讲了这么多，到底什么时候用哪种？我整理了一个对比表：
| 轨迹类型 | 路径可控性 | 计算复杂度 | 典型应用 |
|---|---|---|---|
| 点到点(PTP) | 低 | 低 | 码垛、搬运、上下料 |
| 直线插补 | 高 | 中 | 焊接、涂胶、切割 |
| 圆弧插补 | 高 | 高 | 弧焊、打磨、轮廓加工 |
选型建议：
- 如果空间开阔、没有障碍物 → PTP，简单高效
- 如果需要精确走直线 → 直线插补
- 如果需要走圆弧或曲线 → 圆弧插补
- 如果对运动平滑度要求高 → 配合S型速度曲线
我的经验：实际项目中，往往是多种轨迹组合使用。比如机器人先PTP快速移动到工件附近，然后直线插补慢速接近，最后圆弧插补完成焊接。别死板地只用一种。
3.6 本章知识体系
下面这张图，是我梳理的本章知识结构。你可以把它当作一个快速索引：
这张图把本章的核心内容串起来了。左边是路径规划——选什么路走；右边是速度规划——怎么走才稳。两者结合，就是完整的轨迹规划。
好了，轨迹规划的基础就讲到这里。这些内容看着简单，但真正用好，需要在项目里反复打磨。下次调试的时候，不妨多想想：我选的轨迹类型合适吗？速度曲线够平滑吗？