本發明屬于電力技術領域，具體涉及一種基于虛擬現實帶電作業機械臂防碰撞保護方法。

背景技術：

采用機器人自主完成帶電作業，不僅可以將工作人員從危險的、繁重的、精神緊張的工作中解放出來，有效避免帶電作業時人員傷亡事故的發生，使帶電作業更加安全，提高作業效率；同時還可以提高電網的運行質量，進一步減少供電系統的人員投入，降低人員成本，具有巨大的經濟效益和社會效益。

帶電作業時采用多機械臂共同完成帶電作業，如果沒有系統、安全、可靠的防碰撞方法，那么多機械臂必然無法協調完成帶電作業，甚至引發安全事故，給機器人和操作人員帶來危害。

技術實現要素：

本發明提出一種基于虛擬現實帶電作業機械臂防碰撞保護方法，能夠使得機器人在帶電作業時，多機械臂之間協調工作，互相配合、互不干擾，有序高效安全地完成帶電作業。

為了解決上述技術問題，本發明提供一種基于虛擬現實帶電作業機械臂防碰撞保護方法，根據真實作業場景建立虛擬作業場景，虛擬場景中機械臂模型的尺寸略大于機械臂實際尺寸，在虛擬場景中進行虛擬碰撞檢測，以判斷機械臂是否會發生碰撞；所述虛擬碰撞檢測采用基于包圍盒的虛擬碰撞檢測方法，具體為：

步驟1，預處理階段，對參與碰撞檢測的機械臂模型進行特征采樣，將構成機械臂模型的基本集合元素的包圍盒的中心點作為特征采樣點，根據兩個待檢測包圍盒的特征對建立二維離散搜索空間；

步驟2，碰撞檢測階段，包括以下步驟：

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

步驟2-2，判斷以上距離小于一定碰撞檢測閾值的特征對所對應的包圍盒是否相交，若包圍盒不相交，則進入步驟2-4；若包圍盒相交，則繼續步驟2-3；

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

進一步，步驟2-1具體為：

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

步驟2-1-2，計算每個粒子的適應度值；

步驟2-1-3，對每個粒子，比較其適應度值和個體極值p_best，如果適應度值小于粒子當前的個體極值p_best，則更新個體極值p_best；否則不更新；

步驟2-1-4，對每個粒子，比較其適應度值和全局極值g_best，如果適應度值小于粒子當前的全局極值g_best則更新全局極值g_best；否則不更新；

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

其中，k為迭代代次數；c₁,c₂為學習因子；r₁,r₂為均勻分布在[0,1]之間的兩個隨機數；ω為慣性權重；為粒子i的當前位置；V_i^k+1為粒子i的當前飛行速度；P_i是粒子i經歷的最優位置，稱為個體最優位置；P_g是到目前時間段為止，群體中所有粒子經歷的最優位置，稱為全局最優位置；

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

本發明與現有技術相比，其顯著優點在于：

(1)本發明針對自主帶電作業機器人，采用基于虛擬碰撞檢測的防碰撞保護方法和基于深度傳感器的防碰撞保護方法，多種方法結合，更好地監控機械臂安全、協調地完成帶電作業；

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

(3)虛擬場景中所建立的機械臂模型外圍尺寸比實際機械臂大一些，首先在虛擬場景中檢測到碰撞，然后及時對碰撞做出響應，避免真實場景中的機械臂發生碰撞。

(4)帶電作業機器人作業時分為人工遙操作方式和機器人自主方式，當機器人自主作業時，采用本發明的機械臂防碰撞保護方法；當人工遙操作時，一方面操作者通過監控畫面實時觀測機械臂狀態，另一方面，如果計算機檢測到機械臂即將發生碰撞，立即干預人工操作，停止機械臂的一切動作。

附圖說明

圖1為本發明中帶電作業機器人一種實施例的整體結構示意圖；

圖2為本發明中機器人平臺的結構示意圖；

圖3為本發明的帶電作業虛擬碰撞檢測的流程圖；

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

具體實施方式

容易理解，依據本發明的技術方案，在不變更本發明的實質精神的情況下，本領域的一般技術人員可以想象出本發明基于虛擬現實帶電作業機械臂防碰撞保護方法的多種實施方式。因此，以下具體實施方式和附圖僅是對本發明的技術方案的示例性說明，而不應當視為本發明的全部或者視為對本發明技術方案的限制或限定。

結合附圖，帶電作業機器人包括絕緣斗臂車1、控制室2、伸縮臂3、機器人平臺4。其中，絕緣斗臂車1上架設控制室2和伸縮臂3，伸縮臂3末端連接機器人平臺4，機器人平臺4與控制室2之間采用光纖以太網通信或者無線網絡通信。

絕緣斗臂車1可供操作人員駕駛，從而將機器人平臺4運輸到作業現場。絕緣斗臂車1上裝有支撐腿，支撐腿可以展開，從而將絕緣斗臂車1與地面穩固支撐。絕緣斗臂車1上裝有發電機，從而給控制室2及伸縮臂3供電。

伸縮臂3設有沿伸縮方向的驅動裝置，操作人員可以通過控制驅動裝置，從而將機器人平臺4升降到作業高度。該伸縮臂3由絕緣材料制成，用于實現機器人平臺4與控制室2的絕緣。在本發明中，伸縮臂3可有由剪叉式升降機構或其他機構代替。

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

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

機器人平臺4的絕緣子46用于支撐第一機械臂43、第二機械臂44、輔助機械臂42，將這三個機械臂的外殼與機器人平臺4絕緣。

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

專用工具箱47是放置抓具、扳手等作業工具的場所。機械臂末端安裝有工具快換裝置。機械臂根據作業任務的類型到專用工具箱47中使用工具快換裝置獲取作業工具。

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

為了防止上述帶電作業機器人各機械臂發生碰撞，本發明提出一種基于虛擬現實帶電作業機械臂防碰撞保護方法，包括基于虛擬碰撞檢測的防碰撞保護方法和基于深度傳感器的防碰撞保護方法。

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

步驟1，預處理階段，對參與碰撞檢測的機械臂模型進行特征采樣，對構成機械臂模型的基本集合元素的包圍盒的中心點作為特征采樣點。包括以下步驟：

步驟1-1，輸入兩個機械臂模型；

步驟1-2，用包圍盒將機械臂模型包圍住，對機械臂模型進行不斷的劃分，每一級劃分都用一個簡單的包圍盒包圍住，直到將機械臂模型細分成一些諸如三角形的基本幾何元素；

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

步驟1-4，建立二維離散搜索空間，即兩個待檢測包圍盒的特征對構成的搜索空間。

步驟2，詳細碰撞檢測階段，包括以下步驟：

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

步驟2-2，判斷以上距離小于一定碰撞檢測閾值的特征對所對應的包圍盒是否相交。當包圍盒不相交時，在包圍盒里面的基本幾何元素必然不會相交。當包圍盒發生相交時，基本幾何元素才會相交。

若不相交，進入步驟2-4；若包圍盒相交，繼續步驟2-3；

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

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

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

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

將待檢測的隨機碰撞檢測特征對(即兩個機械臂包圍盒的特征，如點、邊、三角片、四面體等)分別放在兩個集合中，記為集合A和集合B，假設它們分別含有N_a和N_b個元素。判斷兩個機械臂模型是否發生干涉碰撞，第一步需要計算兩個待檢測包圍盒之間是否至少存在一對特征對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的基本幾何元素的包圍盒的中心特征點，它們在三維物體空間中具有坐標屬性(x,y,z)。F為距離函數，δ為碰撞閾值。目標空間D是由特征對任意組合形成的二維離散空間，由于特征對具有對偶特性，D的大小為Na*Nb/2。

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

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，計算每個粒子的適應度值；

適應度函數是兩個包圍盒中心點之間的歐氏距離，為了減少計算量采用距離的平方：

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是物體的空間坐標，F(Xa,Xb)是適應度函數。

在每次迭代后粒子的狀態由它在兩個模型上的速度和位置分量共同決定。粒子P在第k-1次迭代后的狀態是P^k-1，經過一次進化后新的狀態是P^k。

步驟2-1-3，對每個粒子，用它的適應度值和個體極值p_best比較，如果適應度值小于粒子當前的個體極值p_best，則更新p_best；否則不更新；

步驟2-1-4，對每個粒子，用它的適應度值和全局極值g_best比較，如果適應度值小于粒子當前的全局極值g_best則更新g_best；否則不更新；

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

其中，k為迭代代數；c₁,c₂是學習因子，一般取值2；r₁,r₂是均勻分布在[0,1]之間的兩個隨機數；ω為慣性權重，可以對算法的全局搜索能力和局部搜索能力進行平衡調整；X_i為粒子i的當前位置；V_i為粒子i的當前飛行速度；P_i是粒子i經歷的最優位置，稱為個體最優位置；P_g是到目前時間段為止，群體中所有粒子經歷的最優位置，稱為全局最優位置。

步驟2-1-6，滿足結束條件(粒子的適應度值小于一定范圍或到達最大循環次數)退出，得到距離小于一定碰撞檢測閾值的特征對，否則回到步驟2-1-2。

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

另外，帶電作業機器人作業時分為人工遙操作方式和機器人自主方式。當機器人自主作業時，采用上述機械臂防碰撞保護方法；當人工遙操作時，一方面操作者通過監控畫面實時觀測機械臂狀態，另一方面，如果計算機檢測到機械臂即將發生碰撞，立即干預人工操作，停止機械臂的一切動作。