五次轨迹让步进电机的加速度连续起来

目标只差十度，电机却要经历一次完整的启动和停止。小转台、相机调节轴、送料机构，往往反复做这种短行程动作。把最高转速设得很低，并没有回答另一个问题——加速度能不能别在一瞬间跳起来？

梯形速度曲线已经解决了速度突变，但它的加速度仍会在零、正上限和负上限之间切换。对于需要柔和起停的机构，可以再往前走一步，让加速度连续变化，同时把变化速率也纳入约束。

下面做一个可直接运行的五次多项式规划器。输入位置、速度上限、加速度上限和 jerk 上限，输出固定周期的位置表；短行程、反向运动和整数编码器计数都一起处理。先把参考指令做对，再讨论接入电机。

图1　同样的90°行程与速度、加速度上限，梯形参考用时0.7500秒，五次参考用时0.9376秒。曲线来自离线计算，展示的是参考指令，未测量电机振动。

先分清位置表和驱动脉冲

这次用到的控制板由RP2040、MT6816角度传感器、两路电流采样和H桥组成。它的控制链路是位置参考进入位置环，再经过速度环和电流环驱动功率级。上位机提供的是位置序列，不能把它当作常见驱动器的STEP脉冲列表。

从工程中的主控连接可以核对几组关键网络。

图2　实际EDA编辑器重新截取的主控局部。GPIO编号与封装管脚号是两套编号，代码使用前者。

图3　MT6816的3.3V供电及四线读取连接。位置计数进入控制软件后，一机械圈按16384计数处理，因此一计数约为0.02197°。

图4　V11板的顶层和底层视图，均从顶面观察，右图没有镜像翻板。板上既有功率回路，也有角度与电流反馈；本次只新增上位机离线规划模块，不改这块硬件。

源码里还有一个容易忽略的时间单位问题。python_tools/trapezoid.py示例把一个时间单位解释为1/2500秒，而当前mcu/foc.c通过PWM中断每五次置位一次update_speed，位置环再读取FIFO。两者不能直接混用。

本例明确选择200μs作为离线位置表的采样约定，并将它作为dt参数暴露。接入板端前仍要核对实际时钟、PWM计数模式和FIFO消费周期；本文没有完成整套固件编译、联机发送或周期实测，不把名义频率当成已经测到的结果。

用六个边界条件确定一条曲线

这里先解决最常用的一种动作——从静止出发，到目标位置静止结束，起点和终点的加速度也都为零。

设总时长为T，归一化时间u=t/T，位移D=x1−x0。位置写成：

s(u) = 10u³ - 15u⁴ + 6u⁵
x(t) = x0 + D·s(u)          0 ≤ u ≤ 1

五次多项式的六个系数，对应起终点的位置、速度、加速度六个条件。对位置依次求导，就得到速度、加速度和jerk。jerk是加速度随时间的变化率，角度单位下可用°/s³表示。

这条曲线的连续参考峰值可以直接算出，不必靠采样碰运气寻找最大值。

速度峰值出现在u=0.5；加速度的两个极值点是u=(3±√3)/6；jerk的最大绝对值出现在运动区间端点。由此反推三个最短允许时间，再取最大值，就能同时满足三项上限。

D = abs(x1 - x0)
Tv = 1.875 * (D / vmax)
Ta = math.sqrt((10.0 / math.sqrt(3.0)) * (D / amax))
Tj = math.cbrt(60.0 * (D / jmax))
Tmin = max(Tv, Ta, Tj)
N = max(1, math.ceil(Tmin / dt))
T = N * dt

这是规划器的核心计算。文末的完整模块另外处理了零位移、非法参数、采样数量上限和浮点取整边界。位置、速度、加速度、jerk必须使用同一套单位；把速度填成RPM、位置却仍填编码器计数，会得到一条形状漂亮、数值错误的曲线。

这也不是时间最优的七段S曲线。五次曲线通过拉长时间满足约束，实现简单，代价是可能比允许触及各项上限的最优轨迹更慢。另外，起终点处jerk可以从保持段的零跳到有限值；连续的是加速度，不是jerk本身。

行程越短，越不能只盯最高速度

本例统一设置速度上限180°/s、加速度上限720°/s²、jerk上限14400°/s³，然后分别规划约10°、45°和90°。

10°对应的计数不是整数，所以示例用455计数，实际为9.99756°。这里没有把舍入后的目标仍写成精确10°。

图5　约10°时由jerk约束主导，45°时由加速度约束主导，90°时由速度约束主导。虚线是统一上限，图中的曲线没有超过相应限制。

短行程下，最高速度甚至只有54°/s。继续抬高速度上限，并不能明显缩短这次动作，因为约束已经落在加速度爬升过程上。对调节机构而言，这比只给一个“最大转速”参数更容易解释清楚。

固定周期要一直固定到终点

一个常见做法是按dt采样到接近终点，再额外补一个精确终点。这样最后一段往往不满dt，而板端却可能仍按一个完整周期消费它。位置虽然对了，时间含义却变了。

这里先把T向上取整为N·dt，再重新计算整条曲线。输出N+1个点，包含t=0和t=T；所有相邻时间戳都相差dt。上取整使时长增加不足一个采样周期，同时保住连续参考的三项上限。

先看调用方式。将文末完整模块保存为quintic.py，再把下面的代码保存为run.py：

from quintic import plan

counts_per_degree = 16384 / 360
p = plan(
    0, 4096,
    vmax=180 * counts_per_degree,
    amax=720 * counts_per_degree,
    jmax=14400 * counts_per_degree,
    dt=0.0002,
)
rows = list(p.samples())
print(p.duration, len(rows))
print(rows[0][3], rows[-1][3])
# 约0.9376秒，4689行；整数端点为0和4096

参数中的计数位置限定为有符号32位整数。反向运动只改变位移符号，速度、加速度和jerk随之改变方向，时长约束仍使用绝对位移。零位移只输出一个保持点，不伪造一段动作。

需要特别注意，4689行位置表远大于板端512点FIFO的容量。即使周期完全匹配，也不能整张表一次塞入。联机适配还需要流控、预填、同步启动、欠载处理与现有数据压缩协议。本文程序只计算离线位置表，不打开串口、不使能电机。

整数位置表不会自动继承所有光滑性质

连续曲线最后还要变成整数计数。样例采用最接近整数取整，恰好半计数时远离零，正反向规则一致。量化误差不超过半计数，对应这套位置单位约0.01099°。

图6　约10°动作最开始25ms的浮点参考与整数位置。台阶来自计数量化，不是缺失采样点。

但不要再对整数位置连续差分三次，然后要求得到的数值也小于14400°/s³。量化造成的台阶会被微分放大。本文给出的jerk上限针对连续参考，不是针对整数序列的三阶差分。

这也解释了为什么“位置表平滑”与“电机实际平稳”之间还隔着一层工作。位置环的前馈、编码器噪声、环路带宽、负载惯量和机械共振都可能影响结果。把规划器与伺服系统分开验收，才能判断问题到底发生在哪一层。

跑通以后，再检查它到底通过了什么

下面是可复制的完整quintic.py，只依赖Python标准库，要求Python 3.11及以上；本次验证环境为CPython 3.12.14。

"""Offline rest-to-rest quintic setpoints. No serial output or motor control.

Units: encoder counts, seconds. Endpoint coordinates must be signed int32.
Limits apply to the continuous reference, not differences of rounded counts.
"""
from dataclasses import dataclass
import math


def _positive(value, name):
    if isinstance(value, bool) or not math.isfinite(value) or value <= 0:
        raise ValueError(f"{name} must be finite and positive")


@dataclass(frozen=True)
class Plan:
    x0: int
    x1: int
    dt: float
    intervals: int

    @property
    def duration(self):
        return self.intervals * self.dt

    def at(self, t):
        """Return (position, velocity, acceleration, jerk) at finite time t.

        Before/after the move the reference holds. At the endpoints jerk is
        the one-sided value inside the move; velocity and acceleration are 0.
        """
        if not math.isfinite(t):
            raise ValueError("time must be finite")
        T, d = self.duration, self.x1 - self.x0
        if not self.intervals or t < 0 or t > T:
            return (float(self.x0 if t < 0 else self.x1), 0.0, 0.0, 0.0)
        u = t / T
        # Symmetry avoids cancellation in position near the far endpoint.
        z = min(u, 1.0 - u)
        s = z**3 * (10.0 + z * (-15.0 + 6.0 * z))
        x = self.x0 + d*s if u <= 0.5 else self.x1 - d*s
        v = d / T * (30.0 * u*u * (1.0-u)**2)
        a = (d / T) / T * (60.0 * u * (1.0-u) * (1.0-2.0*u))
        j = ((d / T) / T) / T * (60.0 * (1.0 - 6.0*u + 6.0*u*u))
        return x, v, a, j

    def samples(self):
        """N+1 rows, exact fixed dt and both endpoints; round half away from 0."""
        for k in range(self.intervals + 1):
            t = k*self.dt
            x, v, a, j = self.at(t)
            q = math.floor(x+0.5) if x >= 0 else math.ceil(x-0.5)
            yield k, t, x, q, v, a, j


def plan(x0, x1, vmax, amax, jmax, dt=0.0002, max_intervals=2_000_000):
    """Create a fixed-period plan with zero initial/final speed/acceleration.

    Not time optimal. Nonzero boundary velocities are deliberately unsupported.
    max_intervals prevents accidentally generating unreasonable CSV files.
    """
    for x in (x0, x1):
        if type(x) is not int or not -(2**31) <= x < 2**31:
            raise ValueError("endpoints must be signed int32 integers")
    for name, value in (("vmax", vmax), ("amax", amax), ("jmax", jmax), ("dt", dt)):
        _positive(value, name)
    if type(max_intervals) is not int or max_intervals < 1:
        raise ValueError("max_intervals must be a positive integer")
    D = abs(x1-x0)
    if not D:
        return Plan(x0, x1, dt, 0)
    # Use roots of ratios to avoid unnecessary intermediate overflow.
    Tv = 1.875 * (D/vmax)
    Ta = math.sqrt((10.0/math.sqrt(3.0)) * (D/amax))
    Tj = math.cbrt(60.0 * (D/jmax))
    ratio = max(Tv, Ta, Tj) / dt
    if not math.isfinite(ratio) or ratio > max_intervals:
        raise ValueError("requested move exceeds sample budget")
    n = max(1, math.ceil(ratio))
    # Guard the rare floating-point rounding of ceil(T/dt)*dt below T.
    while n*dt < max(Tv, Ta, Tj):
        n += 1
    if n > max_intervals:
        raise ValueError("requested move exceeds sample budget")
    return Plan(x0, x1, dt, n)

将quintic.py和前面的run.py放在同一目录，运行python run.py，即可核对时长、采样行数和整数端点。修改调用中的起终点可检查反向运动。

本次五组自动化测试全部通过。其中500组固定种子的正反向轨迹，用60位十进制展开式交叉核对位置值，同时检查连续参考的速度、加速度和jerk上限；另外通过数值差分检查导数关系，覆盖零位移、一计数运动、32位端点、整数量化、非法输入和过大采样请求。

这些结果证明了离线规划模块在已测范围内的数学一致性与边界处理。它们没有证明电机的定位精度、温升或振动已经改善。若接入实际设备，首先确认消费周期与位置单位，再从低电流、短行程开始，记录实际位置误差与跟随情况。

当动作需要非零起终速度、连续拐角或紧急制动时，这个静止到静止的模型就不够了。下一步应选择支持相应边界条件的规划器，而不是硬把相邻的五次曲线拼在一起。

资料与工程

图中曲线由同版本程序计算；原理图和PCB来自真实EDA编辑器截图，裁剪并排版，底层轻微提亮，未补画线路或器件文字。

硬件与控制链路参考立创开源广场“闭环步进电机（RS485接口）”，https://oshwhub.com/hemn/closed-loop-stepper-motor 。工程代码 https://gitee.com/hemn1990/closed-loop-stepper-motor ，核对版本ae9aac55cda1438cdaac01c573effe1c4e746040，硬件V11，MIT许可，Copyright (c) 2025 Mengning He。本文独立完成五次离线规划模块、约束推导、数值测试与图表；未改动功率硬件，未进行板端固件集成及实体测量。

技术交流，欢迎关注微信公众号：美男子玩编程。