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

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

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

0 引言

目前，多臂鑿巖機(jī)器人廣泛應(yīng)用于煤礦、隧道開挖等領(lǐng)域中。由于鑿巖機(jī)器人屬于工業(yè)液壓重載型機(jī)械臂，具有多臂協(xié)作、結(jié)構(gòu)復(fù)雜等特點(diǎn)，機(jī)械臂在隧道中作業(yè)時(shí)極易發(fā)生碰撞干涉現(xiàn)象，嚴(yán)重時(shí)會(huì)造成施工人員傷亡和極大的經(jīng)濟(jì)損失，因此多臂鑿巖機(jī)器人的碰撞檢測問題亟需解決[1-3]。

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

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

1 鑿巖機(jī)器人碰撞檢測模型

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

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

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

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

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

2 碰撞檢測算法

2.1 兩圓柱體包絡(luò)盒碰撞檢測算法

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

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

設(shè)兩圓柱體包絡(luò)盒的軸線段為l1、l2，底面半徑為r1、r2，4個(gè)端點(diǎn)坐標(biāo)分別為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上任意一點(diǎn)坐標(biāo)用這兩個(gè)向量表示為:

(1)

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

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

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

(2)

(3)

如圖4所示，當(dāng)0≤λ1,λ2≤1時(shí)，表示兩直線的公垂線垂足落在兩條線段中，則此時(shí)的最短距離d2=f(λ1,λ2)；當(dāng)λ1,λ2有一個(gè)不在[0,1]時(shí)，則表示兩個(gè)垂足至少有一個(gè)沒有落在線段內(nèi)，此時(shí)分別求解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)，當(dāng)dmin>0時(shí)，兩圓柱體不發(fā)生碰撞，否則碰撞現(xiàn)象發(fā)生。該方法基于空間向量運(yùn)算實(shí)現(xiàn)，避免了求解直線方程等復(fù)雜過程，便于通過C語言和MATLAB軟件代碼實(shí)現(xiàn)，提高了運(yùn)行效率和檢測精度。

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

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

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

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

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

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

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

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

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

在以O(shè)為極點(diǎn)，射線OT2為極軸，逆時(shí)針方向?yàn)檎臉O坐標(biāo)系中描述E、F、T1、T2這4個(gè)端點(diǎn)：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中的最大極角，此時(shí)矩形投影與圓弧線段無干涉發(fā)生。

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

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

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

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

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

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

3 試驗(yàn)結(jié)果及分析

利用中鐵DJ3E鑿巖機(jī)器人展開試驗(yàn)以驗(yàn)證碰撞檢測算法的有效性。為保證試驗(yàn)安全性，將兩圓柱體包絡(luò)盒間的安全距離設(shè)置為100 mm，實(shí)時(shí)碰撞檢測距離小于安全距離100 mm時(shí)，發(fā)生碰撞。

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

3.1 鑿巖機(jī)器人自身機(jī)械臂碰撞檢測

3.1.1 單個(gè)鉆臂桿件間碰撞檢測

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

表2 MATLAB軟件計(jì)算左臂自身碰撞檢測結(jié)果

由表2和圖7a可知：在左臂移動(dòng)過程中，當(dāng)左臂桿件間最小距離小于安全距離時(shí)，MATLAB軟件實(shí)時(shí)計(jì)算結(jié)果表明鉆臂將發(fā)生碰撞。

3.1.2 兩鉆臂之間碰撞檢測

使用左臂和中臂進(jìn)行試驗(yàn)，初始狀態(tài)下無碰撞發(fā)生。移動(dòng)左臂和中臂，并在MATLAB軟件中計(jì)算鉆臂是否發(fā)生碰撞，如表3所示。左臂和中臂的最終狀態(tài)如圖7b所示，左臂關(guān)節(jié)值θl=[-5.31,-8.52,0,2.01,-6.33,-3.42,548]，中臂關(guān)節(jié)值θc=[-1.88,-14.91,0,1.05,5.61,7.27,501]，左臂和中臂將發(fā)生碰撞，同時(shí)DJ3E鑿巖機(jī)器人顯示器出現(xiàn)預(yù)警信號。

由表3和圖7b可知：在左臂和中臂移動(dòng)過程中，當(dāng)兩臂桿件間最小距離小于安全距離時(shí)，MATLAB軟件實(shí)時(shí)計(jì)算結(jié)果表明兩鉆臂將發(fā)生碰撞。

表3 MATLAB軟件計(jì)算左臂與中臂碰撞檢測結(jié)果

3.1.3 鉆臂與吊籃臂碰撞檢測

表4 MATLAB軟件計(jì)算右臂與吊籃臂碰撞檢測結(jié)果

使用右臂和吊籃臂進(jìn)行試驗(yàn)。初始狀態(tài)下無碰撞發(fā)生。移動(dòng)右臂和吊籃臂，并在MATLAB軟件中計(jì)算此時(shí)鉆臂和吊籃臂實(shí)時(shí)最小距離，如表4所示。右臂和吊籃臂最終狀態(tài)如圖7c所示，此時(shí)右臂關(guān)節(jié)值θr=[-4.99,-30.88,1 285,-0.12,20.24,1.06,1 795]，吊籃臂關(guān)節(jié)值θl=[-3.10,-7.62]，右臂和吊籃臂間將發(fā)生碰撞，DJ3E鑿巖機(jī)器人顯示器出現(xiàn)預(yù)警信號。

(a) 左臂自身?xiàng)U件間碰撞檢測

由表4和圖7c可知：在右臂和吊籃臂移動(dòng)過程中，當(dāng)兩臂桿件間最小距離小于安全距離時(shí)，MATLAB軟件實(shí)時(shí)計(jì)算結(jié)果表明兩鉆臂發(fā)生碰撞。

3.2 鉆臂與隧道碰撞檢測

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

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

由表5和圖8可知：當(dāng)右臂與隧道的最小距離小于安全距離時(shí)，MATLAB軟件實(shí)時(shí)計(jì)算結(jié)果表明右臂與隧道發(fā)生碰撞。

上述試驗(yàn)在實(shí)體DJ3E鑿巖機(jī)器人上完成，同時(shí)在MATLAB軟件中進(jìn)行機(jī)械臂的碰撞檢測計(jì)算，將計(jì)算結(jié)果與實(shí)物碰撞結(jié)果進(jìn)行對比驗(yàn)證，最終證明了該碰撞檢測算法的實(shí)時(shí)性和準(zhǔn)確性。

4 結(jié)論

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