李鐵軍，劉力源，劉今越，賈曉輝，武毅

（河北工業(yè)大學(xué) 機械工程學(xué)院，天津 300401）

相對于非冗余度機器人而言，冗余度雙臂機器 人有良好的容錯性、更好的靈巧性、更廣的適用性等特性[1-2]。冗余度機械臂獨有的自運動特點，使冗余度雙臂機器人末端執(zhí)行器執(zhí)行協(xié)作任務(wù)的同時進行運動學(xué)及動力學(xué)二次優(yōu)化[3]。隨著智能機器人越來越多地用于執(zhí)行復(fù)雜任務(wù)，因此機器人作業(yè)的安全性問題是無法回避的?？紤]機器人作業(yè)過程中與作業(yè)環(huán)境避免發(fā)生碰撞，以及機器人雙臂協(xié)調(diào)作業(yè)時避免雙臂互相碰撞問題變得尤為重要[4-5]。

局部、全局路徑規(guī)劃通常是冗余度機械臂避障研究的兩個方向[6]。對于實時避障而言，局部路徑規(guī)劃比全局路徑規(guī)劃的優(yōu)勢之處在于：其算法運算量小，使用一些視覺傳感器實時獲取作業(yè)環(huán)境中障礙物的位置信息，進而實現(xiàn)避障目標(biāo)[7]。就目前看，國內(nèi)外學(xué)者研究冗余度機器人避障方法時大都是以單臂機器人為研究對象[8-12]，冗余度雙臂機器人協(xié)調(diào)作業(yè)時避障研究僅考慮雙臂自避碰[13-14]或機器人與環(huán)境障礙物的避障問題[15]，同時考慮上述兩種情況的研究相對少。

因此，本文提出一種協(xié)作任務(wù)下基于冗余度機械臂自運動特點的冗余度雙臂機器人實時避障算法。首先對機器人做實時避障規(guī)劃中，需要盡快的完成障礙物與機器人及機器人雙臂之間的位置關(guān)系，且判斷出易出現(xiàn)碰撞的桿件，從而完成避障動作。故提出一種預(yù)篩選投影矢量，說明其建立過程，然后根據(jù)機器人協(xié)作任務(wù)下雙臂躲避障礙物的運動學(xué)逆解；再引入梯度“安全距離”指標(biāo)估算機器人雙臂之間、機械臂與環(huán)境障礙物之間碰撞程度，由此調(diào)整機械臂避障標(biāo)志點的速度，保證機器人避障動作的連續(xù)性和平滑性。實現(xiàn)冗余度雙臂機器人完成協(xié)作任務(wù)的同時機器人雙臂相互之間不碰撞及機器人躲避作業(yè)環(huán)境中的障礙物任務(wù)。

1 預(yù)篩選投影矢量參數(shù)指標(biāo)的建立

1.1 預(yù)篩選投影矢量參數(shù)

作為傳統(tǒng)的避障指標(biāo)Hmin可以很好地衡量障礙物距離機械臂的危險程度[8]。該指標(biāo)主要是先計算障礙物與機械臂的各個連桿標(biāo)志點的距離，再選擇出對應(yīng)距離最小的機械臂連桿，該桿件即為避障桿件。冗余度雙臂機器人在做避障路徑規(guī)劃的場景下，使用Hmin的方法求取避障桿件，需要遍歷所有的機械臂連桿的桿件，比較機器人雙臂連桿之間及雙臂連桿與障礙物之間的最短距離，消耗了大量的時間。

針對上述指標(biāo)的不足，本文提出了預(yù)篩選投影矢量參數(shù)指標(biāo)的方法：首先采用膠囊體包絡(luò)法簡化機械臂，對障礙物采用球形包絡(luò)。在計算障礙物與機械臂連桿間最短距離之前，先根據(jù)障礙物在機械臂桿件上的投影位置情況，粗篩選掉一些不會碰撞的桿件，再去計算可能發(fā)生碰撞的桿件與障礙物最短距離，減少掉一些不必要的運算量。

1.2 預(yù)篩選投影矢量參數(shù)評估

以平面三自由度機械臂為例，假設(shè)障礙物與機械臂的一般位置關(guān)系如圖1 所示，圖中紅色虛線部分為機械臂的膠囊體包絡(luò)體，其包絡(luò)半徑設(shè)為R, 藍色部分為障礙物的球形包絡(luò)體，其包絡(luò)半徑設(shè)為Ro。

圖1 冗余度機器人碰撞模型Fig.1 Collision model of a redundant robot

設(shè)機械臂的第i桿件關(guān)節(jié)端點li、li-1的位置矢量分別為m，n，Li為對應(yīng)的機械臂桿件長度，機械臂的第i桿件在基坐標(biāo)系{OB}下可表示為

在基坐標(biāo)系{OB}下障礙物中心在機械臂的第i桿件上的投影矢量βi為

式中c為基坐標(biāo)系{OB}下障礙物包絡(luò)中心位置。

障礙物在機械臂桿件上的投影位置情況可分為以下3 種情況：

1）當(dāng)βi>（Li+R+Ro+r）時，r為設(shè)定的距離閾值。機械臂的第i桿件為非避碰桿件，在下一步計算障礙物到機械臂的各個桿件最短距離時忽略該桿件；如圖1 中機械臂第一個連桿與障礙物的位置關(guān)系。

2）當(dāng)βi< -（R+Ro+r）時，機械臂的第i桿件為非避碰桿件，在下一步計算障礙物到機械臂的各個桿件最短距離時忽略該桿件；

3）當(dāng)（Li+R+Ro+r）≥βi≥-（R+Ro+r），機械臂的第i桿件為可能避碰桿件，為了進一步確認避碰情況需要計算障礙物到該桿件距離去判斷，如圖1中機械臂第二、三個連桿與障礙物的位置關(guān)系。

通過上述分析，冗余雙臂機器人桿件數(shù)量多，在計算障礙物和機器人各個桿件距離之前，先進行預(yù)篩選，剔除安全桿件，即不發(fā)生碰撞的桿件，只考慮可能與障礙物發(fā)生碰撞的桿件，減少不必要的計算量。

1.3 機械臂與障礙物最短距離解算

假定機械臂的第i桿件在進行1.2 節(jié)的預(yù)篩選檢測之后，判定為不安全桿件，為了進一步判斷該桿件與障礙物碰撞的危險程度，需要計算該桿件與障礙物之間的距離，進一步分析。

在基坐標(biāo)系{OB}下機械臂的第i桿件距離障礙物最近位置標(biāo)志點矢量Mi為

把障礙物和機器人機械臂體積大小考慮在內(nèi)，則障礙物距離機械臂的第i桿件最短距離表達式為

計算所有非安全桿件上標(biāo)志點與障礙物的距離，通過比較各距離值的大小，找到最短距離。

2 冗余度雙臂機器人避障運動空間

在空間機器人雙臂協(xié)作過程中，機械臂的運動是三維空間運動，機械臂躲避障礙物時，機械臂對應(yīng)的標(biāo)志點避障運動方向為沿著障礙物中心指向標(biāo)志點的直線方向，機械臂避障縮減為一維空間運動[5]。減少避障算法的計算量，提高計算效率。

以機器人主臂為例，機器人的從臂為主臂的障礙物，在求解主臂避障標(biāo)志點的避障方向時，先把主的各個桿件用式（1）～式（5）求解，得到主臂的各連桿與障礙物（包括環(huán)境障礙物與從臂）的距離，通過比較各個距離的大小，求解出最短距離d0L對應(yīng)機械臂桿件上的標(biāo)志點M0L的位置記作x0L，如圖2 所示。

圖2 冗余度雙臂機器人避障運動示意圖Fig.2 Schematic diagram of obstacle avoidance motion for redundant dual arm robots

機器人主臂上標(biāo)志點避障方向為

則縮減避障運動空間后，主臂上標(biāo)志點速度?d0L及雅克比矩陣Jd0L分別為：

同理，縮減避障運動空間后，從臂上標(biāo)志點速度?d0F及雅克比矩陣Jd0F分別為：

3 冗余度雙臂機器人協(xié)作運動學(xué)約束方程

冗余度雙臂機器人執(zhí)行搬運任務(wù)過程中，機器人雙臂與被搬運物體之間會形成一條閉式運動鏈。這條閉式運動鏈需要滿足一定的運動關(guān)系[16-17]，方可實現(xiàn)機器人雙臂的協(xié)作任務(wù)。

機器人雙臂和剛體相互間的絕對運動學(xué)約束方程為

式中：?O為剛體中心絕對速度，?O∈R6×1；?O、ωO分別為在參考坐標(biāo)系{OB}下的剛體中心絕對線速度、絕對角速度；JO為對應(yīng)的雅克比矩陣，JO∈R6×14。

機器人雙臂和剛體相互間的相對運動學(xué)約束方程為

式中：?r為剛體中心相對速度，?r∈R6×1；Jr為對應(yīng)的雅克比矩陣，Jr∈R6×14。

結(jié)合式（11）和式（12），得到冗余自由度雙臂機器人運動學(xué)表達式為

4 冗余度雙臂機器人協(xié)作策略下實時避障算法

由式（8） ～ 式（14）及冗余度機器人速度級逆運動學(xué)方程的得到冗余度雙臂機器人協(xié)作策略下的避障算法

為了高效地控制機器人雙臂躲避障礙物保證其協(xié)作任務(wù)，故引入兩個避障因子，式（14）可改為

式中：w1L、w1F分別為關(guān)節(jié)角速度因子；w2L、w2F分別為主、從臂上標(biāo)志點避障速度因子。

引入梯度“安全距離”dn（n=1, 2, 3），確保機器人雙臂避障過程中運動平滑性與連續(xù)性。其中w1L、w1F、w2L、w2F和dn之間的關(guān)系為：

w1L、w1F、w2L、w2F和梯度“安全距離”dn的關(guān)系如圖3 所示。

圖3 w1、w2 與安全距離d 之間的關(guān)系Fig.3 The relationship between w1, w2 and safety distance d

由圖3 可知，當(dāng)機械臂上標(biāo)志點與障礙物之間的距離d0i>d3時，為安全階段，障礙物對機械臂的運動沒有影響，機械臂不產(chǎn)生避障運動。式（15）可寫為

當(dāng)機械臂上標(biāo)志點與障礙物之間的距離d2

當(dāng)機械臂上標(biāo)志點與障礙物之間的距離d1

5 仿真分析

以平面冗余自由度度雙臂機器人為仿真實驗對象，驗證上述算法的可行性。如圖4 所示，機器人的基坐標(biāo)系為{OB}，qi j、lij（i=L，F(xiàn)；j=1，2，3，4）分別為機器人雙臂關(guān)節(jié)轉(zhuǎn)角、機器人雙臂各個桿件長度，其中關(guān)節(jié)轉(zhuǎn)動的正方向記為“ + ”。

圖4 平面冗余度雙臂機器人Fig.4 Planar redundant two-arm robot

避障仿真實驗在MATLAB R2019 軟件下開始。設(shè)定機器人雙臂桿件長度分別為：lL1=lF1= 240 mm，lL2=lF2= 260 mm，lL3=lF3= 200 mm，lL4=lF4= 65 mm；機器人雙臂各個關(guān)節(jié)初始轉(zhuǎn)角分別為：qL1= 60°，qL2=50°，qL3= -40°，qL4= -70°，qF1= 120°，qF2= -50°，qF3=40°，qF4=-70°。機器人包絡(luò)半徑均為10 mm，標(biāo)志點的避障速度為10.8 mm/s，梯度安全距離分別為d1=12 mm，d2= 28 mm，d3= 45 mm。

機器人雙臂協(xié)作任務(wù)是兩機械臂的末端執(zhí)行器一起夾持一個剛性物體，把剛性物體從起始位置（640，0） mm 沿直線搬運至終點位置（380，0） mm，協(xié)作空間內(nèi)環(huán)境障礙物中心的位置分別為O1（230，-210） mm，O2（220，185） mm，障礙物的半徑為 10 mm，整個仿真時長為10 s。

在未使用避障算法對冗余度平面雙臂機器人進行避障路徑規(guī)劃的情況下，機器人雙臂運動軌跡如圖5 所示。該機器人雙臂末端執(zhí)行器能夠很好地完成搬運物體的目標(biāo)協(xié)作任務(wù)。但是在t= 5 s 時，機器人雙臂的第一桿件末端發(fā)生了干涉；在t= 8.1 s時，機器人主臂的第二桿件同環(huán)境障礙物O1發(fā)生了碰撞干涉；在t= 8.5 s 時，從臂的第二桿件同環(huán)境障礙物O2發(fā)生了碰撞干涉，這也從側(cè)邊說明對機器人進行避障路徑規(guī)劃的重要性。

圖5 未使用避障算法時機械臂的軌跡Fig.5 Trajectories of without-obstacle avoidance algorithm

當(dāng)只考慮雙臂自身的影響時，機器人雙臂運動軌跡如圖6 所示。機器人雙臂末端執(zhí)行器能夠完成搬運物體的目標(biāo)協(xié)作任務(wù)，同時機器人雙臂可以相互避開。但在在t=9.3 s 時，機器人主臂的第二桿件同環(huán)境障礙物O1發(fā)生碰撞干涉，在t=9.4 s 時，從臂的第二桿件同環(huán)境障礙物O2發(fā)生碰撞干涉。

圖6 只用自避障算法時機械臂的軌跡Fig.6 Trajectories of using only self-avoidance algorithm

機器人雙臂協(xié)同作業(yè)時，同時考慮雙臂互相干涉以及環(huán)境障礙物的干涉，使用本文提出的冗余度雙臂機器人實時避障算法后，其雙臂運動軌跡如圖7 所示。此時機器人雙臂與障礙物（包括環(huán)境障礙物以及雙臂互為障礙物的情況）之間最短距離變化如圖8 所示。雙臂各個關(guān)節(jié)轉(zhuǎn)角變化如圖9 所示。

圖7 使用環(huán)境障礙物避障與自避障算法機械臂的軌跡Fig.7 Trajectories of using only self-avoidance and environmental obstacle avoidance algorithm

圖8 機器人雙臂與障礙物之間的最短距離Fig.8 The shortest distance between robot's arms and obstacles

圖9 機器人雙臂關(guān)節(jié)轉(zhuǎn)角變化Fig.9 Changes in the angle of rotation of robot arm joints

圖8 和圖9 表明：機器人雙臂桿件間的最短距離以及雙臂與環(huán)境中的障礙物之間的最短距離在t=0～2.8 s 內(nèi)大于安全距離d3，雙臂末端執(zhí)行器執(zhí)行協(xié)作任務(wù)，機械臂不執(zhí)行避障任務(wù)；隨著機器人雙臂各關(guān)節(jié)的運動，在t= 2.8～6.5 s 內(nèi)，機器人雙臂的主臂與從臂的各連桿距離最短，雙臂進入相互避障過程，機器人雙臂各自的第一桿件末端距離逐漸縮短，該距離位于d2到d3區(qū)間內(nèi)，為次安全階段，雙臂末端執(zhí)行器執(zhí)行協(xié)作任務(wù)，同時雙臂執(zhí)行避障任務(wù)；在t=6.5～10 s 時，機器人主臂第二桿件距離環(huán)境障礙物O1的距離小于主臂第一桿件距離從臂的各個桿件的最短距離，主臂開始躲避環(huán)境障礙物，隨著主臂與障礙物O1之間的最短距離逐漸縮減，機械臂的避障動作變大；在t=6.5～10 s 時，從臂的第二桿件距離環(huán)境障礙物O2的距離小于從臂的第一桿件距離主臂各個桿件的最短距離，從臂開始躲避環(huán)境障礙物，隨著從臂與障礙物O2之間的最近距離逐漸縮減，機械臂的避障動作變大。

仿真過程中，其雙臂各桿件間最短距離及雙臂與障礙物間最短距離始終大于梯度安全距離d2，由此可以看出使用本文避障算法后，不僅實現(xiàn)機器人雙臂之間的相互避障，還實現(xiàn)雙臂躲避環(huán)境障礙物，保證機器人在執(zhí)行協(xié)作任務(wù)過程中的安全性。

根據(jù)圖9 可以得出，機器人雙臂在執(zhí)行協(xié)作目標(biāo)任務(wù)以及雙臂躲避障礙物的過程中，機械臂的各個關(guān)節(jié)轉(zhuǎn)角不發(fā)生突變。因此，使用本文提出的避障算法，冗余度平面雙臂機器人順利平穩(wěn)地完成目標(biāo)協(xié)作任務(wù)及避障任務(wù)。

6 實驗驗證

為了進一步驗證文中算法的實踐應(yīng)用效果，以MOTOMAN-SDA10 冗余度空間雙臂機器人為實驗對象，它由兩個七自由度的串聯(lián)機械臂組成，其左臂為從臂（Follow），右臂為主臂（Leader），主、從機械臂為對稱式結(jié)構(gòu)，機械臂相鄰關(guān)節(jié)的旋轉(zhuǎn)軸垂直。其結(jié)構(gòu)簡圖如圖10 所示，所搭建的平臺如圖11 所示。

圖10 機器人結(jié)構(gòu)簡圖Fig.10 Schematic diagram of robot structure

圖11 實驗平臺搭建Fig.11 Establishment of an experimental platform

實驗時，圖漾相機獲取環(huán)境障礙物的位置，把障礙物的位置轉(zhuǎn)換到機器人基坐標(biāo)系下，再把障礙物的位置及機器人末端期望速度信息輸入到避障算法中，得到避障后的機器人的關(guān)節(jié)角數(shù)據(jù)，最后把關(guān)節(jié)角的數(shù)據(jù)轉(zhuǎn)為關(guān)節(jié)角脈沖發(fā)送到機器人控制柜，驅(qū)動關(guān)節(jié)伺服電機，機器人到達指定位置。

機器人雙臂的膠囊體包絡(luò)半徑設(shè)為150 mm，機械臂各關(guān)節(jié)初始轉(zhuǎn)角分別設(shè)置為：qL1= 20°，qL2=20°，qL3= 0°，qL4= 120°，qL5= 0°，qL6= -20°，qL7=0°，qF1= 20°，qF2= 160°，qF3= 0°，qF4= 120°，qF5=0°，qF6= 20°，qF7= 0°。避障礙物的速度為15.6 mm/s，梯度“安全距離”分別為：d1= 10 mm，d2= 45 mm，d3=75 mm。被搬運物體中心的初始、終止位置分別為：（569.2，0，207.2） mm，（919.2，0，-142.8） mm。

冗余度機器人雙臂協(xié)調(diào)操作的任務(wù)為：雙臂協(xié)作搬運剛性工件，工件長度為500 mm，將其從初始位置勻速沿直線搬運到末端位置，給定兩個半徑r=30 mm 的球形障礙物，球中心位置分別為：O1=（500, -488, -8） mm，O2= （475, 545, -7） mm。冗余度雙臂機器人協(xié)作運動過程如圖12 所示，雙臂協(xié)作過程中，機器人雙臂與障礙物（包括環(huán)境障礙物以及雙臂互為障礙物的情況）之間最短距離變化如圖13 所示，此過程中機器人雙臂各個關(guān)節(jié)轉(zhuǎn)角變化如圖14 所示。

圖12 考慮機器人本體互為障礙和環(huán)境障礙物的影響下機器人協(xié)作運動情況Fig.12 The cooperative motion of a robot is considered when the influence of the arms are mutual obstacles and environmental obstacles

圖13 機器人雙臂與障礙物之間的最短距離Fig.13 The shortest distance between the two arms of the robot and obstacles

由圖12～圖14 可知：采用本文算法進行避障規(guī)劃，各關(guān)節(jié)運動連續(xù)、平穩(wěn)，且可保證雙臂各連桿之間、雙臂各連桿與環(huán)境障礙物之間的最近距離始終大于設(shè)定的閾值d2。保證機器人雙臂協(xié)作過程中的安全，且雙臂能夠完成協(xié)作任務(wù)。

7 結(jié)論

針對冗余度雙臂機器人操作環(huán)境的復(fù)雜性，提出了機器人實時自身避障及躲避環(huán)境障礙物的協(xié)作路徑規(guī)劃算法。通過預(yù)篩選投影矢量指標(biāo)，篩選掉機械臂中不會碰撞的桿件，減少計算量，再實時計算可能發(fā)生碰撞的桿件與其余機械臂桿件及環(huán)境障礙物之間的最短距離，并與設(shè)定的梯度“安全距離”比較，評估障礙物與機械臂碰撞的危險程度，并實時改變機械臂上標(biāo)志點的避障速度，最后通過仿真及實驗驗證了該算法的可行性，確保機器人雙臂執(zhí)行協(xié)作作業(yè)過程的安全。