4、运动控制核心算法模块：轨迹规划（梯形、S形）、PID控制算法、前馈控制、插补算法

各位工程师朋友，咱们今天聊聊运动控制里最核心的几个算法模块。说实话，搞了这么多年运动控制，我越来越觉得这些算法就像厨师的刀工——基本功扎实了，什么菜都能做。梯形、S形轨迹规划，PID控制，前馈补偿，还有插补算法，这四样东西你吃透了，大部分运动控制项目都能拿下。

4.1 轨迹规划：让运动“有章可循”

轨迹规划说白了，就是告诉电机“你怎么走”。不是简单地从A到B，而是要考虑速度、加速度、甚至加加速度。我见过太多新手直接给个阶跃信号，结果机器抖得像筛子一样。

4.1.1 梯形速度规划

梯形规划是最基础的。它分三段：加速、匀速、减速。形状像个梯形，所以叫这名。

嗯，这里要注意：梯形规划的加速度是突变的。从0直接跳到a，再从a直接跳到0。这会导致什么？冲击！

梯形规划的代码实现其实很简单：

// 梯形速度规划核心逻辑
float calcTrapezoidalVelocity(float t, float v_max, float a, float total_dist) {
    float t_acc = v_max / a;          // 加速时间
    float t_const = total_dist / v_max - t_acc;  // 匀速时间
    
    if (t < t_acc) {
        return a * t;                 // 加速段
    } else if (t < t_acc + t_const) {
        return v_max;                 // 匀速段
    } else {
        return v_max - a * (t - t_acc - t_const); // 减速段
    }
}

4.1.2 S形速度规划

S形规划就聪明多了。它把加速度也做了平滑处理，分成加加速、减加速、匀速、加减速、减减速五段或七段。你想想看，加速度连续变化，冲击自然就小了。

我个人习惯用七段式S形规划，虽然计算量大了点，但效果确实好。核心思想是让加加速度（Jerk）保持恒定。

// S形规划中的加加速度计算
float calcJerkLimitedVelocity(float t, float v_max, float a_max, float jerk) {
    // 七段式S形规划
    // 段1: 加加速 (t0~t1)
    // 段2: 匀加速 (t1~t2)  
    // 段3: 减加速 (t2~t3)
    // 段4: 匀速   (t3~t4)
    // 段5: 加减速 (t4~t5)
    // 段6: 匀减速 (t5~t6)
    // 段7: 减减速 (t6~t7)
    
    float t_j = a_max / jerk;  // 加加速时间
    // ... 完整实现需要分段判断
}

4.2 PID控制算法：运动控制的“定海神针”

PID控制，搞控制的没人不知道。但说实话，会用和用得好是两码事。PID的核心就三个字：比、积、微。

参数	作用	调大后的影响
Kp（比例）	快速响应误差	响应变快，但可能超调
Ki（积分）	消除稳态误差	消除静差，但可能振荡
Kd（微分）	预测误差趋势	抑制超调，但放大噪声

为什么说PID是定海神针？因为它简单、可靠、可解释。我在一个半导体设备项目里，现场调试时发现电机有低频抖动，排查了半天，最后发现是积分项饱和了。加了个积分限幅，问题就解决了。

// 带积分限幅的增量式PID
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
    float integral_limit;  // 积分限幅
} PIDController;

float PID_Update(PIDController *pid, float setpoint, float feedback) {
    float error = setpoint - feedback;
    
    // 积分项带限幅
    pid->integral += error;
    if (pid->integral > pid->integral_limit) 
        pid->integral = pid->integral_limit;
    if (pid->integral < -pid->integral_limit) 
        pid->integral = -pid->integral_limit;
    
    float output = pid->Kp * error 
                 + pid->Ki * pid->integral 
                 + pid->Kd * (error - pid->prev_error);
    
    pid->prev_error = error;
    return output;
}

4.3 前馈控制：让系统“未卜先知”

PID是“事后诸葛亮”——等误差出来了才去纠正。前馈控制就不一样了，它提前知道你要干什么，直接给一个预判信号。

你想想看，如果系统有较大的惯性或滞后，光靠PID肯定不够。这时候前馈就派上用场了。

举个例子：一个伺服系统，你希望它按S形曲线运动。PID会等位置误差出现再去调整，而前馈直接根据S形曲线的速度和加速度，提前给出对应的电流指令。

// 速度前馈实现
float velocityFeedforward(float target_velocity, float Kvff) {
    return Kvff * target_velocity;  // Kvff是速度前馈系数
}

// 加速度前馈（用于补偿惯性力）
float accelerationFeedforward(float target_acceleration, float Kaff) {
    return Kaff * target_acceleration;  // Kaff是加速度前馈系数
}

4.4 插补算法：让运动“连点成线”

插补，就是把离散的路径点变成连续的运动轨迹。常见的插补方式有直线插补、圆弧插补，还有更复杂的样条插补。

我记得刚入行时，总觉得插补就是简单的线性插值。直到做五轴联动项目，才发现事情没那么简单。

4.4.1 直线插补

直线插补是最基础的。给定起点和终点，中间的点按比例分配。

// 直线插补
typedef struct {
    float x_start, y_start;
    float x_end, y_end;
    float total_length;
} LineInterpolator;

void LineInterp_GetPoint(LineInterpolator *line, float t, float *x, float *y) {
    // t 范围 0~1
    *x = line->x_start + t * (line->x_end - line->x_start);
    *y = line->y_start + t * (line->y_end - line->y_start);
}

4.4.2 圆弧插补

圆弧插补稍微复杂些。需要知道圆心、半径、起始角和终止角。

4.4.3 插补与轨迹规划的配合

这里有个关键点：插补和轨迹规划是配合工作的。轨迹规划决定“速度怎么变”，插补决定“路径怎么走”。两者要协同。

我习惯的做法是：先做轨迹规划，算出每个时刻的目标速度；然后根据这个速度，在插补路径上找到对应的位置点。

// 插补与轨迹规划配合
void MotionPlan_GetCommand(float t, float *pos, float *vel) {
    // 1. 轨迹规划：计算当前时刻的目标速度
    float target_vel = TrajectoryPlan_GetVelocity(t);
    
    // 2. 根据速度计算路径位置
    float path_length = IntegrateVelocity(target_vel, t);
    
    // 3. 插补：根据路径长度获取位置
    Interpolator_GetPosition(path_length, pos);
    
    // 4. 输出速度指令给前馈
    *vel = target_vel;
}

4.5 知识体系总览

说了这么多，我画张图帮你理清思路。这四大模块的关系是这样的：

从这张图你能看出来，轨迹规划和插补负责“生成期望”，PID和前馈负责“跟踪期望”。四者缺一不可。

---

公众号:蓝海资料掘金营，微信deep3321