基于時間最優的機器人多路徑點軌跡規劃

張華文，劉子亮

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

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

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

1 兩點運動的軌跡規劃

用qi和qf分別表示起始和終止時刻關節角度向量和分別表示第j 個關節起始和終止時刻的關節角，kv和ka分別表示關節最大速度向量和關節最大加速度向量，kvj和kaj分別表示第j 個關節的最大速度和最大加速度。 每個關節的最大速度和加速度通常可以根據該關節的電機參數得到。

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

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

1.1 關節同步系數的計算

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

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

求得關節j 最小運行時間為

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

兩個關節的最小運行時間為

同步后的運行時間為

根據式（6）可得：

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

根據式（7）可得：

與之對應的加速時間為

推廣到n 個關節可得：

根據式（7）可以得到其他所有關節的同步系數。

1.2 二次拋物線加速度的軌跡規劃

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

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

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

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

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

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

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

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

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

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

得到整段的運行時間為

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

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

2 多路徑點的軌跡規劃

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

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

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

圖2 關節空間加速度連續過渡的線性規劃Fig.2 Interpolations with continuous acceleration blends in joint space

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

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

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

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

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

式中：tk≤t

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

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

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

3 算法設計

過路徑點的關節空間的軌跡規劃算法流程如圖3 所示。

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

算法在實現時有以下幾個關鍵點：

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

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

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

4 實驗結果與分析

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

表1 示教點參數Tab.1 Teaching point parameters

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

圖4 關節角度變化曲線Fig.4 Joint angle curve

圖5 關節速度變化曲線Fig.5 Joint velocity curve

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

5 結語

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

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