斥力場引導RRT 算法的機械臂路徑規(guī)劃

摘 要： 針對現(xiàn)有的快速搜索隨機樹（RRT）及其改進算法在機械臂路徑規(guī)劃中存在的效率不足、無法快速繞開障礙空間以及路徑冗余等問題，提出一種基于斥力場引導的改進RRT-Connect 算法的機械臂路徑規(guī)劃，以下稱改進RRTConnect（improved RRT-Connect）。首先對已知環(huán)境構(gòu)建柵格化地圖及其斥力勢場；其次利用改進RRT-Connect 以及臂形約束進行路徑規(guī)劃；最后通過逆運動學以及插值算法得到關節(jié)空間路徑點，伺服控制機械臂運動。將所提算法進行仿真與實際操作實驗驗證，結(jié)果表明：基于斥力場引導的改進RRT-Connect 算法的機械臂路徑規(guī)劃可以繞開高效障礙空間，消融實驗表明減少規(guī)劃路徑點17.24% 以上、提高規(guī)劃效率，較RRT 減少規(guī)劃時間99.71%、以及減少路徑冗余，保留10.59% 以下，驗證了算法的可行性。

關鍵詞： 快速搜索隨機樹; 機械臂; 路徑規(guī)劃; 斥力勢場

中圖分類號： TB9; TP241 文獻標志碼： A 文章編號： 1674–5124（2025）01–0126–07

0 引 言

電力運維機械臂是一種應用在電力運維方面的機械臂系統(tǒng)，通過擬人化的靈活操作，代替人力從事危險復雜環(huán)境下的工作，不僅可以節(jié)省人力成本、降低人工風險，工作質(zhì)量和效率也會得以提升[1-2]。傳統(tǒng)工業(yè)正邁向自動化和智能化，機器人技術(shù)迅速發(fā)展。在變電站巡檢場景中，使用機器人巡檢代替人工巡檢是一種趨勢，不僅可以節(jié)約人力成本，在實時性和安全性上也會有很大提高。機械臂面對復雜的作業(yè)環(huán)境和作業(yè)要求，必須擁有一套高效的路徑規(guī)劃方法。

Dijkstra 算法[3-4]、A*算法[5]、人工勢場法[6-7] 等算法都可用作路徑規(guī)劃的方法。盡管Dijkstra 算法可以得到路徑最優(yōu)解，但由于Dijkstra 算法采用遍歷的思想，迭代的過程使得在大型復雜地圖中規(guī)劃需要大量的時間，不滿足實時性要求，A*算法也有同樣如此，所以二者更適用于移動機器人的二維地圖路徑規(guī)劃，而不是更高維空間。人工勢場法不再追求路徑最優(yōu)，相比于前兩種方法效率大幅提升，但在某些情況下會陷入局部最優(yōu)解，而導致規(guī)劃失敗。

快速搜索隨機樹算法[8] 等基于采樣的方法的提出解決了高維空間路徑規(guī)劃中一系列問題。如存在解，由于其隨機采樣機制，通過足夠多的迭代次數(shù)返回一條可行路徑。其也因隨機機制，存在可能收斂慢的不足。

在RRT 的基礎上， RRT-Connect 算法在原本RRT 的基礎上多加了一棵隨機探索樹[9]，各自從起點和終點向外探索拓展，直到兩棵樹相遇時規(guī)劃算法收斂，加快了收斂速度。文獻[10-12] 提出了引入目標偏置策略的Bias-RRT 算法，引入了一定的偏置（Bias）權(quán)重改進傳統(tǒng)算法中采樣隨機性，加快了RRT 算法的收斂速度，能使算法進行路徑規(guī)劃時具有更強的目標性。Karaman 等[13] 提出了RRT*算法[14]，比RRT 多一個選擇重連的過程，來確保新節(jié)點與父節(jié)點連接的成本是最小的，以達到一條漸進最優(yōu)的路徑。

上述三種改進方法均未有效利用環(huán)境信息引導搜索過程，其中RRT-Connect 并未改變采樣隨機性，面對狹長空間仍然收斂慢；Bias-RRT 僅在目標方向上有引導作用，但未利用障礙物信息，無法引導避開障礙物，復雜的環(huán)境下可能陷入局部極小值；RRT*因為需要選擇重連，計算量大，生成路徑的效率一般較RRT 低，甚至在復雜的環(huán)境或者窄長的情況下會出現(xiàn)找不到路徑。

此外，RRT 相關算法由于采樣隨機性和步長限制，在冗余節(jié)點過多時，易使機械臂執(zhí)行軌跡不順滑，產(chǎn)生不必要的姿態(tài)變化[15]。

本文通過在二維空間中改進RRT 算法，并擴展到三維笛卡爾空間中規(guī)劃機械臂工具端路徑進行驗證。首先在RRT-Connect 基礎上，構(gòu)建已知地圖的斥力場與梯度函數(shù)，隨機采樣疊加梯度，使規(guī)劃可有效利用環(huán)境信息引導；其次加入直連碰撞檢測，避免在兩搜索樹交點附近振蕩，使在障礙物較少的環(huán)境下快速收斂；然后利用貪心算法對規(guī)劃路徑進行冗余節(jié)點優(yōu)化，并對每段路徑平均采樣帶入臂形約束，檢驗路徑可行性，如不可行則對該段路徑重新規(guī)劃；最后將規(guī)劃得到的可行笛卡爾空間路徑點做逆運動學變換[16]，得到關節(jié)空間路徑點，并對關節(jié)空間路徑點進行插值，發(fā)送伺服控制器執(zhí)行動作。

本文將通過與其他RRT 相關算法進行比較，以及消融實驗，驗證算法可行性和有效性。本文機械臂實驗部分選用節(jié)卡Zu7 型6-DOF 協(xié)作機械臂。

1 改進RRT-Connect 算法

本文提出了基于斥力場引導的改進RRTConnect機械臂路徑規(guī)劃算法，在RRT-Connect 的基礎上，為利用障礙空間信息快速引導搜索樹繞過障礙空間，減少不必要的冗余節(jié)點，當為滿足目標偏置時，則利用障礙空間的斥力勢場，將隨機節(jié)點疊加此處的梯度矢量，高效繞開障礙。

為避免兩樹在小范圍內(nèi)震蕩，在遍歷到另一棵樹的節(jié)點列表的最近節(jié)點時不再考慮兩節(jié)點的歐幾里得距離，而是直接判斷兩節(jié)點之間的邊是否經(jīng)過障礙空間，若不經(jīng)過障礙空間則直接規(guī)劃成功。并且對規(guī)劃成功的路徑進行二次裁剪，進一步減少冗余節(jié)點，提高機械臂運動效率。