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

趙志偉，劉振宇

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

1 引 言

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

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

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

2 六自由度機械臂運動學(xué)分析

2.1 建立D-H模型

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

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

圖1 Motoman ES165D型機械臂

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

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

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

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

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

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

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

求解θ2時通過矩陣方程：

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

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

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

3 干涉判斷

3.1 環(huán)境模型

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

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

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

3.2 連桿之間干涉判斷

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

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

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

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

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

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

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

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

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

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

Hj為直線連桿j上點Aj或Bj的坐標，dHL為Hj點到連桿i的最短距離。

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

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

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

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

圖6 當時，兩連桿干涉狀況

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

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

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

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

由此得到第三個判斷條件：

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

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

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

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

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

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

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

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

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

4.2 結(jié)果分析

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

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

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

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

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

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

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

5 結(jié)束語

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