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

背景技術(shù)：

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

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

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

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

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

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

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

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

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

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

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

第二步：計(jì)算特征點(diǎn)在測(cè)量坐標(biāo)系中的坐標(biāo)：

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

M₁＝K[I 0]

M₂＝K[R_C T_C]

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

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

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

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

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

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

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

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

附圖說(shuō)明

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

圖2是位姿計(jì)算流程圖。

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

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

圖5是機(jī)器人平臺(tái)示意圖。

具體實(shí)施方式

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

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

具體包括如下步驟：

一、場(chǎng)景構(gòu)建

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

第二步：計(jì)算特征點(diǎn)在測(cè)量坐標(biāo)系中的坐標(biāo)：

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

M₁＝K[I 0]

M₂＝K[R_C T_C]

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

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

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

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

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

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

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