一種基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法與流程

本發(fā)明屬于電力技術(shù)領(lǐng)域，具體涉及一種基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法。

背景技術(shù)：

采用機(jī)器人自主完成帶電作業(yè)，不僅可以將工作人員從危險(xiǎn)的、繁重的、精神緊張的工作中解放出來(lái)，有效避免帶電作業(yè)時(shí)人員傷亡事故的發(fā)生，使帶電作業(yè)更加安全，提高作業(yè)效率；同時(shí)還可以提高電網(wǎng)的運(yùn)行質(zhì)量，進(jìn)一步減少供電系統(tǒng)的人員投入，降低人員成本，具有巨大的經(jīng)濟(jì)效益和社會(huì)效益。

帶電作業(yè)時(shí)采用多機(jī)械臂共同完成帶電作業(yè)，如果沒(méi)有系統(tǒng)、安全、可靠的防碰撞方法，那么多機(jī)械臂必然無(wú)法協(xié)調(diào)完成帶電作業(yè)，甚至引發(fā)安全事故，給機(jī)器人和操作人員帶來(lái)危害。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提出一種基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法，能夠使得機(jī)器人在帶電作業(yè)時(shí)，多機(jī)械臂之間協(xié)調(diào)工作，互相配合、互不干擾，有序高效安全地完成帶電作業(yè)。

為了解決上述技術(shù)問(wèn)題，本發(fā)明提供一種基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法，根據(jù)真實(shí)作業(yè)場(chǎng)景建立虛擬作業(yè)場(chǎng)景，虛擬場(chǎng)景中機(jī)械臂模型的尺寸略大于機(jī)械臂實(shí)際尺寸，在虛擬場(chǎng)景中進(jìn)行虛擬碰撞檢測(cè)，以判斷機(jī)械臂是否會(huì)發(fā)生碰撞；所述虛擬碰撞檢測(cè)采用基于包圍盒的虛擬碰撞檢測(cè)方法，具體為：

步驟1，預(yù)處理階段，對(duì)參與碰撞檢測(cè)的機(jī)械臂模型進(jìn)行特征采樣，將構(gòu)成機(jī)械臂模型的基本集合元素的包圍盒的中心點(diǎn)作為特征采樣點(diǎn)，根據(jù)兩個(gè)待檢測(cè)包圍盒的特征對(duì)建立二維離散搜索空間；

步驟2，碰撞檢測(cè)階段，包括以下步驟：

步驟2-1，采用粒子群算法進(jìn)行搜索，得到距離小于一定碰撞檢測(cè)閾值的特征對(duì)；

步驟2-2，判斷以上距離小于一定碰撞檢測(cè)閾值的特征對(duì)所對(duì)應(yīng)的包圍盒是否相交，若包圍盒不相交，則進(jìn)入步驟2-4；若包圍盒相交，則繼續(xù)步驟2-3；

步驟2-3，判斷機(jī)械臂的基本幾何元素是否相交，若基本幾何元素不相交，進(jìn)入步驟2-4；若基本幾何元素相交，則做出碰撞響應(yīng)。

進(jìn)一步，步驟2-1具體為：

步驟2-1-1，將兩個(gè)機(jī)械臂基本幾何元素的包圍盒特征作為粒子，初始化粒子群，包括群體規(guī)模、每個(gè)粒子的位置和速度；

步驟2-1-2，計(jì)算每個(gè)粒子的適應(yīng)度值；

步驟2-1-3，對(duì)每個(gè)粒子，比較其適應(yīng)度值和個(gè)體極值p_best，如果適應(yīng)度值小于粒子當(dāng)前的個(gè)體極值p_best，則更新個(gè)體極值p_best；否則不更新；

步驟2-1-4，對(duì)每個(gè)粒子，比較其適應(yīng)度值和全局極值g_best，如果適應(yīng)度值小于粒子當(dāng)前的全局極值g_best則更新全局極值g_best；否則不更新；

步驟2-1-5，根據(jù)以下公式更新粒子的速度和位置；

其中，k為迭代代次數(shù)；c₁,c₂為學(xué)習(xí)因子；r₁,r₂為均勻分布在[0,1]之間的兩個(gè)隨機(jī)數(shù)；ω為慣性權(quán)重；為粒子i的當(dāng)前位置；V_i^k+1為粒子i的當(dāng)前飛行速度；P_i是粒子i經(jīng)歷的最優(yōu)位置，稱為個(gè)體最優(yōu)位置；P_g是到目前時(shí)間段為止，群體中所有粒子經(jīng)歷的最優(yōu)位置，稱為全局最優(yōu)位置；

步驟2-1-6，判斷是否滿足粒子群算法結(jié)束條件，若滿足，輸出距離小于一定碰撞檢測(cè)閾值的特征對(duì)，若不滿足，則返回步驟2-1-2。

本發(fā)明與現(xiàn)有技術(shù)相比，其顯著優(yōu)點(diǎn)在于：

(1)本發(fā)明針對(duì)自主帶電作業(yè)機(jī)器人，采用基于虛擬碰撞檢測(cè)的防碰撞保護(hù)方法和基于深度傳感器的防碰撞保護(hù)方法，多種方法結(jié)合，更好地監(jiān)控機(jī)械臂安全、協(xié)調(diào)地完成帶電作業(yè)；

(2)將粒子群算法應(yīng)用到帶電作業(yè)的虛擬碰撞檢測(cè)中，可以實(shí)時(shí)地檢測(cè)到發(fā)生干涉的元素，從而及時(shí)避免碰撞事故；

(3)虛擬場(chǎng)景中所建立的機(jī)械臂模型外圍尺寸比實(shí)際機(jī)械臂大一些，首先在虛擬場(chǎng)景中檢測(cè)到碰撞，然后及時(shí)對(duì)碰撞做出響應(yīng)，避免真實(shí)場(chǎng)景中的機(jī)械臂發(fā)生碰撞。

(4)帶電作業(yè)機(jī)器人作業(yè)時(shí)分為人工遙操作方式和機(jī)器人自主方式，當(dāng)機(jī)器人自主作業(yè)時(shí)，采用本發(fā)明的機(jī)械臂防碰撞保護(hù)方法；當(dāng)人工遙操作時(shí)，一方面操作者通過(guò)監(jiān)控畫(huà)面實(shí)時(shí)觀測(cè)機(jī)械臂狀態(tài)，另一方面，如果計(jì)算機(jī)檢測(cè)到機(jī)械臂即將發(fā)生碰撞，立即干預(yù)人工操作，停止機(jī)械臂的一切動(dòng)作。

附圖說(shuō)明

圖1為本發(fā)明中帶電作業(yè)機(jī)器人一種實(shí)施例的整體結(jié)構(gòu)示意圖；

圖2為本發(fā)明中機(jī)器人平臺(tái)的結(jié)構(gòu)示意圖；

圖3為本發(fā)明的帶電作業(yè)虛擬碰撞檢測(cè)的流程圖；

圖4為采用粒子群算法的流程圖。

具體實(shí)施方式

容易理解，依據(jù)本發(fā)明的技術(shù)方案，在不變更本發(fā)明的實(shí)質(zhì)精神的情況下，本領(lǐng)域的一般技術(shù)人員可以想象出本發(fā)明基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法的多種實(shí)施方式。因此，以下具體實(shí)施方式和附圖僅是對(duì)本發(fā)明的技術(shù)方案的示例性說(shuō)明，而不應(yīng)當(dāng)視為本發(fā)明的全部或者視為對(duì)本發(fā)明技術(shù)方案的限制或限定。

結(jié)合附圖，帶電作業(yè)機(jī)器人包括絕緣斗臂車1、控制室2、伸縮臂3、機(jī)器人平臺(tái)4。其中，絕緣斗臂車1上架設(shè)控制室2和伸縮臂3，伸縮臂3末端連接機(jī)器人平臺(tái)4，機(jī)器人平臺(tái)4與控制室2之間采用光纖以太網(wǎng)通信或者無(wú)線網(wǎng)絡(luò)通信。

絕緣斗臂車1可供操作人員駕駛，從而將機(jī)器人平臺(tái)4運(yùn)輸?shù)阶鳂I(yè)現(xiàn)場(chǎng)。絕緣斗臂車1上裝有支撐腿，支撐腿可以展開(kāi)，從而將絕緣斗臂車1與地面穩(wěn)固支撐。絕緣斗臂車1上裝有發(fā)電機(jī)，從而給控制室2及伸縮臂3供電。

伸縮臂3設(shè)有沿伸縮方向的驅(qū)動(dòng)裝置，操作人員可以通過(guò)控制驅(qū)動(dòng)裝置，從而將機(jī)器人平臺(tái)4升降到作業(yè)高度。該伸縮臂3由絕緣材料制成，用于實(shí)現(xiàn)機(jī)器人平臺(tái)4與控制室2的絕緣。在本發(fā)明中，伸縮臂3可有由剪叉式升降機(jī)構(gòu)或其他機(jī)構(gòu)代替。

控制室2中設(shè)置有第二工控機(jī)、顯示屏、第一主操作手、第二主操作手、輔助主操作手以及通信模塊等。

機(jī)器人平臺(tái)4包括絕緣子46、第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42、第一工控機(jī)48、雙目攝像頭45、全景攝像頭41、深度攝像頭410、蓄電池49、專用工具箱47、通信模塊。

機(jī)器人平臺(tái)4的絕緣子46用于支撐第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42，將這三個(gè)機(jī)械臂的外殼與機(jī)器人平臺(tái)4絕緣。

蓄電池49為第一工控機(jī)48、第一機(jī)械臂43、第二機(jī)械臂44、輔助機(jī)械臂42、全景攝像頭41、雙目攝像頭45、深度攝像頭410、通信模塊供電。

專用工具箱47是放置抓具、扳手等作業(yè)工具的場(chǎng)所。機(jī)械臂末端安裝有工具快換裝置。機(jī)械臂根據(jù)作業(yè)任務(wù)的類型到專用工具箱47中使用工具快換裝置獲取作業(yè)工具。

控制室2中第一主操作手、第二主操作手以及輔助主操作手是一種用于人工遠(yuǎn)程操作機(jī)械臂的操作裝置，他們與第一機(jī)械臂43、第二機(jī)械臂44和輔助機(jī)械臂42構(gòu)成主從操作關(guān)系。機(jī)械臂和主操作手具有相同的結(jié)構(gòu)，只是主操作手尺寸規(guī)格比機(jī)械臂小，以便于操作人員操作。機(jī)械臂和主操作手擁有六個(gè)關(guān)節(jié)，每個(gè)關(guān)節(jié)都有光電編碼器采集角度數(shù)據(jù)，各主操作手的微型控制器通過(guò)串口將六個(gè)關(guān)節(jié)的角度數(shù)據(jù)發(fā)送給第二工控機(jī)。

為了防止上述帶電作業(yè)機(jī)器人各機(jī)械臂發(fā)生碰撞，本發(fā)明提出一種基于虛擬現(xiàn)實(shí)帶電作業(yè)機(jī)械臂防碰撞保護(hù)方法，包括基于虛擬碰撞檢測(cè)的防碰撞保護(hù)方法和基于深度傳感器的防碰撞保護(hù)方法。

所述虛擬碰撞檢測(cè)，即建立機(jī)械臂的運(yùn)動(dòng)虛擬場(chǎng)景，在虛擬場(chǎng)景中機(jī)械臂交互和運(yùn)動(dòng)時(shí)不可避免會(huì)發(fā)生碰撞，系統(tǒng)必須實(shí)時(shí)地檢測(cè)到這些碰撞，避免場(chǎng)景中機(jī)械臂在運(yùn)動(dòng)過(guò)程中發(fā)生穿透等現(xiàn)象。虛擬場(chǎng)景中機(jī)械臂外圍尺寸比實(shí)際機(jī)械臂大一些，當(dāng)虛擬機(jī)械臂檢測(cè)到碰撞時(shí)，實(shí)際機(jī)械臂還未發(fā)生碰撞，此時(shí)虛擬機(jī)械臂進(jìn)行碰撞響應(yīng)，一個(gè)機(jī)械臂向反方向移動(dòng)，另一個(gè)物體(機(jī)械臂或帶電作業(yè)現(xiàn)場(chǎng)其他物體)保持靜止，從而有效避免了機(jī)械臂發(fā)生碰撞。進(jìn)一步，虛擬碰撞檢測(cè)分為兩個(gè)步驟：

步驟1，預(yù)處理階段，對(duì)參與碰撞檢測(cè)的機(jī)械臂模型進(jìn)行特征采樣，對(duì)構(gòu)成機(jī)械臂模型的基本集合元素的包圍盒的中心點(diǎn)作為特征采樣點(diǎn)。包括以下步驟：

步驟1-1，輸入兩個(gè)機(jī)械臂模型；

步驟1-2，用包圍盒將機(jī)械臂模型包圍住，對(duì)機(jī)械臂模型進(jìn)行不斷的劃分，每一級(jí)劃分都用一個(gè)簡(jiǎn)單的包圍盒包圍住，直到將機(jī)械臂模型細(xì)分成一些諸如三角形的基本幾何元素；

步驟1-3，對(duì)機(jī)械臂基本幾何元素的包圍盒進(jìn)行特征采樣，如點(diǎn)、邊、三角片、四面體等；

步驟1-4，建立二維離散搜索空間，即兩個(gè)待檢測(cè)包圍盒的特征對(duì)構(gòu)成的搜索空間。

步驟2，詳細(xì)碰撞檢測(cè)階段，包括以下步驟：

步驟2-1，采用粒子群算法進(jìn)行搜索，得到距離小于一定碰撞檢測(cè)閾值的特征對(duì)；

步驟2-2，判斷以上距離小于一定碰撞檢測(cè)閾值的特征對(duì)所對(duì)應(yīng)的包圍盒是否相交。當(dāng)包圍盒不相交時(shí)，在包圍盒里面的基本幾何元素必然不會(huì)相交。當(dāng)包圍盒發(fā)生相交時(shí)，基本幾何元素才會(huì)相交。

若不相交，進(jìn)入步驟2-4；若包圍盒相交，繼續(xù)步驟2-3；

步驟2-3，判斷機(jī)械臂的基本幾何元素(點(diǎn)、邊、三角片、四面體等)是否相交，若不相交，進(jìn)入步驟2-4；若幾何元素相交，做出碰撞相應(yīng)；

步驟2-4，判斷是否終止，終止則退出，不終止則回到步驟2-1。

進(jìn)一步，步驟2-1中采用的粒子群算法，兩個(gè)機(jī)械臂基本幾何元素的包圍盒特征對(duì)即為粒子，包括以下步驟：

步驟2-1-1，初始化粒子群，包括群體規(guī)模、每個(gè)粒子的位置和速度；

將待檢測(cè)的隨機(jī)碰撞檢測(cè)特征對(duì)(即兩個(gè)機(jī)械臂包圍盒的特征，如點(diǎn)、邊、三角片、四面體等)分別放在兩個(gè)集合中，記為集合A和集合B，假設(shè)它們分別含有N_a和N_b個(gè)元素。判斷兩個(gè)機(jī)械臂模型是否發(fā)生干涉碰撞，第一步需要計(jì)算兩個(gè)待檢測(cè)包圍盒之間是否至少存在一對(duì)特征對(duì)P(a_i,b_j)，其中a_i∈A(0＜i＜N_a)，b_j∈B(0＜j＜N_b)，使F(P)≤δ。a_i,b_j分別是集合A和B的基本幾何元素的包圍盒的中心特征點(diǎn)，它們?cè)谌S物體空間中具有坐標(biāo)屬性(x,y,z)。F為距離函數(shù)，δ為碰撞閾值。目標(biāo)空間D是由特征對(duì)任意組合形成的二維離散空間，由于特征對(duì)具有對(duì)偶特性，D的大小為Na*Nb/2。

一個(gè)粒子P_k(a_i,b_j)代表兩個(gè)機(jī)械臂包圍盒采樣特征的一個(gè)組合，每個(gè)特征在三維物體空間都具有一個(gè)位置。優(yōu)化的目標(biāo)是找到一個(gè)特征間距離最短的組合。每個(gè)粒子的速度和位置向量有兩部分組成：

V＝{Va,Vb}＝{(v_ax,v_ay,v_az),(v_bx,v_by,v_bz)}

X＝{Xa,Xb}＝{(x_a,y_a,z_a),(x_b,y_b,z_b)}

其中，V是速度，Va、Vb分別是特征a、b的速度，v_ax,v_ay,v_az分別代表特征a在x、y、z軸上的速度，v_bx,v_by,v_bz分別代表特征b在x、y、z軸上的速度；X是位置，Xa、Xb分別是特征a、b的位置，x_a,y_a,z_a分別代表特征a在x、y、z軸上的位置，x_b,y_b,z_b分別代表特征b在x、y、z軸上的位置。

步驟2-1-2，計(jì)算每個(gè)粒子的適應(yīng)度值；

適應(yīng)度函數(shù)是兩個(gè)包圍盒中心點(diǎn)之間的歐氏距離，為了減少計(jì)算量采用距離的平方：

F(Xa,Xb)＝(x_a-x_b)²+(y_a-y_b)²+(z_a-z_b)²

其中，x_a,x_b,y_a,y_b,z_a,z_b是物體的空間坐標(biāo)，F(xiàn)(Xa,Xb)是適應(yīng)度函數(shù)。

在每次迭代后粒子的狀態(tài)由它在兩個(gè)模型上的速度和位置分量共同決定。粒子P在第k-1次迭代后的狀態(tài)是P^k-1，經(jīng)過(guò)一次進(jìn)化后新的狀態(tài)是P^k。

步驟2-1-3，對(duì)每個(gè)粒子，用它的適應(yīng)度值和個(gè)體極值p_best比較，如果適應(yīng)度值小于粒子當(dāng)前的個(gè)體極值p_best，則更新p_best；否則不更新；

步驟2-1-4，對(duì)每個(gè)粒子，用它的適應(yīng)度值和全局極值g_best比較，如果適應(yīng)度值小于粒子當(dāng)前的全局極值g_best則更新g_best；否則不更新；

步驟2-1-5，根據(jù)以下公式更新粒子的速度和位置；

其中，k為迭代代數(shù)；c₁,c₂是學(xué)習(xí)因子，一般取值2；r₁,r₂是均勻分布在[0,1]之間的兩個(gè)隨機(jī)數(shù)；ω為慣性權(quán)重，可以對(duì)算法的全局搜索能力和局部搜索能力進(jìn)行平衡調(diào)整；X_i為粒子i的當(dāng)前位置；V_i為粒子i的當(dāng)前飛行速度；P_i是粒子i經(jīng)歷的最優(yōu)位置，稱為個(gè)體最優(yōu)位置；P_g是到目前時(shí)間段為止，群體中所有粒子經(jīng)歷的最優(yōu)位置，稱為全局最優(yōu)位置。

步驟2-1-6，滿足結(jié)束條件(粒子的適應(yīng)度值小于一定范圍或到達(dá)最大循環(huán)次數(shù))退出，得到距離小于一定碰撞檢測(cè)閾值的特征對(duì)，否則回到步驟2-1-2。

所述基于深度傳感器的防碰撞保護(hù)方法，利用深度傳感器測(cè)量得到兩個(gè)物體之間的位置，獲得點(diǎn)云信息，計(jì)算兩個(gè)物體點(diǎn)云數(shù)據(jù)的最小距離d，當(dāng)d小于一定閾值時(shí)，認(rèn)為兩個(gè)物體發(fā)生碰撞，此時(shí)，一個(gè)機(jī)械臂向反方向運(yùn)動(dòng)，另一個(gè)機(jī)械臂保持靜止，從而有效避免了帶電作業(yè)時(shí)發(fā)生意外碰撞事故。

另外，帶電作業(yè)機(jī)器人作業(yè)時(shí)分為人工遙操作方式和機(jī)器人自主方式。當(dāng)機(jī)器人自主作業(yè)時(shí)，采用上述機(jī)械臂防碰撞保護(hù)方法；當(dāng)人工遙操作時(shí)，一方面操作者通過(guò)監(jiān)控畫(huà)面實(shí)時(shí)觀測(cè)機(jī)械臂狀態(tài)，另一方面，如果計(jì)算機(jī)檢測(cè)到機(jī)械臂即將發(fā)生碰撞，立即干預(yù)人工操作，停止機(jī)械臂的一切動(dòng)作。