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

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

0 引言

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

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

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

1 復(fù)合勢(shì)場(chǎng)

1.1 約束環(huán)勢(shì)場(chǎng)

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

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

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

(2)約束環(huán)勢(shì)場(chǎng)的搭建

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

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

(1)

1.2 包絡(luò)面勢(shì)場(chǎng)

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

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

(2)

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

1.3 姿態(tài)勢(shì)場(chǎng)

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

1.3.1 廣義坐標(biāo)

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

1.3.2 角度勢(shì)場(chǎng)的搭建

作業(yè)桿的姿態(tài)分為偏航角和俯仰角。因此姿態(tài)勢(shì)場(chǎng)也分為偏航角度勢(shì)場(chǎng)UYθ和俯仰角度勢(shì)場(chǎng)UPθ。總的姿態(tài)勢(shì)場(chǎng)Uangle為UYθ和UPθ之和。

(3)

(4)

Uangle=UYθVY+UPθVP

(5)

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

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

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

(6)

(7)

1.4 復(fù)合勢(shì)場(chǎng)

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

Usum=kr∑Urep+kf∑Uface+kaUangle

(8)

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

2 OSOA算法

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

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

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

2.2 軌跡尋優(yōu)評(píng)價(jià)函數(shù)的建立

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

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

(9)

(10)

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

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

(11)

(12)

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

為評(píng)價(jià)包絡(luò)面勢(shì)場(chǎng)對(duì)軌跡的約束效果，設(shè)計(jì)評(píng)價(jià)函數(shù)如式(13)所示：

(13)

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

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

f6=1/l

(14)

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

(15)

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

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

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

(16)

式中，vij(n)和xij(n)分別為粒子i在迭代n次過后，第j維參數(shù)的速度和位置；w為粒子的慣性權(quán)重；pij為第i個(gè)粒子j維參數(shù)的極值；gj為全局粒子j維參數(shù)的極值；c1為自生的學(xué)習(xí)因子；c2為社會(huì)學(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ì)場(chǎng)系數(shù)帶入復(fù)合勢(shì)場(chǎng)中，并進(jìn)行軌跡規(guī)劃，得出規(guī)劃軌跡的動(dòng)力學(xué)參數(shù)。

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

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

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

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

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

3.1 復(fù)合勢(shì)場(chǎng)姿態(tài)控制仿真

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

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

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

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

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

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

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

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

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

試驗(yàn)參數(shù)：粒子群規(guī)模為30，最大迭代次數(shù)30次，c1=1，c2=1，w=1。由于桿姿態(tài)無要求，故優(yōu)化參數(shù)無姿態(tài)勢(shì)場(chǎng)系數(shù)，優(yōu)化參數(shù)為(kr,kf)，參數(shù)優(yōu)化空間皆為[1,100]，粒子維度為2。評(píng)價(jià)函數(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。

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

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

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

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

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

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

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

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

表2 OSOA較復(fù)合勢(shì)場(chǎng)性能提升表

4 結(jié)束語

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

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