第9章直线插补

一、直线插补：空间直线插补算法、步长控制、速度规划

各位同学，今天我们来聊聊运动控制里最基础、也最核心的一块——直线插补。

说实话，我入行那会儿，第一个被分配的任务就是写直线插补。当时觉得，不就是两点之间画条线吗？有什么难的。结果一上手才发现，坑多着呢。嗯，今天我就把这些年踩过的坑、总结的经验，一次性讲清楚。

先问大家一个问题：给你起点P0(x0,y0,z0)和终点P1(x1,y1,z1)，你怎么让机器人末端走一条直线？

最简单的思路——参数方程。

空间直线的参数方程长这样：

P(t) = P0 + t * (P1 - P0),  t ∈ [0, 1]

当t从0变到1，点就从起点走到了终点。这个t，就是我们的插补参数。

但这里有个细节：t的变化不一定是均匀的。你想想看，如果t均匀变化，那每个插补周期走的距离一样吗？不一定。因为直线在三维空间里，每个轴的分量可能不一样。

我个人习惯的做法是：先算总长度L：

L = sqrt((x1-x0)² + (y1-y0)² + (z1-z0)²)

然后每个插补周期，根据当前速度算出一个步长Δs，再反推t的变化量：

Δt = Δs / L

这样就能保证每个周期走的空间距离是均匀的。

核心要点：空间直线插补的本质，就是把一维的路径参数t，映射到三维空间坐标上。t的增量由步长决定，步长由速度决定。

步长控制，说白了就是「每个插补周期走多远」。

步长太小，机器人走得慢，效率低；步长太大，轨迹不平滑，甚至可能抖动。我在项目中遇到过一台六轴机器人，步长设大了，末端走直线时手臂一直在抖，像得了帕金森一样。后来把步长减半，问题就解决了。

步长的计算公式很简单：

Δs = v * T

其中v是当前速度，T是插补周期。

但这里有个坑：步长不能超过某个上限。为什么？

你想想看，如果步长太大，一个周期内从A点跳到B点，中间那段路径就丢了。对于直线插补还好，但对于圆弧或样条曲线，丢掉的路径会导致严重误差。

我一般会加一个步长限幅：

if (Δs > Δs_max) {
    Δs = Δs_max;
}

这个Δs_max怎么定？通常取插补周期的1/10到1/5的路径长度。具体要看你的精度要求。

经验之谈：步长控制还有一个容易被忽略的点——最后一个周期的步长处理。如果剩余距离小于一个步长，不能直接跳过，要做一个「收尾」处理。我曾经因为没处理好这个，导致机器人每次走到终点都会「冲过头」。

速度规划，是直线插补里最讲究的部分。

简单粗暴的做法：全程匀速。但这样有两个问题：

所以我们需要一个速度曲线。最常用的是梯形速度曲线：

阶段1：加速段（t0 → t1）
阶段2：匀速段（t1 → t2）
阶段3：减速段（t2 → t3）

每个阶段的速度公式：

加速段：v(t) = v0 + a * t
匀速段：v(t) = v_max
减速段：v(t) = v_max - a * (t - t2)

这里a是加速度，v_max是最大速度。

但梯形曲线有个缺点：加速度突变。在加速段结束的瞬间，加速度从a突然变成0，会产生冲击。对于高精度应用，这个冲击不可接受。

所以更高级的做法是用S形曲线：

阶段1：加加速度段（加速度线性增加）
阶段2：匀加速段
阶段3：减加速度段（加速度线性减小）
阶段4：匀速段
阶段5：加减速度段
阶段6：匀减速段
阶段7：减减速度段

S形曲线有7个阶段，看着复杂，但效果确实好。我记得有一次给半导体设备做运动控制，客户要求末端抖动小于0.01mm，用梯形曲线死活达不到，换成S形曲线一次通过。

注意：速度规划不是越复杂越好。如果你的应用场景是点焊、搬运这种对轨迹精度要求不高的场合，梯形曲线完全够用。S形曲线虽然平滑，但计算量大，实时性要求高。选哪种，要看你的硬件算力和应用需求。

好了，我们把上面三个部分串起来，看看一个完整的直线插补流程长什么样：

这个流程看起来简单，但每个步骤都有细节。比如步骤3的速度规划，你要考虑是否到达了减速点；步骤4的步长限幅，要考虑最后一个周期的特殊处理。

总结一下：直线插补的核心就三件事——怎么走（算法）、走多快（步长）、怎么变速（速度规划）。这三件事做好了，直线插补就稳了。

下一章我们讲圆弧插补，那个比直线复杂不少，但原理是相通的。到时候我会分享一个我当年调试圆弧插补时遇到的「灵异事件」——明明算法没问题，机器人就是画不圆。嗯，到时候再细说。