機械臂時間最優軌跡的樣條曲線擬合與智能規劃

袁旭華，劉 羽，林喜輝

（1.延安大學數學與計算機科學學院，陜西 延安 716000；2.延安大學石油工程與環境工程學院，陜西 延安 716000；3.北海職業學院，廣西 北海 536000）

1 引言

機械臂控制律是在關節空間中設計的，機械臂關節空間軌跡規劃是機械臂控制的基礎。合理的工作軌跡不僅能夠提高工作效率、降低能耗，而且影響產品質量和工作安全[1]，因此研究機械臂在關節空間軌跡規劃問題具有安全意義和經濟意義。

按照研究空間的不同，機械臂軌跡規劃分為笛卡爾空間規劃和關節空間規劃，笛卡爾空間規劃是針對機械臂末端執行器的軌跡規劃，存在奇異解問題和可控性不強的問題[2]，對笛卡爾空間軌跡的跟蹤一般需要首先轉化到關節空間中。關節空間軌跡規劃是通過規劃每個關節角的軌跡，使末端執行器能夠通過某些特定位置而后到達目標位置，關節空間軌跡的可控性極強，但是難以對笛卡爾空間完全跟蹤，只能通過設定的若干位置［3］。就軌跡規劃的目標而言，可以分為時間最優、能量消耗最優、沖擊最小及三者組成的綜合目標［4-5］，其中以時間最優研究最多，這是因為時間最優代表著生產效率最高，生產效率高帶來的經濟效益必然抵消了機械臂能耗成本，因此時間最優軌跡規劃更具有研究價值。文獻［6］以能耗最小為目標，使用蟻群算法規劃了植發機器人工作路徑；文獻［7］使用5次NURBS曲線插值法擬合機械臂工作軌跡，以運行時間、能量消耗、脈動沖擊為優化目標，使用改進人工蜂群算法得到了Pareto最優解集；文獻［8］使用4-3-4多項式插值法對關節空間進行軌跡規劃，在滿足速度約束前提下，使用雜交算法優化了路徑點之間的用時。總結機械臂關節空間軌跡規劃研究熱點和難題，主要包括兩個方面：（1）路徑點間的擬合函數選取，選取什么樣的擬合函數可以獲得最好的規劃性能；（2）優化方法的選擇與改進，使用什么樣的方法搜索到最優路徑。

針對機械臂關節空間軌跡的時間最優規劃問題，本文改進了B樣條曲線，提高了對軌跡點的擬合精度；改進了蜂群算法，提高了算法的尋優質量；在滿足機械臂運動約束前提下，實現了軌跡的時間最優規劃。

2 改進B樣條曲線與問題建模

軌跡規劃的方法是：根據任務需要和避障要求，確定機械臂起始點、目標點和若干中間點，然后使用某種曲線進行擬合，使機械臂依次經過設定中間點并順利到達目標點。軌跡規劃的目的是在滿足約束的前提下，獲得時間最短或能耗最小軌跡。

2.1 改進B樣條曲線擬合技術

中間點之間的軌跡規劃一般使用多項式函數或B 樣條曲線，多項式插值函數具有計算量與連續性之間的矛盾，選擇B樣條曲線規劃中間點之間的軌跡。B樣條曲線擬合分為近似擬合和插值擬合，近似擬合無法保證經過所有中間點，插值擬合能夠經過所有中間點，但是需要將中間點作為型值點反算控制點，此時曲線求解計算量大。為了使B樣條曲線經過所有中間點同時不增加計算量，提出了改進B樣條曲線擬合方法。

傳統三次B樣條曲線，如圖1所示。由4個控制點P0～P3進行控制。圖中為P0P2中點，r0為線段的1/3處；同樣地，為P1P3中點，r1為線段的1/3處。B樣條曲線起始于點r0，終止于點r1，B樣條曲線起點處切線平行于P0P2，終點處切線平行于P1P3。

圖1 傳統B樣條曲線Fig.1 Traditional B-spline Curve

從圖中可以看出，B樣條曲線不經過中間點P0～P3，為了使B樣條曲線經過中間點，同時避免插值法中反算過程帶來的巨大計算量，本文提出改進的B樣條擬合曲線，如圖2所示。以B樣條曲線經過點P1、P2為例，改進基本思想為：通過增加控制點P1，0、P1，1，使B 樣條曲線的起點r0與控制點P1重合；通過增加控制點P2，0、P2，1，使B樣條曲線的終點點r1與控制點P2重合。

圖2 改進B樣條曲線Fig.2 Improved B-Spline Curve

改進的具體方法為：以點P1為中點增加控制點P1，0、P1，1，點P1、P1，0、P1，1成一條直線且平行于P0P2，長度P1，0P1=P1P1，1=為底邊系數比例。以同樣方式在點P2兩側增加控制點P2，0、P2，1。以點P1，0、P1、P1，1、P2，0、P2、P2，1為新的控制點，每連續4個控制點可以確定一段B樣條曲線，那么6個控制點可以確定3段B樣條曲線。第1段B樣條曲線由P1，0、P1、P1，1、P2，0確定，由于P1，0、P1、P1，1在同一直線上，可以將其視為三角形的極限狀態，按照傳統B樣條曲線構造方法，此時點P1、P1′與曲線起點r0重合，即第1段B樣條曲線的起點經過中間點P1。同樣的，第3段B樣條曲線由P1，1、P2，0、P2、P2，1確定，而點P2，0、P2、P2，1在同一直線上，那么第3段B樣條曲線的終點必然經過中間點P2。

總的來講，對于n個中間點，需要將其擴展為3n個控制點，每4個控制點確定一段B樣條曲線，規劃的曲線必然經過n個中間點，且不存在插值法的反算過程。中間點的擴展控制點為：

傳統的三次B樣條曲線方程［9］為：

式中：t—變量；Pi—中間點在三維空間坐標；Bi(t)—B樣條曲線的樣條基。

將式（2）改寫為矩陣方式，得到連續3段B樣條曲線為：

將式（1）代入式（3），使用原控制點替換掉擴展控制點，得到改進B樣條曲線表達式為：

在此需要特別說明的是，式（4）中i不能取0和n-1，因為第一段B樣條曲線和最后一段B樣條曲線的擴展控制點不滿足式（2），只能用式（3）求解。

2.2 時間最優軌跡規劃模型

根據機械臂的任務需求、障礙物分布和關節活動范圍，確定機械臂執行任務過程中必須經過的若干軌跡點為P0，P1，P2，…，Pm，其中，P0—機械臂末端起始位姿；Pm—機械臂末端終止位姿，使用機械臂逆運動學求解軌跡點對應的關節空間點為θ0，θ1，θ2，…，θm。其中，θi={θij}，j=1，2，…n，n為機械臂關節數量。記機械臂末端依次經過點P0，P1，P2，…，Pm的時間為t0，t1，…，tm，這里的規劃思路為通過優化機械臂到達各軌跡點的時間，在滿足約束條件的前提下，使機械臂在最短時間內由起始點到達目標點。根據規劃思路，優化目標函數設計為：

式中：f—機械臂總耗時；Δti=ti-ti-1；i=1，2，…m—由中間點Pi-1運動到點Pi的時間。

機械臂軌跡規劃需滿足機械臂運動條件，包括關節運動范圍約束、角速度約束、角加速度約束、角沖擊約束等，以關節j為例，約束條件為：

式（5）與式（6）將機械臂關節空間軌跡規劃問題轉化為帶約束優化問題，此類問題求解方法有梯度法、尋優法等，使用智能算法尋優進行求解。

3 優化搜索策略蜂群算法

3.1 人工蜂群算法

人工蜂群算法是模擬蜂群搜索最優蜜源過程而提出的，將最優蜜源視為優化問題的最優解，蜂群搜索最優蜜源的過程即為優化問題求解過程。算法將蜜蜂人工地分為引領蜂、跟隨蜂、偵查蜂三類，三種人工蜂協作完成蜜源搜索[10]。

3.1.1 初始化人工蜂位置。

記人工蜂數量為N，搜索維度為D，以隨機方式初始化蜜蜂位置，為：

式中：i∈[1，N]—蜜蜂序號；j∈[1，D]—維度；xij—人工蜂位置；—維度j上下界；rand(0，1)—［0，1］間隨機數。

3.1.2 引領蜂搜索階段

引領蜂以交叉方式更新蜜源，即：

式中：vij—引領蜂的蜜源更新位置；xij—原蜜源位置；xkj—隨機選擇的異于xij的個體；φij—[-1，1]間的隨機數。引領蜂使用貪婪規則選擇蜜源，即選擇xij和vij中蜜源濃度較高的位置。

魏氣沖沖走了，遲恒發慌，得趕緊報警，打開手機翻蓋，顯示屏不亮，按鍵也無反應，該死的手機！他想去追魏昌龍。就在這時，他看見庫區西頭變戲法似地亮起了一溜車燈，散在庫區的手電光束迅速向北壩中段移動。

3.1.3 跟隨蜂選擇與搜索階段

跟隨蜂通過搖擺舞了解引領蜂位置蜜源濃度，依據蜜源濃度確定對引領蜂的選擇概率：

式中：pi—選擇引領蜂i的概率；NB—引領蜂數量；fiti—引領蜂i處蜜源濃度；fi—式（5）構造的目標函數值。通過式（9）給出選擇各引領蜂的概率，然后使用輪盤賭方法確定最終選擇的引領蜂。跟隨蜂選擇引領蜂后，在引領蜂鄰域內按照式（8）進行位置更新。

3.1.4 偵查蜂搜索階段

當某只蜜蜂迭代次數達到設定上限但是適應度卻沒有明顯提高時，此蜜蜂放棄此處蜜源轉化為偵查蜂，以隨機的方式生成新位置，方法與式（7）一致。

3.2 優化搜索策略蜂群算法

為了平衡算法多樣性和收斂性，對引領蜂和跟隨蜂的蜜源搜索策略進行改進。

3.2.1 基于最優解引導的引領蜂蜜源搜索

文獻[11]中提到“DE/rand-to-pbest/1”突變策略可以很好地保持種群差異性，參考這一思想，將引領蜂蜜源搜索策略改進為基于最優解引導的鄰域學習策略，即：

式（10）給出的位置更新策略，如圖3所示。

圖3 引領蜂搜索方式Fig.3 Searching Mode of Leading Bee

以xr1位置向量為基向量，在伸縮因子?ij的作用下使vr1以不同程度向xbest靠近，保證了蜜蜂位置朝著更優方向前進。式（10）中第三項通過高斯縮放因子調節xr2-xr3的步長，使vr1以xbest-xr1為中心分布線，向兩側呈現高斯分布。根據高斯分布特性，vr1以68.27%的概率分布在xbest-xr1兩側(-α，+α)范圍內，保證了以大概率在最優解鄰域內搜索；vr1以31.73%的概率分布在(-α，+α)以外，對于探索新區域、開發新蜜源、增加多樣性意義重大。

3.2.2 基于三角變異的跟隨蜂蜜源搜索

此部分內容對跟隨蜂的蜜源搜索方法進行改進。以人工蜂xr1的位置更新為例，在種群中隨機選取異于xr1的兩只相鄰人工蜂xr2、xr3，三個相異個體組成一個超幾何三角形，利用三角變異引導人工蜂向三者中最優者靠近，方法為：

式中：Gr1，j—高斯分布縮放因子，即Gr1，j～N(0，β)；xtd—位置擾動量；xr1、xr2、xr3—相鄰人工蜂；p1、p2、p3—由各自適應度構造的影響概率。

式（11）給出的三角變異蜜源搜索方法，如圖4所示。以三角形中心位置為基向量，各邊向量對基向量的擾動大小依據適應度函數而構造，使得基向量向三個人工蜂中最優位置靠近，這種蜜源搜索方法可以保證跟隨蜂向適應度靠前的33.3%蜜蜂靠近，達到了位置更新進化的目的。另外，高斯伸縮因子的作用與式（10）中一致，這里不再分析。

圖4 跟隨蜂搜索方式Fig.4 Searching Mode of Following Bee

3.3 優化搜索策略蜂群算法流程

前文對引領蜂和跟隨蜂的蜜源搜索策略進行了改進，并未對算法流程產生影響，因此此處給出的算法流程既適用于改進蜂群算法，也適用于傳統人工蜂群算法，如圖5所示。

圖5 優化搜索策略蜂群算法流程Fig.5 Searching Strategy Improved Bee Swarm Algorithm Flow

4 仿真驗證與分析

仿真驗證包括兩個方面內容，一是驗證B樣條曲線與改進B樣條曲線的擬合精度，二是驗證優化搜索策略蜂群算法的軌跡規劃性能。

4.1 改進B樣條曲線擬合精度驗證

前文中提到，B樣條曲線使用擬合法時無法保證經過所有中間點，插值法能夠經過所有中間點但是存在反算引起的大量計算問題，改進B樣條曲線使用擬合法確保了經過所有中間點，在此對其擬合精度進行驗證。以拋物線y=(x-4)2為例，在[0，8]區間內均勻生成9個中間點，分別使用B樣條曲線擬合法和改進B樣條曲線擬合法結果，如圖6所示。

圖6 B樣條曲線擬合精度Fig.6 B-Spline Curve Fitting Accuracy

統計B樣條曲線和改進B樣條曲線在9個中間點處的擬合誤差標準差，B樣條曲線的擬合誤差為0.082，改進B樣條曲線的擬合誤差為0.041，比傳統B樣條曲線的擬合精度提高一個數量級以上，說明了改進B樣條曲線能夠更加精確地經過中間點，同時改進B樣條曲線計算過程中不存在反算過程。

4.2 優化搜索策略蜂群算法規劃性能驗證

以PUMA560機械臂為研究對象，此機械臂具有6個活動關節，具體構型可參考文獻[12]，這里不再贅述。PUMA 機械臂的活動能力約束，如表1所示。

表1 PUMA560活動能力約束Tab.1 Mobility Constraint of PUMA560

在PUMA560機械臂可到達范圍內設置6個軌跡點，如表2所示。表中：θ0—軌跡起點；θ5—軌跡終點；θ1～θ4—必須經過的中間點。

表2 軌跡點Tab.2 Trajectory Point

使用人工蜂群算法和優化搜索策略蜂群算法對問題進行求解，算法參數統一設置為：種群規模N=40，粒子維度D=5，最大迭代次數tmax=400，單個位置迭代限度為tlimit=20，將任意相鄰兩點間耗時限制在4s內，則每個維度搜索空間為（0，4]，高斯收縮因子參數α=1、β=0.1。兩種算法各自獨立運行10次，選擇兩個算法各自的最優解進行展示，最優解的目標函數值隨迭代過程變化曲線，如圖7所示。

圖7 迭代曲線Fig.7 Iteration Curve

從圖中可以看出，優化搜索策略蜂群算法在迭代至120次時算法搜索到最優解，最優解為13.5141s；傳統人工蜂群算法迭代至200次時搜索到最優解，最優解為14.8726s。人工蜂群算法和優化搜索策略蜂群算法搜索到的最優解位置，如表3所示。

表3 優化結果Tab.3 Optimizing Result

結合圖7和表3，從搜索到最優解時迭代次數看，改進算法比傳統算法提前了80次；從最優解質量看，改進算法比傳統算法減少了1.3585s，即機械臂工作耗時減少了9.13%。

這是因為改進算法中引入了基于最優解引導的引領蜂蜜源搜索和基于三角變異的跟隨蜂蜜源搜索方法，改進的蜜源搜索方法以大比例促進算法向最優解附近靠近，同時以相當比例允許蜜蜂大范圍搜索，兼顧了多樣性。而傳統算法向鄰解學習方式，不僅學習效率低，而且由于鄰解學習價值小，使算法收斂速度也較慢。

根據改進算法搜索的最優蜜源位置和改進B樣條曲線規劃方法，給出PUMA560機械臂6個關節角軌跡，如圖8所示。

結合表1和圖8可以看出：（1）改進蜂群算法規劃的關節空間軌跡滿足動力學約束，可以進行實際應用；（2）6個關節的起點和終點角速度均為0，與實際情況相符；（3）改進B樣條曲線精確地經過了所有軌跡點，相應地機械臂末端精確的經過了設定點。由以上三點可以看出，改進蜂群算法和改進B樣條曲線規劃的軌跡在滿足約束條件，實現了時間最優，可以應用于實際。

圖8 關節角軌跡Fig.8 Joint Angle Trajectory

5 結論

研究了機械臂關節空間的時間最優軌跡規劃問題，改進了B樣條曲線和蜂群算法，經仿真驗證得出了以下結論：

（1）改進B樣條曲線使用擬合法，在不增加計算量同時，提高了對軌跡點的擬合精度；

（2）優化搜索策略蜂群算法規劃的軌跡滿足約束條件，實現了時間最優，可以進行實際應用。