基于量子蟻群算法的機械手臂避障路徑規(guī)劃

李大偉，趙 明

(遼寧科技大學電子與信息工程學院，鞍山 114051)

0 引言

多自由度機械手臂在可操作空間內尋找一條從起始位置到達目標位置的無碰撞最優(yōu)路徑，其各關節(jié)變量求解問題構成機械手臂運動學逆解問題[1]。目前，解決運動學逆解問題的方法主要有傳統(tǒng)方法：幾何解析法、矩陣求逆法、旋量李代數(shù)法和智能優(yōu)化算法。幾何解析法[2]是根據(jù)機械手臂各關節(jié)空間關系求解，只適用于簡單、低自由度機械結構。矩陣求逆法[3]是一種代數(shù)解法，利用矩陣性質來解一個十分復雜的三角超越方程，多數(shù)機械手臂結構無法尋找到解析解形式而不適用，只適用于特定結構。旋量李代數(shù)法[4]是利用Paden-Kahan子問題的理論化簡機械手臂運動學正解方程，然后得到齊次線性方程組并進行聯(lián)立迭代求解。其方程組會隨著機械結構復雜度呈現(xiàn)幾何倍數(shù)上升，從而很難求解。近年來，隨著智能優(yōu)化算法興起，遺傳算法、神經(jīng)網(wǎng)絡、蟻群算法在機械手臂運動學逆解問題中得到新的應用。然而，遺傳算法[5]本質上是一種無啟發(fā)式尋優(yōu)算法，對解決復雜的多自由度機械手臂運動學逆解問題容易陷入局部最優(yōu)。神經(jīng)網(wǎng)絡算法[6]對于復雜結構機械手臂的數(shù)據(jù)獲取和訓練也存在局限性，很難訓練出精確的神經(jīng)網(wǎng)絡函數(shù)。蟻群算法[7]具有啟發(fā)式性、正反饋機制，但受制于螞蟻數(shù)量收斂速度慢，易陷入局部極值。量子蟻群算法(QACA)[8-10]運用于多種機器人應用場合，包括TSP問題[11-12]、無人機協(xié)同軌跡規(guī)劃[14]等。

本文首次將QACA應用到搜索機械手臂的運動學逆解中，利用量子計算強大的并行性[8-9]、三鏈編碼機制[10-11]、量子旋轉門、非門[12]對基本蟻群算法進行改進，解決了機械手臂目標函數(shù)求解過程之初搜索緩慢、甚至停滯問題，并且擴大了螞蟻數(shù)量限制優(yōu)化解的范圍。在建立機械手臂避障目標函數(shù)F[10-14]時，首先利用DH參數(shù)法，建立實際位置矩陣A6；其次是采用方向包圍盒[15](OBB)碰撞檢測技術進行相交性檢測來確定碰撞因子cosllsion；最后再引入6軸關節(jié)的操作空間的約束[16]。同時為了給量子蟻群提供搜索空間，提出一種基于量子Bloch球面的三維量子蟻群搜索空間的建立方法。

1 建立目標優(yōu)化函數(shù)F

1.1 采用DH參數(shù)法建立實際位置矩陣A6

建立通用6R機械手臂DH參數(shù)坐標[12]，如圖1所示。

圖1 6R機械手臂DH坐標簡圖

其中6R是指6個關節(jié)均為旋轉關節(jié)，于是可得其DH參數(shù)，如表1所示。

表1 6R機械手臂的DH參數(shù)

根據(jù)空間平移旋轉變換[13]可得：

(1)

式中，sθi代表sinθi；cθi代表cosθi；sαi代表sinαi；cαi代表cosαi。

按式(1)建立6R機械手臂實際位置矩陣：

(2)

在機械手臂可操作空間內隨機設定末端執(zhí)行器的目標位置矩陣為：

(3)

1.2 方向包圍盒(OBB)碰撞因子

方向包圍盒[15]是用一種包圍被測對象的6面長方體，它是以任意坐標軸方向建立的，并且表面法線滿足兩兩垂直，是對被測物體的最小包圍。

首先用OBB包圍盒包圍住機械手臂各個關節(jié)，通過檢測機械手臂關節(jié)的各個OBB包圍樹的各個節(jié)點是否與障礙物相交，來輸出碰撞因子Collision。定義如下：

(4)

1.3 建立目標優(yōu)化函數(shù)F

為了將6軸機械手臂避障路徑規(guī)劃問題轉化為多元函數(shù)求極值問題，提出了滿足優(yōu)化條件的一種新的目標優(yōu)化函數(shù)F[10,12-13]，如式(5)所示。

(5)

2 應用量子蟻群算法求逆解

2.1 建立量子Bloch球面的三維量子搜索空間

提出一種三維量子蟻群搜索空間方法。具體過程如下：設Tr點為起始點，Ts為目標點；首先以TrTs線段為X軸建立笛卡爾坐標系，再以起始點Tr為原點建立半徑為R的Bloch球面[10]，對起始點位置進行量子三鏈編碼[12]。然后螞蟻按照狀態(tài)轉移概率公式[7]搜索球面上的一點T1并把它作為螞蟻當前的目標點，通過量子門[11]相位角Δθ與Δφ大小和方向進行空間旋轉，到達目標點第一個目標點位置T1并再次進行量子三鏈編碼。依次類推，下一個點都以前一個點作為新起點建立量子Bloch球面直到找到最終目標點。

至此，單只螞蟻依次選出T1,T2,T3,…,Ti，i∈{1,2,…,n}點，并對這些點進行編碼信息素進行局部更新，最終選擇的Tn點趨近目標點Ts，這時單只螞蟻搜索過程結束，需要對整個搜索路徑進行信息素的全局更新[7]；通過多只螞蟻和多次迭代后，最終搜索到的Tn點與目標點之間的距離最小或者基本重合。

可見，搜索這n個點的過程，即是求解F函數(shù)逼近全局極大值的過程，最后將其轉化為關節(jié)解空間變量Xj，映射為多元函數(shù)求極值問題。單只螞蟻三維量子搜索空間如圖2所示。

圖2 單只螞蟻三維量子搜索空間

2.2 目標點的空間約束

假設drs為Tr，Ts之間的歐式距離為已知，它的大小應該在機械手臂可操作空間W內即滿足：

drs≤Pe=Pe(Xi),Xim≤Xj≤XiM

(6)

式中，j=1,…,n；Xim(XiM)為第j個關節(jié)角的最小(最大)限制；Pe為在Xim(XiM)限制下末端執(zhí)行器所能達到的最遠位置；否則空間目標位置矩陣不可達。除此之外，可以設置螞蟻在最多N步內到達最終目標位置Ts。

2.3 建立Bloch球面

經(jīng)典信息比特用0和1表示，量子比特用狄拉克表示法表示為|0〉和|1〉兩種基態(tài)的線性疊加。這種疊加態(tài)可表示為：

|φ〉=cosθ|0〉+eiφsinθ|1〉

對應的矩陣形式表示為：

于是滿足歸一化定律：

(7)

此時量子態(tài)可借助于圖3所示的Bloch球面[10-11]直觀表示，其中θ和φ定義了球面上螞蟻每步選擇的目標點Ti，可以用Bloch球面坐標表示為[cosφsinθsinφsinθcosθ]T。

圖3 疊加態(tài)|φ〉的Bloch球面表示

2.4 量子蟻群的三鏈編碼方式

假設某一時刻有m只螞蟻分布在Ω=[-1,1]n空間中，螞蟻身上存在一組量子比特信息，通常是n位。把單一量子比特的概率幅都用來表示螞蟻對當前的位置信息，形成三鏈編碼結構[11,15]，這樣擴大了搜索空間，增加搜索近似解的數(shù)量。設螞蟻k攜帶量子信息為：

(8)

式中，φk,i=2π×rnd；θk,i=2π×rnd；rnd為(0,1)之間的隨機數(shù)；k=1,2,…,m；n是量子位為1,2,…,n。在量子蟻群算法中把每一位量子位的概率幅包括余弦和正弦概率幅當做3個并聯(lián)的信息串，則螞蟻k占據(jù)空間Ω三個位置分別為：

Pkx=(cosφk,1sinθk,1,…,cosφk,nsinθk,n)
Pky=(sinφk,1sinθk,1,…,sinφk,nsinθk,n)
Pkz=(cosθk,1,…,cosθk,n)

(9)

經(jīng)過解空間變換后，用于優(yōu)化目標函數(shù)F，該過程中螞蟻當前的位置對應優(yōu)化問題的近似解。

2.5 目標位置的選擇

設螞蟻k從位置Tr移動到位置Ts，其位置的轉移規(guī)則滿足式(10)。

(10)

式中，Pk為螞蟻k選擇位置Ts的概率；Tn為螞蟻占據(jù)單位空間In點的集合；τ(Ts)為Tr與Ts位置之間的信息素含量；α為信息素啟發(fā)因子[7]。

2.6 通過量子旋轉門傳送螞蟻到目標點位置

目標點選擇完畢后，螞蟻通過改變自身攜帶量子信息的量子比特相位角來實現(xiàn)向目標點的移動，即改變目標點位置的三鏈編碼，這種改變稱為量子旋轉門作用[10]。從位置Tr移動到位置Ts。

(11)

(12)

滿足Ts=URTr；其中量子旋轉門為：

(13)

所以，

(14)

可見量子旋轉門UR通過改變量子位的相位實現(xiàn)空間的旋轉，使得量子信息從一個量子態(tài)轉移到另一個量子態(tài)，從而到達空間另一個點位置。

2.7 量子旋轉門方向與大小

(1)旋轉門方向[11]：令當代螞蟻中的第i只螞蟻攜帶的量子信息中第j位的Bloch球面坐標為qij(xij,yij,zij)，其中當代最優(yōu)螞蟻量子信息第j位Bloch球面坐標為q0j(x0j,y0j,z0j)，記

(15)

①Δφ方向：當A≠0時，sgn(Δφ)=-sgn(A)；否則，正反向均可。

②Δθ方向：當B≠0時，sgn(Δθ)=-sgn(B)；否則，正反向均可。

(2)旋轉門大?。?/p>

Δφi=-sgn(A)φ0e-i
Δθi=-sgn(B)θ0e-i

(16)

式中，φ0、θ0為迭代初值；i為優(yōu)化步數(shù)。

2.8 變異處理

為防止算法過早收斂、陷入局部極值，采用量子非門[12]進行變異處理。令第k只螞蟻攜帶量子信息的第i個量子位為Tki=[cosφsinθsinφsinθcosθ]T，實施非門的過程可表示為：

(17)

即：

(18)

得到：

(19)

2.9 信息素的更新

(1)信息素的局部更新[7]。螞蟻每一步移動都將進行信息素的改變，即局部更新，如式(20)所示。

(20)

(2)信息素的全局更新[7]。全局更新采取選擇每組螞蟻路徑規(guī)劃當中的最優(yōu)解(使F函數(shù)取最大的解)所對應的從Tr點到Ts點的每一條路徑進行信息素更新的方法，即全局更新公式為：

(21)

式中，(1-ρ)為信息素的揮發(fā)系數(shù)；Q為強度；F(Ts)、F(Tr)分別對應Ts、Tr位置時目標優(yōu)化函數(shù)值。由于目標優(yōu)化F函數(shù)介于(0,1)之間，將目標優(yōu)化函數(shù)F增量值F(Ts)-F(Tr)作為比例系數(shù)直接作用于信息素全局更新上。

2.10 量子空間向關節(jié)空間解的變換

(22)

第k只螞蟻完成一次搜索后，將搜索到的點代入目標優(yōu)化函數(shù)F(式5)得到一個Fk值，把它與目前最優(yōu)Fmax進行比較，選取最大值進行替代。最大1000次迭代之后，找出最大Fmax對應的[θ1θ2θ3θ4θ5θ6]T即為機械手臂各個關節(jié)角。

2.11 量子蟻群搜索算法求解步驟

歸納得到量子蟻群算法求解步驟如下：

步驟1：按2.1節(jié)方法建立螞蟻搜索空間即三維量子旋轉搜索空間，在式(6)約束條件下將m只螞蟻放置量子搜索空間起點，對m只螞蟻在起始點位置按式(8)進行三鏈編碼，初始化參數(shù)ρ(信息素揮發(fā)因子)，信息素影響因子，初值Tau，變異概率為pv，轉角步長初值φ0和θ0，Max_Iter(最大迭代次數(shù))，當前螞蟻k=1，當前迭代次數(shù)Iter=1。

步驟2：第k只螞蟻從起點S出發(fā)，按照概率轉移式(10)選擇下一個目標位置，通過量子旋轉門即式(14)實現(xiàn)螞蟻移動，旋轉門方向、大小分別按式(15)和式(16)確定，這樣每一個目標位置的三鏈編碼都通過量子旋轉門進行改變；按照變異機制，隨機選擇種群中若干螞蟻進行量子非門變異即式(18)。

步驟3：按式(20)進行信息素局部更新。

步驟4：若第k只螞蟻經(jīng)過N步后到達終點Ts，按式(21)進行信息素全局更新。按照式(22)將單位空間In的解變換為關節(jié)變量空間Ω的解，計算該只螞蟻的目標優(yōu)化函數(shù)F即式(5)的Fk值；記當前最優(yōu)解為Fmax，若Fk≥Fmax，則Fmax=Fk；k=k+1；否則，繼續(xù)按照概率轉移式(10)選擇下一個目標點，轉移到步驟3。

步驟5：若k=m，則選出該組m只螞蟻中最優(yōu)解F值對應的路徑，Iter=Iter+1；否則，轉移到步驟2。

步驟6：若Iter=Max_Iter，則輸出該組最優(yōu)F值對應的結果；否則k=1，轉移到步驟2。

3 MATLAB仿真與實驗

3.1 量子蟻群算法在KDX1上的應用

因不同的6R機械手臂具有不同的機械結構，這里只針對一款基于通用6R機械手臂結構而設計的KDX1進行仿真，其DH參數(shù)，如表2所示。

表2 6R機械手臂的DH參數(shù)

由式(3)隨機產(chǎn)生目標位置矩陣：

(23)

設置障礙物坐標為兩個球體，其中一個以(10,4,15)為圓心，半徑為3；另一個以(12,20,11)為圓心，半徑為3；起點為(0,0,0)。

由式(4)建立目標優(yōu)化函數(shù)F，并設α=0.5；ρ=0.7；α=6；Max_Iter=1000；m=20；Tau=0.01；pv=0.05；Δφ0和θ0均為0.05π。按式(2)的DH參數(shù)法建立最優(yōu)實際位置矩陣A6為：

(24)

按照2.11步驟求解，經(jīng)量子蟻群算法搜索出關節(jié)變量最優(yōu)解為θ1～θ6，表3給出量子尋優(yōu)后的結果。

表3 量子蟻群尋優(yōu)結果

圖4 量子蟻群算法優(yōu)化趨勢

尋優(yōu)運算時間及收斂情況如表4所示?？梢娝惴ㄔ谇?00次就迅速收斂，到0.1以下且運行時間不足0.5 s。

表4 運算時間及收斂情況表

3.2 量子蟻群與基本蟻群在優(yōu)化F中的比較

圖5 量子蟻群算法與基本蟻群算法最優(yōu)解變化趨勢比較圖 圖6 基本蟻群算法與量子蟻群算法目標函數(shù)F值比較

表5 兩種蟻群算法尋優(yōu)比較

由仿真結果可見，相對基本蟻群算法而言，量子蟻群算法收斂速度更快，運行時間更少，這也充分證明量子蟻群計算具有強大的并行能力以及采用量子非門跳出極值的優(yōu)勢。

3.3 MATLAB動態(tài)仿真及CCS聯(lián)合開發(fā)實驗

為了直觀地觀察經(jīng)量子蟻群算法尋優(yōu)出的逆解，即各個關節(jié)角，能夠在障礙物空間下具有避障能力并移動到設定的目標點整個過程，利用MATLAB中的robotic toolbox for MATLAB 9.10工具箱[18]構建系統(tǒng)仿真實驗平臺。根據(jù)表3關節(jié)變量θ1=0.734 0，θ2=0.026 5，θ3=-0.876 1，θ4=0.755 4，θ5=1.231 8，θ6=-0.125 7。

根據(jù)表2的DH參數(shù)建立6R機械手臂KDX1模型：

Joint(1)=Link([θ1,d1,a1,α1,θrange1],′standard′)
Joint(2)=Link([θ1,d2,a2,α2,θrange2],′standard′)
Joint(3)=Link([θ1,d3,a3,α3,θrange3],′standard′)
Joint(4)=Link([θ1,d4,a4,α4,θrange4],′standard′)
Joint(5)=Link([θ1,d5,a5,α5,θrange5],′standard′)
Joint(6)=Link([θ1,d6,a6,α6,θrange6],′standard′)
robot=SerialLink(Joint,′name′,6R機械手臂KDX1′)

運行仿真如圖7所示。

圖7 機械臂運行仿真

PC端MATLAB安裝TI CCS9.3硬件開發(fā)工具C2000，6個關節(jié)電機的運動控制器采用TI DSP TMS320F28335開發(fā)板，與6個關節(jié)機械手臂KDX1連接方式如圖8所示。

圖8 實驗控制結構示意圖

實驗過程：QACA通過MATLAB的S函數(shù)實現(xiàn)對F函數(shù)最值尋優(yōu)，得到機械手臂KDX1運動學逆解即6個關節(jié)變量θ=[θ1,θ2,θ3,θ4,θ5,θ6]T。以全局變量形式傳給CCS工程DSP控制部分，作為PWM輸入數(shù)組，經(jīng)占空比計算、PID控制送入驅動板。驅動板連接6個關節(jié)電機，各關節(jié)執(zhí)行角度旋轉，運動到相應的Joint1～Joint6位置，觀察末端執(zhí)行器的位置坐標與預定目標位置是否接近，期間是否觸碰障礙物球體。

關節(jié)采用MG996舵機，6路PWM輸出，20 ms控制周期，其中開通脈寬時間介于0.5 ms～2.5 ms，對應旋轉0～180°。每增加1°，需增加時長：2 ms÷180=11.1 μs。達到目標閾值小于0.01，DSP PWM波占空比按照表6設計。

表6 關節(jié)變量θ占空比對照表

整個實驗過程執(zhí)行流程如圖9所示。

圖9 實驗過程流程圖

最后觀察機械手臂末端執(zhí)行器反饋的實際位置是否達到預設目標位置閾值范圍內，即為成功求取逆解。實驗執(zhí)行結果如圖10所示。

圖10 KDX1運行結果圖

仿真實驗結果表明：利用量子蟻群算法能夠為6R機械手臂規(guī)劃出一條從起始位置S到目標位置T的一條無碰撞路徑，目標點位置誤差小于閾值0.01，可見算法應用有效且可行。

4 結論

本文利用方向層次包圍盒OBB碰撞因子和各關節(jié)的可操作空間約束以及多元函數(shù)函數(shù)求極值的思想，應用量子蟻群算法(QACA)，提出了一種新的機械手臂避障路徑規(guī)劃方法，有效地實現(xiàn)了躲避空間障礙物。提出建立基于量子Bloch球面的三維量子旋轉搜索空間方法，合理地利用蟻群算法的啟發(fā)式搜索以及正反饋機制，以及充分利用量子計算強大的并行能力、量子旋轉門以及三鏈編碼方案，并有機結合，達到了在螞蟻種群數(shù)量不變或者較少的情況下，擴展了解的空間范圍、提高了搜索效率、不易陷入局部極值的效果。與基本蟻群算法(ACA)對比仿真及實驗結果，證明了此算法在求解機械手臂運動學逆解問題中有效實用。