技術(shù)特征：

1.一種初加速度不為零的S型曲線速度控制方法，其特征在于：所述方法分解為三個(gè)層面，即速度規(guī)劃層、步進(jìn)規(guī)劃層和位置規(guī)劃層；

其中速度規(guī)劃層根據(jù)給定的起始速度v₀、末點(diǎn)速度v_e、起始加速度a₀、最大加加速度j_max和最大加速度a_max，進(jìn)行變速規(guī)劃，得到變速過程的加加速截止時(shí)間t₁、勻加速截止時(shí)間t₂、減加速截止時(shí)間t₃以及當(dāng)前的加加速度j；

步進(jìn)規(guī)劃層根據(jù)速度規(guī)劃層計(jì)算結(jié)果，計(jì)算任意時(shí)刻t時(shí)的位置x_r、速度v_rx和加速度a_rx；位置規(guī)劃層根據(jù)速度規(guī)劃層和步進(jìn)規(guī)劃層的計(jì)算結(jié)果、起始點(diǎn)位置x₀、末點(diǎn)位置x_e以及最大速度v_max，計(jì)算當(dāng)前起始點(diǎn)的下一步運(yùn)動(dòng)位置、速度和加速度，并保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于v_e；當(dāng)當(dāng)前段鏈規(guī)劃長度不夠時(shí)，將當(dāng)前段鏈鏈接下一條段鏈，并根據(jù)鏈接后段鏈總長度重新計(jì)算。

2.根據(jù)權(quán)利要求1所述的初加速度不為零的S型曲線速度控制方法，其特征在于：設(shè)向量y₀＝[x₀ v₀ a₀]表示起始點(diǎn)位置、速度、加速度，設(shè)向量y_e＝[x_e v_e 0]表示末點(diǎn)位置、速度、加速度，設(shè)向量y₁＝[x₁ v₁ a₁]表示下一步運(yùn)動(dòng)位置、速度、加速度，設(shè)向量y＝[x v a]為預(yù)測的運(yùn)動(dòng)位置、速度、加速度，設(shè)給定的最大速度為v_max，所述位置規(guī)劃層包括以下步驟：

步驟1：針對一條段鏈，如果末點(diǎn)位置x_e與起始點(diǎn)位置x₀之差ΔL小于一常量Ⅰδ₁，即ΔL＝x_e-x₀＜δ₁，且末點(diǎn)速度v_e與起始點(diǎn)速度v₀之差的絕對值|Δv|小于一常量Ⅱδ₂，即|Δ_v|＝|v_e-v₀|＜δ₂，則設(shè)置下一步運(yùn)動(dòng)向量y₁為末點(diǎn)向量y_e，即y₁＝y(tǒng)_e，返回參數(shù)y₁；否則進(jìn)入步驟2；

步驟2：以v₀為起始速度，a₀為起始加速度，v_max為末點(diǎn)速度調(diào)用速度規(guī)劃層函數(shù)，并按照計(jì)算結(jié)果調(diào)用步進(jìn)規(guī)劃層函數(shù)往前預(yù)測一步，得y₁；接著以y₁為起始點(diǎn)向量，y_e為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測達(dá)到末點(diǎn)速度v_e所需要運(yùn)行到的位置x，如果x≤x_e，表明下一步運(yùn)動(dòng)到y(tǒng)₁可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于v_e，因此y₁即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y₁；否則進(jìn)入步驟3；

步驟3：如果步驟2往前預(yù)測一步使用的加加速度j＝j(luò)_max，則轉(zhuǎn)到步驟4，否則轉(zhuǎn)到步驟5；

步驟4：以勻加速即j＝0調(diào)用步進(jìn)規(guī)劃層函數(shù)往前預(yù)測一步，得y₁；接著以y₁為起始點(diǎn)向量，y_e為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測達(dá)到末點(diǎn)速度v_e所需要達(dá)到的位置x，如果x≤x_e，表明下一步運(yùn)動(dòng)到y(tǒng)₁可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度不大于v_e，因此y1即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y1；否則進(jìn)入步驟5；

步驟5：以y₀為起始點(diǎn)向量，y_e為末點(diǎn)向量調(diào)用速度規(guī)劃層函數(shù)和步進(jìn)規(guī)劃層函數(shù)往前預(yù)測一步，得y₁，并往前預(yù)測達(dá)到末點(diǎn)速度v_e所需要達(dá)到的位置x，如果x≤x_e，表明下一步運(yùn)動(dòng)到y(tǒng)₁可以保證最終運(yùn)動(dòng)到末點(diǎn)時(shí)速度等于v_e，因此y₁即為下一步運(yùn)動(dòng)目標(biāo)，返回參數(shù)y₁；否則進(jìn)入步驟6；

步驟6：當(dāng)前段鏈規(guī)劃長度不夠，將當(dāng)前段鏈鏈接下一條段鏈，并返回步驟2。

3.根據(jù)權(quán)利要求2所述的初加速度不為零的S型曲線速度控制方法，其特征在于：設(shè)置常量Ⅰδ₁＝0.5(a_max-a₀)T²，設(shè)置常量Ⅱδ₂＝J_maxT²。

4.根據(jù)權(quán)利要求2所述的初加速度不為零的S型曲線速度控制方法，其特征在于：設(shè)v₀為起始速度，v_e為S型速度規(guī)劃結(jié)束速度，首末速度差Δv＝v₀-v_e，a₀為起始加速度，j_max為給定最大加加速度，a_max為給定最大加速度，標(biāo)志速度常量v_x＝Δv×flag，加速度常量a_x＝a₀×flag；所述速度規(guī)劃層函數(shù)包括以下步驟，其中t1、t2、t3和j分別為加加速截止時(shí)間、勻加速截止時(shí)間、減加速截止時(shí)間和當(dāng)前的加加速度：

步驟A：當(dāng)a_x＜0，且時(shí)，取

$j=j_{\max },t_1=\frac{-a_x+\sqrt{\frac{1}{2}{a_x}^2-j_{max}{v}_x}}{j_{max}},t_2=t_1,t_3=t_1+t_2+\frac{a_x}{j};$

步驟B：當(dāng)a_x≥0或時(shí)，

當(dāng)時(shí)，取j＝-j_max，

$t_1=\frac{a_x}{j_{max}}+\frac{\sqrt{\frac{1}{2}{a_x}^2+j_{max}{v}_x}}{j_{max}},t_2=t_1,t_3=t_1+t_2+\frac{a_x}{j};$

當(dāng)時(shí)，取j＝-j_max,

$t_1=\frac{a_x+a_{max}}{j_{\max }},t_2=\frac{v_x}{a_{max}}+\frac{1}{2}\frac{{a_x}^2}{a_{max}{j}_{max}}+\frac{a_x}{j_{\max }},t_3=t_1+t_2+\frac{a_x}{j};$

步驟C：更新給定加加速度，即取j＝j(luò)×flag。

5.根據(jù)權(quán)利要求4所述的初加速度不為零的S型曲線速度控制方法，其特征在于：設(shè)t為當(dāng)前時(shí)刻，所述步進(jìn)規(guī)劃層函數(shù)根據(jù)以下公式計(jì)算當(dāng)前時(shí)刻t的位置x_r、速度v_rx和加速度a_rx：

(1)當(dāng)t≤t₁時(shí)，

a_rx＝a₀+jt

$v_{rx}=v_0+a_{0}t+\frac{1}{2}{\mathrm{jt}}^2$

$x_r=x_0+v_{0}t+\frac{1}{2}{a}_0{t}^2+\frac{1}{6}{\mathrm{jt}}^3$

(2)當(dāng)t₁＜t≤t₂時(shí)，

a_rx＝a₀+jt₁

$v_{rx}=v_0+a_{0}t-\frac{1}{2}{{\mathrm{jt}}_1}^2+{\mathrm{jt}}_{1}t$

$x_r=x_0+v_{0}t+\frac{1}{2}{a}_0{t}^2+\frac{1}{6}{{\mathrm{jt}}_1}^3-\frac{1}{2}{{\mathrm{jt}}_1}^{2}t+\frac{1}{2}{\mathrm{jt}}_1{t}^2$

(3)當(dāng)t₂＜t≤t₃時(shí)，

a_rx＝a₀+jt₁-j(t-t₂)

$v_{rx}=v_0+a_{0}t+{\mathrm{jt}}_{1}t-\frac{1}{2}{{\mathrm{jt}}_1}^2-\frac{1}{2}j{(t-t_2)}^2$

$x_r=x_0+v_{0}t+\frac{1}{2}{a}_0{t}^2-\frac{1}{6}{\mathrm{jt}}^3+\frac{1}{6}j({t_1}^3+{t_2}^3)+\frac{1}{2}j(t_1+t_2)t^2-\frac{1}{2}j({t_1}^2+{t_2}^2)t$

(4)當(dāng)t≥t₃時(shí)，

$\begin{array}{c}{a}_{rx}=a_0+{\mathrm{jt}}_1-j(t_3-t_2)\\ v_{rx}=v_0+a_0{t}_3+{\mathrm{jt}}_1{t}_3-\frac{1}{2}{{\mathrm{jt}}_1}^2-\frac{1}{2}j{(t_3-t_2)}^2\\ x_r=x_0+v_0{t}_3+\frac{1}{2}{a}_0{t_3}^2-\frac{1}{6}{{\mathrm{jt}}_3}^3+\frac{1}{6}j({t_1}^3+{t_2}^3)+\frac{1}{2}j(t_1+t_2){t_3}^2-\frac{1}{2}j({t_1}^2+{t_2}^2)t_3\end{array}.$