機械臂與障礙物的雙層次碰撞檢測算法仿真

高嘉材，李麗宏，原 鋼

(1. 太原理工大學電氣與動力工程學院，山西 太原 030000； 2. 中國煤炭科工集團太原研究院，山西 太原 030000)

1 引言

當今工業社會中出現了許多具有復雜功能的機械臂，如裝配機械臂、焊接機械臂、打磨機械臂等。這些機械臂的工作環境復雜，任務量大，對運動的穩定性、安全性提出了較高要求。高效的碰撞檢測方法能夠提前檢測出危險情況，避免對機械臂本體以及環境設施的損壞，是機械臂能夠應用于各工作場景的必要條件。

目前常用的檢測方法有傳感器法和幾何體模型法。傳感器法的檢測效果易受環境影響，成本高，效果不穩定。Lumelsky.V.J等提出在機械臂上布置感知皮膚來檢測碰撞，這種方式雖然能夠檢測出所有碰撞，但檢測過程復雜，需要處理的數據很多，檢測速度依賴于處理器的性能[1]。相較而言，幾何體模型法更為可靠，其主要思想是計算機械臂運動軌跡插補點位置與障礙物幾何體模型位置的距離來判斷是否碰撞。陳友東[2]等提出將機械臂的關節和連桿等效成球體及膠囊體，將碰撞問題轉換為距離計算問題，一定程度上簡化了檢測過程。這種方法的弊端在于若選擇檢測的插補點間隔數目過大會損失檢測精度，遺漏碰撞情況；反之，會增加碰撞檢測的時間，特別是當機械臂與障礙物距離較遠時，這種方法的效率很低。因此，連續碰撞檢測(continuous collision detection)的概念受到廣泛關注。F.Schwarzer[3]等提出了一種自適應動態碰撞檢測方法，通過估算運動軌跡的長度判斷機械臂與障礙物的碰撞情況；金明杰[4]等將其加入到運動規劃過程，提高了路徑規劃過程的完備性。這種方法能夠判斷出連續運動軌跡是否與障礙物發生碰撞，雖然速度更快，但該方法僅與障礙物的特定模型進行判斷，檢測精度有待提高。

本文結合幾何體模型法與自適應動態碰撞檢測法的思想，提出了一種更為有效的雙層次碰撞檢測算法。初次判斷過程利用自適應動態碰撞檢測法快速篩選出運動軌跡中可能與障礙物球體模型發生碰撞的軌跡范圍，二次判斷過程采用分離軸法準確判斷該范圍內的軌跡點與障礙物OBB模型的碰撞情況。研究對象選用以UR5為原型的六軸協作機械臂，通過仿真分別與自適應動態碰撞檢測法及幾何體模型法進行對比，驗證了本文算法在確保實時性的同時，檢測結果更為準確。

2 機械臂及障礙物模型

本文的研究對象為連桿關節式機械臂，具有六個自由度，由兩個連桿依次連接關節模組組成，其結構如圖1(a)所示，機械臂自身的碰撞檢測算法可以參考文獻[5]中的圓柱體等效模型法。使用D-H法[6]建立的機械臂連桿坐標系如圖1(b)所示，對應的參數見表1，其中：ai-1表示沿Xi-1方向從Zi-1到Zi移動的距離；αi-1表示繞Xi-1軸Zi-1轉到Zi的角度；di表示沿Zi方向從Xi-1到Xi移動的距離；θi表示繞Zi軸Xi-1轉到Xi的角度。

圖1 六自由度機械臂示意圖

表1 六自由度機械臂D-H參數

由正運動學得到的末端連桿坐標系相對世界坐標系的位置方程為

式中s1=sinθ1，c1=cosθ1，c23=cos(θ2+θ3)，s23=sin(θ2+θ3)，其余類推。從上式可看出，六軸機械臂的位置僅與前三個關節的角度有關，末端腕部位置可等效為第三軸末端的位置。因此，可將機械臂等效為三自由度機械臂[2]。

對于障礙物來說，球體模型是完全包圍障礙物的最小球體，通過選取物體上最遠兩點的距離作為直徑來獲得[7]。雖然球體模型的包圍面積過大，包含多余空間，但其生成方法簡單，計算速度快，適用于以快速篩選可疑碰撞軌跡為目的的初次判斷過程。OBB(Oriented Bounding Box)，即有向包圍盒，是根據障礙物自身的幾何條件生成的緊湊長方體包圍盒子[8]，具有方向性，緊密性好，不會損失避障空間。相較于球體模型，OBB更加逼近不規則障礙物，適用于以準確判斷碰撞情況為目的的二次判斷過程。機械臂等效模型由qa狀態運動到qb狀態與障礙物球體模型的平面位置關系如圖2，此時的運動軌跡可能與障礙物碰撞。

圖2 機械臂等效模型與障礙物位置關系

3 自適應動態碰撞檢測

3.1 算法原理

機械臂在關節空間中運行時，碰撞檢測需要在軌跡插補運動中判斷自身與障礙物之間是否出現重合或越界的情況。以二維平面為例分析動態碰撞檢測算法的理論基礎。

假設物體1由起始位置a運動到終點位置c，物體2由起始位置b運動到終點位置d，其一般的運動軌跡為曲線ac、bd；當物體1與物體2在運動中發生碰撞，曲線ac與曲線bd交于o點，兩種運動狀態如圖3所示。

圖3 平面運動示意圖

在發生碰撞的情況中可推出

根據三角形兩邊之和大于第三邊定理，得到

由上述兩個式子可推出：

因此得到結論：若平面中兩條曲線ac、bd相交，則曲線ac、bd的長度之和大于等于直線段ab、cd的長度之和。進而推出逆否命題，若曲線ac、bd的長度之和小于直線段ab、cd的長度之和，即滿足(1)式時，兩條曲線ac、bd不相交。

(1)

將上述結論擴展到三維空間，得到結論1：

若空間中兩物體Ai、Aj分別沿曲面從qa狀態到qb狀態的最大移動距離λi(qa，qb)、λj(qa，qb)之和小于Ai、Aj分別在qa、qb狀態時的直線距離ηij(qa)、ηij(qb)之和，即(2)式成立，則Ai、Aj在從qa狀態到qb狀態的運動過程中不會發生碰撞。

λi(qa，qb)+λj(qa，qb)<ηij(qa)+ηij(qb)

(2)

采用反證法證明此結論：

假設兩物體Ai、Aj的運動過程在滿足(2)式的前提下發生碰撞，設發生碰撞時的狀態為qc，則有

進而推出

又由已知條件得

進而推出：λi(qa，qb)+λj(qa，qb)≥ηij(qa)+ηij(qb)。這與假設條件(2)式成立相矛盾，從而結論1得證。

將結論1應用于機械臂Ai與固定障礙物Aj的碰撞檢測情況中，此時λj(qa，qb)=0，則(2)式變為

λi(qa，qb)<ηij(qa)+ηij(qb)

(3)

即在已知機械臂移動的最大距離λi(qa，qb)及qa、qb狀態時機械臂與障礙物的直線距離ηij(qa)、ηij(qb)的條件下，若(3)式成立，則機械臂從qa狀態到qb狀態的運動軌跡不會與障礙物發生碰撞。

3.2 估計運動軌跡的最大長度

文獻[3]中的引理2給出了估算最大運動軌跡長度的公式，內容如下：

當機械臂從qa狀態運動到qb狀態時，第i軸運動軌跡的最大長度為

4 碰撞檢測過程

4.1 初次判斷

初次判斷過程將自適應動態碰撞檢測法與二分法融合，對運動軌跡與障礙物球體模型進行判斷，篩選出可疑碰撞軌跡范圍，過程如下：

已知軌跡從qa狀態運動到qb狀態的最大移動距離λi(qa，qb)，并計算出兩種狀態下機械臂與球體模型的直線距離ηij(qa)、ηij(qb)，接著判斷：

若λi(qa，qb)≥ηij(qa)+ηij(qb)

若qa到qb中的軌跡插補點數目大于2

獲取qa與qb之間的中間點狀態qmid，對(qa，qmid)、(qmid，qb)按照上述步驟計算；

否則：

(qa，qb)之間可能與障礙物發生碰撞，進行二次判斷；

否則：

(qa，qb)軌跡之間不發生碰撞。

相較于文獻[4]中使用的自適應動態檢測算法需要不停計算中間狀態軌跡點與特定障礙物模型的距離判斷是否碰撞，本文算法去除了該計算部分，初次判斷過程僅將可能碰撞的軌跡段篩選出來，由二次判斷來進行更準確的檢測。

4.2 二次判斷

對于可能發生碰撞的軌跡段需要進行更嚴格的檢測，使用分離軸法判斷該范圍內的軌跡點與障礙物OBB模型的碰撞關系，具體過程如下：

設障礙物OBB模型的長度寬度為2d，高度為h，上下端面的中心點AB的坐標分別為(xA，yA，zA)，(xB，yB，zB)，軌跡點M坐標為(x1，y1，z1)，如圖4所示。以世界坐標系中的X軸、Y軸、Z軸作為分離軸，將OBB模型與軌跡點分別向X軸、Y軸、Z軸投影，(4)式成立則判斷軌跡點M與障礙物碰撞，否則該點不會與障礙物碰撞。

(4)

圖4 軌跡點與OBB投影示意圖

若待檢軌跡段內任一軌跡點滿足(4)式則終止檢測并得出機械臂會與障礙物碰撞的結論；否則該軌跡段不會與障礙物發生碰撞。當所有可疑軌跡段都判斷為不碰撞情況時，得出最終結論：機械臂的規劃軌跡不會與障礙物發生碰撞。

5 實驗分析

本節將通過兩次實驗從運行時間方面對比雙層次碰撞檢測算法、自適應動態碰撞檢測法及幾何體模型法，其中后兩種算法用到的障礙物模型皆為球體模型。

實驗平臺為個人筆記本，參數配置為：Intel(R)core(TM)i5-8250U CPU @1.6GHz，8GB內存，使用matlab軟件編寫三種算法的腳本程序。測試數據選用關節空間下以[-30；-120；120；-130；120；0]為各軸起始角度，[-90；-90；180；-90；90；50]為各軸終止角度，插補周期為100ms的S型曲線速度控制算法[10]規劃得到的95組末端軌跡點，各軸位置、速度變化曲線及世界坐標系中軌跡點的位置變化如圖5所示。

圖5 機械臂各部變化圖

1)運動軌跡不與障礙物球體模型發生碰撞的情況。設障礙物的球體模型半徑為20，中心點坐標為(250，-200，50)，在xoy平面上的投影如圖6所示。此時，λ3(qa，qb)=504.64，ηij(qa)+ηij(qb)=577.34，(3)式成立，本文算法及自適應動態碰撞檢測算法得出不發生碰撞的結論；幾何體模型法經過95次計算軌跡點與障礙物的距離后得出不碰撞的結論。分別記錄三種算法運行100次的平均時間，結果見表2。

圖6 未碰撞情況運動軌跡與障礙模型投影圖

表2 未碰撞情況下各算法運行時間記錄表

由實驗結果看出，對于運動軌跡不會與障礙物球體模型發生碰撞的情況，三種算法都能得到正確結論。雙層次碰撞檢測算法僅需運行初次判斷過程即可得到正確結果，因此與自適應動態碰撞檢測法的運行時間大體一致；相較與幾何體模型法，雙層次碰撞檢測算法僅需一次判斷即可得到正確結果，運行時間縮短了22.96%。

上述結果說明在運動軌跡不與障礙物球體模型發生碰撞的情況中，雙層次碰撞檢測算法的檢測結果準確，計算量小，速度快。

圖7 碰撞情況運動軌跡與障礙模型投影圖

2)運動軌跡與障礙物球體模型碰撞但不與OBB模型碰撞的情況。設障礙物中心點坐標為(250，-140，108)，球體模型半徑為20，OBB模型的三條棱長為37、14、6，在xoy平面上的投影如圖7所示。本文算法的初次判斷過程經過6次迭代篩選后得到第45、46、47三組可疑軌跡點，坐標分別為(254.56，-130.82，117.75)、(250.32，-131.64，108.83)、(245.98，-132.12，100.08)，二次判斷過程的(4)式判別情況見表3，得出不與OBB模型發生碰撞的結論；自適應動態碰撞檢測算法在僅與球體模型判斷的條件下得出發生碰撞的結論；幾何體模型法在經過95次距離計算后同樣得出了發生碰撞的結論。再次記錄三種算法運行100次的平均時間，結果見表4。

表3 二次判斷可疑點碰撞情況分析表

表4 碰撞情況下各算法運行時間記錄表

由實驗結果看出，相較與自適應動態碰撞檢測法，雙層次碰撞檢測算法多了二次判斷步驟，結果更準確，但檢測速度稍有減慢；與幾何體模型法相比，雙層次碰撞檢測算法在得到準確結果的同時，運行時間縮短了18.68%。

上述結果表明，在運動軌跡與障礙物球體模型發生碰撞但不與OBB模型碰撞的情況中，雙層次碰撞檢測算法在不失速度性的前提下，能夠得到更為準確的檢測結果。

6 結論

本文提出了一種機械臂與障礙物之間的雙層次碰撞檢測算法，初次判斷過程通過自適應動態碰撞檢測算法快速檢測軌跡是否與障礙物球體模型碰撞并篩選出可能碰撞的軌跡點范圍；二次判斷過程采用分離軸法判斷可疑軌跡范圍是否與障礙物OBB模型碰撞，得到最終的檢測結果。實驗結果表明，本文算法能提升自適應動態碰撞檢測法的準確性，同時能夠克服幾何體模型法計算量大、實時性較差的問題，更適用于對實時性、準確性有較高要求的機械臂控制系統。