本發(fā)明涉及機器人控制技術(shù)領(lǐng)域，特別涉及一種速度規(guī)劃方法。

背景技術(shù)：

機器人屬于多軸機構(gòu)。在執(zhí)行任務(wù)時，機器人各軸之間往往需要進行同步，即，同時到達終點。為了實現(xiàn)同步，往往先對所有軸按照各自的特性及系統(tǒng)要求分別進行速度規(guī)劃，然后選取時間最長者作為基準軸，對其他軸按照基準軸進行同步。常見的用于機器人同步的方法主要有兩種：(1)基于位移比例的同步方法，(2)給定時間的S型速度規(guī)劃方法。

在實現(xiàn)本發(fā)明的過程中，發(fā)明人發(fā)現(xiàn)現(xiàn)有技術(shù)中至少存在以下問題：基于位移比例的同步方法，只能對單段進行同步，不同段之間的轉(zhuǎn)折點速度及加速度需降為零，否則會引起速度突變，導(dǎo)致機械抖動，產(chǎn)生機械損傷。S型速度規(guī)劃方法，需要大量的計算及迭代，實時性差。

技術(shù)實現(xiàn)要素：

本發(fā)明實施方式的目的在于提供一種速度規(guī)劃方法，該方法可用于機器人各關(guān)節(jié)軸之間的同步，亦可用于位置與姿態(tài)之間的同步，可解決同步過程中始末速度不為零的同步問題，并且，計算量較小，實時性較高。

為解決上述技術(shù)問題，本發(fā)明的實施方式提供了一種速度規(guī)劃方法，包括：根據(jù)第一約束條件，對多軸機構(gòu)的所有軸進行S型速度規(guī)劃；其中，第一約束條件包括運動的路程、起點速度、末點速度、起點加速度、末點加速度、運行速度、最大運行速度、最大加速度以及最大加加速度；從規(guī)劃結(jié)果中，選取時間最長的軸作為基準軸；根據(jù)第二約束條件，以三次B樣條曲線為規(guī)劃曲線，對其余的軸重新進行給定時間的速度規(guī)劃；其中，第二約束條件包括運動的路程、起點速度、末點速度、起點加速度、末點加速度、最大運行速度、最大加速度、最大加加速度以及基準軸的總運行時間。

本發(fā)明實施方式相對于現(xiàn)有技術(shù)而言，根據(jù)第二約束條件，以三次B樣條曲線為規(guī)劃曲線，對多軸機構(gòu)的基準軸以外的其余的軸進行給定時間的速度規(guī)劃，其中，第二約束條件可以包括運動的路程、起點速度等，這樣，各軸不同段之間的轉(zhuǎn)折點的速度以及加速度不需要降為零。這樣，規(guī)劃過程中的計算量較小，且實時性較高。

另外，根據(jù)第二約束條件，并以三次B樣條曲線為規(guī)劃曲線，對其余的軸重新進行給定時間的速度規(guī)劃具體包括：根據(jù)所述第二約束條件，并以三次準均勻B樣條曲線為規(guī)劃曲線，對基準軸以外的其余的軸進行給定時間的速度規(guī)劃。本實施方式中，以三次準均勻B樣條曲線為規(guī)劃曲線，使規(guī)劃曲線更簡單，速度、加速度更加平滑，規(guī)劃過程更容易實現(xiàn)。

另外，規(guī)劃曲線為六個控制點的三次準均勻B樣條曲線；六個控制點的三次準均勻B樣條曲線的表達式為：

其中，t為時間，s為路程，u為節(jié)點，B_i(u)為基函數(shù)，cp_i為控制點；i為控制點在規(guī)劃曲線上的次序。本實施方式中，規(guī)劃曲線上包括六個控制點，并且給出六個控制點的三次準均勻B樣條曲線的表達式，使得規(guī)劃過程中，計算量較小，規(guī)劃過程更容易實現(xiàn)。

另外，根據(jù)第二約束條件，并以三次B樣條曲線為規(guī)劃曲線，對其余的軸重新進行給定時間的速度規(guī)劃具體包括：根據(jù)第二約束條件與規(guī)劃曲線，獲取6個控制點信息；控制點信息包括控制點所對應(yīng)的時間與路程；根據(jù)獲取的控制點信息與規(guī)劃曲線，獲取各時刻所對應(yīng)的位置、速度與加速度。本實施方式中，通過獲取6個控制點信息，并根據(jù)獲取的控制點信息與規(guī)劃曲線，可以獲取各時刻所對應(yīng)的位置、速度與加速度，進而，可以獲取多軸的位置、速度等信息，有利于多軸同步過程的實現(xiàn)。

附圖說明

圖1是根據(jù)本發(fā)明第一實施方式中的給定時間速度規(guī)劃示意圖；

圖2是根據(jù)本發(fā)明第一實施方式的速度規(guī)劃方法的流程圖；

圖3是根據(jù)本發(fā)明第二實施方式的速度規(guī)劃方法的流程圖；

圖4是根據(jù)本發(fā)明第二實施方式的速度規(guī)劃方法的流程圖；

圖5是根據(jù)本發(fā)明第三實施方式的起始點的加速度大于零時，可行域及最優(yōu)解的示意圖；

圖6是根據(jù)本發(fā)明第三實施方式的起始點的加速度小于或者等于零時，可行域及最優(yōu)解的示意圖。

具體實施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚，下面將結(jié)合附圖對本發(fā)明的各實施方式進行詳細的闡述。然而，本領(lǐng)域的普通技術(shù)人員可以理解，在本發(fā)明各實施方式中，為了使讀者更好地理解本申請而提出了許多技術(shù)細節(jié)。但是，即使沒有這些技術(shù)細節(jié)和基于以下各實施方式的種種變化和修改，也可以實現(xiàn)本申請所要求保護的技術(shù)方案。

本發(fā)明實施方式中，時間t與位移s的規(guī)劃曲線采用樣條曲線，如圖1所示，根據(jù)位移隨時間的變化曲線101，可以得出包括控制點103的控制多邊形102，圖1中以六個控制點103為例。具體地說，在進行速度規(guī)劃時，首先需要給出始末點的位置、速度、加速度及時刻，然后根據(jù)約束條件，求出控制點，進而，得到相應(yīng)的時間與位移。

本發(fā)明的第一實施方式涉及一種速度規(guī)劃方法，如圖2所示，包括：

步驟201：根據(jù)第一約束條件，對多軸機構(gòu)的所有軸進行S型速度規(guī)劃，其中，第一約束條件包括運動的路程、起點速度、末點速度、起點加速度、末點加速度、運行速度、最大運行速度、最大加速度以及最大加加速度。

步驟202：從規(guī)劃結(jié)果中，選取時間最長的軸作為基準軸。具體地說，從規(guī)劃結(jié)果中，選取出運行所用時間最長的軸，并將該軸作為基準軸。

步驟203：根據(jù)第二約束條件，以三次B樣條曲線為規(guī)劃曲線，對其余的軸重新進行給定時間的速度規(guī)劃，其中，第二約束條件包括運動的路程、起點速度、末點速度、起點加速度、末點加速度、最大運行速度、最大加速度、最大加加速度以及基準軸的總運行時間。

具體地說，將基準軸的總運行時間作為約束時間，并以運動的路程、起點速度、末點速度、起點加速度、末點加速度、最大運行速度、最大加速度、最大加加速度以及基準軸的總運行時間作為第二約束條件，對其余的軸重新進行給定時間的速度規(guī)劃。

本發(fā)明實施方式相對于現(xiàn)有技術(shù)而言，根據(jù)第二約束條件，以三次B樣條曲線為規(guī)劃曲線，對多軸機構(gòu)的基準軸以外的其余的軸進行給定時間的速度規(guī)劃，其中，第二約束條件可以包括運動的路程、起點速度等，這樣，各軸不同段之間的轉(zhuǎn)折點的速度以及加速度不需要降為零。這樣，規(guī)劃過程中的計算量較小，且實時性較高。

本發(fā)明的第二實施方式涉及一種速度規(guī)劃方法，本實施方式在第一實施方式的基礎(chǔ)上做了進一步細化，給出了一種更為具體的根據(jù)第二約束條件，以三次B樣條曲線為規(guī)劃曲線，對其余的軸重新進行給定時間的速度規(guī)劃的方法，如圖3所示，包括：

步驟301：根據(jù)第一約束條件，對多軸機構(gòu)的所有軸進行S型速度規(guī)劃，其中，第一約束條件包括運動的路程、起點速度、末點速度、起點加速度、末點加速度、運行速度、最大運行速度、最大加速度以及最大加加速度。

步驟302：從規(guī)劃結(jié)果中，選取時間最長的軸作為基準軸。

具體地說，從規(guī)劃結(jié)果中，選取出運行所用時間最長的軸，并將該軸作為基準軸。

步驟303：根據(jù)第二約束條件，以三次準均勻B樣條曲線為規(guī)劃曲線，對基準軸以外的其余的軸進行給定時間的速度規(guī)劃。其中，規(guī)劃曲線可以為六個控制點的三次準均勻B樣條曲線。六個控制點的三次準均勻B樣條曲線的表達式可以為：

其中，t為時間，s為路程，u為節(jié)點，B_i(u)為基函數(shù)，cp_i為控制點；i為控制點在所述規(guī)劃曲線上的次序。

更具體地說，步驟303中還可以包括以下子步驟，具體如圖4所示。

步驟401：根據(jù)節(jié)點的個數(shù)、控制點個數(shù)以及曲線次數(shù)之間的關(guān)系，獲取規(guī)劃曲線的節(jié)點矢量。

具體地說，根據(jù)節(jié)點u的個數(shù)與控制點個數(shù)、曲線次數(shù)之間的關(guān)系，可以得到規(guī)劃曲線的節(jié)點矢量為：

U＝[0 0 0 0 1/3 2/3 1 1 1 1]

步驟402：根據(jù)節(jié)點矢量，獲取節(jié)點分別為0與1時，基函數(shù)與基函數(shù)的導(dǎo)數(shù)值。其中，基函數(shù)的導(dǎo)數(shù)值包括：基函數(shù)的一階導(dǎo)數(shù)值與基函數(shù)的二階導(dǎo)數(shù)值。

具體地說，節(jié)點u為0與1時，基函數(shù)、基函數(shù)的一階導(dǎo)數(shù)值以及基函數(shù)的二階導(dǎo)數(shù)值如下表所示：

預(yù)設(shè)起點時刻、起點位置均為0，速度為v_s，加速度為a_s，末點的時刻為t，位置為s，速度為v_e，加速度為0。對于起點，在節(jié)點u為0時，基函數(shù)值為(0,0)，基函數(shù)的一階導(dǎo)數(shù)值為基函數(shù)的二階導(dǎo)數(shù)值為其中，

步驟403：根據(jù)約束條件、基函數(shù)及基函數(shù)的導(dǎo)數(shù)值，獲取6個控制點信息。其中，控制點信息包括控制點所對應(yīng)的時間與路程。

具體地說，根據(jù)約束條件、基函數(shù)、基函數(shù)的一階導(dǎo)數(shù)與基函數(shù)的二階導(dǎo)數(shù)，可以計算得出前三個控制點依次為：第一個控制點為(0,0)，第二個控制點為第三個控制點為

由于末點加速度為0，根據(jù)規(guī)劃曲線中路程隨著時間的增加而增加的規(guī)律，后三個控制點可以按照線性方法直接給定。后三個控制點可以依次為：第四個控制點為(s-k₁*v_e,t-k₁)，第五個控制點為(s-k₂*v_e,t-k₂)，其中，k₁、k₂為常數(shù)，k₁>k₂>0，第六個控制點為(s,t)。

步驟404：根據(jù)獲取的控制點信息與規(guī)劃曲線，獲取各時刻所對應(yīng)的位置、速度與加速度。

具體地說，根據(jù)獲取的控制點信息與規(guī)劃曲線，可以獲取規(guī)劃所得樣條曲線，并且可以獲取各時刻所對應(yīng)的節(jié)點，進一步，將獲取的節(jié)點帶入規(guī)劃所得樣條曲線中，可以獲得各時刻所對應(yīng)的位置、速度與加速度。

更具體地說，由反函數(shù)與原函數(shù)之間的關(guān)系可得：

其中，與可直接由樣條曲線得到，因此，根據(jù)二階泰勒展式，可以得到節(jié)點u：

其中，u₀為上一時刻的節(jié)點u，Δt為這一時刻與上一時刻的時間差。進一步，可以將計算得出的節(jié)點u代入規(guī)劃所得樣條曲線，這樣，就可以得出各時刻所對應(yīng)的位置，并且根據(jù)可以計算得出各時刻所對應(yīng)的速度，根據(jù)可以計算得出各時刻所對應(yīng)的加速度。

此外，當末點加速度不為0時，可以類似前三個控制點的求解方法，根據(jù)第二約束條件與規(guī)劃曲線，求解得出第四個控制點、第五個控制點與第六個控制點。

此外，當起點加速度a_s為0時，可以根據(jù)規(guī)劃曲線中路程隨著時間的增加而增加的規(guī)律，按照線性方法，給定第一個控制點、第二個控制點與第三個控制點。

本發(fā)明實施方式中，以三次準均勻B樣條曲線為規(guī)劃曲線，使規(guī)劃曲線更簡單。規(guī)劃曲線上包括六個控制點，并且給出六個控制點的三次準均勻B樣條曲線的表達式，使得規(guī)劃過程中，計算量較小，規(guī)劃過程更容易實現(xiàn)。本實施方式通過獲取6個控制點信息與規(guī)劃曲線，獲取各時刻所對應(yīng)的位置、速度與加速度，從而，可以獲取多軸的位置、速度等信息。這樣，有利于多軸同步過程的實現(xiàn)。

本發(fā)明的第三實施方式涉及一種速度規(guī)劃方法，第三實施方式在第二實施方式的基礎(chǔ)上作了進一步的改進，改進之處主要在于：本發(fā)明第三實施方式，對控制點進行了進一步的優(yōu)化，如圖5及圖6所示。

第二實施方式中獲得的六個控制點依次為：第一個控制點為(0,0)，第二個控制點為第三個控制點為第四個控制點為(s-k₁*v_e,t-k₁)，第五個控制點為(s-k₂*v_e,t-k₂)，其中，k1、k2為常數(shù)，k1>k2>0，第六個控制點為(s,t)。由于上述控制點的表達式中與不唯一確定，因此，六個控制點的值同樣不唯一確定。本實施方式中，根據(jù)規(guī)劃曲線中路程隨著時間的增加而增加的規(guī)律，對上述控制點作了進一步的優(yōu)化。

根據(jù)第一個控制點與第二個控制點，得出第一不等式；其中，第一不等式為根據(jù)第二個控制點與第三個控制點，得出第二不等式與第三不等式；其中，第二不等式為第三不等式為根據(jù)第三個控制點與第四個控制點，得出第四不等式與第五不等式；其中，第四不等式為第五不等式為其中，第四個控制點為(cp4_t,cp4_s)。

當起點加速度a_s大于0時，與的可行域如圖5所示。

第一可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第一可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第一可行域為滿足第一不等式、第二不等式與第五不等式的可行域。

第二可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第二可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第二可行域為滿足第一不等式、第二不等式、第四不等式與第五不等式的可行域。

第三可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第三可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第三可行域為滿足第一不等式、第二不等式與第四不等式的可行域。

當起點加速度a_s小于等于0時，與的可行域如圖6所示。

第四可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第四可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第四可行域為滿足第一不等式、第三不等式與第四不等式的可行域。

第五可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第五可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第五可行域為滿足第一不等式、第三不等式、第四不等式與第五不等式的可行域。

第六可行域的中心點的橫坐標可以作為的第一最優(yōu)解，第六可行域的中心點的縱坐標可以作為的第一最優(yōu)解。其中，第六可行域為滿足第一不等式、第三不等式與第五不等式的可行域。

此外，上述最優(yōu)解也可以采用其他優(yōu)化方法對其他指標進行優(yōu)化得到。

本發(fā)明實施方式中，根據(jù)規(guī)劃曲線中路程隨著時間的增加而增加的規(guī)律，獲得多個不等式，進而，獲得控制點需要滿足的可行域，并選用可行域的中心點作為最優(yōu)解。這樣，得到的控制點唯一確定，且各個控制點相對分散，同時，得到的運行軌跡更加平滑。

此外，本發(fā)明實施方式中，還可以根據(jù)第二約束條件，并以三次非均勻B樣條曲線為規(guī)劃曲線，對多軸機構(gòu)的基準軸以外的其余的軸進行給定時間的速度規(guī)劃。

上面各種方法的步驟劃分，只是為了描述清楚，實現(xiàn)時可以合并為一個步驟或者對某些步驟進行拆分，分解為多個步驟，只要包含相同的邏輯關(guān)系，都在本專利的保護范圍內(nèi)；對算法中或者流程中添加無關(guān)緊要的修改或者引入無關(guān)緊要的設(shè)計，但不改變其算法和流程的核心設(shè)計都在該專利的保護范圍內(nèi)。

本領(lǐng)域的普通技術(shù)人員可以理解，上述各實施方式是實現(xiàn)本發(fā)明的具體實施例，而在實際應(yīng)用中，可以在形式上和細節(jié)上對其作各種改變，而不偏離本發(fā)明的精神和范圍。