基于時間最優(yōu)的機器人多路徑點軌跡規(guī)劃

張華文，劉子亮

（1.河南省工業(yè)機器人應用工程技術研究中心，南陽 473000；2.河南工業(yè)職業(yè)技術學院 自動化工程學院，南陽 473000；3.天地上海采掘裝備科技有限公司，上海 201401）

為了避免機器人運動過程中發(fā)生碰撞，往往需要在起始點和終止點之間插入一些中間點來約束機器人的運動。 合理的軌跡規(guī)劃可使機器人運行平滑，減少機械沖擊，同時讓機器人在通過這些中間點時不用停止，達到最優(yōu)的運行時間，提高工作效率[1]。 文獻[2]構造了關節(jié)空間下經(jīng)過中間點的五次多項式軌跡規(guī)劃函數(shù)，并利用MATLAB 仿真計算出機器人運動過程中各關節(jié)參數(shù)-時間曲線， 但運行過程中加速度變化并不平滑。 文獻[3]對五次多項式函數(shù)插值方法中軌跡中間點角速度的設置方法提出改進， 能使關節(jié)角速度和角加速度更加平滑，但是沒有考慮運行時間最優(yōu)的問題。 文獻[4]以工業(yè)機器人搬運作業(yè)為研究對象，完成了關節(jié)空間中的軌跡規(guī)劃實施過程，并就多項式插值的效果進行了對比，但在中間點各關節(jié)速度都降為零，降低了運行效率。 文獻[5]和文獻[6]基于遺傳算法和高次多項式來實現(xiàn)時間最優(yōu)的軌跡規(guī)劃，提高了軌跡跟蹤精度，但是算法較為復雜，增加系統(tǒng)的運算負擔。

本文采用基于加速度連續(xù)過渡的線性規(guī)劃方法，將路徑點用直線連接起來，每兩個路徑點之間的運動看作是勻速運動，每個運動段之間用加速度連續(xù)變化的運動進行過渡。 該算法只需向前看一個路徑點就可以將整條運動軌跡計算出來， 計算量小，便于編程實現(xiàn)。

1 兩點運動的軌跡規(guī)劃

用qi和qf分別表示起始和終止時刻關節(jié)角度向量和分別表示第j 個關節(jié)起始和終止時刻的關節(jié)角，kv和ka分別表示關節(jié)最大速度向量和關節(jié)最大加速度向量，kvj和kaj分別表示第j 個關節(jié)的最大速度和最大加速度。 每個關節(jié)的最大速度和加速度通?？梢愿鶕?jù)該關節(jié)的電機參數(shù)得到。

在qi和qf之間的軌跡規(guī)劃可以用式（1）表示：

式中：D=qf-qi。 規(guī)劃算法的目標是求出r（t）的表達式。

1.1 關節(jié)同步系數(shù)的計算

首先對梯形速度運動進行分析，位移表達式如式（2）所示，其中τj表示加速段時間。

對速度進行時間的積分，得到梯形的面積，即為運動的距離，如式（3）所示：

求得關節(jié)j 最小運行時間為

所有關節(jié)需要在相同的時間內(nèi)完成各自的運動，為了達到同步的效果，每個關節(jié)需要根據(jù)運動距離、關節(jié)能夠達到的最大速度和加速度來調(diào)整運行時的最大速度和加速度[7]。 設定任意一個關節(jié)為參考關節(jié)，然后為每個關節(jié)設定最大速度同步系數(shù)λj和最大加速度同步系數(shù)υj。 關節(jié)j 的最大同步速度為λjKvj，最大同步加速度為υjKaj。 下文先以兩個關節(jié)為例說明同步系數(shù)的求解。

兩個關節(jié)的最小運行時間為

同步后的運行時間為

根據(jù)式（6）可得：

最大同步速度不可能超過最大運行速度，因此最大速度同步系數(shù)需滿足：

根據(jù)式（7）可得：

與之對應的加速時間為

推廣到n 個關節(jié)可得：

根據(jù)式（7）可以得到其他所有關節(jié)的同步系數(shù)。

1.2 二次拋物線加速度的軌跡規(guī)劃

與梯形速度運動相似，二次拋物線加速度運動也分為加速、勻速和減速3 個階段。 但是其加速度不是恒定的， 而是關于時間的二次拋物線函數(shù)，如圖1 所示。 這樣，保證了加速度、速度和位移的連續(xù)，有效地減少沖擊，確保機器人的平滑運動。

圖1 二次拋物線加速度運動模型Fig.1 Quadratic parabola acceleration motion model

根據(jù)條件可知，在加速運動段，關節(jié)j 的位移、速度和加速度應滿足以下5 個條件：

由這5 個限制條件，可以推出位移、速度和加速度關于時間的表達式：

式中：1≤j≤n，0≤t<τ′。 加速度在t=τ′/2 處取得最大值：

由式（15）可知，二次拋物線加速度運動的加速時間是梯形速度運動加速時間的1.5 倍。 在勻速時間段h′中，位移關于時間的表達式如下：

式中：τ′≤t<τ′+h′。

比如，某學生屢屢遲到，傳統(tǒng)做法可能是，了解學生遲到原因，幫助學生找到避免遲到的方法，同時輔以必要的懲戒。而設計思維的做法是引導學生對人生有目標，對學習感興趣，對班級產(chǎn)生歸屬感，對交往有渴望等，然后再根據(jù)這些更高層次的目標設計解決方案。

減速段和加速段是對稱的，即：tf′=2τ′+h′。

根據(jù)式（13）和式（15）可得加速段運動的距離為

得到整段的運行時間為

為了能達到最大的同步速度，運行距離必須滿足：

如果該條件不能滿足，必須降低最大運行速度，新的最大運行速度為

2 多路徑點的軌跡規(guī)劃

為了避免機器人運動過程中發(fā)生碰撞，往往需要在起始點和終止點之間插入一些中間點來約束機器人的運動。 使用q1，q2，...，qm表示一段軌跡中的所有路徑點。 根據(jù)第1 節(jié)中的方法，假設過每個路徑點的速度都為0，可以計算得到同步系數(shù)和加速段時間τk′，其中k 代表由qk和qk+1所組成的段。

假設第k 段之間是勻速運動，則第k 段的運行時間hk為

為了生成一條在中間路徑點不停止的平滑軌跡，需要在k-1 段和k 段之間進行平滑過渡，如圖2所示。 在第k 個路徑點的過渡段的時間為2Tk。 為了保證加速度的連續(xù)，需要用一個二次函數(shù)來表示過渡段加速度的變化。 在過渡段，每個關節(jié)都以能夠達到的最大加速度運行，以保證軌跡能夠盡可能地逼近中間點。 過渡時間的表達式如下：

圖2 關節(jié)空間加速度連續(xù)過渡的線性規(guī)劃Fig.2 Interpolations with continuous acceleration blends in joint space

式中：k=2，3，…，m-1；j=1，2，…，n。

所有關節(jié)只在整個軌跡的起始點和終止點的過渡是同步的，根據(jù)式（15）可計算得到起始點和終止點的過渡時間Tkj=τ′/2。

為了方便表示，用Tk代表Tkj。如圖2 所示，關節(jié)j 在第k 段中勻速段的位移關于時間的表達如下：

考慮第k 個點的過渡段，有以下幾個限制條件：

根據(jù)上述條件，可以得到關節(jié)j 在第k 段中過渡段的位移關于時間的表達式是一個四次多項式，表達式如下：

式中：tk≤t

速度和加速度關于時間的表達式如下：

加速度在t=tk+Tk處取得最大值：

根據(jù)最大加速度kaj，該表達式可用來計算Tk。需要注意的是，必須滿足hk≥Tk+Tk+1。如果該條件不滿足，需要降低該段的運行速度，重新進行規(guī)劃。

3 算法設計

過路徑點的關節(jié)空間的軌跡規(guī)劃算法流程如圖3 所示。

圖3 多路徑點的關節(jié)空間的軌跡規(guī)劃算法流程Fig.3 Flow chart of trajectory planning with via points in joint space

算法在實現(xiàn)時有以下幾個關鍵點：

（1）必須保證每個關節(jié)運動的同步。

（2）需要判斷每段的距離是否滿足hk≥Tk+Tk+1，如果不滿足，則重新計算該段的運行速度，重新進行規(guī)劃。 可能會出現(xiàn)不論怎樣調(diào)整速度，都無法完成規(guī)劃的情況，此時程序便陷入死循環(huán)。 因此，需要設定循環(huán)次數(shù)上限，如果循環(huán)達到上限后，仍然無法完成規(guī)劃，則程序給出相應提示。

（3）在計算插補點時，為了確定時刻t 的位移，首先要確定t 所處的時間段。為了提高查找效率，可以使用二分法判斷t 所在的時間段。

4 實驗結(jié)果與分析

使用MATLAB 進行算法仿真驗證。分別在時刻0 s，0.2 s，0.7 s 處設置3 個示教點，如表1 所示。

表1 示教點參數(shù)Tab.1 Teaching point parameters

設定插補周期為0.01 s，可得到各關節(jié)角度、速度和加速度的變化曲線。 由圖4 可知，各關節(jié)位移變化平滑連續(xù)且運動同步，終止點位置精確。由圖5可知，各關節(jié)以逼近最大速度運行，變化連續(xù)且運行同步，初始速度和終止速度均為0。 由圖6 可知，各關節(jié)加速度呈現(xiàn)二次拋物線變化方式，變化連續(xù)且運行同步， 最大運行加速度逼近關節(jié)最大加速度，初始加速度和終止加速度都為0。 結(jié)果表明，該算法能夠根據(jù)教點正確規(guī)劃各關節(jié)的運動軌跡；確保各關節(jié)能夠同步運行；位移、速度和加速度都能夠平滑連續(xù)的變化，提高運行平穩(wěn)性；速度和加速度都能夠逼近關節(jié)最大值， 有效地優(yōu)化運行時間，提高運行效率。

圖4 關節(jié)角度變化曲線Fig.4 Joint angle curve

圖5 關節(jié)速度變化曲線Fig.5 Joint velocity curve

圖6 關節(jié)加速度變化曲線Fig.6 Joint acceleration curve

5 結(jié)語

以六自由度工業(yè)機器人為研究對象，分析只有起始和終止兩點的運動軌跡，為實現(xiàn)各關節(jié)運動同步引入速度和加速度同步系數(shù)2 個參數(shù)，為保證運行平衡采用二次拋物線加速度運行方式推算出位移、速度和加速度關于時間的表達式，并給出運行限制條件；分析多路徑點的運動軌跡，把每兩個路徑點之間看作是勻速運動，每個運動段之間用加速度連續(xù)變化的運動進行過渡；分析算法設計的關鍵點，給出算法流程；使用MATLAB 進行算法仿真驗證。

該算法保證了多路徑點運動中各關節(jié)的加速度、速度和位移的變化平滑連續(xù)，實現(xiàn)各關節(jié)同步運行和時間最優(yōu)，確保機器人能夠平滑高效地運行。 通過將軌跡分解成多個低階多項式的方法有效地降低運算復雜度，減小運算量，易于編程實現(xiàn)。