目录
1. 引言
S曲线轨迹规划方法通过平滑的控制速度和加速度的变化,确保机器人在规划起点与目标点之间的平稳过渡,减少运动冲击,提高运动和控制精度。S曲线轨迹一般分为加加速(T1)、匀加速(T2)、减加速(T3)、匀速(T4)、加减速(T5)、匀减速(T6)、减减速(T7)七个阶段,又称为七段式S曲线,如图所示:
其中,图中 T1~T6 分别表示各运动阶段的持续时间,t1~t6 表示各运动阶段的过渡时刻,Ta表示加速运动持续时间,Tv表示匀速运动持续时间,Td表示减速运动持续时间。
2. 规划原理
2.1 输入和输出
七段式S曲线轨迹规划方法的输入和输出分别为:
输入:1.初始状态:初始位置 、初始速度
;
2.目标状态:目标位置 、目标速度
;
3.运动限制:最大速度 、最大加速度
、最大加加速度(jerk)
;
4.运动位移: ;
输出:位置轨迹 、速度轨迹
、加速度轨迹
,输出结果表示为一系列控制信号随时间 t 变化的函数。
2.2 分段规划
· 加加速度段T1(): 加速度
以
线性增加至
· 匀加速度段T2(): 加速度恒定,速度
以
线性增加
· 减加速度段T3(): 加速度
以
线性减小至0,速度增加至
· 匀速段T4(): 加速度
,
,
· 加减速段T5(): 加速度以
从0减少至
· 匀减速段T6(): 加速度
,
· 减减速段T7(): 加速度以
从
增加至
2.3 实际求解
在实际工程应用过程中,由于初速度、末速度、实际路径长度等限制,不能保证每次规划都存在上述完整的七段过程。所以又可以把 S 曲线的七段过程分为加速过程、匀速过程、减速过程三个阶段,这三个阶段的持续时间分别用、
、
表示。
2.3.1 速度能够到最大(存在匀速阶段)
(1)判断加速段能否达到最大加速度
首先判断加速阶段是否能够达到最大加速度,其中,存在关系式:
,
由于加加速阶段()与减加速阶段(
)是对称的(即加速度
以
增加/减少到
/
的过程中时间相等,表示为
,速度增量相等),在临界情况(刚好没有匀加速阶段
,加速度
达到
后马上减少)下,可以推导出:
则当时,不能达到最大加速度
,也没有匀加速阶段,此时有关系式:
此时,系统最大加速度为 。
当时,能达到最大加速度
,存在匀加速阶段(
),此时有关系式:
进一步得到:
(2)判断减速段能否达到最大减速度
同理,减减速阶段()与加减速阶段(
)是对称的(即加速度
以
增加/减少到
/
的过程中时间相等,表示为
,速度增量相等),临界情况为刚好没有匀减速阶段
。
同理,当 时,达不到最大减速度
,存在:
当 时,可以达到最大减速度
,存在:
(3) 计算匀速阶段时间
由于加速阶段中加速过程的对称性,加速阶段平均速度可以表示为,同理减速阶段平均速度为
,则可以求出匀速阶段持续时间
:
如果,则存在匀速阶段,可以按照上述各阶段公式,求出七个阶段各段的位置、速度、加速度函数。如果
,则不存在匀速阶段,系统达不到最大速度
。
2.3.2 速度达不到最大(不存在匀速阶段)
,不存在匀速阶段,系统达不到最大速度
。存在:
其中,推导参考【4】,此时,关于系统能否达到最大加速度
,需要讨论
与
,
与
大小关系。
当 且
时,系统不能达到最大速度
,但可以达到最大加速度,存在:
当 且
时,系统不能达到最大速度
,也达不到最大加速度,此时需逐渐减少设定的最大加速度
,直至满足
且
为止。
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()
运行结果:
4. 参考文章
[1]一文教你快速搞懂速度曲线规划之S形曲线(超详细+图文+推导+附件代码)_s型速度曲线-CSDN博客
[2]【机器人学】5-2.六自由度机器人轨迹规划-速度规划- S型曲线【附MATLAB代码】_scurve速度规划 matlab代码-CSDN博客
[3]分段式S形速度规划算法_s型加减速曲线 不存在匀加速-CSDN博客
[4]田军锋,林浒,姚壮,等.数控系统S型曲线加减速快速规划研究[J].小型微型计算机系统,2013,34(01):168-172.