S曲线轨迹规划方法原理解析（附Python代码）

叉车司机

已于 2024-12-13 14:15:11 修改

阅读量4.4k

点赞数 20

分类专栏：轨迹规划文章标签：算法

于 2024-08-19 16:11:33 首次发布

本文链接：https://blog.csdn.net/m0_61616957/article/details/141220514

版权

轨迹规划专栏收录该内容

3 篇文章

订阅专栏

2.3.1 速度能够到最大（存在匀速阶段）

2.3.2 速度达不到最大（不存在匀速阶段）

3.代码实现

4. 参考文章

1. 引言

S曲线轨迹规划方法通过平滑的控制速度和加速度的变化，确保机器人在规划起点与目标点之间的平稳过渡，减少运动冲击，提高运动和控制精度。S曲线轨迹一般分为加加速（T1）、匀加速(T2)、减加速(T3)、匀速(T4)、加减速(T5)、匀减速(T6)、减减速(T7)七个阶段，又称为七段式S曲线，如图所示：

其中，图中 T1~T7 分别表示各运动阶段的持续时间，t1~t7 表示各运动阶段的过渡时刻，Ta表示加速运动持续时间，Tv表示匀速运动持续时间，Td表示减速运动持续时间。

2. 规划原理

2.1 输入和输出

七段式S曲线轨迹规划方法的输入和输出分别为：

输入：1.初始状态：初始位置 $x_{0}$ 、初始速度 $v_{0}$ ;

2.目标状态：目标位置 $x_{g}$ 、目标速度 $v_{g}$ ;

3.运动限制：最大速度 $v_{max}$ 、最大加速度 $a_{max}$ 、最大加加速度(jerk) $j_{max}$ ;

4.运动位移： $s=x_{g}-x_{0}$ ;

输出：位置轨迹 $x(t)$ 、速度轨迹 $v(t)$ 、加速度轨迹 $a(t)$ ，输出结果表示为一系列控制信号随时间 t 变化的函数。

2.2 分段规划

· 加加速度段T1( $0< t\leq t_{1}$ ): 加速度 $a=0$ 以 $j=j_{max}$ 线性增加至 $a = a_{max}$

$a(t)=j_{max}t$

$v(t) =v_{0}+ \frac{1}{2}j_{max}t^{2}$

$x(t)=x_{0}+\frac{1}{6}j_{max}t^{3}$

· 匀加速度段T2( $t_{1}< t\leq t_{2}$ ): 加速度恒定，速度 $v = v(t_{1})$ 以 $a=a_{max}$ 线性增加

$a(t)=a_{max}$

$v(t) =v(t_{1})+ a_{max}(t-t_{1})$

$x(t)=x(t_{1})+v(t_{1})(t-t_{1})+\frac{1}{2}a_{max}(t-t_{1})^{2}$

· 减加速度段T3( $t_{2}< t\leqslant t_{3}$ ): 加速度 $a=a_{max}$ 以 $j=-j_{max}$ 线性减小至0，速度增加至 $v=v_{max}$

$a(t)=a_{max}-j_{max}(t-t_{2})$

$v(t) =v_{t_{2}}+a_{max}(t-t_{2})- \frac{1}{2}j_{max}(t-t_{2})^{2}$

$x(t)=x(t_{2})+v(t_{2})(t-t_{2})+\frac{1}{2}a_{max}(t-t_{2})^{2}-\frac{1}{6}j_{max}(t-t_{2})^{3}$

· 匀速段T4( $t_{3}< t\leqslant t_{4}$ ): 加速度 $a=0$ ， $j=0$ ， $v = v_{max}$

$a(t)=0$

$v(t) =v(t_{3})=v_{max}$

$x(t)=x(t_{3})+v_{max}(t-t_{3})$

· 加减速段T5( $t_{4}< t\leqslant t_{5}$ ): 加速度以 $j=-j_{max}$ 从0减少至 $a=-a_{max}$

$a(t)=-j_{max}(t-t_{4})$

$v(t)=v_{max}-\frac{1}{2}j_{max}(t-t_{4})^{2}$

$x(t)=x(t_{4})+v_{max}(t-t_{4})-\frac{1}{6}j_{max}(t-t_{4})^{3}$

· 匀减速段T6( $t_{5}< t\leqslant t_{6}$ ): 加速度 $a=-a_{max}$ ， $j=0$

$a(t)=-a_{max}$

$v(t)=v(t_{5})-a_{max}(t-t_{5})$

$x(t)=x(t_{5})+v(t_{5})(t-t_{5})-\frac{1}{2}a_{max}(t-t_{5})^{2}$

· 减减速段T7( $t_{6}< t\leqslant t_{7}$ ): 加速度以 $j=j_{max}$ 从 $a=-a_{max}$ 增加至 $a=0$

$a(t)=-a_{max}+j_{max}(t-t_{6})$

$v(t)=v(t_{6})-\frac{1}{2}j_{max}(t-t_{6})^{2}-a_{max}(t-t_{6})$

$x(t)=x(t_{6})+v(t_{6})(t-t_{6})-\frac{1}{2}a_{max}(t-t_{6})^{2}-\frac{1}{6}j_{max}(t-t_{6})^{3}$

2.3 实际求解

在实际工程应用过程中，由于初速度、末速度、实际路径长度等限制，不能保证每次规划都存在上述完整的七段过程。所以又可以把 S 曲线的七段过程分为加速过程、匀速过程、减速过程三个阶段，这三个阶段的持续时间分别用 $T_{a}$ 、 $T_{v}$ 、 $T_{j}$ 表示。

2.3.1 速度能够到最大（存在匀速阶段）

(1)判断加速段能否达到最大加速度

首先判断加速阶段是否能够达到最大加速度 $a_{max}$ ，其中，存在关系式：

$v_{1}=v_{0}+\frac{1}{2}j_{max}T_{1}^{2}$ ， $a_{max}=j_{max}T_{1}$

由于加加速阶段( $T_{1}$ )与减加速阶段( $T_{3}$ )是对称的(即加速度 $a$ 以 $j=\pm j_{max}$ 增加/减少到 $a=a_{max}$ / $a=0$ 的过程中时间相等，表示为 $T_{j1}$ ，速度增量相等)，在临界情况(刚好没有匀加速阶段 $T_{2}$ ，加速度 $a$ 达到 $a_{max}$ 后马上减少)下，可以推导出：

$v_{1}-v_{0}=\frac{a_{max}^{2}}{2j_{max}}$

$v_{max}-v_{0}=2(v_{1}-v_{0})=\frac{a_{max}^{2}}{j_{max}}$

$v_{max}=v_{1}+\frac{1}{2}j_{max}T_{j1}^{2}=v_{0}+j_{max}T_{j1}^{2}$

则当 $(v_{max}-v_{0})j_{max}<a_{max}^{2}$ 时，不能达到最大加速度 $a_{max}$ ，也没有匀加速阶段，此时有关系式：

$T_{j_{1}}=\sqrt{\frac{v_{max}-v_{0}}{j_{max}}}$

$T_{a}=2T_{j1}$

此时，系统最大加速度为 $a_{lim}=j_{max}T_{j1}$ 。

当 $(v_{max}-v_{0})j_{max}>a_{max}^{2}$ 时，能达到最大加速度 $a_{max}$ ，存在匀加速阶段( $T_{2}$ )，此时有关系式：

$T_{j_{1}}=\frac{a_{max}}{j_{max}}$

$v_{max}-v_{0}=\frac{1}{2}j_{max}T_{j1}^{2}+a_{max}T_{2}+\frac{1}{2}j_{max}T_{j1}^{2}=j_{max}T_{j1}^{2}+a_{max}T_{2}$

$T_{a}=2T_{j1}+T_{2}$

进一步得到：

$T_{2}=\frac{v_{max}-v_{0}}{a}-T_{j1}$

$T_{a}=T_{j1}+\frac{v_{max}-v_{0}}{a_{max}}$

(2)判断减速段能否达到最大减速度

同理，减减速阶段( $T_{7}$ )与加减速阶段( $T_{5}$ )是对称的(即加速度 $a$ 以 $j=\pm j_{max}$ 增加/减少到 $a=a_{max}$ / $a=0$ 的过程中时间相等，表示为 $T_{j2}$ ，速度增量相等)，临界情况为刚好没有匀减速阶段 $T_{6}$ 。
同理，当 $(v_{max}-v_{g})j_{max}<a_{max}^{2}$ 时，达不到最大减速度 $a=-a_{max}$ ，存在：

$T_{j_{2}}=\sqrt{\frac{v_{max}-v_{g}}{j_{max}}}$

$T_{d}=2T_{j2}$

当 $(v_{max}-v_{g})j_{max}>a_{max}^{2}$ 时，可以达到最大减速度 $a=-a_{max}$ ，存在：

$T_{j_{2}}=\frac{a_{max}}{j_{max}}$

$T_{d}=T_{j2}+\frac{v_{max}-v_{g}}{a_{max}}$

(3) 计算匀速阶段时间

由于加速阶段中加速过程的对称性，加速阶段平均速度可以表示为 $\frac{v_{0}+v_{max}}{2}$ ，同理减速阶段平均速度为 $\frac{v_{max}+v_{g}}{2}$ ，则可以求出匀速阶段持续时间 $T_{v}$ ：

$\frac{v_{0}+v_{max}}{2}T_{a}+v_{max}T_{v}+\frac{v_{max}+v_{g}}{2}T_{d}=s$

$T_{v}=\frac{s}{v_{max}}-\frac{T_{a}}{2}(1+\frac{v_{0}}{v_{max}})-\frac{T_{d}}{2}(1+\frac{v_{g}}{v_{max}})$

如果 $T_{v}>0$ ，则存在匀速阶段，可以按照上述各阶段公式，求出七个阶段各段的位置、速度、加速度函数。如果 $T_{v}<0$ ，则不存在匀速阶段，系统达不到最大速度 $v_{max}$ 。

2.3.2 速度达不到最大（不存在匀速阶段）

$T_{v}<0$ ，不存在匀速阶段，系统达不到最大速度 $v_{max}$ 。存在：

$T_{j1}=T_{j2}=\frac{a_{max}}{j_{max}}$

$T_{a}=\frac{\frac{a_{max}^{2}}{j_{max}}-2v_{0}+\sqrt{\Delta }}{2a_{max}}$

$T_{d}=\frac{\frac{a_{max}^{2}}{j_{max}}-2v_{g}+\sqrt{\Delta }}{2a_{max}}$

$\Delta =\frac{a_{max}^{4}}{j^{2}}+2(v_{0}^{2}+v_{g}^{2})+a_{max}(\frac{4s-2(v_{0}+v_{g})a_{max}}{j_{max}})$

其中， $\Delta$ 推导参考【4】，此时，关于系统能否达到最大加速度 $a=\pm a_{max}$ ，需要讨论 $T_{a}$ 与 $2T_{j1}$ ， $T_{d}$ 与 $2T_{j2}$ 大小关系。

当 $T_{a}\geq 2T_{j1}$ 且 $T_{d}\geq 2T_{j2}$ 时，系统不能达到最大速度 $v_{max}$ ，但可以达到最大加速度，存在：

$v_{max}^{'}=v_{0}+a_{max}(T_{a}-T_{j1})$

当 $T_{a}< 2T_{j1}$ 且 $T_{d}< 2T_{j2}$ 时，系统不能达到最大速度 $v_{max}$ ，也达不到最大加速度，此时需逐渐减少设定的最大加速度 $a_{max}$ ，直至满足 $T_{a}\geq 2T_{j1}$ 且 $T_{d}\geq 2T_{j2}$ 为止。

3.代码实现

import numpy as np
import matplotlib.pyplot as plt

# 定义初始变量
x_0 = 0
x_g = 60
v_max = 20
v_0 = 0
v_g = 0
a_max = 15
j_max = 20
count = 0

# 计算 Ta, Td, Tj1, Tj2, Tv

if (v_max - v_0) * j_max < a_max ** 2:   # 不能达到最大加速度
    if v_0 > v_max:
        Tj1 = 0
        Ta = 0
        alima = 0
    else:
        Tj1 = np.sqrt((v_max - v_0) / j_max)
        Ta = 2 * Tj1
        alima = Tj1 * j_max
else:                               # 能达到最大加速度
    Tj1 = a_max / j_max
    Ta = Tj1 + (v_max - v_0) / a_max
    alima = a_max

if (v_max - v_g) * j_max < a_max ** 2:   # 不能达到最大减速度
    Tj2 = np.sqrt((v_max - v_g) / j_max)
    Td = 2 * Tj1
    alimd = Tj2 * j_max
else:                              # 能达到最大减速度
    Tj2 = a_max / j_max
    Td = Tj2 + (v_max - v_g) / a_max
    alimd = a_max

# 计算匀速时间
Tv = (x_g - x_0) / v_max - Ta / 2 * (1 + v_0 / v_max) - Td / 2 * (1 + v_g / v_max)
# 轨迹总时间
T = Tv + Ta + Td

# 定义轨迹序列
p = []
vc = []
ac = []
jc = []

if Tv > 0:        # 存在匀速阶段
    vlim = v_max
    T = Tv + Ta + Td
else:             # 不存在匀速阶段
    Tv = 0
    amax_org = a_max
    delta = (a_max ** 4) / (j_max ** 2) + 2 * (v_0 ** 2 + v_g ** 2) + a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))
    Tj1 = a_max / j_max
    Ta = (a_max ** 2 / j_max - 2 * v_0 + np.sqrt(delta)) / (2 * a_max)
    Tj2 = a_max / j_max
    Td = (a_max ** 2 / j_max - 2 * v_g + np.sqrt(delta)) / (2 * a_max)
    vlim = v_0 + (Ta - Tj1) * alima
    # 逐渐减少a_max
    while Ta < 2 * Tj1 or Td < 2 * Tj2:
        count += 1
        a_max -= amax_org * 0.1
        alima = a_max
        alimd = a_max
        if a_max > 0:
            delta = (a_max ** 4) / (j_max ** 2) + 2 * (v_0 ** 2 + v_g ** 2) + a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))  # 加速
        else:
            delta = (a_max ** 4) / (j_max ** 2) + 2 * (v_0 ** 2 + v_g ** 2) - a_max * (4 * (x_g - x_0) - 2 * a_max / j_max * (v_0 + v_g))  # 减速
        Tj1 = a_max / j_max
        Ta = (a_max ** 2 / j_max - 2 * v_0 + np.sqrt(delta)) / (2 * a_max)
        Tj2 = a_max / j_max
        Td = (a_max ** 2 / j_max - 2 * v_g + np.sqrt(delta)) / (2 * a_max)
        vlim = v_0 + (Ta - Tj1) * alima
        vlima = vlim
        vlimb = v_g - (Td - Tj2) * alimd

    print("调整后信息:")
    print("TJ1:",Tj1)
    print("Ta:",Ta)
    print("Td",Td)
    print("a_max:",a_max)

    if Ta < 0 or Td < 0:
        if v_0 > v_g:
            Ta = 0
            Tj1 = 0
            alima = 0
            Td = 2 * (x_g - x_0) / (v_g + v_0)
            Tj2 = (j_max * (x_g - x_0) - np.sqrt(j_max * (j_max * (x_g - x_0) ** 2 + (v_g + v_0) ** 2 * (v_g - v_0)))) / (j_max * (v_g + v_0))
            alimd = -j_max * Tj2
            vlim = v_g - (Td - Tj2) * alimd
            alimd = -alimd
        else:
            Td = 0
            Tj2 = 0
            Ta = 2 * (x_g - x_0) / (v_g + v_0)
            Tj1 = (j_max * (x_g - x_0) - np.sqrt(j_max * (j_max * (x_g - x_0) ** 2 - (v_g + v_0) ** 2 * (v_g - v_0)))) / (j_max * (v_g + v_0))
            alima = j_max * Tj1
            vlim = v_0 + (Ta - Tj1) * alima

    print(Tj1)
    print(Tj2)
    print(Ta)
    print(Td)
    print("565")
    print(alima)
    print(alimd)
    T = Tv + Ta + Td

for t in np.arange(0, T, 0.001):
    # 加加速T1
    if 0 <= t < Tj1:
        x = x_0 + v_0 * t + j_max * t ** 3 / 6
        p.append(x)
        v = v_0 + j_max * t ** 2 / 2
        vc.append(v)
        a = j_max * t
        ac.append(a)
        jc.append(j_max)
    # 匀加速T2
    elif Tj1 <= t < (Ta - Tj1):
        x = x_0 + v_0 * t + alima / 6 * (3 * t ** 2 - 3 * Tj1 * t + Tj1 ** 2)
        p.append(x)
        v = v_0 + alima * (t - Tj1 / 2)
        vc.append(v)
        a = alima
        ac.append(a)
        jc.append(0)
    # 减加速T3
    elif (Ta - Tj1) <= t < Ta:
        x = x_0 + (vlim + v_0) * Ta / 2 - vlim * (Ta - t) + j_max * (Ta - t) ** 3 / 6
        p.append(x)
        v = vlim - j_max * (Ta - t) ** 2 / 2
        vc.append(v)
        a = j_max * (Ta - t)
        ac.append(a)
        jc.append(-j_max)
    # 匀速T4
    elif Ta <= t < (Ta + Tv):
        x = x_0 + (vlim + v_0) * Ta / 2 + vlim * (t - Ta)
        p.append(x)
        v = vlim
        vc.append(v)
        a = 0
        ac.append(0)
        jc.append(0)
    # 加减速T5
    elif (T - Td) <= t < (T - Td + Tj2):
        x = x_g - (vlim + v_g) * Td / 2 + vlim * (t - T + Td) - j_max * (t - T + Td) ** 3 / 6
        p.append(x)
        v = vlim - j_max * (t - T + Td) ** 2 / 2
        vc.append(v)
        a = -j_max * (t - T + Td)
        ac.append(a)
        jc.append(-j_max)
    # 匀减速T6
    elif (T - Td + Tj2) <= t < (T - Tj2):
        x = x_g - (vlim + v_g) * Td / 2 + vlim * (t - T + Td) - alimd / 6 * (3 * (t - T + Td) ** 2 - 3 * Tj2 * (t - T + Td) + Tj2 ** 2)
        p.append(x)
        v = vlim - alimd * (t - T + Td - Tj2 / 2)
        vc.append(v)
        a = -alimd
        ac.append(a)
        jc.append(0)
    # 减减速T7
    elif (T - Tj2) <= t < T:
        x = x_g - v_g * (T - t) - j_max * (T - t) ** 3 / 6
        p.append(x)
        v = v_g + j_max * (T - t) ** 2 / 2
        vc.append(v)
        a = -j_max * (T - t)
        ac.append(a)
        jc.append(j_max)

t = np.arange(0, T, 0.001)

# 绘图
plt.figure(figsize=(12, 8))

plt.subplot(4, 1, 1)
plt.plot(t, p)
#plt.title('Position vs Time')
plt.ylabel('Position')
plt.legend()

plt.subplot(4, 1, 2)
plt.plot(t, vc)
#plt.title('Velocity vs Time')
plt.ylabel('Velocity')
plt.legend()

plt.subplot(4, 1, 3)
plt.plot(t, ac)
plt.ylabel('Acceleration')
plt.xlabel('Time')
plt.legend()

plt.subplot(4, 1, 4)
plt.plot(t, jc)
plt.ylabel('Jerk')
plt.xlabel('Time')
plt.legend()

# 显示图形
plt.show()

运行结果：