七段式s轨迹分析及在不同情况下的仿真

最新推荐文章于 2024-07-19 13:59:11 发布

微不足_道

最新推荐文章于 2024-07-19 13:59:11 发布

阅读量303

点赞数 1

文章标签：算法 python

本文链接：https://blog.csdn.net/weixin_43321137/article/details/131115704

版权

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开始往上加。