趙 京, 李世龍, 龔世秋

(北京工業(yè)大學(xué)材料與制造學(xué)部, 北京 100124)

隨著機器人的應(yīng)用范圍越來越廣,冗余機械臂憑其在避障、避奇異、避極限、關(guān)節(jié)容錯等方面突出的性能,成為當(dāng)前研究的熱點[1].

避障路徑規(guī)劃是冗余機械臂控制的基本問題之一[2-3],目前常用的冗余機械臂動態(tài)避障算法主要為人工勢場法和梯度投影法. 人工勢場法憑借其算法簡單、適應(yīng)動態(tài)障礙以及不需要逆運動學(xué)求解的獨特優(yōu)勢得到廣泛研究[4],針對其算法本身具有局部最小解、目標(biāo)不可達等問題，已經(jīng)有許多學(xué)者進行了相關(guān)的改進擴展研究,如針對移動式機械臂的路徑規(guī)劃問題[5],通過修改勢能函數(shù)繞開局部極小點[6]等. 梯度投影法能夠在保證末端位姿約束的前提下,利用零空間矩陣映射將避障速度映射到冗余機械臂的自運動中[7],在避障的同時能夠良好地體現(xiàn)出機械臂的冗余特性,實現(xiàn)避障次要任務(wù)[8].

上述方法在其各自適應(yīng)的領(lǐng)域取得了良好的避障效果,但卻有各自的局限性:梯度投影法,主要用在機械臂末端有具體的任務(wù)軌跡約束情況下[9],其避障效果常常因自運動的較小轉(zhuǎn)動幅度而受到限制[10-11];而改進人工勢場法,是以引入額外的計算項來解決原算法的缺陷[12-13],其在機械臂的運動控制應(yīng)用中主要有2個層面:一種是在笛卡兒空間構(gòu)建引力和斥力,并將合力通過雅可比矩陣轉(zhuǎn)換為對機械臂的速度控制[14],另一種是基于關(guān)節(jié)鄰近空間搜索的不斷迭代,對當(dāng)前關(guān)節(jié)位形下的所有相鄰位形全部進行勢能計算以篩選最優(yōu)解[15],這2種方法都會隨著機械臂自由度的增多導(dǎo)致計算量急劇增大,已無法滿足在動態(tài)障礙物環(huán)境下的實時需求.

因此,針對當(dāng)前廣泛采用的七自由度冗余機械臂,以及無末端軌跡約束的條件下,本文通過分析冗余機械臂的結(jié)構(gòu)特點,提出了具有高實時性的避障算法,并通過仿真驗證了算法的有效性.

1 避障策略

1.1 避障基本思路

目前,在有障礙物環(huán)境下的機器人的規(guī)劃控制策略有很多種,如通過不同手段以求解運動軌跡的A*、RRT等,或是引入新的評判概念以減小計算量的人工勢場、偽距離等. 但無論控制策略的形式如何多樣化,其關(guān)鍵過程主要分為2個階段:1)達點過程中與障礙物的靠近與遠離; 2)即將碰撞時的避障規(guī)劃,如圖1所示.

圖1 機械臂避障策略Fig.1 Obstacle avoidance strategy of manipulator

本文著手于冗余機械臂在向目標(biāo)達點運動過程中如何快速避開障礙物. 對于冗余機械臂而言,其末端滿足目標(biāo)位姿的關(guān)節(jié)位形解有無數(shù)種,如何在無數(shù)種解中選擇一種作為目標(biāo)關(guān)節(jié)位形進行達點運動,同時如何在運動過程中即將靠近障礙物時進行避障規(guī)劃,是本文的主要研究內(nèi)容.

1.2 7R機械臂模型

本文以常見的七自由度冗余串聯(lián)正交機械臂為研究對象,其結(jié)構(gòu)及關(guān)節(jié)配置如圖2所示.

圖2 7R機械臂的結(jié)構(gòu)模型Fig.2 Structure model of 7R manipulator

此類型的七自由度機械臂可模仿人手臂結(jié)構(gòu)進行關(guān)節(jié)布置,θ1、θ2、θ3組合模擬人臂肩關(guān)節(jié)運動,θ5、θ6、θ7組合模擬腕關(guān)節(jié)運動,中間θ4模擬肘關(guān)節(jié)運動. 該構(gòu)型的機械臂能夠在末端位姿保持不變的同時,其肘部E點可以繞肩部S點與腕部W點的連線進行自運動,能夠良好地體現(xiàn)出冗余機械臂的冗余特性,如圖3所示.

圖3 7R機械臂自運動示意圖Fig.3 Self motion diagram of 7R manipulator

1.3 達點階段

在給定了機械臂末端需要達點的目標(biāo)位姿,關(guān)節(jié)空間中存在著無數(shù)組解,這些不同的解的集合即為滿足目標(biāo)位形的自運動解集. 如何從解集中求得合適的位形解,就需要從這些解的集合中根據(jù)條件需求進行選擇,按相關(guān)指標(biāo)確定合適的肘部自運動角,即可得到唯一逆解[16].

以自運動角變量φ為輸入?yún)?shù),即可得到對應(yīng)的關(guān)節(jié)角度逆解集合Q,再根據(jù)目標(biāo)點、障礙物以及機械臂三者之間的關(guān)系,按設(shè)定指標(biāo)即可快速求出解集中的合適逆解,主體表達公式為

Q={q|q=f(R,P,φ),φ∈[0,2π]}

(1)

式中:R、P分別為機械臂末端的姿態(tài)矩陣和位置向量；φ為自運動角變量；q為對應(yīng)自運動角φ的關(guān)節(jié)逆解向量；Q為對應(yīng)不同自運動角的q的集合.

1.4 避障階段

當(dāng)障礙物靠近機械臂連桿時,將最接近障礙物的桿件設(shè)為主要危險桿件,對其進行避障引導(dǎo). 根據(jù)串聯(lián)機械臂的運動特性分析,其控制方式有2個特點：1)單根連桿的位置和姿態(tài)僅由靠近基座一側(cè)(前側(cè))的多個關(guān)節(jié)進行耦合控制,而與靠近機械臂末端執(zhí)行器一側(cè)(后側(cè))的關(guān)節(jié)無關(guān),若前側(cè)的關(guān)節(jié)不變,無論后側(cè)的關(guān)節(jié)怎么變化,這根連桿的位置和姿態(tài)都不會變化,因此要使最近桿遠離障礙物僅需對桿件前側(cè)的關(guān)節(jié)進行控制即可;2)當(dāng)連桿運動形式以靠近基座的上一根連桿為參照物時,可視為以桿長為半徑、兩桿連接點為球心的球體運動，如圖4所示.

圖4 避障的等效控制示意圖Fig.4 Equivalent control schematic of avoidance

圖4中距障礙物最近桿為L3桿,其相對于L2桿的運動由連桿所在的前2個關(guān)節(jié)θ3、θ4進行控制,因此通過前2個關(guān)節(jié)即可對最近桿進行一定程度的運動控制,在求逆解時也具有計算量低的優(yōu)勢.

但是,僅靠控制單一桿件避障會有一定的局限性. 仍以圖4所示為例,當(dāng)障礙物較靠近連接點E時,僅對L3桿控制時無法高效地避開障礙物,無論L3桿如何轉(zhuǎn)動,障礙物距連接點E仍較近,因此結(jié)合串聯(lián)機械臂的控制特點,將對L3桿的避障控制退化到前側(cè)的L2桿上,在對最近桿避障控制的同時,也對靠近基座的上一連桿進行避障控制,從而使機械臂有效地避開障礙物.

2 避障實施算法

2.1 障- 桿距離計算

機械臂在執(zhí)行操作任務(wù)過程中,與障礙物的碰撞檢測是進行避障規(guī)劃的基礎(chǔ). 目前常用的碰撞檢測采用線段- 球體包絡(luò)盒檢測：將障礙物用球體包絡(luò),同時將機械臂的桿件用圓柱體包絡(luò). 對于隨機運動的動態(tài)障礙物,保證障礙物位置檢測的效率尤為重要,傳統(tǒng)的在歐式空間的點線距離計算方法在高維機械臂運動過程中,需要對每根連桿構(gòu)建直線方程,計算量大,且易引起計算誤差. 因此本文引入機械臂連桿之間的坐標(biāo)系轉(zhuǎn)換概念,每根桿件i都有相對于基系的轉(zhuǎn)換矩陣0Ti,將障礙物坐標(biāo)逆向投影到每根連桿的局部坐標(biāo)系iPobs(x,y,z)中,以簡單的矩陣運算來快速計算障礙物與每根桿件的距離,障礙物在連桿i坐標(biāo)系下的坐標(biāo)為

iPobs=(0Ti)-10Pobs

(2)

依據(jù)障礙物與桿件的位置關(guān)系,可知在連桿坐標(biāo)系下的桿障位置情況主要有3種,如圖5所示.

圖5 障礙物在桿件坐標(biāo)系中的3種位置情況Fig.5 Three positions of obstacles in the rod coordinate

根據(jù)不同的位置關(guān)系,可快速計算得到障礙物到桿件i的距離di為

(3)

(4)

式中:Li連桿的桿長；robs、rlink分別為障礙物和桿件的包絡(luò)盒半徑；dmin為障礙物到機械臂連桿的最小距離. 當(dāng)障礙物靠近連桿L1時,由于此桿件固連于基座,關(guān)節(jié)的轉(zhuǎn)動對于桿障關(guān)系沒有影響,故此情況條件忽略不予探究. 同時可得到連桿i上與障礙物最近點Pn的坐標(biāo).

2.2 單桿件避障

在確定與障礙物最接近的連桿后,根據(jù)桿障的幾何位置關(guān)系,對此連桿進行避障規(guī)劃.

如圖6(a)所示,E-W-末端為機械臂在達點運動過程中的當(dāng)前位形,Et-Wt-目標(biāo)位形的末端為從自運動解集中優(yōu)選出的目標(biāo)位形,Pobs為障礙物. 此時桿件已無法由當(dāng)前位形直接運動到目標(biāo)位形,本文在最近點處引入2個避障向量vavo與vatt,兩者共同作用引導(dǎo)此桿件避開障礙物,如圖6(b)所示. 圖中Pn為連桿上與障礙物最近點,Pt,n為在目標(biāo)位形上的對應(yīng)相同點,Pi,1與Pi,2分別為連桿i靠近基座和機械臂末端的2個端點,vavo為垂直躲避分量,其方向垂直于最近點Pn與障礙物Pobs的連線,vatt為吸引分量,沿Pn指向Pt,n,兩者表示為

圖6 機械臂與障礙物碰撞圖Fig.6 Collision diagram of robot arm and obstacle

(5)

式中vavo的默認方向為由點E、W、Pobs三點構(gòu)成的平面的法線方向,而本文需要其方向為指向遠離障礙物的一側(cè),即在平面的兩側(cè)中與點Pt,n相同的一側(cè),以達到快速避障并達點的效果. 因此,對vavo的方向進行判別并優(yōu)化,若其方向與點Pt,n在平面不同側(cè),則避障分量vavo的方向取反,若相同則不變.

(6)

式(6)得到的是標(biāo)準(zhǔn)單位大小的避障引導(dǎo)分量,還需要根據(jù)當(dāng)前位形、障礙物、目標(biāo)位形三者的位置關(guān)系,實時調(diào)節(jié)兩分量的大小.

vs=kθvavo+(1-kθ)vattkθ=(tanh (kθ,1+kθ,2θ)+1)/kθ,3θ=arccos (vPn,Pobsvatt/(‖vPn,Pobs‖‖vatt‖))

(7)

式中:θ為向量vPn,Pobs與vatt的夾角；vs為垂直躲避分量和吸引分量的合向量；kθ為與θ相關(guān)的曲線調(diào)節(jié)權(quán)重；kθ,1、kθ,2、kθ,3為相關(guān)調(diào)節(jié)參數(shù).

如圖7所示，當(dāng)θ<π/2時,障礙物位于當(dāng)前位形和目標(biāo)位形中間,k趨近于1,垂直躲避分量vavo起主要引導(dǎo)作用,引導(dǎo)機械臂快速避開障礙物;當(dāng)θ逐漸增大至π時,k由1單調(diào)減小至0,分量vatt起主要引導(dǎo)作用,引導(dǎo)機械臂快速脫離障礙物.

圖7 等效的機械臂與障礙物Fig.7 Equivalent manipulator and obstacle

機械臂的連桿可視為剛體,桿件上每一點處的運動方向都相同,故最近點Pn處的避障向量可以等效為在桿端Pi,2處的方向相同的避障引導(dǎo)向量. 基于式(5)～(9)得到的避障引導(dǎo)總向量vs為單位向量,還需要引入與桿障最小距離dmin相關(guān)的動態(tài)調(diào)節(jié)步長，即

vLi=kdvskd=(1-tanh (kd,1(dmin-dsafe)+kd,2))/kd,3

(8)

式中:vLi為等效的連桿i末端處的避障引導(dǎo)速度；kd為與桿障距離相關(guān)的動態(tài)調(diào)節(jié)步長；kd,1、kd,2、kd,3為相關(guān)調(diào)節(jié)參數(shù)；dsafe為桿障距離的安全閾值,當(dāng)dmin>dsafe時,kd=0;當(dāng)dmin逐漸由dsafe減小至0時,kd由0單調(diào)增大至1,避免了在達點運動和避障運動兩者間切換時產(chǎn)生的關(guān)節(jié)速度突變,如圖8所示.

圖8 避障速度權(quán)重的變化趨勢Fig.8 Change trend of weight for avoidance speed

根據(jù)桿件與關(guān)節(jié)的構(gòu)型關(guān)系分析,桿件i末端的運動控制可簡化為上一連桿i-1的2個關(guān)節(jié)驅(qū)動,這種簡化關(guān)系可將單根桿件的運動控制視為定長半徑的球坐標(biāo)系控制,如圖9所示.

圖9 單根連桿的等效控制Fig.9 Equivalent control of single link

連桿i的桿長可視為球坐標(biāo)系的半徑r,上一連桿i-1的2個關(guān)節(jié)θ2i-1、θ2i可視為球坐標(biāo)系的兩坐標(biāo)分軸θ、φ.在得到連桿i末端處的引導(dǎo)速度后,可求得對應(yīng)的關(guān)節(jié)角變化

i-1PLi,2=(0Ti-1)-1(0Pi,2+vi)

(9)

θ2i=arccos (i-1Pi,2(z)/Li)θ2i-1=arccos (i-1Pi,1(x)/(Lisinθ2i))

(10)

式中i-1Pi,2為連桿i末端在上一連桿i-1的局部坐標(biāo)系下的位置投影.

2.3 多桿聯(lián)合避障

僅依靠對單根連桿進行避障控制在某些桿障位置下有一定的局限性,如圖10所示.

圖10 單根連桿規(guī)劃的局限情況Fig.10 Limitations of single link planning

隨著障礙物的位置變化和機械臂的運動,障礙物最近點Pn在連桿i上的位置也不斷變化,當(dāng)其較接近連桿的桿端Pi,1時,盡管避障引導(dǎo)向量vs仍然起效,但連桿需要很大行程的角度才能躲避繞開障礙物,這在實際的運動控制中是不合理的. 因此面對上述情況,根據(jù)所確定的主要危險連桿和次要連桿進行聯(lián)合避障規(guī)劃.

當(dāng)最近點Pn接近連桿的桿端Pi,1時,在保留對連桿i規(guī)劃的同時,將避障引導(dǎo)向量vs退化至上一連桿i-1,對其進行避障引導(dǎo),并引入改進的Sigmod函數(shù)作為權(quán)重,平滑最近點Pn在Pi,1與Pi,2兩連桿之間運動切換時的抖動,表示為

wd=k1/(1+exp (-k2dPn,Pi,1))-k3

(11)

(12)

式中d為最近點到其所在桿件上一端點的距離,距離越小則當(dāng)前桿件的關(guān)節(jié)避障角度變化的權(quán)重越小,上一桿件的權(quán)重越大. 當(dāng)d大于一半桿長Li/2時,wd趨近于1,此時對主要危險桿i進行規(guī)劃引導(dǎo);當(dāng)d小于Li/2并大于0時,wd由1單調(diào)遞減至0,此時同時對主次危險桿i和i-1進行避障規(guī)劃;當(dāng)d接近0時,wd=0,此時最近危險桿件已由連桿i轉(zhuǎn)變?yōu)閕-1,則算法將對連桿i-1和i-2進行規(guī)劃引導(dǎo).

2.4 最優(yōu)解選擇

在避開障礙物后,繼續(xù)向目標(biāo)關(guān)節(jié)位形進行達點運動,若此時繼續(xù)以靠近障礙物之前的目標(biāo)位形進行達點,可能會出現(xiàn)陷入局部振蕩的現(xiàn)象,原因是障礙物與之前選定的目標(biāo)位形發(fā)生了干涉,即上一步規(guī)劃引導(dǎo)避開了障礙物,下一步進行達點運動時又重復(fù)接近障礙物. 因此,在對機械臂避障引導(dǎo)規(guī)劃完成后,根據(jù)此時障礙物與機械臂連桿之間的位置關(guān)系,重新在自運動解集Q中選擇最優(yōu)目標(biāo)解.

參考勢場原理,以與當(dāng)前關(guān)節(jié)的行程差值最小和到達目標(biāo)時與障礙物距離最大為優(yōu)化目的,建立勢場函數(shù)為

(13)

Ure=kre/(Pφ,E-Pobs)2

(14)

(15)

式中:θnow為當(dāng)前時刻的關(guān)節(jié)角位形；Ujoi為當(dāng)前位形距自運動解集中的不同目標(biāo)位形的關(guān)節(jié)行程勢能函數(shù)；kjoi為相關(guān)引力增益參數(shù)；Q(φ)n為自運動解集中自運動角為φ所對應(yīng)的關(guān)節(jié)位形解；Ure為解集中不同目標(biāo)位形下與肘關(guān)節(jié)點E和障礙物之間的距離成反比的排斥勢場函數(shù)；kre為相關(guān)斥力增益調(diào)節(jié)參數(shù)；Pφ,E為自運動角變量為φ時的肘關(guān)節(jié)端點E在基座標(biāo)系下的位置. 可計算得到與當(dāng)前關(guān)節(jié)位形相距較近,同時又遠離障礙物的新的最優(yōu)目標(biāo)解θtar.

算法整體流程圖如圖11所示.

圖11 算法流程Fig.11 Algorithm flowchart

3 避障仿真

為驗證本文所提出的避障方法的有效性,本文以KUKA iiwa 14 R820機械臂模型對算法進行了仿真,機械臂的具體尺寸參數(shù)如表1所示.

仿真實驗采用CoppeliaSim4.0(CS)與Matlab(M)的跨平臺聯(lián)合仿真,算法單次迭代時由CS提供實時的目標(biāo)點、障礙物位置以及機械臂關(guān)節(jié)角信息,將其傳輸給M進行實時計算避障規(guī)劃,同時將規(guī)劃結(jié)果回傳給CS進行仿真驗證,為檢驗算法的有效性,仿真在靜態(tài)和動態(tài)2種障礙物環(huán)境下分別進行.

表1 7R機械臂DH參數(shù)

3.1 靜態(tài)障礙物的避障

設(shè)定機械臂初始為零位形,待達點的末端目標(biāo)位置和姿態(tài)歐拉角為

Pt,1=[-160 mm,850 mm,750 mm]TPt,2=[-115°,-10°,80°]T

連桿和障礙物的參數(shù)

dsafe=100 mm,robs=rL=50 mmPobs=[350 mm,400 mm,900 mm]T

避障算法相關(guān)參數(shù)

[kθ,1,kθ,2,kθ,3]=[6.5,-10/π,2]T[kd,1,kd,2,kd,3]=[30,3,100]T[kjoi,kre]=[150,10]T

仿真避障過程如圖12所示,可以看出機械臂末端在運動到目標(biāo)位姿的過程中,連桿有效地避開了障礙物. 末端的運動軌跡如圖12(f)所示,圖中綠色小球為目標(biāo)點,紅色小球為初始點,黑色小球及表面藍色網(wǎng)格為障礙物以及觸發(fā)避障規(guī)劃的安全邊界.

圖12 機械臂靜態(tài)避障的仿真過程Fig.12 Simulation process of static obstacle avoidance

仿真的數(shù)值結(jié)果如圖13所示,從初始零位形開始達點運動,在1.21 s時刻檢測到接近障礙物,開始避障引導(dǎo)規(guī)劃,如圖12(b)所示;在2.77 s時刻,完全避開障礙物,繼續(xù)向目標(biāo)達點運動,如圖12(d)所示. 機械臂的連桿L1為基座-E桿,由于此連桿固連于基座,關(guān)節(jié)的轉(zhuǎn)動對于桿障關(guān)系沒有影響,故忽略對于此連桿的距離檢測. 從圖13(a)可看出在機械臂的運動過程中,各連桿始終與障礙物保持在安全距離之外,結(jié)果表明了本文算法在靜態(tài)障礙物環(huán)境中的有效性.

圖13 靜態(tài)避障仿真結(jié)果Fig.13 Results of static obstacle avoidance simulation

3.2 正弦動態(tài)障礙物的避障

保持其他參數(shù)條件不變,將障礙物由靜態(tài)改為動態(tài)正弦運動,設(shè)定初始位置和運動速度為

Pobs=[500 mm,400 mm,700 mm]Tvobs=[150 mm/s,100 mm/s,150sin (5t) mm/s]T

障礙物將以正弦波的形式在機械臂達點軌跡上運動,隨機性地多次靠近又遠離機械臂,如圖14所示為在動態(tài)障礙物下的仿真過程.

在每次障礙物靠近機械臂時,本文算法都有效地使機械臂避開了障礙物,同時在避障完成后機械臂末端順利地抵達了目標(biāo)位姿,完成達點任務(wù).

仿真的數(shù)值結(jié)果如圖15所示,從初始零位形開始運動,在1.44 s時刻首次檢測到接近障礙物,開始避障引導(dǎo)規(guī)劃,在2.11 s時刻,障礙物遠離,機械臂繼續(xù)向目標(biāo)達點運動,在2.64、3.97、5.22 s時刻,多次與障礙物靠近并在之后及時遠離, 每時刻機械臂連桿與障礙物都未發(fā)生碰撞,且始終與障礙物保持安全距離,最終實現(xiàn)了達點并避障的任務(wù).

圖14 機械臂動態(tài)避障的仿真過程Fig.14 Simulation process of dynamic obstacle avoidance

圖15 動態(tài)避障仿真結(jié)果Fig.15 Simulation results of dynamic obstacle avoidance

根據(jù)式(2)～(4)計算,在整個運動過程中桿障最小距離為82.27 mm,較好地滿足了機械臂在達點運動過程中的避障要求,能及時高效地對機械臂進行實時避障規(guī)劃,提高在動態(tài)環(huán)境中的交互安全性. 為進一步驗證本文算法的高效性,將本文所提出的實時避障方法與傳統(tǒng)人工勢場法,在相同的仿真環(huán)境和參數(shù)條件下進行計算性能對比,實驗操作系統(tǒng)為Win10平臺,硬件主要參數(shù)為CPU:AMD Ryzen1600 3.2 GHz, GPU:GTX1060, RAM:32G,實驗結(jié)果求取多次計算平均值,如表2所示.

表2 算法性能比較

從表2中可看出,本文避障規(guī)劃方法的平均單步迭代時間遠遠小于傳統(tǒng)算法,主要由于本文算法將整體的避障規(guī)劃轉(zhuǎn)化為僅對2根主次危險桿的引導(dǎo)規(guī)劃,避免了傳統(tǒng)算法隨著機械臂自由度增多而計算量成倍增加的問題. 因此本文方法能夠有效地在動態(tài)障礙物環(huán)境下實現(xiàn)快速避障的任務(wù).

4 結(jié)論

1) 根據(jù)桿障的位置關(guān)系,確定主次危險桿件,同時對2根連桿進行避障規(guī)劃,并引入改進的Sigmod函數(shù)平滑危險桿件切換時的抖動問題,具有明確的解析形式.

2) 在求解出自運動解集的基礎(chǔ)上,構(gòu)建了關(guān)節(jié)行程的引力勢場和桿障安全距離的斥力勢場,在避障完成后從解集中再次選擇最優(yōu)位形解,進行達點運動,避免了陷入局部最小解振蕩的問題.

3) 通過仿真證明了本文方法的有效性和實時性,能較好地實現(xiàn)了機械臂在動態(tài)障礙物環(huán)境下的避障問題,后續(xù)將基于本文算法,研究在有關(guān)節(jié)約束及多障礙物下的避障問題.