📚 课程目录第4章梯形速度规划

4. 梯形速度规划

任务目标

本案例实现STM32环境下的梯形速度规划算法：给定总位移、最大速度、加速度与减速度，计算加速段、匀速段、减速段的位移与时间参数，并输出各阶段边界速度。该算法是点位运动控制的基础，可直接用于步进电机或伺服电机的速度曲线生成。

核心算法

梯形速度规划将运动过程分为三个阶段：加速段（从0加速至v_max）、匀速段（以v_max恒速运行）、减速段（从v_max减速至0）。若总位移不足以达到最大速度，则退化为三角形曲线（仅加速+减速）。

输入参数：pos_total（总位移，单位：脉冲或mm）、v_max（最大速度）、a_acc（加速度）、a_dec（减速度）。输出：t_acc、t_dec、t_const、pos_acc、pos_dec、pos_const，以及实际峰值速度v_actual。

完整实现

// trapezoid_planner.h
#ifndef TRAPEZOID_PLANNER_H
#define TRAPEZOID_PLANNER_H

typedef struct {
    float v_max;        // 最大速度限制
    float a_acc;        // 加速度
    float a_dec;        // 减速度
    float pos_total;    // 总位移
    // 输出参数
    float v_actual;     // 实际峰值速度
    float t_acc;        // 加速段时间
    float t_const;      // 匀速段时间
    float t_dec;        // 减速段时间
    float pos_acc;      // 加速段位移
    float pos_const;    // 匀速段位移
    float pos_dec;      // 减速段位移
    float t_total;      // 总时间
} TrapezoidProfile;

// 梯形速度规划计算函数
void Trapezoid_Plan(TrapezoidProfile *profile);

#endif

// trapezoid_planner.c
#include "trapezoid_planner.h"
#include <math.h>

void Trapezoid_Plan(TrapezoidProfile *p) {
    // 理论加减速段位移（若能达到v_max）
    float pos_acc_theory = p->v_max * p->v_max / (2.0f * p->a_acc);
    float pos_dec_theory = p->v_max * p->v_max / (2.0f * p->a_dec);

    // 判断能否达到最大速度
    if (pos_acc_theory + pos_dec_theory <= p->pos_total) {
        // 梯形：三段完整
        p->v_actual = p->v_max;
        p->pos_acc = pos_acc_theory;
        p->pos_dec = pos_dec_theory;
        p->pos_const = p->pos_total - p->pos_acc - p->pos_dec;
        p->t_acc = p->v_max / p->a_acc;
        p->t_dec = p->v_max / p->a_dec;
        p->t_const = p->pos_const / p->v_max;
    } else {
        // 三角形：无法达到v_max，需联立求解实际峰值速度
        // 位移方程: v^2/(2a_acc) + v^2/(2a_dec) = pos_total
        float denom = 1.0f / (2.0f * p->a_acc) + 1.0f / (2.0f * p->a_dec);
        p->v_actual = sqrtf(p->pos_total / denom);
        p->pos_acc = p->v_actual * p->v_actual / (2.0f * p->a_acc);
        p->pos_dec = p->v_actual * p->v_actual / (2.0f * p->a_dec);
        p->pos_const = 0.0f;
        p->t_acc = p->v_actual / p->a_acc;
        p->t_dec = p->v_actual / p->a_dec;
        p->t_const = 0.0f;
    }
    p->t_total = p->t_acc + p->t_const + p->t_dec;
}

// main.c 调用示例
#include <stdio.h>
#include "trapezoid_planner.h"

int main(void) {
    TrapezoidProfile profile = {
        .v_max = 100.0f,    // 最大速度 100 mm/s
        .a_acc = 200.0f,    // 加速度 200 mm/s^2
        .a_dec = 200.0f,    // 减速度 200 mm/s^2
        .pos_total = 500.0f // 总位移 500 mm
    };

    Trapezoid_Plan(&profile);

    printf("v_actual = %.2f mm/s\n", profile.v_actual);
    printf("t_acc = %.3f s, t_const = %.3f s, t_dec = %.3f s\n",
           profile.t_acc, profile.t_const, profile.t_dec);
    printf("pos_acc = %.2f mm, pos_const = %.2f mm, pos_dec = %.2f mm\n",
           profile.pos_acc, profile.pos_const, profile.pos_dec);
    printf("t_total = %.3f s\n", profile.t_total);
    return 0;
}

运行与验证

编译运行：将上述三个文件置于同一工程，使用任意C编译器（如GCC）编译：

gcc -o planner main.c trapezoid_planner.c -lm
./planner

预期输出（梯形工况）：

v_actual = 100.00 mm/s
t_acc = 0.500 s, t_const = 3.000 s, t_dec = 0.500 s
pos_acc = 25.00 mm, pos_const = 450.00 mm, pos_dec = 25.00 mm
t_total = 4.000 s

验证方法：

将pos_total改为30.0f（小于理论加减速位移之和50mm），重新编译运行，应输出三角形曲线结果：v_actual ≈ 77.46 mm/s，t_const = 0，三段位移之和等于30mm。
检查位移守恒：pos_acc + pos_const + pos_dec应严格等于pos_total（浮点误差小于0.001）。
检查速度连续性：v_actual = a_acc × t_acc = a_dec × t_dec，误差在浮点精度范围内。

STM32集成说明

在STM32工程中，将trapezoid_planner.c/h加入项目，在定时器中断或运动控制任务中调用Trapezoid_Plan()获得各段时间，再根据当前所处阶段计算瞬时速度并输出脉冲频率或PWM占空比。本算法不依赖任何硬件外设，纯数学计算，可直接移植。