基于空間位置的工業(yè)機(jī)器人干涉判斷研究

趙志偉，劉振宇

(沈陽工業(yè)大學(xué)信息科學(xué)與工程學(xué)院,沈陽110870)

1 引 言

近年來，工業(yè)機(jī)器人在自動(dòng)化領(lǐng)域受到廣泛應(yīng)用，其中應(yīng)用最多的就是串聯(lián)型六自由度機(jī)器人。這種機(jī)器人在決定位移范圍時(shí)，必須考慮連桿之間的干涉問題；同時(shí)在運(yùn)動(dòng)過程中，也要考慮是否會(huì)和障礙物發(fā)生碰撞。國內(nèi)外許多專家學(xué)者都對此問題展開研究。

方彬等人[1]曾嘗試在初步檢測階段采用AABB包圍盒和間隔減半來剔除場景中不相交的物體；在詳細(xì)檢測階段通過對物體建立AABB來精確相交區(qū)域。Ayala等人[2]為提高檢測精度，應(yīng)用管狀物體進(jìn)行自碰撞檢測。靳雁霞等人[3]曾提出融合R-Sphere包圍球的碰撞檢測算法，該算法首先將有公共頂點(diǎn)的三角片構(gòu)造成包圍球，利用包圍球快速剔除不相交的物體，再通過粒子群算法將三維問題轉(zhuǎn)換成二維問題；王鵬等人[4]利用在三角形網(wǎng)格中增加特征元素的信息(點(diǎn)、邊、面)形成特征描述三角形，再結(jié)合方向包圍盒OBB(Oriented bounding box)來進(jìn)行碰撞檢測。胡詠梅[5]提出將AABB和OBB相結(jié)合的碰撞檢測算法，該算法通過對象投影來剔除不相交的物體，繼而用AABB構(gòu)建剩下的物體，初步判斷可能相交的物體，最后用OBB來精確檢測，獲得了較好的實(shí)時(shí)性。

然而以上研究成果大多是將機(jī)器人桿件與障礙物當(dāng)成幾何多面體、橢球體或者球體。求解連桿與障礙物的距離時(shí)，為了安全起見，這些幾何元件的尺寸均大于實(shí)際連桿和障礙物的尺寸，因此雖然容易做出判斷，但卻無法精確判斷是否真正有干涉現(xiàn)象發(fā)生[6-10]。故此，提出一種新方法，利用工業(yè)機(jī)器人外型特征，將機(jī)器人各關(guān)節(jié)替換為更接近其形狀的圓柱或圓錐體，可迅速判斷出哪些連桿不發(fā)生干涉現(xiàn)象，并利用可能發(fā)生干涉的連桿及障礙物兩者中心線的距離來判斷是否真正發(fā)生干涉。所提方法不需要用到代表連桿及障礙物的幾何方程，且不需要判斷每一連桿與障礙物是否發(fā)生干涉，即可快速準(zhǔn)確地判斷是否有干涉發(fā)生。

2 六自由度機(jī)械臂運(yùn)動(dòng)學(xué)分析

2.1 建立D-H模型

以安川公司生產(chǎn)的六自由度點(diǎn)焊機(jī)器人Motoman ES165D為研究對象進(jìn)行運(yùn)動(dòng)學(xué)建模。圖1(a)即為產(chǎn)品實(shí)物圖。該機(jī)器人結(jié)構(gòu)如圖1(b)所示，其運(yùn)動(dòng)學(xué)正解采用傳統(tǒng)的D-H法進(jìn)行描述。DH法建立各個(gè)關(guān)節(jié)坐標(biāo)系如圖1(c)所示(Unity3D世界坐標(biāo)系中，χ-z平面為水平面，豎直方向?yàn)閥軸，這里按照Unity坐標(biāo)規(guī)則定義坐標(biāo)系)，其中θi為連桿i與連桿i-1的夾角，αi為連桿i與連桿i-1間的扭角，ai為連桿i長度，di為連桿i與連桿i-1的距離，得到D-H參數(shù)如表1所示，第i-1坐標(biāo)系與第i坐標(biāo)系之間的關(guān)系可以用平移和旋轉(zhuǎn)來實(shí)現(xiàn)。最終得到機(jī)械臂末端相對于基座上參考坐標(biāo)X0-Y0-Z0的位姿矩陣T,如下式：

式中：nχ,ny,nz,oχ,oy,oz,aχ,ay,az為機(jī)械臂末端的姿態(tài),pχ,py,pz為機(jī)械臂末端的位置坐標(biāo)。

圖1 Motoman ES165D型機(jī)械臂

表1 Motoman ES165D機(jī)械臂D-H參數(shù)

2.2 六自由度串聯(lián)機(jī)械臂反解分析

已知機(jī)器人連桿的幾何參數(shù)，給定機(jī)器人末端相對于參考坐標(biāo)系的期望位置和姿態(tài)，通過機(jī)器人運(yùn)動(dòng)方程的反解，得到機(jī)器人能夠到達(dá)預(yù)期位姿的關(guān)節(jié)變量，這就需要對運(yùn)動(dòng)方程求解，即逆運(yùn)動(dòng)學(xué)。求解θ1時(shí)將式(1)等號(hào)兩邊同時(shí)左乘得到：

化簡可求得：θ1=±90-arctan2(pz,px),式中正、負(fù)號(hào)對應(yīng)θ1的兩個(gè)可能解。

令矩陣方程(2)兩邊的(2,4)和(3,4)分別對應(yīng)相等，得到方程組：

其中s1是sinθ1的縮寫，c1是cosθ1的縮寫，以此類推，s23=sin(θ2+θ3)，c1=cos(θ2+θ3)，化簡得：

由于θ1存在兩個(gè)可能解，對應(yīng)θ3也存在兩種可能解。由于結(jié)構(gòu)的限制，根據(jù)表1變量的范圍，有些解不能實(shí)現(xiàn)。故此根據(jù)實(shí)際情況選擇其中一組解來滿足機(jī)器人的工作要求。

求解θ2時(shí)通過矩陣方程：

令矩陣方程(3)兩端(1,4)對應(yīng)相等，即有：

將-a2移到等式左邊后對等式兩邊平方，化簡后得：

同理可求得θ4,θ5,θ6：

3 干涉判斷

3.1 環(huán)境模型

在進(jìn)行干涉判斷之前，首先需要對機(jī)械臂工作環(huán)境進(jìn)行合理的描述。在干涉判斷時(shí)采用對環(huán)境中的障礙物以及機(jī)器人進(jìn)行兩次投影的思想[11-12]。H-平面垂直于機(jī)械臂基坐標(biāo)Y軸軸線；V-平面垂直于H-平面并且經(jīng)過基坐標(biāo)Y軸軸線，利用機(jī)械臂與障礙物分別在H-平面內(nèi)以及V-平面內(nèi)進(jìn)行投影截取，這樣機(jī)械臂在三維工作空間中運(yùn)動(dòng)的問題便轉(zhuǎn)換為H-平面及V-平面兩個(gè)平面的二維問題，減少了計(jì)算量，提升了系統(tǒng)的實(shí)時(shí)性。

首先在SolidWorks中對Motoman ES165D機(jī)器人本體進(jìn)行建模，然后應(yīng)用3D Max對各個(gè)關(guān)節(jié)所在的坐標(biāo)系進(jìn)行修改，完成關(guān)節(jié)間坐標(biāo)變換的設(shè)置。同時(shí)利用3D Max中提供的高級建模工具對機(jī)器人工作的操作平臺(tái)、障礙物等相關(guān)物件創(chuàng)建模型，最后以FBX格式導(dǎo)入U(xiǎn)nity3D中。通過Unity3D自帶的物理引擎對零件附加剛體屬性，使零件具有質(zhì)量、摩擦等物理效果，參照生產(chǎn)車間進(jìn)行虛擬環(huán)境搭建，最后搭建了如圖2所示的本體模型及環(huán)境模型。

圖2 Unity3D工作環(huán)境模擬

3.2 連桿之間干涉判斷

在串聯(lián)式機(jī)械臂干涉判斷方面，因?yàn)檫B桿多為不規(guī)則形狀，所以將各連桿等效為圓錐或者圓柱形狀來簡化問題?？臻g中兩連桿干涉情況如圖3所示。

圖3 空間中兩連桿相互干涉

通過兩個(gè)連桿之間中心線的垂直距離dij來判斷是否發(fā)生干涉，其表達(dá)式為：

其中dij為兩中心線在空間中與公垂線相交兩點(diǎn)的距離。為垂直于連桿i與連桿j的單位向量，為兩連桿的中心線方向。

空間中兩直線Li與Lj的參數(shù)方程為：

AiBi和AjBj分別為連桿i與連桿j的上下端點(diǎn)的坐標(biāo)，λi,λj為交點(diǎn)位置參數(shù)。兩連桿中心線的公垂線與兩連桿中心線的交點(diǎn)Qi與Qj，由聯(lián)立方程求得λi及λj，并分別帶回式 (4)求得兩交點(diǎn)的位置Qi與Qj。當(dāng) 0≤λi點(diǎn)位于連桿i長度范圍內(nèi)；當(dāng) 0≤λj≤1，Qj點(diǎn)位于連桿j長度范圍內(nèi)；所以連桿i與連桿j發(fā)生干涉的條件為0≤λi≤1和0≤λj≤1且垂直距離小于干涉點(diǎn)的兩桿半徑和，以此得到判斷條件為：

情況一：當(dāng)Δi≥0，Δj≥0，交點(diǎn)位于i、j連桿范圍內(nèi),由交點(diǎn)位置參數(shù)λj(當(dāng)λj為0則交點(diǎn)為Aj，直徑為Dj;當(dāng)λj為1則交點(diǎn)為Bj，直徑為dj)，則可計(jì)算出連桿j在交點(diǎn)位置的直徑大小為則發(fā)生干涉，否則不發(fā)生干涉。

情況二：Δi×Δj＜0，代表交點(diǎn)位于i、j二連桿其中一個(gè)的范圍內(nèi)，無法確定是否發(fā)生干涉，需要通過求解軸與點(diǎn)的最短距離來作判斷，如圖4所示。

圖4 兩連桿發(fā)生干涉情況二

為了使干涉點(diǎn)固定在障礙物端面的邊上，將圓錐體改視為圓柱體來簡化判斷條件，其直徑由大圓Dj取代。利用點(diǎn)到直線最短距離公式來計(jì)算，以Δi＞0為例（對于Δj＞0的情況，其求解過程中只需將式中i、j做交換即可），計(jì)算結(jié)果如下：

Hj為直線連桿j上點(diǎn)Aj或Bj的坐標(biāo)，dHL為Hj點(diǎn)到連桿i的最短距離。

圖5 兩連桿發(fā)生干涉夾角為銳角、鈍角情況

情況三：當(dāng)Δi＜0，Δj＜0，代表連桿i與連桿j的交點(diǎn)都位于兩者范圍之外。如圖6所示，當(dāng)時(shí)，其計(jì)算過程與情況二的Δi＞0一樣；反之，當(dāng)時(shí)，對其求解只需將式中i、j做交換即可。

3.3 連桿與障礙物之間的干涉判斷

第一種方法與連桿之間的干涉判斷方法相同，只需將障礙物視為錐柱體或圓柱體進(jìn)行判別；另外考慮到在現(xiàn)實(shí)當(dāng)中，障礙物多為垂直于水平面的物體，提出第二種較為簡易的判斷方法：首先，將障礙物設(shè)定為圓柱體，圓面的直徑為D0；同時(shí)也將每根連桿設(shè)定為圓柱體，其直徑為Di，連桿i的頂點(diǎn)為Ai；而為障礙物的中心線方向，A0為障礙物地面的圓心點(diǎn)，B0為障礙物頂點(diǎn)的圓心點(diǎn)，dOL為連桿2坐標(biāo)原點(diǎn)O與障礙物中心線在空間中點(diǎn)到直線的距離,其表達(dá)式為：

圖6 當(dāng)時(shí)，兩連桿干涉狀況

連桿2與障礙物發(fā)生的第一種干涉，將連桿2與障礙物投影到χ-z平面,如圖7(a)所示為基坐標(biāo)原點(diǎn)O與障礙物投影后的圓心C連線與χ軸的夾角，θL2為θ1造成連桿2與障礙物產(chǎn)生干涉的偏轉(zhuǎn)角大小，其表達(dá)式為：

其次，若上述條件成立，需要進(jìn)一步判斷連桿2的長度是否進(jìn)入障礙物干涉的范圍。將連桿2與障礙物都投影到r-y平面，其中r軸為坐標(biāo)原點(diǎn)O與障礙物底圓圓心A0連線的軸，將連桿2的末端投影至r軸，比較其長度是否有干涉發(fā)生。如圖7(b)所示，得到連桿2投影到r軸的長度，其中B2r為連桿2投影到r軸的末端點(diǎn)，配合正弦函數(shù)將連桿2的直徑投影到r軸，得到：

所以當(dāng)OEr≤OCr，即時(shí)，連桿2可能發(fā)生干涉，否則不發(fā)生干涉。

最后，第二軸角θ2也會(huì)影響連桿2與障礙物的干涉狀況判定，若上述兩個(gè)判斷條件都成立，同樣在r-y平面比較連桿2在y軸方向的高度是否與障礙物發(fā)生干涉。如圖7(c)所示，其中E為連桿2與障礙物的干涉點(diǎn)，Er為E點(diǎn)投影到r軸的點(diǎn)，Y為連桿2和障礙物的干涉點(diǎn)到連桿2軸的y方向長度：

由此得到第三個(gè)判斷條件：

圖7 連桿2與障礙物干涉情況

當(dāng)同時(shí)滿足上述三個(gè)條件時(shí)，連桿2發(fā)生干涉，否則連桿2無干涉發(fā)生。當(dāng)連桿2無干涉發(fā)生時(shí)，由于干涉點(diǎn)不一定在連桿2上，所以要繼續(xù)對連桿3進(jìn)行干涉判斷。

第一步，與連桿2相同，計(jì)算連桿3與障礙物之間的造成干涉的偏轉(zhuǎn)角度θL3。

圖8 連桿3與障礙物干涉情況

4 仿真實(shí)現(xiàn)及結(jié)果分析

4.1 基于Unity3D的仿真實(shí)現(xiàn)

Unity3D支持C#和JavaScript語言的腳本編程，由于C#語言具有面向?qū)ο蟮奶匦院蛷?qiáng)大的函數(shù)庫，故此選用C#作為開發(fā)語言。在仿真初始階段，為生成相關(guān)參數(shù)，需要編寫Script腳本，用以定義目標(biāo)點(diǎn)位置、連桿長度等參數(shù)，并制成參數(shù)控制面板。

根據(jù)相關(guān)參數(shù)對正運(yùn)動(dòng)學(xué)進(jìn)行仿真驗(yàn)證，運(yùn)動(dòng)學(xué)正解的關(guān)鍵代碼如下，建立FK類，包含GetMatrix()、MatrixCalcu-Lation()等關(guān)鍵函數(shù)：

進(jìn)行正運(yùn)動(dòng)學(xué)、逆運(yùn)動(dòng)學(xué)驗(yàn)證之后，編寫干涉判斷的相關(guān)腳本，在Update函數(shù)（Update函數(shù)是unity提供函數(shù)，其在每一幀被調(diào)用）中進(jìn)行條件判斷，當(dāng)發(fā)生干涉時(shí)輸出是哪一連桿發(fā)生了干涉。

4.2 結(jié)果分析

由于Motoman ES165D型機(jī)器人的末端三個(gè)關(guān)節(jié)軸線交于一點(diǎn)，所以只考慮前三個(gè)關(guān)節(jié)干涉的情況。Unity3D中機(jī)械臂與障礙物干涉情況如圖9。

圖9 Unity3D中機(jī)械臂與障礙物干涉狀況示意圖

在已經(jīng)搭建好的Unity3D環(huán)境下，選取機(jī)械臂路徑中末端點(diǎn)為（11.6,61.5,11.6）的一點(diǎn)，障礙物D0坐標(biāo)位置為（30,0,-30），障礙物直徑為14cm，高度為67cm，通過逆運(yùn)動(dòng)學(xué)得到每個(gè)連桿的旋轉(zhuǎn)角度為：θ1=45°，θ2=45°，θ3=0°，θ4=45°，θ5=30°，θ6=30°。已知每個(gè)關(guān)節(jié)旋轉(zhuǎn)角度之后，利用機(jī)器人正運(yùn)動(dòng)學(xué)得到各個(gè)關(guān)節(jié)在基坐標(biāo)中的位姿矩陣：

結(jié)合連桿參數(shù)坐標(biāo)系，得到連桿2的下端點(diǎn)坐標(biāo)（14.1,65,14.1），上端點(diǎn)坐標(biāo)（31.6,89.7,31.6），連桿3下端點(diǎn)坐標(biāo)（31.6,89.7,31.6），連桿3上端點(diǎn)坐標(biāo)（11.6,61.5,11.6）。得到各個(gè)連桿與障礙物干涉判斷狀況如表2、表3所示。

表2 各連桿與障礙物第一種判斷方法結(jié)果數(shù)據(jù)

表3 各連桿與障礙物的第二種判斷方法結(jié)果數(shù)據(jù)

由兩表可以看出，兩種方法都可以快速地判斷出當(dāng)末端點(diǎn)位于（11.6,61.5,11.6）時(shí)，機(jī)械臂第三連桿將與障礙物發(fā)生碰撞，其中第一種方法較為復(fù)雜，須聯(lián)立兩線性方程式求解λ，但其能判斷圓錐型障礙物或連桿；第二種方法雖然不能判斷圓錐型物體，但其計(jì)算量較小，干涉判斷速度比第一種方法更快。

5 結(jié)束語

將連桿與障礙物簡化為更接近實(shí)物的圓柱體與圓錐體，通過兩種干涉判斷法，分析連桿與連桿之間、連桿與障礙物之間干涉發(fā)生的可能情況，能夠有效判斷干涉是否發(fā)生及干涉點(diǎn)發(fā)生位置，提高了干涉判斷精確度。以Motoman ES165D機(jī)器人為研究對象，通過分析其結(jié)構(gòu)，構(gòu)建了本體模型及工作車間場景模型。運(yùn)用D-H參數(shù)法對機(jī)器人正逆運(yùn)動(dòng)學(xué)求解，并在Unity3D中通過虛擬仿真，驗(yàn)證了機(jī)器人運(yùn)動(dòng)過程中干涉判斷方法的可行性，在實(shí)際應(yīng)用中能夠有效預(yù)防工作中機(jī)器人的損壞。