移植解析安川伺服代码—S形轨迹规划工程实现

麻烦不断的杰哥

17人浏览 · 2026-08-24 16:50:50

麻烦不断的杰哥 · 2026-08-24 16:50:50 发布

在做电机控制算法（特别是多轴联动的机械臂、人形机器人关节）时，我们经常会遇到一个痛点：下发的指令如果不够平滑，底层的 FOC 速度环和电流环就会疯狂震荡，导致电机发出刺耳的尖叫，甚至把减速器打坏。

为了解决这个问题，工业界普遍采用 S形轨迹规划（S-Curve Trajectory Planning）。它通过引入对加加速度（Jerk）的限制，让加速度也保持连续不突变，从而生成极其丝滑的运动指令。

但在学术界，S 曲线通常被描述为极其复杂的解析几何问题，包含大量的高阶多项式、开根号甚至一元三次方程。如果你敢把这些复杂的数学库函数直接塞进单片机每秒执行一万次的控制中断（ISR）里，控制周期立刻就会被打乱，系统直接死机。

数学公式是完美的，但 MCU 的算力是残缺的。

今天，我们来移植解析下安川伺服的 S 形轨迹规划的代码。看看真正的工程实现是如何通过将复杂的微积分问题降维，变成极致高效的底层代码的。

下面这张图，就是整个 S 形轨迹规划在单片机里的完整工作流程：

// 轨迹规划器依赖的上下文结构体 (精简版)
typedef struct {
    float position_cmd_rate_limit;   // 限制：最大速度 (V_max)
    float speed_cmd_rate_limit;      // 限制：最大加速度 (A_max)
    float position_cmd_jerk_limit;   // 限制：最大加加速度 (Jerk_max)
    // ... 其他状态变量
    float position_cmd_shaped;       // 规划出的目标位置
    float position_trajectory_segment_time[7]; // 7 段 S 曲线的时间表
} ControllerStruct;

接下来是核心的代码，分为单侧时间计算和总规划：

#include "math.h"

// 【重点 1：面积投射】计算单侧（加速或减速过程）的时间分配
static void TrajectoryPlanner_GetHalfProfile(float velocity, float acceleration,
                                             float jerk, float *jerk_time,
                                             float *constant_accel_time, float *total_time) {
    // 灵魂拷问：这个 v >= A^2/J 是怎么来的？
    if(velocity >= acceleration * acceleration / jerk) {
        *jerk_time = acceleration / jerk;
        *constant_accel_time = velocity / acceleration - *jerk_time;
    } else {
        *jerk_time = sqrtf(velocity / jerk);
        *constant_accel_time = 0.0f;
    }
    *total_time = 2.0f * *jerk_time + *constant_accel_time;
}

// 核心规划函数：生成时间表
static void TrajectoryPlanner_PlanSCurve(ControllerStruct *controller, float target) {
    float distance = fabsf(target - controller->position_cmd_shaped);
    float max_velocity = fabsf(controller->position_cmd_rate_limit);
    float accel_limit = fabsf(controller->speed_cmd_rate_limit);
    float jerk_limit = fabsf(controller->position_cmd_jerk_limit);
    
    // ... [省略变量声明] ...

    // 先假设能跑到最大速度，试算一下
    TrajectoryPlanner_GetHalfProfile(max_velocity, accel_limit, jerk_limit,
                                     &accel_jerk_time, &accel_hold_time, &accel_total_time);
    TrajectoryPlanner_GetHalfProfile(max_velocity, decel_limit, jerk_limit, ...);
    
    // 如果全速跑，需要的最小距离
    float minimum_distance = 0.5f * max_velocity * (accel_total_time + decel_total_time);

    // 【重点 2：二分法逼近】如果目标距离太短，达不到最大速度怎么办？
    if(minimum_distance > distance) {
        float low_velocity = 0.0f;
        float high_velocity = max_velocity;
        
        // 死磕 16 次迭代！绝对不准用 while！
        for(uint8_t iteration = 0U; iteration < 16U; iteration++) {
            float peak_velocity = 0.5f * (low_velocity + high_velocity);
            
            TrajectoryPlanner_GetHalfProfile(peak_velocity, accel_limit, jerk_limit, ...);
            TrajectoryPlanner_GetHalfProfile(peak_velocity, decel_limit, jerk_limit, ...);
            
            minimum_distance = 0.5f * peak_velocity * (accel_total_time + decel_total_time);
            
            if(minimum_distance < distance) {
                low_velocity = peak_velocity;
            } else {
                high_velocity = peak_velocity;
            }
        }
    } 
    // ... [后续赋值逻辑省略]
}

代码看完了，接下来，咱们挑出最值钱的两个地方，掰开揉碎了讲。

一、用一个数组，干掉几十行 switch-case

一听到“7 段式 S 曲线”，第一反应肯定是建一个枚举，然后写一个几十行的巨大 switch-case：

switch (segment) {
    case STAGE_ACCEL_UP:   current_jerk = jerk_limit;  break;
    case STAGE_ACCEL_HOLD: current_jerk = 0.0f;        break;
    case STAGE_ACCEL_DOWN: current_jerk = -jerk_limit; break;
    // ...
}

这种代码不仅臃肿，而且过多的逻辑分支会破坏 CPU 的指令流水线。安川是怎么处理这种 7 段状态机的？他直接在脑子里把这 7 个阶段的物理本质抽离出来——不管你是哪一段，唯一改变的只有加加速度（Jerk）的方向极性！

graph TD
    A[段1: 加加速区 J>0] --> B[段2: 匀加速区 J=0]
    B --> C[段3: 减加速区 J<0]
    C --> D[段4: 匀速运行 J=0]
    D --> E[段5: 加减速区 J<0]
    E --> F[段6: 匀减速区 J=0]
    F --> G[段7: 减减速区 J>0]

在执行代码时，定义一个静态数组：

// 周期性更新函数：在每个控制周期（如1ms）执行
static void TrajectoryPlanner_UpdateSCurve(ControllerStruct *controller, float target, float control_period) {
    // 定义7个阶段的极性数组，替代所有 switch-case
    static const float jerk_sign[7] = {1.0f, 0.0f, -1.0f, 0.0f, -1.0f, 0.0f, 1.0f};
    
    float direction;
    float remaining_time;

    // ... [状态检查与初始化省略] ...

    direction = (controller->position_trajectory_target >= controller->position_cmd_shaped) ? 1.0f : -1.0f;
    remaining_time = control_period;

    // 处理跨段逻辑
    while((remaining_time > 1.0e-9f) && (controller->position_trajectory_segment < 7U)) {
        uint8_t segment = controller->position_trajectory_segment;
        float segment_remaining = controller->position_trajectory_segment_time[segment] - controller->position_trajectory_segment_elapsed;
        float step_time;
        float jerk;

        if(segment_remaining <= 1.0e-9f) {
            controller->position_trajectory_segment++;
            controller->position_trajectory_segment_elapsed = 0.0f;
            continue;
        }

        step_time = fminf(remaining_time, segment_remaining);
        
        // 直接用段号 segment 做索引查表，获取当前 Jerk
        jerk = direction * controller->position_cmd_jerk_limit * jerk_sign[segment];
        
        // 无脑扔进积分流水线！没有哪怕一个 if-else 干扰！
        controller->position_cmd_shaped += 
            controller->position_cmd_velocity * step_time + 
            0.5f * controller->position_cmd_acceleration * step_time * step_time + 
            jerk * step_time * step_time * step_time / 6.0f;
            
        controller->position_cmd_velocity += 
            controller->position_cmd_acceleration * step_time + 
            0.5f * jerk * step_time * step_time;
            
        controller->position_cmd_acceleration += jerk * step_time;
        
        controller->position_trajectory_segment_elapsed += step_time;
        remaining_time -= step_time;

        if(controller->position_trajectory_segment_elapsed + 1.0e-8f >= controller->position_trajectory_segment_time[segment]) {
            controller->position_trajectory_segment++;
            controller->position_trajectory_segment_elapsed = 0.0f;
        }
    }
}

二、用“面积投射”搞定极值判定

GetHalfProfile 里的第一句话：if (velocity >= acceleration * acceleration / jerk)。很多人第一次看这个公式一脸懵，查遍了论文也不知道这个 $\frac{A^2}{J}$ 是从哪冒出来的。其实啊，不要死盯着速度曲线看，咱们要把目光往下降一维，去盯着加速度曲线（A-t 图）看。。在加速阶段，为了让加速度不突变，加速度只能以斜率 $J$ （加加速度）慢慢爬升。如果你想把加速度推到最大值 $A$ ，你需要的时间就是 $\frac{A}{J}$ 。同理，从 $A$ 回落到 $0$ ，也需要 $\frac{A}{J}$ 。我们在物理上学过，加速度曲线包围的面积，就是速度！这个爬升又回落的过程，在 A-t 图上是个什么形状？一个等腰三角形！三角形的高是：最大加速度 $A$ 三角形的底是：爬升时间 + 回落时间 = $2AJ\frac{2A}{J}$ 。来，我们算一下这个三角形的面积（也就是这脚油门踩到底再松开，电机获得的速度）： $\frac{1}{2} \times 底 \times 高 = \frac{1}{2} \times \frac{2A}{J} \times A = \frac{A^2}{J}$

这就是工程上的“面积投射”！不需要去解复杂的积分方程，只要在脑子里画个三角形就明白了：只要你给我的目标速度 $V$ 大于这个三角形的面积 $A2J\frac{A^2}{J}$ ，就说明这脚油门能踩到底，还能在最大加速度上保持一会儿（呈现梯形）。如果给我的目标速度比这个面积还小？不好意思，你油门还没踩到底就得松开，加速度曲线只能是个砍掉尖顶的小三角形！一个小小的 if，背后是对物理极值极其透彻的降维打击。

把这段逻辑画成图，就是下面这个样子：

三、用“二分法”避开要命的开根号

大家再看代码里的短距离处理。当目标距离极短（微小挪动）时，电机根本没空间加速到你设置的 max_velocity。这时候，实际能达到的最高速度 peak_velocity 是多少呢？如果是搞学术的，肯定马上拿起笔列方程：纯限制状态下，位移 $\cdot v_{peak} \cdot \sqrt{\frac{v_{peak}}{J}}$ ，推导一下，解出 $vpeak=(S2J4)13v_{peak} = \left( \frac{S^2 J}{4} \right)^{\frac{1}{3}}$ 。如果是加减速不对称，还要解一元三次方程！你要是敢把开三次方（pow(x, 1.0/3.0)）或者解一元三次方程的代码写进每秒执行一万次的中断函数里，电机的控制周期立刻就会被打乱，系统直接死机。工业界的规矩是：能用加减乘除解决的，绝不碰开方。安川是怎么做的？既然正着算距离容易（三角形面积推导），反推速度难，那咱们就“猜”！

for(iteration = 0U; iteration < 16U; iteration++) {
    // 1. 先取个中间值猜一猜
    peak_velocity = 0.5f * (low_velocity + high_velocity);
    
    // 2. 用猜的速度，正向推算所需时间...
    TrajectoryPlanner_GetHalfProfile(peak_velocity, accel_limit, jerk_limit, ...);
    TrajectoryPlanner_GetHalfProfile(peak_velocity, decel_limit, jerk_limit, ...);
    
    // 3. 算出：如果跑到这个速度，起步加刹车一共需要多少距离？
    minimum_distance = 0.5f * peak_velocity * (accel_total_time + decel_total_time);
    
    // 4. 核心逻辑：对准目标靶心，收缩边界
    if(minimum_distance < distance) {
        low_velocity = peak_velocity;
    } else {
        high_velocity = peak_velocity;
    }
}

重点来看看这段 if-else。很多人觉得这不就是个“猜数字”游戏吗？没错，但你要理解它背后的物理意义。

我们打个开车的比方。
假设前面 50 米处有个红绿灯（用户下发的实际位移 distance = 50）。你现在要在不超载的情况下，尽可能快地开过去并精准停下。

你先在脑子里猜一个最高车速，比如 60km/h（peak_velocity）。
然后你按物理公式一算，一脚油门踩到 60km/h，然后再一脚刹车踩死停下来，这一套动作最少需要 80 米（试算出的 minimum_distance = 80）。

这时候，判断逻辑就来了：

整个二分逼近的流程，可以用下面这张图来概括：

分支一：如果算出来的距离比实际长

对应代码的 else 分支（即 minimum_distance >= distance）。

物理情景：刹车距离需要 80 米，但路况只有 50 米。如果你真跑到 60 km/h，绝对会闯红灯（过冲，Overshoot）。

如何修正：这说明你猜的速度太快了！实际的极限速度一定比 60 km/h 小。

代码动作：high_velocity = peak_velocity;（把当前猜的速度设为新的“天花板”，下次猜的时候必须比这个慢）。

分支二：如果算出来的距离比实际短

对应代码的 if (minimum_distance < distance) 分支。

物理情景：假设你第二次猜了 20 km/h。你一算，加减速到 20 km/h 只需要 15 米（minimum_distance = 15）。但实际路况有 50 米。这意味着什么？意味着你太保守了！跑完 15 米后，你还有足足 35 米的距离可以在匀速下“滑行”。

如何修正：既然我们追求极限，且空间还有富裕，说明实际的极限速度一定比 20 km/h 大。我们可以再开快点！

代码动作：low_velocity = peak_velocity;（把当前猜的速度设为新的“地板”，下次猜的时候必须比这个快）。

为什么这个“土办法”万无一失？

在数学上，这叫单调递增性。速度越大，需要的加减速距离必然越长，这个关系是绝对单调的。正是因为物理世界严格遵守这个单调性，二分法才永远不会找错方向。

为什么死守 16 次 for 循环？

喜欢写 while(abs(误差) > 0.001)。在实时控制里，这是大忌！中断服务里的执行时间必须绝对恒定。不论距离多短，跑满 16 次迭代，CPU 消耗的周期永远一样。
16 次精度够吗？

二分法 16 次，区间缩小到 $1216\frac{1}{2^{16}}$ （约 0.0015%）。在微小位移下，这已经逼近单精度浮点数的极限了。对于齿轮、皮带的机械间隙来说，这个精度早就溢出了。
避开了所有的非线性运算。

这 16 次循环里，只有基础的加法、乘法和一次除法。单片机跑起来连汗都不会出，这就是工程的极致妥协与优雅。

三、跑跑仿真看看效果

说得再好听，不如拉出来遛遛。我给大伙准备了一份高度还原这套 C 代码逻辑的 MATLAB 脚本。你们把它粘进去跑一跑，看看工业级的微元法是怎么切出 S 形曲线的。

% 安川 S-Curve 轨迹规划 MATLAB 仿真
clc; clear; close all;

% --- 用户参数配置 (老底子参数) ---
target_pos = 100;     % 目标位置
dt = 0.001;           % 控制周期 (1ms)
v_max = 20;           % 最大速度
a_max = 10;           % 最大加速度
d_max = 10;           % 最大减速度
j_max = 50;           % 加加速度限制 (Jerk)

% --- 仿真变量初始化 ---
p = 0; v = 0; a = 0;
pos_record = []; vel_record = []; acc_record = []; jerk_record = []; time_record = [];

% --- 模拟 Plan 阶段：算算七段时间表 ---
t_acc_jerk = a_max / j_max;
t_acc_hold = (v_max / a_max) - t_acc_jerk;
t_dec_jerk = d_max / j_max;
t_dec_hold = (v_max / d_max) - t_dec_jerk;
acc_dist = 0.5 * v_max * (2*t_acc_jerk + t_acc_hold);
dec_dist = 0.5 * v_max * (2*t_dec_jerk + t_dec_hold);
t_cruise = (target_pos - acc_dist - dec_dist) / v_max;

T_seg = [t_acc_jerk, t_acc_hold, t_acc_jerk, t_cruise, t_dec_jerk, t_dec_hold, t_dec_jerk];
J_sign = [1, 0, -1, 0, -1, 0, 1];

% --- 模拟 Update 阶段：微元积分（按表操课） ---
current_time = 0;
for i = 1:7
    seg_time = T_seg(i);
    j_current = j_max * J_sign(i);
    
    t_elapsed = 0;
    while t_elapsed < seg_time
        step_time = min(dt, seg_time - t_elapsed);
        
        % 泰勒微元积分：不碰绝对时间多项式，只做短步长累加
        p = p + v*step_time + 0.5*a*step_time^2 + (j_current * step_time^3)/6;
        v = v + a*step_time + 0.5*j_current*step_time^2;
        a = a + j_current*step_time;
        
        t_elapsed = t_elapsed + step_time;
        current_time = current_time + step_time;
        
        pos_record(end+1) = p;
        vel_record(end+1) = v;
        acc_record(end+1) = a;
        jerk_record(end+1) = j_current;
        time_record(end+1) = current_time;
    end
end

% --- 绘图 ---
figure('Name', 'S 曲线', 'Color', 'w', 'Position', [100, 100, 800, 800]);
subplot(4,1,1); plot(time_record, pos_record, 'b', 'LineWidth', 2); title('位置 (Position)'); grid on;
subplot(4,1,2); plot(time_record, vel_record, 'r', 'LineWidth', 2); title('速度 (Velocity) - 完美的 S 形'); grid on;
subplot(4,1,3); plot(time_record, acc_record, 'k', 'LineWidth', 2); title('加速度 (Acceleration) - 梯形且连续无跳变'); grid on;
subplot(4,1,4); plot(time_record, jerk_record, 'm', 'LineWidth', 2); title('Jerk (加加速度) - 纯逻辑切换'); grid on; xlabel('时间 (s)');