基于改進(jìn)灰狼算法的冗余機(jī)械臂軌跡跟蹤與避障

崔靖凱，周宇飛，賀順鋒，徐振邦，朱明超*

（1.中國(guó)科學(xué)院 長(zhǎng)春光學(xué)精密機(jī)械與物理研究所，吉林 長(zhǎng)春 130033；2.中國(guó)科學(xué)院大學(xué)，北京 100049）

1 引言

機(jī)械臂在工業(yè)自動(dòng)化的進(jìn)程中具有不可替代的作用，由于其靈活性高、穩(wěn)定性強(qiáng)、成本低等優(yōu)勢(shì)，已被廣泛應(yīng)用于各個(gè)領(lǐng)域［1-2］。在工業(yè)生產(chǎn)中，機(jī)械臂常用于搬運(yùn)、組裝、焊接等任務(wù)。若機(jī)械臂的自由度數(shù)大于完成任務(wù)所需的最小自由度數(shù)，則稱其為冗余機(jī)械臂［3］。冗余的自由度有助于實(shí)現(xiàn)避障等次要的任務(wù)目標(biāo)。但同時(shí)，冗余也帶來(lái)了諸多挑戰(zhàn)，這體現(xiàn)在運(yùn)動(dòng)規(guī)劃的計(jì)算量增大，以及逆運(yùn)動(dòng)學(xué)的多解性和奇異性等方面。使冗余機(jī)械臂在跟蹤目標(biāo)軌跡的同時(shí)避開障礙物，是精密制造的重要需求，而將軌跡跟蹤和避障規(guī)劃統(tǒng)一到同一框架，設(shè)計(jì)一種計(jì)算簡(jiǎn)單且通用的解決方法，是一項(xiàng)具有實(shí)際意義和挑戰(zhàn)性的工作［4］。

機(jī)械臂的軌跡跟蹤問題是指設(shè)計(jì)控制指令使機(jī)械臂的末端執(zhí)行器沿指定的目標(biāo)軌跡運(yùn)動(dòng)。若目標(biāo)軌跡在笛卡爾工作空間給定，需要通過逆運(yùn)動(dòng)學(xué)將其映射到關(guān)節(jié)空間。對(duì)于冗余機(jī)械臂，逆運(yùn)動(dòng)學(xué)映射不是唯一的，即同一條笛卡爾軌跡將被反解為無(wú)限多條關(guān)節(jié)軌跡。傳統(tǒng)方法使用雅可比矩陣偽逆（Jacobian-Matrix-PseudoInverse，JMPI）來(lái)解決冗余問題［5］。然而，JMPI 方法具有很多局限性。首先，它只適用于求解等式約束，因此無(wú)法考慮關(guān)節(jié)角的限制；其次，它無(wú)法解決避障問題，因?yàn)楸苷铣１唤椴坏仁郊s束；此外，JMPI 方法的計(jì)算量很大，難以應(yīng)對(duì)冗余機(jī)械臂的多解性和奇異性等挑戰(zhàn)。

近年來(lái)，諸如遺傳算法、粒子群算法等元啟發(fā)式優(yōu)化算法被廣泛應(yīng)用于軌跡跟蹤。這些以優(yōu)化為核心的方法能夠在滿足軌跡跟蹤需求的前提下解決額外的不等式約束，且不依賴于機(jī)械臂的構(gòu)型和關(guān)節(jié)數(shù)量，計(jì)算量小、通用性強(qiáng)。LOPEZ-FRANCO C［6］等提出了一種元啟發(fā)式框架，通過最小化末端執(zhí)行器的位置和方向誤差完成冗余機(jī)械臂的軌跡跟蹤任務(wù)；ROKBANI N［7］等基于β-SSA 算法設(shè)計(jì)了逆運(yùn)動(dòng)學(xué)求解器，并將其應(yīng)用于冗余機(jī)械臂的圓形軌跡跟蹤。然而，這些方法沒有考慮到機(jī)械臂與周圍環(huán)境可能存在的碰撞問題。

避障是冗余機(jī)械臂在工業(yè)應(yīng)用中應(yīng)該滿足的重要需求，它能夠保證生產(chǎn)的安全和穩(wěn)定。傳統(tǒng)的避障方法使用“人工勢(shì)場(chǎng)”的概念，例如姜力［8］等利用虛擬排斥力實(shí)現(xiàn)了七自由度冗余機(jī)械臂的避障任務(wù)。元啟發(fā)式優(yōu)化算法憑借其靈活簡(jiǎn)單的優(yōu)勢(shì)，成為解決避障問題的新趨勢(shì)。AGARWAL D［9］等使用改進(jìn)的黏菌算法規(guī)劃了自主移動(dòng)機(jī)器人的無(wú)碰撞路徑；ZHANG J X［10］等將障礙物視為圓，將機(jī)械臂連桿簡(jiǎn)化為直線，基于改進(jìn)的粒子群算法實(shí)現(xiàn)了冗余機(jī)械臂的避障規(guī)劃；常寧東［11］等基于改進(jìn)的遺傳算法，設(shè)計(jì)了野外環(huán)境下車輛的無(wú)碰撞路徑。傳統(tǒng)的優(yōu)化方法通常將避障作為一系列不等式約束，這些約束不會(huì)鼓勵(lì)優(yōu)化器主動(dòng)避障，而是被動(dòng)地將解決方案標(biāo)記為不可行的解。

本文提出了一種避障跟蹤優(yōu)化器，通過制定適應(yīng)度函數(shù)來(lái)實(shí)現(xiàn)冗余機(jī)械臂的軌跡跟蹤和避障規(guī)劃。適應(yīng)度函數(shù)有兩個(gè)目標(biāo)：首先是軌跡跟蹤，即最小化機(jī)械臂末端執(zhí)行器與目標(biāo)軌跡點(diǎn)之間的位置誤差；其次是避障規(guī)劃，即最大化機(jī)械臂連桿與障礙物之間的距離。不同于傳統(tǒng)方法，本文將避障設(shè)計(jì)為獎(jiǎng)勵(lì)項(xiàng)，而不是作為不等式約束。避障獎(jiǎng)勵(lì)項(xiàng)可以實(shí)時(shí)評(píng)估解決方案的優(yōu)劣，以主動(dòng)獎(jiǎng)勵(lì)優(yōu)化器避開障礙物。因此，跟蹤和避障的本質(zhì)被簡(jiǎn)化為求解同一個(gè)優(yōu)化問題。

GJK（Gilbert-Johnson-Keerthi）算法［12］是一種高效的碰撞檢測(cè)算法，它可以計(jì)算任意形狀的三維凸多邊形之間的最小距離。在避障空間建模時(shí)，本文采用GJK 算法進(jìn)行碰撞檢測(cè)。該方法考慮了障礙物與機(jī)械臂連桿的幾何形狀，而不是將其簡(jiǎn)化為點(diǎn)或直線。因此，避障空間更切合實(shí)際情況，碰撞檢測(cè)的準(zhǔn)確度得以保證。

為了求解優(yōu)化問題，本文采用了一種元啟發(fā)式算法——灰狼算法［13］?；依撬惴M了自然界中灰狼的社會(huì)階級(jí)和狩獵行為，結(jié)構(gòu)簡(jiǎn)單、收斂性強(qiáng)。然而，經(jīng)典灰狼算法存在過早收斂和種群多樣性低的缺點(diǎn)［14］。本文通過引入隨機(jī)分散策略提出了一種改進(jìn)的灰狼算法，以增強(qiáng)算法的全局搜索能力。九自由度冗余機(jī)械臂被用于驗(yàn)證所提出方法的有效性和優(yōu)越性。

2 避障跟蹤優(yōu)化器

本節(jié)提出了一種避障跟蹤優(yōu)化器，將冗余機(jī)械臂的軌跡跟蹤和避障規(guī)劃統(tǒng)一到一個(gè)優(yōu)化框架中。首先，基于GJK 算法和包圍盒法對(duì)避障空間進(jìn)行了建模；然后，設(shè)計(jì)了適應(yīng)度函數(shù)，闡述了優(yōu)化器的基本框架。

2.1 避障空間的建模

在傳統(tǒng)方法中，機(jī)械臂連桿通常被簡(jiǎn)化為直線，障礙物被簡(jiǎn)化為圓或球，通過判斷兩者的位置關(guān)系實(shí)現(xiàn)碰撞檢測(cè)［14］。這些方法沒有考慮機(jī)械臂與障礙物的三維幾何形狀。本文使用GJK算法進(jìn)行碰撞檢測(cè)，它的基本原理是計(jì)算任意形狀的三維凸多邊形之間的最小距離。

考慮三維空間中的兩個(gè)凸多邊形A和B，它們的頂點(diǎn)由矩陣VA∈RnA×3和VB∈RnB×3定義，其中R表示實(shí)數(shù)集，nA和nB是A和B的頂點(diǎn)數(shù)量。VA和VB的每一行表示對(duì)應(yīng)多邊形頂點(diǎn)的位置坐標(biāo)。GJK 算法利用這些矩陣計(jì)算兩個(gè)多邊形頂點(diǎn)之間的最小距離：

其中：Vi：表示矩陣V的第i行；‖ · ‖2表示2-范數(shù)。

本文的研究對(duì)象是九自由度冗余機(jī)械臂，其構(gòu)型和坐標(biāo)系如圖1 所示。坐標(biāo)系使用MDH（Modified Denavit-Hartenberg）參數(shù)法構(gòu)造，其中，基坐標(biāo)系被描述為坐標(biāo)系｛b｝或｛0｝，關(guān)節(jié)i（i=1-9）的坐標(biāo)系被描述為坐標(biāo)系｛i｝，末端執(zhí)行器的坐標(biāo)系被描述為坐標(biāo)系｛e｝。xi，yi，zi分別是坐標(biāo)系｛i｝的x軸、y軸和z軸。dij是從坐標(biāo)系｛i｝的原點(diǎn)到坐標(biāo)系｛j｝的原點(diǎn)的距離。機(jī)械臂由9 個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)和8 個(gè)連桿組成。關(guān)節(jié)1，3，5，7，9繞質(zhì)心軸旋轉(zhuǎn)，不影響避障。由于機(jī)械臂連桿不是凸多邊形，本文使用包圍盒法將其建模為立方體，以滿足GJK 算法的使用條件。

圖1 九自由度冗余機(jī)械臂構(gòu)型圖Fig.1 Configuration diagram of a 9-DOF redundant robotic manipulator

圖2 展示了包圍盒的建立過程。由圖2（a）所示，連桿2-3，4-5，6-7 以及連桿8 被最小的圓柱體包圍，得到4 個(gè)虛擬連桿L1-L4，其中，每個(gè)圓柱體的底面半徑為Ri（i=1-4），虛擬連桿兩端的圓心坐標(biāo)J2，J4，J6，J8和Je分別對(duì)應(yīng)關(guān)節(jié)2，4，6，8 和末端執(zhí)行器的質(zhì)心坐標(biāo)。由圖2（b）所示，使用最小的長(zhǎng)方體將虛擬連桿包圍，根據(jù)連桿的圓心坐標(biāo)J以及半徑R，可以得到初始構(gòu)型下，長(zhǎng)方體包圍盒的8 個(gè)頂點(diǎn)坐標(biāo)。同樣地，將障礙物O用最小的長(zhǎng)方體包圍，使用式（1）可以求得機(jī)械臂與障礙物之間的最小距離dg，如圖2（c）所示。

圖2 避障空間的建模Fig.2 Model of obstacle avoidance space

需要注意的是，隨著機(jī)械臂的運(yùn)動(dòng)，頂點(diǎn)的坐標(biāo)會(huì)發(fā)生變化，即它是關(guān)節(jié)角θ的函數(shù)。設(shè)計(jì)如下的計(jì)算方法來(lái)實(shí)時(shí)獲得頂點(diǎn)坐標(biāo)：

其中：Vi(θ)∈表示第i個(gè)虛擬連桿的頂點(diǎn)矩陣，Vi(0) 是初始時(shí)刻的頂點(diǎn)信息；θ={θ1，θ2，…，θ9}是當(dāng)前時(shí)刻的關(guān)節(jié)角向量，θ1-θ9是關(guān)節(jié)1-9 的角度；Ri(θ)和Ti(θ)表示第i個(gè)虛擬連桿的旋轉(zhuǎn)和平移矩陣。

2.2 適應(yīng)度函數(shù)的設(shè)計(jì)

為了將軌跡跟蹤和避障規(guī)劃統(tǒng)一到一個(gè)優(yōu)化框架中，本文設(shè)計(jì)了如下所示的適應(yīng)度函數(shù)：

其中：O∈代表障礙物的頂點(diǎn)矩陣，no是障礙物的頂點(diǎn)數(shù)量；xt代表目標(biāo)軌跡點(diǎn)的位置坐標(biāo)；θ代表關(guān)節(jié)角向量；ferr(xt，θ)是跟蹤誤差項(xiàng)，表示機(jī)械臂末端執(zhí)行器與目標(biāo)軌跡之間的位置誤差，由式（4）計(jì)算，其中FK(·)是機(jī)械臂的正運(yùn)動(dòng)學(xué)方程；foa(O，θ)是避障獎(jiǎng)勵(lì)項(xiàng)，取機(jī)械臂與障礙物之間最小GJK 距離的倒數(shù)，如式（5）所示，其中μ是常參數(shù)，一般取μ=1；Λ是避障開關(guān)，用于控制避障任務(wù)在軌跡跟蹤中所占的比例，如Λ=0 即關(guān)閉避障功能，僅進(jìn)行軌跡跟蹤。

式（3）的設(shè)計(jì)基于這樣的動(dòng)機(jī)：最小化末端執(zhí)行器與目標(biāo)軌跡的位置誤差，同時(shí)最大化機(jī)械臂與障礙物的距離。因此，本文提出的優(yōu)化框架可以描述為：

其中：θc，，分別代表關(guān)節(jié)位置、速度和加速度約束，t代表時(shí)間；dmin是機(jī)械臂與障礙物之間最小距離的約束。對(duì)于違反約束的解，采用文獻(xiàn)［15］提出的約束處理方法，其原理是賦予這些解更差的適應(yīng)度值。

通過求解式（6）～式（8）所述的優(yōu)化問題，即可在不違反關(guān)節(jié)約束的前提下，獲得同時(shí)滿足機(jī)械臂軌跡跟蹤和避障需求的最優(yōu)關(guān)節(jié)角配置θ。值得注意的是，本文提出的優(yōu)化框架可以將笛卡爾空間的目標(biāo)軌跡映射到關(guān)節(jié)空間，而不需要計(jì)算雅可比矩陣的偽逆。這是由于式（4）所示的跟蹤誤差項(xiàng)只需要機(jī)械臂的正運(yùn)動(dòng)學(xué)方程。對(duì)于冗余機(jī)械臂，這減小了軌跡跟蹤的計(jì)算量，同時(shí)避免了傳統(tǒng)方法可能存在的奇異性問題。此外，本文引入的式（5）所示的避障獎(jiǎng)勵(lì)項(xiàng)能鼓勵(lì)優(yōu)化器主動(dòng)避開障礙物，因?yàn)楸苷闲阅芨玫慕獗活~外賦予了更好的適應(yīng)度。

3 灰狼算法與改進(jìn)

為了求解第2 節(jié)構(gòu)造的優(yōu)化問題，本節(jié)提出了一種改進(jìn)的灰狼算法。首先介紹了經(jīng)典灰狼算 法（Grey Wolf Optimizer，GWO）［13］的基本 結(jié)構(gòu)，包括社會(huì)階級(jí)、包圍獵物以及攻擊獵物；然后，提出了隨機(jī)分散策略，以改進(jìn)GWO 的性能。

3.1 社會(huì)階級(jí)

灰狼社會(huì)由四個(gè)階級(jí)組成：阿爾法（α）、貝塔（β）、德爾塔（δ）和歐米伽（ω）。α，β和δ分別是當(dāng)前種群中最優(yōu)、第二優(yōu)和第三優(yōu)的解。它們代表狼群的首領(lǐng)，距離獵物（全局最優(yōu)解）最近。剩余的狼用ω表示，它們的位置在每次迭代中根據(jù)α，β和δ而改變。

3.2 包圍獵物

狩獵的第一步是包圍獵物，這一行為由以下方程描述：

其中：Xp和X分別表示獵物和灰狼的位置；D代表灰狼和獵物之間的距離；t是當(dāng)前的迭代次數(shù)；A和C為系數(shù)變量，計(jì)算公式如下：

其中：r1和r2是［0，1］中的隨機(jī)數(shù)；在整個(gè)迭代過程中，a由2 線性減小到0，即：

其中，MaxIter表示最大迭代次數(shù)。

3.3 攻擊獵物

第二步是攻擊獵物，即狼群ω跟隨首領(lǐng)α、β和δ向獵物移動(dòng)。ω的位置更新方程如下：

其中：Xα，Xβ和Xδ分別表示α，β和δ的位置；X1，X2和X3分別表示由α，β和δ引起的位移；A1，A2和A3分別表 示α，β和δ對(duì)應(yīng)的 決定灰 狼移動(dòng) 距離的隨機(jī)變量，由式（11）計(jì)算；C1，C2和C3分別表示α，β和δ對(duì)應(yīng)的決定獵物位置在灰狼運(yùn)動(dòng)中所占比例的隨機(jī)變量，由式（12）計(jì)算。

3.4 隨機(jī)分散策略

在經(jīng)典GWO 中，由式（17）可以看出，狼群的位置完全由首領(lǐng)α，β和δ決定，這導(dǎo)致了種群多樣性的缺失，容易使算法過早收斂而陷入局部最優(yōu)。本文提出了一種隨機(jī)分散策略，引入隨機(jī)個(gè)體來(lái)影響種群更新，描述如下：

其中：Xm和Xn是種群中隨機(jī)挑選的兩個(gè)不同的個(gè)體；ρ是［0，1］中的隨機(jī)數(shù)，它用于決定隨機(jī)分散的距離。

由式（18）引起的位置更新是隨機(jī)的，這給個(gè)體逃離局部最優(yōu)提供了可能。此外，Xm和Xn的引入增加了種群多樣性，這對(duì)克服過早收斂具有重要作用。隨機(jī)分散策略模擬了灰狼在自然界中為了應(yīng)對(duì)食物短缺而分散覓食的行為。如果首領(lǐng)找到的食物不足以供應(yīng)整個(gè)狼群，部分灰狼可能會(huì)遷移到食物更加豐富的新地區(qū)。

3.5 改進(jìn)的灰狼算法

將隨機(jī)分散策略引入到經(jīng)典GWO，得到如下所示的種群更新方程：

其中：Xi(t+1)表示第i個(gè)灰狼個(gè)體的新位置；=(X1(t)+X2(t)+X3(t)和 式（17）一致，表示由領(lǐng)導(dǎo)者引起的運(yùn)動(dòng);的計(jì)算如式（18），表示由隨機(jī)個(gè)體引起的運(yùn)動(dòng)；ρ1，ρ2∈[0，1]是隨機(jī)數(shù)，用于決定運(yùn)動(dòng)的距離；B1和B2是權(quán)重因子，用于自適應(yīng)地平衡算法的局部搜索能力和全局搜索能力，計(jì)算如下：

其中，B1和B2是迭代次數(shù)t的線性變量。在搜索的初期，通過設(shè)置較大的B2使個(gè)體隨機(jī)分散到整個(gè)搜索空間，有利于全局搜索；在搜索的末期，較大的B1可以使個(gè)體快速收斂到最優(yōu)解，強(qiáng)調(diào)局部搜索。改進(jìn)GWO 的流程圖如圖3 所示。

圖3 改進(jìn)灰狼算法的流程圖Fig.3 Flowchart of improved grey wolf optimizer

3.6 計(jì)算復(fù)雜度分析

本節(jié)將評(píng)估經(jīng)典GWO 與提出的改進(jìn)GWO 的計(jì)算復(fù)雜度。首先，在種群初始化階段，經(jīng)典GWO 與改進(jìn)GWO 的計(jì)算復(fù)雜度均為O（N），N是種群大?。粚?duì)于迭代的主循環(huán)，兩個(gè)算法都要更新每個(gè)個(gè)體的位置向量，其計(jì)算復(fù)雜度為O（N·D），D是位置向量的維度；更新種群后，在O（N）的時(shí)間內(nèi)評(píng)估所有個(gè)體的適應(yīng)度值。

因?yàn)榉N群更新和適應(yīng)度計(jì)算都在迭代次數(shù)達(dá)到最大次數(shù)T時(shí)停止，所以上述計(jì)算復(fù)雜度需乘以T。截至此操作步驟，經(jīng)典GWO 與改進(jìn)GWO 的計(jì)算復(fù)雜度均為O（T·N·D）。在改進(jìn)GWO 中，隨機(jī)分散策略將部分個(gè)體分散到搜索空間中的新區(qū)域，其計(jì)算復(fù)雜度為O（N）。因此，改進(jìn)GWO 的計(jì)算復(fù)雜度依然是O（T·N·D），與經(jīng)典GWO 相同。

4 仿真與實(shí)驗(yàn)

本節(jié)使用九自由度冗余機(jī)械臂對(duì)提出的優(yōu)化器進(jìn)行了仿真和實(shí)驗(yàn)研究。對(duì)于經(jīng)典GWO 和改進(jìn)的GWO，種群大小被設(shè)置為30，最大迭代次數(shù)被設(shè)置為100，搜索的停止條件是當(dāng)前迭代次數(shù)達(dá)到最大值。目標(biāo)軌跡是由6 325 個(gè)點(diǎn)組成的半徑為0.1 m 的圓，障礙物是邊長(zhǎng)為0.1 m 的立方體。

4.1 仿真1：關(guān)閉避障功能

為了驗(yàn)證式（5）所示的避障獎(jiǎng)勵(lì)項(xiàng)的有效性，首先設(shè)置式（3）中的Λ=0，即關(guān)閉優(yōu)化器的避障功能。使用改進(jìn)的GWO 對(duì)優(yōu)化器進(jìn)行求解。圖4 展示了Λ=0 時(shí)，機(jī)械臂跟蹤圓形軌跡的構(gòu)型變化及其三視圖。從圖4 可以看出，機(jī)械臂與障礙物發(fā)生了明顯的碰撞。這是因?yàn)閮?yōu)化器計(jì)算的關(guān)節(jié)運(yùn)動(dòng)軌跡僅致力于降低末端執(zhí)行器的跟蹤誤差，而沒有考慮機(jī)械臂與障礙物之間的距離。

圖4 關(guān)閉避障功能時(shí)的構(gòu)型變化Fig.4 Configuration changes when obstacle avoidance function is turned off

4.2 仿真2：開啟避障功能

在仿真2 中，設(shè)置式（3）中的Λ=0.02，即開啟避障功能。另外，設(shè)置式（8）中的dmin=0.02 m，以約束機(jī)械臂與障礙物之間的最小距離。使用改進(jìn)的GWO 對(duì)優(yōu)化器進(jìn)行求解，機(jī)械臂的構(gòu)型變化如圖5 所示。由圖5 可以看出，開啟避障功能后，機(jī)械臂在完成軌跡跟蹤的同時(shí)成功避開了障礙物。這是由于在避障獎(jiǎng)勵(lì)項(xiàng)的鼓勵(lì)下，優(yōu)化器選擇了無(wú)碰撞的運(yùn)動(dòng)軌跡。

圖5 開啟避障功能時(shí)的構(gòu)型變化Fig.5 Configuration changes when obstacle avoidance function is turned on

圖6 展示了Λ=0.02 時(shí)，關(guān)節(jié)和末端執(zhí)行器的運(yùn)動(dòng)軌跡。圖6（a）展示了軌跡跟蹤過程中，機(jī)械臂九個(gè)關(guān)節(jié)的位置變化。關(guān)節(jié)的運(yùn)動(dòng)軌跡存在一些輕微的抖動(dòng)，這是由元啟發(fā)式算法的隨機(jī)性造成的。然而，這些抖動(dòng)并不會(huì)影響末端執(zhí)行器的跟蹤精度，且幅度沒有超出機(jī)械臂安全運(yùn)行的限制。此外，考慮到元啟發(fā)式算法對(duì)計(jì)算效率和尋優(yōu)精度帶來(lái)的顯著提升，這些輕微的抖動(dòng)是可以接受的。末端執(zhí)行器在三個(gè)方向上位置分量的變化如圖6（b）所示，該軌跡與目標(biāo)圓完全重合。

圖6 Λ=0.02 時(shí)的運(yùn)動(dòng)軌跡Fig.6 Motion trajectory for Λ=0.02

Λ=0.02 時(shí)，機(jī)械臂末端的軌跡跟蹤誤差以及機(jī)械臂連桿與障礙物之間的最小距離分別如圖7 和圖8 所示。由圖7 可以看出，跟蹤誤差集中在［10-7m，10-3m］，這證明了算法的全局收斂性，即隨迭代次數(shù)的增加，跟蹤誤差收斂到0。誤差在0 附近的波動(dòng)是由元啟發(fā)式算法固有的隨機(jī)性造成的。經(jīng)計(jì)算，平均跟蹤誤差為0.21 mm，滿足毫米級(jí)的跟蹤要求。由圖8 可知，軌跡跟蹤過程中，機(jī)械臂與障礙物的最小距離不小于70 mm，且沒有低于設(shè)定的最小限制dmin。較高的距離值降低了在障礙物位置不準(zhǔn)確或機(jī)械臂模型有誤差的情況下，發(fā)生碰撞的風(fēng)險(xiǎn)，這提高了避障的魯棒性。

圖7 Λ=0.02 時(shí)的軌跡跟蹤誤差Fig.7 Trajectory tracking error for Λ=0.02

圖8 機(jī)械臂連桿與障礙物之間的最小距離Fig.8 Minimum distance between the links and the obstacle

圖9 展示了Λ=0.02 時(shí)，優(yōu)化器在目標(biāo)軌跡的第3 000 個(gè)點(diǎn)處的收斂曲線。需要注意的是，本文研究的優(yōu)化問題是最小化問題，即適應(yīng)度值越小，解決方案越好。從圖9 可以看出，優(yōu)化器以很快的速度收斂到了很小的適應(yīng)度值。在迭代次數(shù)為20 左右時(shí)，優(yōu)化器陷入了局部最優(yōu)；然而，隨著迭代次數(shù)的增加，優(yōu)化器突破局部最優(yōu)停滯，繼續(xù)向縮小適應(yīng)度值的方向收斂。這說(shuō)明改進(jìn)GWO 中的隨機(jī)分散策略能有效地克服過早收斂，增強(qiáng)算法的全局搜索能力。優(yōu)化器在目標(biāo)軌跡的其他點(diǎn)處的收斂過程與該點(diǎn)類似，由于篇幅限制，本文不再展示。

圖9 優(yōu)化器的收斂曲線Fig.9 Convergence curve of optimizer

需要注意的是，式（3）所示的適應(yīng)度函數(shù)是由跟蹤誤差項(xiàng)和避障獎(jiǎng)勵(lì)項(xiàng)的和構(gòu)成的，避障開關(guān)Λ的取值會(huì)影響優(yōu)化器的跟蹤性能和避障性能。即增大Λ的值，將獲得更好的避障性能，但跟蹤性能會(huì)變差，這體現(xiàn)在機(jī)械臂與障礙物之間的距離增加，但跟蹤誤差變大。本文中，Λ=0.02 是通過多次實(shí)驗(yàn)獲得的能夠平衡跟蹤和避障性能的最佳值。在實(shí)際應(yīng)用中，應(yīng)綜合考慮跟蹤和避障的需求，選取合適的Λ。

為了驗(yàn)證改進(jìn)GWO 的優(yōu)越性，在相同的仿真條件下，使用經(jīng)典GWO 對(duì)優(yōu)化器進(jìn)行求解。軌跡跟蹤誤差的對(duì)比結(jié)果如表1 所示。由表1 可以看出，相比于經(jīng)典GWO，改進(jìn)的GWO 使軌跡跟蹤誤差的平均值降低了13%，這證明了改進(jìn)GWO 具有更優(yōu)越的性能。

表1 跟蹤誤差的對(duì)比結(jié)果Tab.1 Comparison results of tracking errors （mm）

最后，本節(jié)定性分析了基于改進(jìn)灰狼算法的避障跟蹤優(yōu)化器相比于傳統(tǒng)方法的區(qū)別和優(yōu)勢(shì)，如表2 所示。由表2 可以看出，本文提出的方法計(jì)算量小、通用性強(qiáng)，具有一定的應(yīng)用價(jià)值。

表2 本文方法與傳統(tǒng)方法的比較Tab.2 Comparison between the proposed method and traditional methods

4.3 實(shí)驗(yàn)驗(yàn)證

為了進(jìn)一步證明提出的避障跟蹤優(yōu)化器的可行性，使用9 自由度冗余機(jī)械臂，對(duì)仿真2 的結(jié)果進(jìn)行實(shí)驗(yàn)驗(yàn)證。實(shí)驗(yàn)平臺(tái)如圖10 所示，采樣時(shí)間設(shè)置為5 ms。

圖10 9 自由度冗余機(jī)械臂的實(shí)驗(yàn)平臺(tái)Fig.10 Experimental platform for the 9-DOF redundant robotic manipulator

將仿真2 獲得的關(guān)節(jié)運(yùn)動(dòng)軌跡輸入給上位機(jī)執(zhí)行，以驅(qū)動(dòng)機(jī)械臂進(jìn)行軌跡跟蹤和避障。障礙物由邊長(zhǎng)為0.1 m 的泡沫立方體模擬，使用塑膠桿固定。實(shí)驗(yàn)前，障礙物的位置使用激光跟蹤儀進(jìn)行了標(biāo)定。將靶標(biāo)固定在障礙物上，由激光跟蹤儀發(fā)射激光，經(jīng)靶標(biāo)反射后，獲得障礙物在跟蹤儀坐標(biāo)系中的球面極坐標(biāo)。經(jīng)過坐標(biāo)變換，即可確定障礙物在機(jī)械臂坐標(biāo)系中的三維坐標(biāo)，以保證實(shí)驗(yàn)場(chǎng)景的設(shè)置與仿真場(chǎng)景一致。實(shí)驗(yàn)結(jié)果如圖11 所示，圖中1 到4 表示機(jī)械臂的運(yùn)動(dòng)順序。由上位機(jī)測(cè)量的關(guān)節(jié)運(yùn)動(dòng)軌跡如圖12（a）所示，該軌跡與圖6（a）的仿真結(jié)果完全吻合。圖12（b）展示了由圖12（a）的關(guān)節(jié)軌跡，經(jīng)機(jī)械臂的正運(yùn)動(dòng)學(xué)方程，求解的末端執(zhí)行器的運(yùn)動(dòng)軌跡，該軌跡與圖6（b）的仿真結(jié)果一致。由此可見，避障跟蹤優(yōu)化器生成的關(guān)節(jié)軌跡安全可行，機(jī)械臂能夠準(zhǔn)確地完成軌跡跟蹤和避障任務(wù)。

圖11 軌跡跟蹤與避障的實(shí)驗(yàn)結(jié)果Fig.11 Photos of experimental results of trajectory tracking and obstacle avoidance

圖12 實(shí)驗(yàn)測(cè)量的運(yùn)動(dòng)軌跡Fig.12 Motion trajectories obtained from the experiment

5 結(jié)論

本文提出了一種基于改進(jìn)灰狼算法的避障跟蹤優(yōu)化器，以解決冗余機(jī)械臂的軌跡跟蹤和避障問題。首先，使用包圍盒法對(duì)避障空間進(jìn)行了建模，基于GJK 算法獲得機(jī)械臂連桿與障礙物的最小距離，該方法考慮了機(jī)械臂連桿與障礙物的三維幾何形狀，而不是將其簡(jiǎn)化為直線或點(diǎn)。其次，設(shè)計(jì)了適應(yīng)度函數(shù)，將軌跡跟蹤和避障任務(wù)統(tǒng)一到同一個(gè)優(yōu)化問題。其中，跟蹤誤差項(xiàng)只基于正運(yùn)動(dòng)學(xué)實(shí)現(xiàn)笛卡爾空間的軌跡向關(guān)節(jié)空間的映射，而不需要雅可比逆矩陣；避障獎(jiǎng)勵(lì)項(xiàng)能積極地鼓勵(lì)優(yōu)化器避開障礙物，而不是將避障問題簡(jiǎn)化為不等式約束。然后，基于隨機(jī)分散策略提出了改進(jìn)的灰狼算法，該算法具有更強(qiáng)的全局搜索能力。最后，使用九自由度冗余機(jī)械臂對(duì)提出的避障跟蹤優(yōu)化器進(jìn)行了仿真和實(shí)驗(yàn)驗(yàn)證。

結(jié)果表明，軌跡跟蹤的平均誤差為0.21 mm，機(jī)械臂與障礙物的最小距離不小于70 mm；相比于經(jīng)典灰狼算法，改進(jìn)灰狼算法使跟蹤誤差降低了13%?；诟倪M(jìn)灰狼算法的避障跟蹤優(yōu)化器能安全、準(zhǔn)確地實(shí)現(xiàn)9 自由度冗余機(jī)械臂的軌跡跟蹤和避障任務(wù)，隨機(jī)分散策略能有效提高經(jīng)典灰狼算法的性能。