長作業(yè)桿復(fù)雜環(huán)境下自尋優(yōu)避障規(guī)劃方法研究*

朱瑞明，李啟光，馬 飛，李 超

(1.北京信息科技大學(xué)機(jī)電工程學(xué)院，北京 100192；2.北京北開電氣股份有限公司，北京 100176)

0 引言

在壓鑄工藝中有從半封閉熔爐中打撈殘渣的工序，目前多是高危環(huán)境下的人工作業(yè)，亟需采用機(jī)械臂取代人工。機(jī)械臂控制作業(yè)桿末端杷沿著設(shè)定的作業(yè)軌跡清理，同時避免作業(yè)桿與熔爐口碰撞；作業(yè)過程中為實現(xiàn)打撈作業(yè)要求及防止殘渣掉落，需兼顧作業(yè)桿姿態(tài)限制；此外控制作業(yè)桿的機(jī)械臂腕關(guān)節(jié)若與熔爐以及其他周邊設(shè)備發(fā)生干涉，還需要對腕關(guān)節(jié)軌跡做一定的空間范圍限制。在這類半封閉殼體環(huán)境下，作業(yè)桿作業(yè)需滿足避開障礙物、姿態(tài)可控、機(jī)械臂工作空間范圍可調(diào)三類規(guī)劃要求。

避障路徑規(guī)劃最常用的是人工勢場法[1]，因計算效率高且實時性好，廣泛運用于避障規(guī)劃中；由于在很多環(huán)境下都無法直接應(yīng)用，所以大量學(xué)者對該方法進(jìn)行了改進(jìn)。陳勁峰等[2]引入距離調(diào)節(jié)因子和屏蔽無效障礙的策略，解決動態(tài)避障逃離最優(yōu)問題；郭彤穎等[3]將蟻群算法和人工勢場法結(jié)合，實現(xiàn)更佳的逃離局部最優(yōu)效果；陳滿意等[4]提出了低振蕩人工勢場-ARRT混合算法，實現(xiàn)多障礙環(huán)境下的機(jī)械臂末端路徑規(guī)劃；WANG等[5]提出了一種基于改進(jìn)吸引力勢函數(shù)的人工勢場法，實現(xiàn)機(jī)械臂避障過程中關(guān)節(jié)角速度的平滑過渡；劉冰雁等[6]研究了改進(jìn)人工勢場法，實現(xiàn)衛(wèi)星變軌過程中的避障路徑規(guī)劃；曹博等[7]提出了基于冗余機(jī)械臂的人工勢場避障算法實現(xiàn)冗余機(jī)械臂逃離局部最小的規(guī)劃；史亞飛等[8]在傳統(tǒng)勢場中添加速度勢場，可以有效逃離局部最小點。根據(jù)以上分析，前人研究都是基于機(jī)械臂或移動機(jī)器人繞開障礙物以及逃離局部最小點這兩類問題的改進(jìn)算法[9-10]，并沒有涉及到半封閉殼體環(huán)境下的長桿避障與姿態(tài)控制等運用場景。

針對上述問題，本文提出一個在線自優(yōu)化的避障算法OSOA。該算法先將半封閉殼體障礙物簡化為若干空間桿組成的約束環(huán)等效模型，提出約束環(huán)勢場概念，以解決半封閉殼體避障問題；在此基礎(chǔ)上又引入姿態(tài)勢場以解決作業(yè)桿姿態(tài)控制問題；引入包絡(luò)面斥力勢場適配驅(qū)動端軌跡空間范圍約束問題，并將上述三種勢場加權(quán)疊加形成復(fù)合勢場。由于作業(yè)障礙環(huán)境變化不定，按經(jīng)驗適配出的各勢場權(quán)重值難以規(guī)劃出最優(yōu)作業(yè)軌跡，本文進(jìn)一步將復(fù)合勢場與智能尋優(yōu)算法結(jié)合，實現(xiàn)了在線自優(yōu)化的避障算法，即OSOA算法。

1 復(fù)合勢場

1.1 約束環(huán)勢場

(1)約束環(huán)模型

如圖1a所示為作業(yè)桿出入熔爐殼體通道，標(biāo)示的兩個碰撞邊緣是殼體與作業(yè)桿易發(fā)生碰撞區(qū)域，提取出此邊緣輪廓，形成如圖1b所示的兩個空間線段組成的閉環(huán)，稱該閉環(huán)為半封閉殼體的等效約束模型。作業(yè)桿穿過約束環(huán)內(nèi)部，不會與殼體碰撞。組成約束閉環(huán)的線段稱為障礙桿，約束環(huán)是空間障礙桿組成的空間閉環(huán)。

(a)殼體三維圖 (b)殼體等效約束環(huán)模型

(2)約束環(huán)勢場的搭建

在約束環(huán)勢場中，傳統(tǒng)人工勢場中的兩點之間的斥力勢場轉(zhuǎn)變成障礙桿和作業(yè)桿兩條直線之間的斥力勢場。該斥力勢場將作業(yè)桿限制在約束環(huán)內(nèi)部，避免與約束環(huán)觸碰。如圖2所示是處于約束環(huán)勢場下作業(yè)桿作業(yè)過程示意圖，其中作業(yè)端沿著預(yù)定作業(yè)軌跡運動，作業(yè)桿在兩個約束環(huán)共8個障礙桿的斥力勢場約束下，規(guī)劃出驅(qū)動端的驅(qū)動軌跡，即避障軌跡。式(1)為約束環(huán)勢場Urep計算方法，其中，LS為障礙桿和作業(yè)桿之間的距離；Vr為勢場的單位方向向量，由障礙桿垂直指向作業(yè)桿；L0為勢場的最大作用范圍。

圖2 約束環(huán)軌跡規(guī)劃示意圖

(1)

1.2 包絡(luò)面勢場

為保障驅(qū)動端避障軌跡處于機(jī)械臂工作空間內(nèi)并避開干涉空間，提出了包絡(luò)面勢場概念。本文將包絡(luò)空間設(shè)定為一空間長方體，組成約束空間的各個面即為包絡(luò)面，如圖3所示。驅(qū)動端軌跡上的各個節(jié)點都會受到指向包絡(luò)空間內(nèi)部的驅(qū)動力，使整個驅(qū)動端軌跡都會約束在包絡(luò)空間內(nèi)部。式(2)為包絡(luò)面勢場公式。

圖3 包絡(luò)面示意圖

(2)

式中，Uface為包絡(luò)面勢場；Kf為包絡(luò)面勢場系數(shù)；Lf為作業(yè)桿驅(qū)動端節(jié)點到包絡(luò)面的距離；L1為限幅常數(shù)；Vf為包絡(luò)面勢場單位方向向量。

1.3 姿態(tài)勢場

姿態(tài)勢場可以在避障的前提下對作業(yè)桿的姿態(tài)進(jìn)行小范圍的約束控制。

1.3.1 廣義坐標(biāo)

作業(yè)桿的作業(yè)路徑坐標(biāo)不只空間三維坐標(biāo)，還包括對應(yīng)的姿態(tài)坐標(biāo)，即S=[P,θ]。其中，S為廣義坐標(biāo)；P為笛卡爾空間下三維坐標(biāo)；θ為作業(yè)桿的姿態(tài)坐標(biāo)。

1.3.2 角度勢場的搭建

作業(yè)桿的姿態(tài)分為偏航角和俯仰角。因此姿態(tài)勢場也分為偏航角度勢場UYθ和俯仰角度勢場UPθ?？偟淖藨B(tài)勢場Uangle為UYθ和UPθ之和。

(3)

(4)

Uangle=UYθVY+UPθVP

(5)

式(3)中，UYθ為偏航角度勢場大?。沪萗S和θYB分別為某作業(yè)節(jié)點上作業(yè)桿的偏航角設(shè)定的最小角度和最大角度，其組成了偏航角度的合適范圍；Kθ為角度勢場系數(shù)；θY為當(dāng)前作業(yè)桿的偏航角度；θ0為限幅常數(shù)，俯仰角度勢場同理。

式(5)中，Uangle為總的角度勢場；VY與VP分別為偏航角度勢場和俯仰角度勢場的單位方向向量。如圖4所示,OA為作業(yè)桿；O為作業(yè)桿作業(yè)端；A為作業(yè)桿驅(qū)動端；OB為作業(yè)桿在XOY平面上的投影?？梢郧蟪鯲Y與VP如式(6)、式(7)所示。

圖4 作業(yè)桿空間位姿示意圖

(6)

(7)

1.4 復(fù)合勢場

將約束環(huán)勢場、姿態(tài)勢場、包絡(luò)面勢場加權(quán)疊加后形成復(fù)合勢場如下：

Usum=kr∑Urep+kf∑Uface+kaUangle

(8)

式中，Usum為復(fù)合勢場；kr、kf、ka分別為約束環(huán)勢場、包絡(luò)面勢場、姿態(tài)勢場的權(quán)重系數(shù)。

2 OSOA算法

雖然運用復(fù)合勢場可以實現(xiàn)在半封閉殼體下的避障路徑規(guī)劃，但很難在不同環(huán)境下憑經(jīng)驗權(quán)重系數(shù)規(guī)劃出最優(yōu)的軌跡。因此本文提出了復(fù)合勢場法和智能尋優(yōu)算法相結(jié)合的OSOA算法，利用尋優(yōu)算法在線優(yōu)化復(fù)合勢場的各權(quán)重系數(shù)。

2.1 優(yōu)化參數(shù)

因為在障礙環(huán)境不變的情況下，復(fù)合勢場各權(quán)重系數(shù)是影響規(guī)劃避障路徑的主要因素，設(shè)置待優(yōu)化各權(quán)重系數(shù)為向量k=[kr,kf,ka]T。

2.2 軌跡尋優(yōu)評價函數(shù)的建立

作業(yè)桿驅(qū)動端的速度、加速度、角速度、角加速度、工作空間等指標(biāo)均與驅(qū)動機(jī)械臂有關(guān)。因此這些指標(biāo)可視為判別軌跡優(yōu)劣的參考指標(biāo)。

最大速度與最大角速度會受到機(jī)械臂額定性能限值，故提出評價函數(shù)如下：

(9)

(10)

式(9)中，f1為軌跡規(guī)劃過程中最大速度的評價函數(shù)；s為避障軌跡路徑；ds為軌跡規(guī)劃過程中的速度值；式(10)中，f2為最大加速度的評價函數(shù)；β為作業(yè)桿的角度路徑；dβ為角速度值。

避障軌跡中加速度和角加速度大小會影響機(jī)械臂的穩(wěn)定及動態(tài)響應(yīng)，因此設(shè)計最大加速度和角加速度評價函數(shù)如下：

(11)

(12)

式中，f3為加速度評價函數(shù)；d2s為軌跡規(guī)劃的加速度；f4為角加速度評價函數(shù)；d2β為角加速度。

為評價包絡(luò)面勢場對軌跡的約束效果，設(shè)計評價函數(shù)如式(13)所示：

(13)

式中，f5為包絡(luò)空間約束效果的目標(biāo)優(yōu)化函數(shù)；pi為軌跡上的點；p∈c為避障軌跡在包絡(luò)空間內(nèi)部；ci-pi為點pi到各個包絡(luò)空間各個面的距離。

此外增加了一個遠(yuǎn)離約束環(huán)的評價函數(shù)，如式(14)中f6所示，l為作業(yè)桿與約束環(huán)最近距離。

f6=1/l

(14)

綜合式(9)～式(14)，得尋優(yōu)評價函數(shù)如下：

(15)

式中，f為綜合評價函數(shù)w1～w6為各評價函數(shù)的權(quán)值且和為1；f1goal～f6goal為估計優(yōu)化值，作用為使f1～f6各評價函數(shù)優(yōu)化值調(diào)整到同一數(shù)量級。

2.3 運用粒子群算法優(yōu)化權(quán)重系數(shù)

本文以粒子群算法為例對各權(quán)重系數(shù)k=[kr,kf,ka]T進(jìn)行優(yōu)化，粒子群算法的維度設(shè)為3，即第i個粒子可以編碼成xi={xi1,xi2,xi3},在粒子群算法中，算法的每次迭代都會更新自身位置和速度，公式如下：

(16)

式中，vij(n)和xij(n)分別為粒子i在迭代n次過后，第j維參數(shù)的速度和位置；w為粒子的慣性權(quán)重；pij為第i個粒子j維參數(shù)的極值；gj為全局粒子j維參數(shù)的極值；c1為自生的學(xué)習(xí)因子；c2為社會學(xué)習(xí)因子；r1與r2為(0，1)之間的隨機(jī)數(shù)。

2.4 OSOA算法流程

OSOA算法流程圖如圖5所示，具體算法流程如下：

圖5 OSOA算法流程圖

步驟1: 確定算法參數(shù)，如種群規(guī)模，迭代次數(shù)，權(quán)值范圍，優(yōu)化區(qū)間，初始化粒子群等；

步驟2: 將粒子參數(shù)作為復(fù)合勢場系數(shù)帶入復(fù)合勢場中，并進(jìn)行軌跡規(guī)劃，得出規(guī)劃軌跡的動力學(xué)參數(shù)。

步驟3: 將步驟2中的動力學(xué)參數(shù)帶入評價函數(shù)式(15)中，計算各個粒子的適應(yīng)度。

步驟4: 由步驟3求得的粒子適應(yīng)度帶入式(17)中更新粒子的位置、速度以及粒子群的極值。

步驟5: 判斷算法是否超出最大迭代次數(shù)或粒子是否收斂于設(shè)定區(qū)間，若否，轉(zhuǎn)至步驟2繼續(xù)迭代，若是，輸出最優(yōu)粒子。

步驟6：將最優(yōu)粒子參數(shù)作為復(fù)合勢場系數(shù)，規(guī)劃出最優(yōu)軌跡。

3 仿真及結(jié)果分析

3.1 復(fù)合勢場姿態(tài)控制仿真

為驗證復(fù)合勢場中的姿態(tài)勢場對作業(yè)桿姿態(tài)規(guī)劃能力，構(gòu)建仿真試驗環(huán)境如圖6所示，其中約束環(huán)由一個四邊型框和一個空間立體的六邊形組成。作業(yè)軌跡為點畫線S,起點S1、終點S2。假設(shè)工序要求作業(yè)桿在S1姿態(tài)為(10,80)度,S2姿態(tài)為(0,80)度。姿態(tài)第一個數(shù)值為作業(yè)桿偏航角，第二個數(shù)值為俯仰角。

圖6 姿態(tài)控制三維圖

具體試驗步驟如下：在作業(yè)桿作業(yè)端沿著軌跡S從S1運動到S2時，無姿態(tài)勢場下規(guī)劃的驅(qū)動端位姿從L1運動到L2；姿態(tài)勢場下的驅(qū)動端位姿會從L1θ運動到L2θ。

仿真規(guī)劃出的驅(qū)動端試驗數(shù)據(jù)如表1所示?？梢钥闯鲎藨B(tài)勢場下L1θ、L2θ角度貼近工序要求的角度，而未經(jīng)姿態(tài)勢場規(guī)劃的L1、L2角度不滿足要求。

表1 作業(yè)軌跡坐標(biāo)集

作業(yè)桿角度規(guī)劃如圖7所示。從該試驗看出無姿態(tài)勢場規(guī)劃的避障軌跡中作業(yè)桿姿態(tài)不可控，加上姿態(tài)勢場可以有效地控制作業(yè)桿在作業(yè)時的姿態(tài)。

圖7 作業(yè)桿姿態(tài)圖

3.2 OSOA路徑尋優(yōu)仿真

為驗證OSOA算法的尋優(yōu)能力，搭建試驗環(huán)境為實際熔爐的等效約束環(huán)，作業(yè)軌跡為一橢圓，四周無干涉物，作業(yè)任務(wù)對桿姿態(tài)無要求，作業(yè)時間為20 s。

試驗參數(shù)：粒子群規(guī)模為30，最大迭代次數(shù)30次，c1=1，c2=1，w=1。由于桿姿態(tài)無要求，故優(yōu)化參數(shù)無姿態(tài)勢場系數(shù)，優(yōu)化參數(shù)為(kr,kf)，參數(shù)優(yōu)化空間皆為[1,100]，粒子維度為2。評價函數(shù)參數(shù)為：w1=0.1，w2=0，w3=0.1，w4=0，w5=0.4，w6=0.4，f1goal=0.1，f2goal=0.05，f3goal=0.1，f4goal=0.05，f5goal=0.2，f6goal=0.01。

試驗結(jié)果分析：初始規(guī)劃如圖8所示，粒子群初始在優(yōu)化空間隨機(jī)分布，初始軌跡由復(fù)合勢場規(guī)劃得出，可以看出雖然避障軌跡在包絡(luò)空間內(nèi)，但所占空間范圍較大。經(jīng)過5次迭代優(yōu)化后，粒子群分布圖以及軌跡優(yōu)化圖如圖9所示，粒子逐漸往評價函數(shù)最小處靠攏，避障軌跡逐漸縮小。迭代30次完成后，形成如圖10所示的最終優(yōu)化圖，粒子收斂于kr=5.1，kf=100處，此時規(guī)劃出最優(yōu)軌跡。

(a)初始化粒子分布圖 (b)初始軌跡

(a)迭代5次粒子分布圖 (b)迭代5次優(yōu)化軌跡

(a)迭代完成粒子分布圖 (b)最終優(yōu)化軌跡

初始軌跡和最優(yōu)軌跡的運動學(xué)性能對比如圖11～圖13所示，由圖11可以明確看出最優(yōu)軌跡所占空間比初始軌跡小，對機(jī)械臂的靈活工作空間范圍需求更?。挥蓤D12可以看出最優(yōu)軌跡的速度比初始軌跡的速度更平緩，對電機(jī)的負(fù)載要求更??；由圖13可以看出最優(yōu)軌跡加速度整體趨近于零，表明最優(yōu)軌跡對機(jī)械臂的負(fù)載明顯小于初始軌跡。最終的優(yōu)化幅度如表2所示。

圖11 規(guī)劃軌跡位置曲線圖

圖12 規(guī)劃軌跡速度曲線圖

圖13 規(guī)劃軌跡加速度曲線圖

表2 OSOA較復(fù)合勢場性能提升表

4 結(jié)束語

提出復(fù)合勢場方法，解決了半封閉殼體多約束環(huán)境下長作業(yè)桿避障規(guī)劃問題，且復(fù)合勢場不限于文中提出的幾種拓展勢場，可以根據(jù)約束環(huán)境進(jìn)一步拓展新的勢場。在此基礎(chǔ)上提出OSOA算法，實現(xiàn)在不同作業(yè)障礙環(huán)境下自尋優(yōu)獲取最優(yōu)路徑。OSOA算法將作業(yè)桿驅(qū)動端位置、速度、加速度等不同量綱的約束條件融于評價函數(shù)中，為后續(xù)進(jìn)一步依據(jù)作業(yè)桿驅(qū)動系統(tǒng)自身特性指標(biāo)優(yōu)化最優(yōu)路徑提供可行的途徑。

本文算例中OSOA算法仿真時間約為128 s左右，只適用于靜態(tài)障礙環(huán)境的避障規(guī)劃，對于動態(tài)環(huán)境下的實時避障規(guī)劃需要進(jìn)一步提高尋優(yōu)效率。