本發明屬于電力技術領域，具體涉及一種帶電作業機器人作業環境3D重構方法。

背景技術：

目前，我國高壓帶電作業，主要采用絕緣手套作業法，要求操作人員攀爬高壓鐵塔或借助絕緣斗臂車進行不停電作業。但人工帶電作業稍有不慎就容易發生人身傷亡事故，給家庭和社會帶來嚴重的損失。

目前，國內已研發的一款帶電作業機器人，仍然需要操作人員在絕緣斗內隨機器人升至線路附近，沒有從根本上解決操作人員的生命安全問題。并且，該作業機器人完全由操作人員控制，不能自主完成帶電作業，較傳統的絕緣手套作業法反而效率更低。

技術實現要素：

本發明提出一種帶電作業機器人作業環境3D重構方法，在三維空間中對障礙物建模，并應用3D重構技術在監控顯示器顯示中，再現機器人作業的真實場景，以便操作人員發出相應的指令。

為了解決上述技術問題，本發明提供一種帶電作業機器人作業環境3D重構方法，利用雙目攝像頭確定單個元器件和機械手工具的位姿參數，將獲得的位姿參數導入帶電作業場景的模型中，生成帶電作業現場實時監控場景并顯示在監控設備上。

進一步，利用雙目攝像頭，從兩個視點同時觀察同一場景獲取的目標圖像，通過三角測量原理，計算圖像像素間的視差來解算特征點在測量坐標系下的三維坐標，根據物體上對應特征點在測量坐標系下空間坐標以及物體坐標系下空間坐標，求解目標的位置姿態參數。

進一步，求解目標的位置姿態參數的具體過程為：

第一步：建立坐標系：

建立物體坐標系O_O-x_oy_oz_o，設目標上第i個特征點在物體坐標系上的坐標為q_i＝(x_wi,y_wi,z_wi)^T；

建立測量坐標系，將第一個攝像頭的攝像頭坐標系為測量坐標系，目標上第i個特征點在測量坐標系上的坐標為p_i＝(x_ci,y_ci,z_ci)^T；

建立兩個攝像機所采集的像素坐標系o-uv和o′-u′v′，o-uv為左攝像機像素坐標，o′-u′v′為右攝像機像素坐標，以成像平面左上角為原點，u、v軸分別平行于像平面坐標系的x軸和y軸。目標第i個特征點在兩雙目圖像上的投影在其像素坐標系下的坐標分別為(u,v)^T和(u′,v′)^T；

第二步：計算特征點在測量坐標系中的坐標：

雙目攝像機中兩攝像機的投影矩陣分別為M₁和M₂，

M₁＝K[I 0]

M₂＝K[R_C T_C]

其中，K為攝像機的內參數矩陣，R_C和T_C為右攝像頭相對于左攝像頭的旋轉矩陣和平移向量。

所求該特征點在測量坐標系下的齊次坐標用下式解出，

M₁₁、M₁₂、M₁₃為投影矩陣M₁的行向量，M₂₁、M₂₂、M₂₃為投影矩陣M₂的行向量，設左攝像頭采集圖像上第i個特征點在像素坐標系下的齊次坐標為(u₁,v₁,1)^T；

第三步：求解旋轉矩陣N：一個四元數包含一個標量分量和一個3D向量分量，的最大值是旋轉矩陣N的最大特征值，為其對應的特征向量，N的求解公式如下，式中的S為M的分塊矩陣；p為特征點在測量坐標系下的三維坐標，q為特征點在物體坐標系下的三維坐標；

假設n個特征點的質心在測量坐標系和物體坐標系中的坐標分別為和

則可以得到分別以質心為原點的坐標系下的新坐標

第四步：計算出目標對象的位移向量：通過公式計算出位移向量由，位移向量即確定了相關的位姿參數。

本發明與現有技術相比，其顯著優點在于，本發明中，操作人員通過主從遙操作方式，將帶電作業機器人平臺送至自主作業工位。機器人到達自主作業工位后，將根據操作人員下達的任務指令，完成帶電作業任務。在作業機械手自主作業時，必須避免碰到障礙物，否則可能會引起線路損壞或斷電。為使系統自主作業時具有避障功能，需要在三維空間中對障礙物建模，并應用3D重構技術在監控顯示器顯示中，再現機器人作業的真實場景，以便操作人員發出相應的指令。

附圖說明

圖1是帶電作業場景結構示意圖。

圖2是位姿計算流程圖。

圖3是作業環境重構流程圖。

圖4是帶電作業機器人系統示意圖。

圖5是機器人平臺示意圖。

具體實施方式

容易理解，依據本發明的技術方案，在不變更本發明的實質精神的情況下，本領域的一般技術人員可以想象出本發明帶電作業機器人作業環境3D重構方法的多種實施方式。因此，以下具體實施方式和附圖僅是對本發明的技術方案的示例性說明，而不應當視為本發明的全部或者視為對本發明技術方案的限制或限定。

結合附圖，基于高壓帶電機器人作業環境的3D重構方法，利用雙目攝像頭確定單個元器件和機械手工具的位姿參數，將獲得的位姿參數導入帶電作業場景的模型中，生成帶電作業現場實時監控場景并顯示在監控設備上。

具體包括如下步驟：

一、場景構建

所構建場景包括天空遠景、電線桿塔、機械臂(包括第一機械臂43、第二機械臂44、輔助機械臂42)、電線、單個元器件和機械手專用工具。其中，機械臂包括第一機械臂43、第二機械臂44和輔助機械臂42。單個元器件包括絕緣子、跌落式熔斷器以及隔離刀閘等。機械手專用工具包括電動扳手、自動破螺母工具、夾持器以及自動剝皮器等等。

基于系統雙機械臂、輔助機械手、電線桿塔、單個元器件，電線等的實體裝置按比例建立三維模型。

機械臂建模。建模過程中釆用測繪技術或機械臂的結構參數，先建立機械臂的等比例的三維模型。由于機械臂機械結構復雜，為了保證仿真的有效性，先對機械臂各個構件單獨建立三維模型，然后利用三維軟件的裝配功能對機械臂各個構件按按照自由度進行構件裝配。

單個元器件建模。需要先根據單個元器件詳細的尺寸參數，建立一個平面圖，然后再通過各種特征的建立和修改，構造其三維模型。

電線桿塔建模。通過單個元器件的建模，當桿塔的各部分組成元器件模型均建立后，根據三維場景中模型的全部可見面，去除被物體遮擋面和不可見面，將桿塔上的電力元器件裝配成一個整體。

天空遠景建模。天空背景通過實地拍攝，由攝像機傳入計算機，運用Photoshop軟件進行修正，生成紋理，再轉換成渲染引擎軟件可用的格式。采用半圓球法給天空建模，用加蓋一個籠罩整個地形的半球面作為“頂”，加上適當光照效果，產生強烈的縱深感。

在給所有部件建模之后，布局帶電作業場景時依據單個元器件、電線桿塔、天空遠景、機械臂之間的機械連接關系，需對照部件實物及其機械手冊，獲取模型尺寸的比例關系，需對作業場景和帶電作業機器人系統，進行多個角度的全景拍攝，通過對這些圖像的分析組成線路桿塔的各種設備和機械手各機械部件的標準參數。

在渲染引擎里集成各個部件后，要給場景進行加工修飾，準確匹配各個部件在虛擬光源照射下所產生的實體亮度、陰影強度。

機械臂模型的渲染驅動：接收第一工控機48發送給機械臂的各個電機的控制信號，依據機械臂模型的物理性質以及機電特性，模擬出機械臂的運動軌跡并顯示于監控畫面。

連續拍攝第一機械臂43、第二機械臂44、輔助機械臂42雙目圖片，對包圍待識別物體的特征點的區域進行特征點形態開閉運算邊緣檢測提取，與事先準備好的特征點模板進行匹配，匹配度最高的即為特征點，將特征點中心坐標輸入對象三維模型，作為相機坐標系下的坐標。

利用雙目攝像頭45，從兩個視點同時觀察同一場景獲取的目標圖像，通過三角測量原理，計算圖像像素間的視差來解算特征點在測量坐標系下的三維坐標，根據物體上對應特征點在測量坐標系下空間坐標以及物體坐標系下空間坐標，求解目標的位置姿態參數。下面是具體的計算過程

第一步：建立坐標系：

建立物體坐標系O_O-x_oy_oz_o，設目標上第i個特征點在物體坐標系上的坐標為q_i＝(x_wi,y_wi,z_wi)^T；

建立測量坐標系，將第一個攝像頭的攝像頭坐標系為測量坐標系，目標上第i個特征點在測量坐標系上的坐標為p_i＝(x_ci,y_ci,z_ci)^T；

建立兩個攝像機所采集的像素坐標系o-uv和o′-u′v′，o-uv為左攝像機像素坐標，o′-u′v′為右攝像機像素坐標，以成像平面左上角為原點，u、v軸分別平行于像平面坐標系的x軸和y軸。目標第i個特征點在兩雙目圖像上的投影在其像素坐標系下的坐標分別為(u,v)^T和(u′,v′)^T。

第二步：計算特征點在測量坐標系中的坐標：

雙目攝像機中兩攝像機的投影矩陣分別為M₁和M₂，

M₁＝K[I 0]

M₂＝K[R_C T_C]

其中，K為攝像機的內參數矩陣，R_C和T_C為右攝像頭相對于左攝像頭的旋轉矩陣和平移向量。

所求該特征點在測量坐標系下的齊次坐標用下式解出，

M₁₁、M₁₂、M₁₃為投影矩陣M₁的行向量，M₂₁、M₂₂、M₂₃為投影矩陣M₂的行向量，設左攝像頭采集圖像上第i個特征點在像素坐標系下的齊次坐標為(u₁,v₁,1)^T。

第三步：求解旋轉矩陣N。一個四元數包含一個標量分量和一個3D向量分量，的最大值是旋轉矩陣N的最大特征值，為其對應的特征向量，N的求解公式如下，式中的S為M的分塊矩陣。p為特征點在測量坐標系下的三維坐標，q為特征點在物體坐標系下的三維坐標。

假設n個特征點的質心在測量坐標系和物體坐標系中的坐標分別為和

則可以得到分別以質心為原點的坐標系下的新坐標

第四步：計算出目標對象的位移向量：位移向量由公式可以算出，位移向量即確定了相關的位姿參數，其中s的求解辦法如下，