s曲线介绍
相比于T型轨迹来说,S轨迹的冲击比较小,加速度连续,在加速,匀速和减速衔接处做了优化,使得曲线柔顺,但是相比于算法,S轨迹运算远超T型轨迹。
仿真模拟
这边在S规划中做了一点小判断,满足现在项目中的JOG运动加减速的标准,但是还是有欠缺
后续需要优化
下面这个是标准的一个S曲线轨迹图像,这种适合初始速度和末段速度都小于匀速速度的情况
下面这个做了一个判断,当初始速度大于匀速速度时,轨迹图像。注意:此时的轨迹必须要有匀速,否则会报错,这个后面得优化才行
当末速度大于平均速度时候轨迹图像,此时依然需要匀速段
等等。。
python代码
首先先根据输入的参数计算总时间T,在根据能否达到最大速度的判断来运行
def _compute_maximum_speed_reached_or_not_reached(self,q0,q1, v0, v1, v_max, a_max, j_max):
if (v_max - v0) * j_max < a_max ** 2:
# # a_max is not reached
Tj1 = np.sqrt(abs((v_max - v0)) / j_max)
Ta = 2 * Tj1
alima = Tj1 * j_max
else:
# a_max is reached
Tj1 = a_max / j_max
Ta = Tj1 + (v_max - v0) / a_max
alima = a_max
# Deceleration period
if (v_max-v1)*j_max < a_max**2:
# a_min is not reached
Tj2 = np.sqrt(abs((v_max-v1))/j_max)
Td = 2*Tj2
alimd = Tj2 * (-j_max)
else:
# a_min is reached
Tj2 = a_max/j_max
Td = Tj2 + (v_max-v1)/a_max
alimd = (-a_max)
Tv = (q1-q0)/v_max - (Ta/2)*(1+v0/v_max)-(Td/2)*(1+v1/v_max)
下面这段参考了论文以及一些博主的代码来写的,但是运行起来还是有些状态没考虑到
if Tv > 0:
vlim = v_max
else:
Tv = 0
amax_org = a_max
delta = (a_max**4)/(j_max**2) + 2*(v0**2+v1**2) + a_max*(4*(q1-q0) -2*a_max/j_max*(v0 + v1))
Tj = a_max/j_max
Tj1 = Tj
Ta = (a_max**2/j_max - 2*v0 + np.sqrt(delta))/2/a_max
Tj2 = Tj
Td = ((a_max**2)/j_max - 2*v1 + np.sqrt(delta))/2/a_max
if Ta < 0 or Td < 0:
if Ta < 0:
Ta = 0
Tj1 = 0
Td = 2*(q1-q0)/(v1 + v0)
Tj2 = (j_max*(q1-q0) - np.sqrt(j_max*(j_max*((q1-q0)**2) + ((v1 + v0)**2)*(v1 - v0))))/j_max/(v1 + v0)
alima = 0
alimd = -j_max*Tj2
vlim = v0
elif Td < 0:
Td = 0
Tj2 = 0
Ta = 2 * (q1 - q0) / (v1 + v0)
Tj1 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) - ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
alima = j_max * Tj1
alimd = 0
vlim = j_max * Tj1
elif Ta >= 2*Tj and Td >= 2*Tj:
alima = a_max
alimd = -a_max
vlim = v0 + alima * (Ta - Tj)
else:
count = 0
while Ta < 2*Tj or Td < 2*Tj:
count += 1
a_max = a_max - amax_org*0.01
Tj = a_max / j_max
Tj1 = Tj
Tj2 = Tj
delta = (a_max ** 4) / (j_max ** 2) + 2 * (v0 ** 2 + v1 ** 2) + a_max * (4 * (q1 - q0) - 2 * a_max / j_max * (v0 + v1))
Ta = ((a_max**2)/j_max - 2*v0 + np.sqrt(delta))/2/a_max
Td = ((a_max ** 2) / j_max - 2 * v1 + np.sqrt(delta)) / 2 / a_max
if Ta<0 or Td <0:
if Ta <0:
Ta = 0
Tj1 = 0
Td = 2*(q1-q0)/(v0+v1)
Tj2 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) + ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
alima = 0
alimd = -j_max*Tj2
vlim = v0
elif Td < 0:
Td = 0
Tj2 = 0
Ta = 2*(q1-q0)/(v0+v1)
Tj1 = (j_max * (q1 - q0) - np.sqrt(j_max * (j_max * ((q1 - q0) ** 2) - ((v1 + v0) ** 2) * (v1 - v0)))) / j_max / (v1 + v0)
alima = j_max*Tj1
alimd = 0
vlim = j_max * Tj1
elif Ta >= 2*Tj and Td >= 2*Tj:
alima = a_max
alimd = -a_max
vlim = v0+alima*(Ta -Tj)
self.T = Tv + Ta + Td
self.Tj1 = Tj1
self.Tj2 = Tj2
self.Ta = Ta
self.Tv = Tv
self.Td = Td
self.alima = alima
self.alimd = alimd
self.vlim = vlim
return self.T
下面就是S曲线计算点位的公式了。根据7段时间来规划:
def _get_trajectory_func(self,q0,q1,v0,v1,v_max,a_max,j_max):
step = 0.0001
t_list = np.arange(0, self.T+0.01, step)
q_list = []
v_list = []
a_list = []
j_list = []
for t in t_list:
if v0 > v_max:
if 0 <= t < self.Tj1:
q = q0 + v0*t - j_max*(t**3)/6
qd = v0 - j_max * (t ** 2) / 2
qdd = -j_max * t
qddd = j_max
elif self.Tj1 <= t < (self.Ta - self.Tj1):
q = q0 + v0*t - self.alima/6*(3*(t**2) - 3*self.Tj1*t + self.Tj1**2)
qd = v0 - self.alima * (t - self.Tj1 / 2)
qdd = -self.alima
qddd = 0
elif (self.Ta-self.Tj1) <= t < self.Ta:
q = q0 + (self.vlim+v0)*self.Ta/2 - self.vlim*(self.Ta - t) - j_max*(self.Ta - t)**3/6
qd = self.vlim + j_max * ((self.Ta - t) ** 2) / 2
qdd = -j_max * (self.Ta - t)
qddd = -j_max
else:
if 0 <= t < self.Tj1:
q = q0 + v0 * t + j_max * (t ** 3) / 6
qd = v0 + j_max * (t ** 2) / 2
qdd = j_max * t
qddd = j_max
elif self.Tj1 <= t < (self.Ta - self.Tj1):
q = q0 + v0 * t + self.alima / 6 * (3 * (t ** 2) - 3 * self.Tj1 * t + self.Tj1 ** 2)
qd = v0 + self.alima * (t - self.Tj1 / 2)
qdd = self.alima
qddd = 0
elif (self.Ta - self.Tj1) <= t < self.Ta:
q = q0 + (self.vlim + v0) * self.Ta / 2 - self.vlim * (self.Ta - t) + j_max * (self.Ta - t) ** 3 / 6
qd = self.vlim - j_max * ((self.Ta - t) ** 2) / 2
qdd = j_max * (self.Ta - t)
qddd = -j_max
# Constant velocity phase
if self.Ta <= t < (self.Ta + self.Tv):
q = q0 + (self.vlim+v0)*self.Ta/2 + self.vlim*(t-self.Ta)
qd = self.vlim
qdd = 0
qddd = 0
# Deceleration phase
if v1 > v_max:
if (self.T - self.Td) <= t < (self.T-self.Td+self.Tj2):
q = q1 - (self.vlim+v1)*self.Td/2 + self.vlim*(t-self.T+self.Td) +j_max*((t-self.T+self.Td)**3)/6
qd = self.vlim + j_max * ((t-self.T+self.Td) ** 2) / 2
qdd = j_max * (t-self.T+self.Td)
qddd = -j_max
elif (self.T-self.Td+self.Tj2) <= t < (self.T-self.Tj2):
q = q1 - (self.vlim+v1)*(self.Td/2) + self.vlim*(t-self.T+self.Td) -self.alimd/6*(3*((t-self.T+self.Td)**2) - 3*self.Tj2*(t-self.T+self.Td) + self.Tj2**2)
qd = self.vlim - self.alimd * ((t-self.T+self.Td) - self.Tj2 / 2)
qdd = -self.alimd
qddd = 0
elif (self.T-self.Tj2) <= t <= self.T:
q = q1 - v1*(self.T-t) + j_max*(((self.T-t)**3)/6)
qd = v1 - j_max * ((self.T-t) ** 2) / 2
qdd = j_max * (self.T-t)
qddd = j_max
else:
if (self.T - self.Td) <= t < (self.T - self.Td + self.Tj2):
q = q1 - (self.vlim + v1) * self.Td / 2 + self.vlim * (t - self.T + self.Td) - j_max * (
(t - self.T + self.Td) ** 3) / 6
qd = self.vlim - j_max * ((t - self.T + self.Td) ** 2) / 2
qdd = -j_max * (t - self.T + self.Td)
qddd = -j_max
elif (self.T - self.Td + self.Tj2) <= t < (self.T - self.Tj2):
q = q1 - (self.vlim + v1) * (self.Td / 2) + self.vlim * (t - self.T + self.Td) + self.alimd / 6 * (
3 * ((t - self.T + self.Td) ** 2) - 3 * self.Tj2 * (
t - self.T + self.Td) + self.Tj2 ** 2)
qd = self.vlim + self.alimd * ((t - self.T + self.Td) - self.Tj2 / 2)
qdd = self.alimd
qddd = 0
elif (self.T - self.Tj2) <= t <= self.T:
q = q1 - v1 * (self.T - t) - j_max * (((self.T - t) ** 3) / 6)
qd = v1 + j_max * ((self.T - t) ** 2) / 2
qdd = -j_max * (self.T - t)
qddd = j_max
# else:
# qdd = 0
# qd= v1
# q = q1
# qddd = 0
q_list.append(q)
v_list.append(qd)
a_list.append(qdd)
j_list.append(qddd)
with open("data.csv", 'w+') as f:
f.write(str( q_list))
f.close()
return q_list, v_list, a_list, j_list, t_list
该代码可以正常正向运行,参数合理的情况下。如果要反向运行,可以修改一下运行机制,在运行前做判断,当给定的初始位置小于目标位置,那么正常运行,当初始位置大于目标位置,则交换这两个位置,速度,加速度,加加速则取绝对值(都为正,位置可以为负数),然后计算出来T,这时候的点位从后往前取点,也就是T从本身开始减,并不是从0开始往上加。