多臂鑿巖機器人的碰撞檢測方法研究

方夢娟，徐巧玉，李坤鵬，張 正

(1.河南科技大學 機電工程學院，河南 洛陽 471003；2.洛陽銀杏科技有限公司，河南 洛陽 471003)

0 引言

目前，多臂鑿巖機器人廣泛應用于煤礦、隧道開挖等領域中。由于鑿巖機器人屬于工業液壓重載型機械臂，具有多臂協作、結構復雜等特點，機械臂在隧道中作業時極易發生碰撞干涉現象，嚴重時會造成施工人員傷亡和極大的經濟損失，因此多臂鑿巖機器人的碰撞檢測問題亟需解決[1-3]。

迄今為止，眾多學者在鑿巖機器人的碰撞檢測方面進行的研究[4-12]，主要分為基于外部傳感器的碰撞檢測方法、基于視覺的碰撞檢測方法和通過幾何建模實現的碰撞檢測方法。對于工業液壓重載機器人，由于使用幾何建模實現的碰撞檢測方法易于實現且處理數據效率高，所以應用廣泛。該方法通常將機器人鉆臂桿件進行模型簡化，通過計算空間幾何模型距離實現機器人的碰撞檢測。文獻[13-15]采用球體和膠囊體包圍盒簡化機械臂模型，能夠實現有效的碰撞檢測，但因機械臂關節機械結構的不規則性，使用球體包絡會對檢測精度造成一定的影響。文獻[16-18]采用圓柱體包圍盒簡化機械臂模型，基于空間向量運算實現碰撞檢測。文獻[19-20]使用空間凸多面體模型對復雜物體進行建模，能夠準確實現碰撞檢測，但鑿巖機器人所處隧道壁相對規則，用該種方法會降低機器人與隧道的碰撞檢測效率。依賴幾何建模展開的鑿巖機器人碰撞檢測研究多應用于鑿巖機器人的多個機械臂之間，而鑿巖機器人機械臂與隧道的碰撞檢測研究較少，仍待深入研究。同時，在依賴幾何建模實現的碰撞檢測方法中，復雜的建模方法和計算方法會降低檢測算法的效率和精度。

針對上述問題，本文提出了一種多臂鑿巖機器人的碰撞檢測方法，將鑿巖機器人機械臂桿件簡化為圓柱體包絡盒，通過實時計算圓柱體包絡盒間的距離，實現鑿巖機器人自身的碰撞檢測；將圓柱體包絡盒投影在隧道壁面上，通過判斷矩形投影與隧道壁面凸多邊形輪廓線的碰撞情況，實現鑿巖機器人與隧道的碰撞檢測。在中鐵DJ3E鑿巖機器人上進行實驗，以驗證算法的實時性和準確性。

1 鑿巖機器人碰撞檢測模型

本文以中鐵DJ3E多臂鑿巖機器人為對象展開研究，建立碰撞檢測模型。該鑿巖機器人如圖1所示，共有4條機械臂：左中右3條工作鉆臂和1條吊籃臂。

多臂鑿巖機器人在隧道工作中碰撞現象，通常發生在單個鉆臂桿件間、鉆臂之間、鉆臂和吊籃臂之間、鉆臂和隧道面之間，如圖2所示，共包含12類碰撞檢測內容。

①.左臂自身碰撞;②.中臂自身碰撞;③.右臂自身碰撞;④.中臂與左臂碰撞;⑤.中臂與右臂碰撞;⑥.左臂與右臂碰撞;⑦.左臂與吊籃臂碰撞;⑧.中臂與吊籃臂碰撞;⑨.右臂與吊籃臂碰撞;.左臂與隧道碰撞;.中臂與隧道碰撞;.右臂與隧道碰撞。圖1 多臂鑿巖機器人圖2 多臂鑿巖機器人碰撞檢測內容

由于鑿巖機器人機械臂結構復雜，實際結構模型很難描述，因此建立3條鉆臂和吊籃臂的包絡盒模型，如圖3所示。鑿巖機器人三臂的簡化模型一致，將鉆臂桿件簡化為4個圓柱體包絡盒，機械臂關節包含于圓柱體包絡盒中。由于在實際作業時，包絡盒CS3所占空間范圍較小，幾乎不發生碰撞，因此CS3包絡盒忽略不計，不參與碰撞檢測。此時的碰撞檢測模型更加簡潔，有利于提高碰撞檢測效率。同理，將吊籃臂的桿件簡化為兩個圓柱體包絡盒。

鑿巖機器人自身的碰撞檢測等同于求解空間圓柱體包絡盒間的最小距離問題；將隧道模型簡化為平面凸多邊形，機器人鉆臂與隧道碰撞檢測等同于求解各圓柱體包絡盒與隧道簡化模型的碰撞問題。

2 碰撞檢測算法

2.1 兩圓柱體包絡盒碰撞檢測算法

將多臂鑿巖機器人每條機械臂上的桿件簡化為圓柱體包絡盒后，通過計算圓柱體包絡盒間的最小距離判斷鑿巖機器人自身的碰撞情況。

兩圓柱體軸線段最小距離計算方法如下：

設兩圓柱體包絡盒的軸線段為l1、l2，底面半徑為r1、r2，4個端點坐標分別為P1(x1,y1,z1)、P2(x2,y2,z2)、Q1(x3,y3,z3)、Q2(x4,y4,z4)，如圖4所示。記向量p=P2-P1，向量q=Q2-Q1，線段l1、l2上任意一點坐標用這兩個向量表示為:

(1)

其中：0≤λ1；λ2≤1。

線段之間的最短距離可以轉化為有約束的最優化問題，如式(2)所示：

minf(λ1,λ2)=[P(λ1)-Q(λ2)]2=[(P1+λ1p)-(Q1+λ2q)]2。

(2)

(3)

如圖4所示，當0≤λ1,λ2≤1時，表示兩直線的公垂線垂足落在兩條線段中，則此時的最短距離d2=f(λ1,λ2)；當λ1,λ2有一個不在[0,1]時，則表示兩個垂足至少有一個沒有落在線段內，此時分別求解P1到l2的距離d1、P2到l2的距離d2、Q1到l1的距離d3、Q2到l2的距離d4，兩圓柱體軸線段最小距離d={d1,d2,d3,d4}min。

(a) λ1,λ2∈[0,1] (b) λ1∈[0,1],λ2?[0,1] (c) λ1?[0,1],λ2∈[0,1] (d) λ1,λ2?[0,1]

已知兩圓柱體底面半徑分別為r1、r2，兩圓柱體軸線間最小距離d，則圓柱體之間的最小距離dmin=d-(r1+r2)，當dmin>0時，兩圓柱體不發生碰撞，否則碰撞現象發生。該方法基于空間向量運算實現，避免了求解直線方程等復雜過程，便于通過C語言和MATLAB軟件代碼實現，提高了運行效率和檢測精度。

2.2 圓柱體包絡盒與隧道輪廓碰撞檢測算法

隧道壁面簡化為由直線段和圓弧線段組成的凸多邊形，隧道輪廓簡化模型如圖5所示，同時將空間圓柱體投影到隧道壁面上，通過對圓柱體的投影矩形與隧道直線段、圓弧線段碰撞判別實現鉆臂與隧道的碰撞檢測。

T1T3.隧道壁面的左輪廓直線； T2T4.隧道壁面的右輪廓直線； T3T4.地平面； O.圓弧T1T2的圓心；R.圓弧半徑；矩形ABCD.圓柱體在隧道壁面的矩形投影；E、F.圓柱體軸線兩端點；l.圓柱體軸線；r.圓柱體底面半徑。

圓柱體的矩形投影與直線的碰撞檢測分析：

如圖5所示，通過計算矩形投影ABCD分別與直線T1T3、T2T4、T3T4的最小距離，反映圓柱體與直線的碰撞情況。已知圓柱體底面半徑r，設軸線l與各直線的最小距離為d5，當d5

軸線l與各直線的距離計算方法參考上文兩圓柱體軸線段最小距離計算方法。

圓柱體的矩形投影與圓弧碰撞檢測分析：

矩形投影在扇形T1OT2上的分布情況見圖6。如圖6所示，矩形投影與圓弧的位置關系共有4種情況。

(a) 投影全部在扇形外 (b) 投影部分在扇形內 (c) 投影部分在扇形內 (d) 投影全部在扇形內

在以O為極點，射線OT2為極軸，逆時針方向為正的極坐標系中描述E、F、T1、T2這4個端點：E(‖OE‖,α1)、F(‖OF‖,α2)、T1(‖OT1‖,β1)、T2(‖OT2‖,β2)，其中‖OT1‖=‖OT2‖=R，β2=0。

(Ⅰ)如圖6a所示，矩形投影全部落在扇形T1OT2范圍外，即E和F中最大極角小于T1和T2中的最小極角或E和F中最小極角大于T1和T2中的最大極角，此時矩形投影與圓弧線段無干涉發生。

(Ⅱ)如圖6b所示，矩形投影部分落在扇形T1OT2范圍內，T1和T2的最大極角介于E和F中最小和最大極角之間，且T1和T2的最小極角小于E和F中的最小極角，矩形投影軸線與扇形邊緣的交點處為G，當‖OG‖

(Ⅲ)如圖6c所示，矩形投影部分落在扇形T1OT2范圍內，T1和T2的最小極角介于E和F中最小和最大極角之間，且T1和T2的最大極角大于E和F中的最大極角，矩形投影軸線與扇形邊緣的交點處為G，當‖OG‖

(Ⅳ)如圖6d所示，矩形投影全部落在扇形T1OT2范圍內，當‖OE‖

總結以上多種情況，可得矩形投影與隧道圓弧輪廓的碰撞情況如表1所示。

表1 矩形投影與隧道圓弧輪廓的碰撞情況

由上述分析及計算公式能夠快速地檢測出機器人鉆臂與隧道的碰撞情況，通過在極坐標中判斷極角區間的方式節省了算法的處理時間，提高了碰撞算法的檢測效率。

3 試驗結果及分析

利用中鐵DJ3E鑿巖機器人展開試驗以驗證碰撞檢測算法的有效性。為保證試驗安全性，將兩圓柱體包絡盒間的安全距離設置為100 mm，實時碰撞檢測距離小于安全距離100 mm時，發生碰撞。

試驗分為鑿巖機器人自身機械臂碰撞檢測、鉆臂與隧道碰撞檢測兩大部分進行，其中，自身機械臂的碰撞檢測包含了單個鉆臂自身碰撞檢測、兩鉆臂之間碰撞檢測、鉆臂與吊籃臂碰撞檢測。

3.1 鑿巖機器人自身機械臂碰撞檢測

3.1.1 單個鉆臂桿件間碰撞檢測

為便于現場觀察，使用鑿巖機器人左臂進行試驗。移動左臂并在MATLAB軟件中計算鉆臂各桿件是否發生碰撞，桿件間的實時最小距離如表2所示。圖7為多臂鑿巖機器人自身機械臂碰撞檢測圖。左臂最終狀態如圖7a所示，此時左臂關節值θl=[6.55,-3.28,0,0.57,-6.33,7.21,548]，左臂自身將發生碰撞，DJ3E鑿巖機器人顯示器出現預警信號。

表2 MATLAB軟件計算左臂自身碰撞檢測結果

由表2和圖7a可知：在左臂移動過程中，當左臂桿件間最小距離小于安全距離時，MATLAB軟件實時計算結果表明鉆臂將發生碰撞。

3.1.2 兩鉆臂之間碰撞檢測

使用左臂和中臂進行試驗，初始狀態下無碰撞發生。移動左臂和中臂，并在MATLAB軟件中計算鉆臂是否發生碰撞，如表3所示。左臂和中臂的最終狀態如圖7b所示，左臂關節值θl=[-5.31,-8.52,0,2.01,-6.33,-3.42,548]，中臂關節值θc=[-1.88,-14.91,0,1.05,5.61,7.27,501]，左臂和中臂將發生碰撞，同時DJ3E鑿巖機器人顯示器出現預警信號。

由表3和圖7b可知：在左臂和中臂移動過程中，當兩臂桿件間最小距離小于安全距離時，MATLAB軟件實時計算結果表明兩鉆臂將發生碰撞。

表3 MATLAB軟件計算左臂與中臂碰撞檢測結果

3.1.3 鉆臂與吊籃臂碰撞檢測

表4 MATLAB軟件計算右臂與吊籃臂碰撞檢測結果

使用右臂和吊籃臂進行試驗。初始狀態下無碰撞發生。移動右臂和吊籃臂，并在MATLAB軟件中計算此時鉆臂和吊籃臂實時最小距離，如表4所示。右臂和吊籃臂最終狀態如圖7c所示，此時右臂關節值θr=[-4.99,-30.88,1 285,-0.12,20.24,1.06,1 795]，吊籃臂關節值θl=[-3.10,-7.62]，右臂和吊籃臂間將發生碰撞，DJ3E鑿巖機器人顯示器出現預警信號。

(a) 左臂自身桿件間碰撞檢測

由表4和圖7c可知：在右臂和吊籃臂移動過程中，當兩臂桿件間最小距離小于安全距離時，MATLAB軟件實時計算結果表明兩鉆臂發生碰撞。

3.2 鉆臂與隧道碰撞檢測

使用鑿巖機器人右臂進行與隧道的碰撞檢測試驗，移動右臂使其逐漸靠近隧道頂部，并在MATLAB軟件中計算此時右臂和隧道實時最小距離，如表5所示。右臂最終狀態如圖8所示，此時右臂關節值θr=[-2.71,-12.08,322,0.05,9.35,1.09,555]，右臂和隧道墻壁之間將發生碰撞，DJ3E鑿巖機器人顯示器出現預警信號。

表5 MATLAB軟件計算鉆臂與隧道碰撞檢測結果編號最小距離/mm是否碰撞1(初始狀態)658.4否2(中間過程)491.8否3(中間過程)106.3否4(最終狀態)<100是圖8 右臂與隧道碰撞檢測

由表5和圖8可知：當右臂與隧道的最小距離小于安全距離時，MATLAB軟件實時計算結果表明右臂與隧道發生碰撞。

上述試驗在實體DJ3E鑿巖機器人上完成，同時在MATLAB軟件中進行機械臂的碰撞檢測計算，將計算結果與實物碰撞結果進行對比驗證，最終證明了該碰撞檢測算法的實時性和準確性。

4 結論

本文針對多臂鑿巖機器人的結構和碰撞特點建立一種基于圓柱體包絡的碰撞檢測模型，提出一種空間兩圓柱體碰撞檢測方法、鉆臂與隧道輪廓的碰撞檢測方法，MATLAB軟件仿真數據和現場試驗數據證明，該檢測方法可以快速且全面地檢測鑿巖機器人的碰撞情況。