3rd S-curve velocity profile

3rd S-curve

 (1)

(2)

(3)

 

(4)

 完整的三次S曲線包括上面的七個階段。前面三個階段爲加速階段，從初始速度Vs加速到Vmax:

(5)

整個加速階段的位移爲：

(6)

後面三個階段爲減速階段：

(7)

(8)

也能夠看做爲反向加速階段，即速度從最終速度Ve加速到Vmax:

(9)

(10)

中間的階段爲勻速階段：

 (11)

可是在實際中，受限於Vs,Ve,以及位移L，整條速度曲線並不包含完整的七個階段，一般Jerka = Jerkd = Jerk。那麼求速度曲線能夠轉換爲如下數學問題：已知Vs,Ve,L,Amax,Jerk,Fmax，求解下面的四元三次方程組：

(12)

 其中,Sa和Sd分別由式(6)和(10)計算，而且須要知足如下限制條件：

(13)

這是一個非齊次的非線性的方程組。四個未知數，可是隻有兩個方程以及一些限制條件。給定初值而後利用迭代法計算也許是求解的一種途徑。在設定初值上，能夠分狀況進行討論。流程圖以下圖所示。

首先使用Vs,Ve,Fmax,Amax,Jerk的值計算T1,T2,T5,T6。計算過程當中先假設T2=0，計算速度從Vs達到Fmax所需的時間T1，若是T1<Amax/Jerk，則沒有上面的第二個階段，即加速度勻速的階段。若是T1>Amax/Jerk,則T1會受到最大加速度的限制，即T1=Amax/Jerk,T2=(Fmax-Vs)/(Jerk*T1)-T1，而且根據T1,T2經過式(6)能夠計算出加速階段的位移Sa。同理能夠計算出T5,T6和Sd。

若是Sa+Sd>=L，則說明最大速度能夠保持勻速一段時間T4，T4=(L-Sa-Sd)/Fmax

若是Sa+Sd<L，則說明T4=0，且整條速度曲線的峯值達不到最大速度Fmax。所以問題轉換爲以下的數學問題：

(14)

求T1,T2,T5,T6。

當Vs=Ve時，T1=T5,T2=T6，則求解式(14)就變成求解式(15)的解：

(15)

也是先假設T2=0，則式(15)是一個關於未知數T1的一元三次方程，且因其判別式大於零，其有惟一解。所以能夠求得T1。若T1<Amax/Jerk,則T2=0,若T1>Amax/Jerk,則T1=Amax/Jerk,而後再經過式(15)計算T2。

當Vs≠Ve時，方程組有四個未知數，求不到其惟一解。所以本文中採用的是Python.scipy庫中的求最小值的問題來計算T1,T2,T5,T6。即解決以下數學問題：

求T=[T1,T2,T5,T6]，使得

(16)

值最小，其中，Vmax=Vs+J*T1*(T1+T2)。並知足以下的條件：

(17)

下面爲這段python代碼：

import numpy as np
from scipy.optimize import minimize
import math

def motion_profile(args):
    vs,J,Fmax,L,t = args
    s=lambda x: (vs*t*(2*x[0]+x[1])+J*x[0]*math.pow(t,3)*(2*x[0]+x[1])*(x[0]+x[1])+(vs+J*x[0]*math.pow(t,2)*(x[0]+x[1]))*(2*x[2]+x[3])*t-J*x[2]*math.pow(t,3)*(2*x[2]+x[3])*(x[2]+x[3])-L)**2
    return s

def cons(args):
    Fmax,ve,Amax,J,vs,t = args
    cons = ({'type':'ineq',
             'fun':lambda x:np.array([Amax/J - x[0]*t,
                                      x[0],
                                      x[1],
                                      x[2],
                                      x[3],
                                      Amax/J - x[2]*t,
                                      ve-vs-J*(x[0]*t)**2 -J*x[0]*t*x[1]*t + J*x[2]*t*x[3]*t + J*(x[2]*t)**2,
                                      Fmax-ve-J*x[2]*math.pow(t,2)*(x[2]+x[3]),
                                      vs + J*(x[0]*t)**2 + J*x[0]*t*x[1]*t - J*x[2]*t*x[3]*t -J*(x[2]*t)**2])})
    return cons

def optimization(args,args1,x0):
    conditions = cons(args1)
    res = minimize(motion_profile(args),x0,method='SLSQP',constraints = conditions)

    time_list = []
    time_list.append(res.x[0])
    time_list.append(res.x[1])
    time_list.append(res.x[2])
    time_list.append(res.x[3])
    return time_list

整個代碼能夠見github:

https://github.com/Larissa1990/S-curve-Velocity-Profile

Example1:

Vs=40,Ve=35,Fmax=80,Amax=2000,Jerk=80000,L=5,interpolation_period=0.002

T1=0.022,T2=0,T4=0,T5=0.016,T6=0.02

Example2:

Vs=35,Ve=35,Fmax=80,Amax=2000,Jerk=80000,L=5,interpolation_period=0.002

 

T1=0.024,T2=0,T4=0,T5=0.024,T6=0