本發(fā)明屬于電力技術(shù)領(lǐng)域，具體涉及一種帶電作業(yè)機器人作業(yè)環(huán)境3D重構(gòu)方法。

背景技術(shù)：

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

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

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

本發(fā)明提出一種帶電作業(yè)機器人作業(yè)環(huán)境3D重構(gòu)方法，在三維空間中對障礙物建模，并應(yīng)用3D重構(gòu)技術(shù)在監(jiān)控顯示器顯示中，再現(xiàn)機器人作業(yè)的真實場景，以便操作人員發(fā)出相應(yīng)的指令。

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

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

進一步，求解目標(biāo)的位置姿態(tài)參數(shù)的具體過程為：

第一步：建立坐標(biāo)系：

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

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

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

第二步：計算特征點在測量坐標(biāo)系中的坐標(biāo)：

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

M₁＝K[I 0]

M₂＝K[R_C T_C]

其中，K為攝像機的內(nèi)參數(shù)矩陣，R_C和T_C為右攝像頭相對于左攝像頭的旋轉(zhuǎn)矩陣和平移向量。

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

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

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

假設(shè)n個特征點的質(zhì)心在測量坐標(biāo)系和物體坐標(biāo)系中的坐標(biāo)分別為和

則可以得到分別以質(zhì)心為原點的坐標(biāo)系下的新坐標(biāo)

第四步：計算出目標(biāo)對象的位移向量：通過公式計算出位移向量由，位移向量即確定了相關(guān)的位姿參數(shù)。

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

附圖說明

圖1是帶電作業(yè)場景結(jié)構(gòu)示意圖。

圖2是位姿計算流程圖。

圖3是作業(yè)環(huán)境重構(gòu)流程圖。

圖4是帶電作業(yè)機器人系統(tǒng)示意圖。

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

具體實施方式

容易理解，依據(jù)本發(fā)明的技術(shù)方案，在不變更本發(fā)明的實質(zhì)精神的情況下，本領(lǐng)域的一般技術(shù)人員可以想象出本發(fā)明帶電作業(yè)機器人作業(yè)環(huán)境3D重構(gòu)方法的多種實施方式。因此，以下具體實施方式和附圖僅是對本發(fā)明的技術(shù)方案的示例性說明，而不應(yīng)當(dāng)視為本發(fā)明的全部或者視為對本發(fā)明技術(shù)方案的限制或限定。

結(jié)合附圖，基于高壓帶電機器人作業(yè)環(huán)境的3D重構(gòu)方法，利用雙目攝像頭確定單個元器件和機械手工具的位姿參數(shù)，將獲得的位姿參數(shù)導(dǎo)入帶電作業(yè)場景的模型中，生成帶電作業(yè)現(xiàn)場實時監(jiān)控場景并顯示在監(jiān)控設(shè)備上。

具體包括如下步驟：

一、場景構(gòu)建

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

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

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

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

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

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

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

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

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

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

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

第一步：建立坐標(biāo)系：

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

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

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

第二步：計算特征點在測量坐標(biāo)系中的坐標(biāo)：

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

M₁＝K[I 0]

M₂＝K[R_C T_C]

其中，K為攝像機的內(nèi)參數(shù)矩陣，R_C和T_C為右攝像頭相對于左攝像頭的旋轉(zhuǎn)矩陣和平移向量。

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

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

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

假設(shè)n個特征點的質(zhì)心在測量坐標(biāo)系和物體坐標(biāo)系中的坐標(biāo)分別為和

則可以得到分別以質(zhì)心為原點的坐標(biāo)系下的新坐標(biāo)

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