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

趙志偉，劉振宇

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

1 引 言

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

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

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

2 六自由度機械臂運動學分析

2.1 建立D-H模型

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

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

圖1 Motoman ES165D型機械臂

表1 Motoman ES165D機械臂D-H參數

2.2 六自由度串聯機械臂反解分析

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

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

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

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

由于θ1存在兩個可能解，對應θ3也存在兩種可能解。由于結構的限制，根據表1變量的范圍，有些解不能實現。故此根據實際情況選擇其中一組解來滿足機器人的工作要求。

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

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

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

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

3 干涉判斷

3.1 環境模型

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

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

圖2 Unity3D工作環境模擬

3.2 連桿之間干涉判斷

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

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

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

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

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

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

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

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

圖4 兩連桿發生干涉情況二

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 仿真實現及結果分析

4.1 基于Unity3D的仿真實現

Unity3D支持C#和JavaScript語言的腳本編程，由于C#語言具有面向對象的特性和強大的函數庫，故此選用C#作為開發語言。在仿真初始階段，為生成相關參數，需要編寫Script腳本，用以定義目標點位置、連桿長度等參數，并制成參數控制面板。

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

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

4.2 結果分析

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

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

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

結合連桿參數坐標系，得到連桿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 各連桿與障礙物第一種判斷方法結果數據

表3 各連桿與障礙物的第二種判斷方法結果數據

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

5 結束語

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