运动控制中的S曲线加减速规划

运动控制系统的核心在于如何让电机从静止平滑地加速到目标速度，再平稳地停止。传统的梯形加减速算法虽然简单，但在加速和减速的拐点处加速度突变，会产生机械冲击和振动。S曲线加减速规划通过限制加加速度的变化率，解决了这一问题，广泛应用于高精度的数控机床、机器人及自动化设备中。

1. 理解S曲线的七个阶段

S曲线速度规划的核心在于将加速度的变化也纳入控制范围，使得加速度的变化率（即加加速度 Jerk）为常数或零。一个完整的S曲线运动过程通常包含七个阶段。掌握这七个阶段是进行参数计算和逻辑编程的基础。

以下是七个阶段的具体定义：

在实际工程应用中，如果目标位移较短，可能无法达到最大速度或最大加速度，导致某些阶段缺失（例如没有匀速段，甚至没有匀加速段）。

2. 规划流程与算法逻辑

在进行代码编写前，必须理清参数之间的约束关系。核心输入参数通常包括：最大速度 $V_{max}$、最大加速度 $A_{max}$、最大加加速度 $J_{max}$ 以及目标位移 $S_{target}$。

算法的主要任务是判断在有限的位移内，能否达到预设的 $V_{max}$ 或 $A_{max}$。

上图展示了决策逻辑。为了在代码中实现这一点，我们需要构建几个关键的判断公式。

3. 关键参数的计算步骤

为了确保运动规划的准确性，必须按顺序计算以下关键物理量。

步骤 1：计算加加速度时间

加加速度时间 $T_j$ 是加速度从 0 增加到 $A_{max}$（或从 $A_{max}$ 减少到 0）所需的时间。计算公式如下：

$$ T_j = \frac{A_{max}}{J_{max}} $$

步骤 2：判断加速度曲线形状

根据设定的最大速度 $V_{max}$ 和计算出的 $T_j$，判断加速度曲线是呈“三角形”还是“梯形”。

计算仅依靠加加减速能达到的速度 $V_{tri}$：

$$ V_{tri} = A_{max} \cdot T_j $$

比较 $V_{max}$ 与 $V_{tri}$：

• 如果 $V_{max} \le V_{tri}$：说明加速度还没上升到 $A_{max}$ 就需要开始下降，加速度曲线为三角形。
• 如果 $V_{max} > V_{tri}$：说明加速度可以维持一段时间在 $A_{max}$，加速度曲线为梯形。

步骤 3：计算加速过程的总位移

无论加速度曲线是三角形还是梯形，都需要计算加速阶段（包括加加速、匀加速、减加速）的总位移 $S_{acc}$。

当 $V_{max} > V_{tri}$ 时（梯形加速度模式）：

$$ S_{acc} = V_{max} \cdot T_j + \frac{V_{max}^2}{2 \cdot A_{max}} $$

注意：此处 $T_j$ 是加加速段的时间。实际上，加速段的总位移也可以分解为加加速、匀加速、减加速三段位移之和。

更直观的分段计算方式（针对梯形加速度模式）：

1. 加加速段位移：$S_1 = \frac{1}{6} \cdot J_{max} \cdot T_j^3$
2. 匀加速段位移：$S_2 = V_{start} \cdot T_{a} + 0.5 \cdot A_{max} \cdot T_{a}^2$ （其中 $T_{a}$ 为匀加速时间，$V_{start}$ 为加加速段结束速度）
3. 减加速段位移：$S_3$ （与 $S_1$ 类似，取决于起始速度和加速度）

通常我们会先计算达到最大速度所需的总时间 $T_{acc\_total}$ 和匀速时间 $T_{const}$。

步骤 4：判断是否达到最大速度

计算完整加速和减速所需的“极限位移” $S_{limit}$。

$$ S_{limit} = S_{acc} + S_{dec} $$

其中 $S_{dec}$（减速段位移）在数值上通常等于 $S_{acc}$。

比较 $S_{target}$ 与 $S_{limit}$：

• 如果 $S_{target} \ge S_{limit}$：运动过程包含完整的加加速、匀加速、减加速、匀速、加减速、匀减速、减减速七个阶段。
• 如果 $S_{target} < S_{limit}$：运动没有匀速阶段。此时需要降低实际运行的最大速度 $V_{peak}$，使得加速段和减速段的位移之和正好等于 $S_{target}$。这涉及到求解一个一元二次方程或三次方程，通常使用迭代法或解析法求解新的 $V_{peak}$。

4. 代码实现示例 (Python)

以下是一个简化的 Python 函数，用于计算S曲线规划的时间参数。此代码假设位移足够长，能够达到最大加速度和最大速度，即标准的7段模型。

import math

def calculate_s_curve_times(V_max, A_max, J_max, S_target):
    """
    计算7段S曲线的时间参数
    :param V_max: 最大速度
    :param A_max: 最大加速度
    :param J_max: 最大加加速度
    :param S_target: 目标位移
    :return: 包含各阶段时间的字典
    """

    # 1. 计算加加速度时间
    T_j = A_max / J_max

    # 2. 计算加速度达到最大时的临界速度
    V_tri = A_max * T_j

    # 3. 判断加速度形状 (此处演示梯形情况: V_max > V_tri)
    if V_max <= V_tri:
        # 如果是三角形加速度，逻辑会更复杂，此处简化处理，返回提示
        return {"error": "三角形加速度模式，请降低加速度或提高速度"}

    # 4. 计算加速段（加速+匀加速+减加速）的位移
    # 加加速段结束时的速度 V1
    V1 = 0.5 * J_max * T_j**2
    # 减加速段结束时的速度 V2 (即最大速度 V_max)

    # 加速段总位移公式 (梯形加速度)
    # S_acc = V_max * T_j + V_max^2 / (2 * A_max) 
    # 推导较为繁琐，这里使用物理积分的逻辑近似表达：
    # S_acc = S1 + S2 + S3
    # S1 (加加速) = 1/6 * J * Tj^3
    # S2 (匀加速) = V1 * Ta + 0.5 * A * Ta^2, 其中 Ta = (Vmax - 2*V1) / A ? 不完全是
    # 让我们使用能量守恒或标准公式：S_acc = V_max * (V_max / A_max + T_j) / 2

    S_acc = V_max * (V_max / A_max + T_j) / 2
    S_dec = S_acc # 对称性

    # 5. 计算极限位移
    S_limit = S_acc + S_dec

    # 6. 判断是否有匀速段
    if S_target < S_limit:
        return {"error": "位移不足，无法达到最大速度，需要削顶处理"}

    # 7. 计算匀速段时间
    S_const = S_target - S_limit
    T_const = S_const / V_max

    # 计算匀加速段时间 (Ta)
    # V_max = V1 + A_max * Ta + (V_max - V1)  <-- 这里注意，减加速段的速度变化量也是 V1
    # 实际上：V_max = 2 * V1 + A_max * Ta
    # 所以：Ta = (V_max - 2 * V1) / A_max
    # V1 = 0.5 * A_max * T_j
    # V_max - A_max * T_j = A_max * Ta
    Ta = (V_max - A_max * T_j) / A_max

    return {
        "T_j1": T_j,      # 加加速时间
        "T_a": Ta,        # 匀加速时间
        "T_j2": T_j,      # 减加速时间
        "T_const": T_const, # 匀速时间
        "T_j3": T_j,      # 加减速时间
        "T_d": Ta,        # 匀减速时间
        "T_j4": T_j,      # 减减速时间
        "Total_Time": 4*T_j + 2*Ta + T_const
    }

# 示例参数
params = calculate_s_curve_times(V_max=100, A_max=50, J_max=100, S_target=500)
print(params)

5. 实时插补输出

计算出各阶段的时间后，控制系统的每个伺服周期（例如 1ms）都需要根据当前所处的时间段，计算当前的瞬时速度、加速度和位置。

假设当前时间 $t$ 处于加加速段 ($0 \le t < T_j$)：

• 加速度：$a(t) = J_{max} \cdot t$
• 速度：$v(t) = v_0 + 0.5 \cdot J_{max} \cdot t^2$ (假设初速度 $v_0=0$)
• 位移：$p(t) = p_0 + v_0 \cdot t + \frac{1}{6} \cdot J_{max} \cdot t^3$

假设当前时间 $t$ 处于匀加速段 ($T_j \le t < T_j + T_a$)：
令 $t' = t - T_j$

• 加速度：$a(t) = A_{max}$
• 速度：$v(t) = v_{T_j} + A_{max} \cdot t'$
• 位移：$p(t) = p_{T_j} + v_{T_j} \cdot t' + 0.5 \cdot A_{max} \cdot t'^2$

其中 $v_{T_j}$ 和 $p_{T_j}$ 是上一阶段结束时的状态值，必须存储并传递给下一阶段。

在每个伺服周期中断服务程序（ISR）中：

1. 读取当前累计时间 $T_{curr}$。
2. 判断 $T_{curr}$ 落在上述哪个时间段。
3. 代入对应的时间区间公式，计算本周期应增加的位移增量 $\Delta P$。
4. 累加 $\Delta P$ 到总位置计数器，并发送给驱动器。