王 楊,王 犇,麥曉明

（1.廣東電網(wǎng)有限公司電力科學(xué)研究院人工智能與機(jī)器人研究所，廣東 廣州 510000 2.華北電力大學(xué)（保定）能源動(dòng)力與機(jī)械學(xué)院，河北 保定 017000）

1 引言

配網(wǎng)帶電作業(yè)機(jī)器人一直是機(jī)器人研究的熱點(diǎn)和難點(diǎn)[1]。配網(wǎng)作業(yè)環(huán)境非確定性強(qiáng)，遙操作操控方式能有效提高機(jī)器人的適應(yīng)性。但是，當(dāng)線路為同塔多回架設(shè)時(shí)，作業(yè)區(qū)域內(nèi)非作業(yè)相導(dǎo)線或設(shè)備分布密集，安全作業(yè)空間狹窄受限，作業(yè)機(jī)器人本體無(wú)法靈活移動(dòng)，傳統(tǒng)的固定式視覺感知系統(tǒng)難以繞過(guò)作業(yè)空間內(nèi)障礙遮蔽為操作人員提供空間高覆蓋的作業(yè)環(huán)境感知和目標(biāo)定位信息。而隨著民用VR/AR設(shè)備技術(shù)成熟、成本降低，將VR/AR設(shè)備、串聯(lián)機(jī)器人和遙操作技術(shù)相融合，設(shè)計(jì)可根據(jù)操作人員觀測(cè)需求，避讓視場(chǎng)內(nèi)障礙對(duì)作業(yè)目標(biāo)多角度觀測(cè)和定位、輔助機(jī)器人帶電作業(yè)的感動(dòng)態(tài)視覺感知系統(tǒng)，不僅有現(xiàn)實(shí)需求，更有實(shí)現(xiàn)基礎(chǔ)。

現(xiàn)有文獻(xiàn)中利用AR/VR設(shè)備搭建視覺遙操作系統(tǒng)的研究較少。目前，遙操作與AR/VR技術(shù)相結(jié)合的研究主要集中于兩個(gè)方向。

（1）將AR/VR設(shè)備僅作為顯示設(shè)備向操作人員反饋現(xiàn)場(chǎng)視頻圖像，提升一定的遙操作臨場(chǎng)感[2-5]；（2）利用AR/VR技術(shù)構(gòu)建現(xiàn)場(chǎng)虛擬圖像或者虛擬力、虛擬觸覺降低系統(tǒng)延時(shí)對(duì)操作臨場(chǎng)感的影響[6]。

在遙操作實(shí)現(xiàn)方面，異構(gòu)型遙操作的逆運(yùn)動(dòng)學(xué)問(wèn)題是難點(diǎn)之一，目前常用的方法是采用D-H法建立遙操作機(jī)器人主從端的運(yùn)動(dòng)學(xué)模型，通過(guò)對(duì)模型求逆的方法求解逆運(yùn)動(dòng)學(xué)問(wèn)題[7]。

綜上所述，現(xiàn)有針對(duì)將遙操作與VR/AR技術(shù)相結(jié)合設(shè)計(jì)搭建能跟隨操控者頭部位姿靈活運(yùn)動(dòng)，為操控者實(shí)時(shí)提供對(duì)目標(biāo)多角度觀測(cè)的動(dòng)態(tài)視覺感知系統(tǒng)的研究尚處空白。

這里搭建了一套由VR頭戴設(shè)備、6自由度機(jī)器人和雙目相機(jī)構(gòu)成的動(dòng)態(tài)視覺感知系統(tǒng)，提出了一種針對(duì)該系統(tǒng)的逆運(yùn)動(dòng)學(xué)求解方法，并基于雙目相機(jī)采集的視場(chǎng)內(nèi)深度信息，實(shí)現(xiàn)對(duì)視場(chǎng)范圍內(nèi)固定小球位置的多角度觀測(cè)和絕對(duì)位置粗定位。

2 系統(tǒng)設(shè)計(jì)

2.1 系統(tǒng)組成與工作流程

受到人類在多視角觀測(cè)同一目標(biāo)時(shí)脊柱姿態(tài)變化的啟發(fā)，這里搭建了動(dòng)態(tài)視覺系統(tǒng)。樣機(jī)測(cè)試平臺(tái)，如圖1所示。各組件明細(xì)，如表1所示。

表1 系統(tǒng)硬件與主要技術(shù)指標(biāo)Tab.1 System Hardware and Key Parameters

圖1 動(dòng)態(tài)視覺系統(tǒng)試驗(yàn)平臺(tái)Fig.1 Experiment Platform of Dynamic Vision System

動(dòng)態(tài)視覺系統(tǒng)工作流程如下：一方面，VR頭戴設(shè)備將實(shí)時(shí)反饋雙目相機(jī)采集的可見光圖像；佩戴人員根據(jù)觀察需要在一定范圍內(nèi)移動(dòng)或轉(zhuǎn)動(dòng)頭部；主控制器通過(guò)采集VR 設(shè)備內(nèi)置慣導(dǎo)（Inertial Measurement Unit，IMU）及基站信息反饋的VR 設(shè)備實(shí)時(shí)姿態(tài)與位置信息，基于逆運(yùn)動(dòng)學(xué)模型求解末端搭載雙目相機(jī)的6自由度串聯(lián)機(jī)器人各關(guān)節(jié)角，并通過(guò)伺服模式驅(qū)動(dòng)機(jī)器人關(guān)節(jié)運(yùn)動(dòng)，從而實(shí)現(xiàn)雙目相機(jī)對(duì)佩戴VR設(shè)備的人員頭部空間運(yùn)動(dòng)的實(shí)時(shí)跟蹤；

另一方面，控制器根據(jù)雙目相機(jī)返回的視場(chǎng)內(nèi)深度信息和可見光信息，對(duì)目標(biāo)進(jìn)行識(shí)別和絕對(duì)空間位置粗定位，并將信息反饋至VR設(shè)備中，為操作人員提供目標(biāo)位置信息[8]。

基于遙操作的動(dòng)態(tài)視覺系統(tǒng)信號(hào)傳遞路徑，如圖2所示。

圖2 系統(tǒng)信號(hào)傳遞路徑圖Fig.2 Diagram of Signal Transmission

2.2 主從異構(gòu)型遙操作系統(tǒng)逆運(yùn)動(dòng)學(xué)求解

由于VR頭戴設(shè)備（主設(shè)備）與6自由度串聯(lián)機(jī)器人（從設(shè)備）構(gòu)型相異，動(dòng)態(tài)視覺系統(tǒng)遙操作需解決主從異構(gòu)型遙操作的逆運(yùn)動(dòng)學(xué)問(wèn)題。采用VR 設(shè)備繞笛卡爾坐標(biāo)系（由標(biāo)定過(guò)程決定）x，y，z軸旋轉(zhuǎn)角（[φ，θ，φ]m）描述VR 設(shè)備姿態(tài)，以及VR 設(shè)備分別在三個(gè)軸上的投影[x，y，z]m描述VR設(shè)備空間位置；采用與相機(jī)（雙目相機(jī)的左鏡頭）固連的機(jī)器人末端分別繞笛卡爾坐標(biāo)系的x，y，z軸轉(zhuǎn)角（[φ，θ，φ]s）描述相機(jī)空間姿態(tài)，以及末端分別在三個(gè)軸上的投影[x，y，z]s描述相機(jī)空間位置。VR設(shè)備和機(jī)器人坐標(biāo)系定義，如圖3所示。

圖3 遙操作系統(tǒng)坐標(biāo)系定義Fig.3 Coordinate System Definition of Teleoperation System

該遙操作系統(tǒng)的逆運(yùn)動(dòng)學(xué)問(wèn)題可描述為：根據(jù)VR設(shè)備的空間位姿求解相應(yīng)的機(jī)器人關(guān)節(jié)角。即：

式中：f—由VR設(shè)備位姿到機(jī)器人末端位姿的映射模型；g—由機(jī)器人的末端位姿到機(jī)器人各關(guān)節(jié)角大小的映射模型，q=[q1，q2，…，q6]—機(jī)器人關(guān)節(jié)角。

2.2.1 VR設(shè)備與動(dòng)態(tài)視覺系統(tǒng)的位姿映射模型

在位置映射方面，VR設(shè)備空間位置與動(dòng)態(tài)視覺系統(tǒng)末端相機(jī)鏡頭的位置映射采用比例映射，即：

式中：a—主從設(shè)備之間位置映射的比例系數(shù)。為實(shí)現(xiàn)高臨場(chǎng)感，a=1。[x0，y0，z0]m和[x0，y0，z0]s分別為VR設(shè)備和機(jī)器人在末端初始狀態(tài)空間位置。

姿態(tài)映射是主從異構(gòu)型遙操作系統(tǒng)逆運(yùn)動(dòng)學(xué)問(wèn)題求解的關(guān)鍵。為保證佩戴者的高臨場(chǎng)感體驗(yàn)，需保證VR頭戴設(shè)備的姿態(tài)與機(jī)器人末端相機(jī)的姿態(tài)一一映射。由于主從設(shè)備的相對(duì)坐標(biāo)系定義不同，首先引入旋轉(zhuǎn)矩陣實(shí)現(xiàn)對(duì)VR頭戴設(shè)備和機(jī)器人末端空間姿態(tài)的唯一表達(dá)，即：

其次，在映射關(guān)系下，機(jī)器人末端相對(duì)坐標(biāo)系的n，o，a軸分別與VR設(shè)備的a，o，n對(duì)應(yīng)且在笛卡爾坐標(biāo)系z(mì)-軸上的投影等大反向，即：

2.2.2 機(jī)器人末端位姿與關(guān)節(jié)角度映射模型

6自由度串聯(lián)機(jī)器人機(jī)構(gòu)圖，如圖4所示。機(jī)器人的正運(yùn)動(dòng)學(xué)模型可描述為：

圖4 機(jī)器人坐標(biāo)系定義Fig.4 Coordinate System Definition of Robot

因此，g的求解就是根據(jù)機(jī)器人理想末端位姿M(q)求解其各關(guān)節(jié)角大小q=[q1，q2，…，q6]的問(wèn)題。

為方便描述求解過(guò)程，定義關(guān)節(jié)4回轉(zhuǎn)軸線和關(guān)節(jié)5回轉(zhuǎn)軸線的交點(diǎn)為機(jī)器人“頸”。這里采用求解方法的關(guān)鍵是利用機(jī)器人的幾何約束，首先根據(jù)末端位姿確定“頸”部空間位置，從而將一個(gè)描述機(jī)器人末端位姿和機(jī)器人6個(gè)關(guān)節(jié)角度關(guān)系的六元方程組求解問(wèn)題，轉(zhuǎn)化成為分別描述機(jī)器人“頸”部位置與機(jī)器人[q1，q2，q3]等3個(gè)關(guān)節(jié)角度之間關(guān)系，以及描述機(jī)器人末端姿態(tài)與剩余[q4，q5，q6]等3個(gè)關(guān)節(jié)角度之間關(guān)系的兩個(gè)三元方程組求解問(wèn)題。具體的求解過(guò)程請(qǐng)見參考文獻(xiàn)［9］。

2.3 被觀測(cè)目標(biāo)絕對(duì)空間位置粗定位

基于剛體的坐標(biāo)變換原理，觀測(cè)目標(biāo)在相機(jī)中的位置與在笛卡爾坐標(biāo)系中的位置存在以下轉(zhuǎn)換關(guān)系：

式中：pin_eye—觀測(cè)目標(biāo)在相機(jī)坐標(biāo)系中的位置；pin_cartesian—觀測(cè)目標(biāo)在笛卡爾坐標(biāo)系中的位置。

3 樣機(jī)試驗(yàn)及結(jié)果分析

采用樣機(jī)試驗(yàn)的方法驗(yàn)證這里搭建的動(dòng)態(tài)視覺系統(tǒng)的感知和定位功能有效性。

3.1 樣機(jī)系統(tǒng)參數(shù)設(shè)定

系統(tǒng)的軟件編寫采用Visual Studio 2015 并在工作站運(yùn)行。工作站為主控制器，通過(guò)調(diào)用配套API函數(shù)以125Hz的工作頻率采集HTC VIVE（主端設(shè)備）位姿信號(hào)，并將經(jīng)逆運(yùn)動(dòng)學(xué)模型計(jì)算后的機(jī)器人關(guān)節(jié)角度由socket建立的TCP通訊以125Hz工作頻率發(fā)送至UR3（從端設(shè)備）的30003端口。UR3采用關(guān)節(jié)伺服控制，控制指令參考文獻(xiàn)［10］中的相關(guān)命令格式編寫（具體參看servoj命令，這里將控制時(shí)間參數(shù)設(shè)置為t=0.1）。

試驗(yàn)過(guò)程圖，如圖5所示。UR3機(jī)器人的詳細(xì)尺寸參數(shù)，及機(jī)器人本身的逆運(yùn)動(dòng)學(xué)問(wèn)題求解精度在參考文獻(xiàn)［9］中詳述。

圖5 試驗(yàn)過(guò)程Fig.5 Testing Process

3.2 環(huán)境多方位動(dòng)態(tài)感知遙操作試驗(yàn)

初始狀態(tài)下，人員佩戴VR設(shè)備后，VR設(shè)備初始空間位姿為：

動(dòng)態(tài)視覺系統(tǒng)在工作空間和關(guān)節(jié)空間位姿跟蹤試驗(yàn)結(jié)果分別，如圖6、圖7所示。

圖6 遙操作位姿軌跡跟蹤Fig.6 Trajectory Tracking of Teleoperation

圖7 關(guān)節(jié)跟蹤試驗(yàn)結(jié)果Fig.7 Trajectory Tracking of Robot Joints Position

圖6中，VR設(shè)備生成的參考信號(hào)已經(jīng)換算為在UR3絕對(duì)坐標(biāo)系下的表達(dá)，下標(biāo)ref表示VR頭戴設(shè)備生成的末端參考位姿軌跡信號(hào)，下標(biāo)act表示與UR3末端固連的雙目相機(jī)實(shí)際空間位姿信號(hào)。系統(tǒng)在工作空間和關(guān)節(jié)空間的跟蹤平均誤差和標(biāo)準(zhǔn)差統(tǒng)計(jì)分別，如表2、表3所示。

由試驗(yàn)結(jié)果可知，在位姿跟蹤精度方面：這里動(dòng)態(tài)視覺系統(tǒng)及遙操作模型能實(shí)現(xiàn)串聯(lián)6自由度機(jī)器人末端及搭載雙目相機(jī)對(duì)VR 頭戴設(shè)備的空間位姿跟蹤。位置跟蹤平均誤差不大于1.7mm，姿態(tài)跟蹤平均誤差＜2.36°。動(dòng)態(tài)視覺系統(tǒng)在對(duì)環(huán)境多方位感知方面具有較高的臨場(chǎng)感。

在遙操作的位姿跟蹤誤差來(lái)源方面：首先，誤差主要是由主、從設(shè)備之間的固有工作頻率差異導(dǎo)致。這在誤差統(tǒng)計(jì)方面，表現(xiàn)為標(biāo)準(zhǔn)差遠(yuǎn)大于誤差均值，如表2、表3所示。而在跟蹤效果上表現(xiàn)為參考軌跡變化快速發(fā)生變化后，跟蹤誤差會(huì)有大幅上升并逐漸回落，如圖7中虛線及圖6和圖7中圓圈標(biāo)識(shí)區(qū)域曲線變化所示。VR頭戴設(shè)備質(zhì)量輕、慣性小且由操作人員直接驅(qū)動(dòng)運(yùn)動(dòng)，工作頻率較高；串聯(lián)機(jī)器人關(guān)節(jié)質(zhì)量較大且由電機(jī)驅(qū)動(dòng)，工作頻率較低，這導(dǎo)致該主從遙操作系統(tǒng)本身是一個(gè)低通濾波器，從端的機(jī)器人始終無(wú)法跟蹤主端VR 設(shè)備生成的軌跡信號(hào)中的高頻成分。其次，6自由度機(jī)器人本身的逆運(yùn)動(dòng)學(xué)求解精度，也產(chǎn)生了位姿跟蹤的部分系統(tǒng)誤差。

表2 末端跟蹤誤差統(tǒng)計(jì)Tab.2 Tracking Error of Teleoperation

表3 關(guān)節(jié)跟蹤誤差統(tǒng)計(jì)Tab.3 Error of Robot Joint Position

3.3 目標(biāo)多方位觀測(cè)粗定位試驗(yàn)

在目標(biāo)識(shí)別與定位方面，系統(tǒng)采用顏色和邊緣檢測(cè)的方法實(shí)現(xiàn)對(duì)目標(biāo)小球的識(shí)別，并利用雙目相機(jī)采集的深度信息完成對(duì)目標(biāo)小球球心的三維空間定位。在圖像反饋方面，VR設(shè)備向操作人員展示經(jīng)三維渲染后的雙目相機(jī)采集的可見光圖像、識(shí)別目標(biāo)高亮選定框、識(shí)別目標(biāo)絕對(duì)空間位置等信息。小球識(shí)別方法、雙目視覺定位原理和VR 圖像顯示方法不在這里詳述。視覺定位試驗(yàn)取樣（5次），如圖8所示。

圖8 視覺定位試驗(yàn)取樣（5次）Fig.8 Samples of Vision Location Testing（5 Times）

試驗(yàn)過(guò)程包含5次觀測(cè)，記錄機(jī)器人處于不同位姿狀態(tài)下，其末端雙目相機(jī)采集到的目標(biāo)小球在相機(jī)坐標(biāo)系中的位置，經(jīng)（4）式所示模型計(jì)算，求得小球在笛卡爾坐標(biāo)系（機(jī)器人絕對(duì)坐標(biāo)系）下的位置。5次觀測(cè)對(duì)應(yīng)的機(jī)器人姿態(tài)，如表4所示。經(jīng)雙目相機(jī)反饋的小球在相機(jī)坐標(biāo)系中位置，如圖9、表5所示。笛卡爾坐標(biāo)系下小球的實(shí)際位置及經(jīng)計(jì)算所得的小球位置，如表6 所示。試驗(yàn)結(jié)果表明，這里所設(shè)計(jì)的動(dòng)態(tài)視覺系統(tǒng)能夠?qū)崿F(xiàn)在不同觀測(cè)位姿下對(duì)小球絕對(duì)位置的粗定位，球心定位誤差約為1%（小球與機(jī)器人底座水平距離為1.1m）。

表4 觀測(cè)取樣對(duì)應(yīng)的機(jī)器人關(guān)節(jié)角度Tab.4 Joint Positions Corresponding to the Samples for Vision Location Testing

表5 觀測(cè)取樣對(duì)應(yīng)的小球在相機(jī)坐標(biāo)系中定位Tab.5 Location of the Ball in the Coordination System of Camera Corresponding to the Samples for Vision Location Testing

表6 觀測(cè)取樣對(duì)應(yīng)小球在笛卡爾坐標(biāo)系中定位Tab.6 Location of the Ball in the Cartesian Coordination System Corresponding to the Samples for Vision Location Testing

圖9 定位結(jié)果對(duì)比（5次）Fig.9 Comparison of the Calculated Position of the Ball in Cartesian Coordinate System

4 結(jié)論

這里設(shè)計(jì)并搭建了包含VR頭戴設(shè)備、六自由度串聯(lián)機(jī)器人和雙目相機(jī)的動(dòng)態(tài)視覺系統(tǒng)，并提出了針對(duì)該系統(tǒng)的異構(gòu)型主從遙操作逆運(yùn)動(dòng)學(xué)模型，實(shí)現(xiàn)了在笛卡爾坐標(biāo)系下雙目相機(jī)對(duì)VR頭戴設(shè)備的空間位姿跟蹤。其中，位置跟蹤平均誤差不大于1.7mm，姿態(tài)跟蹤平均誤差小于2.36°；另一方面，這里提出了被觀測(cè)目標(biāo)在相機(jī)中位置與在笛卡爾坐標(biāo)系中位置映射模型，實(shí)現(xiàn)了相機(jī)在不同位姿狀態(tài)下對(duì)一個(gè)固定小球在笛卡爾坐標(biāo)系下的空間粗定位，定位精度優(yōu)于1%。這里研究成果可應(yīng)用于配網(wǎng)帶電作業(yè)機(jī)器人，以及其他在高危環(huán)境下開展作業(yè)的機(jī)器人系統(tǒng)研發(fā)。以應(yīng)對(duì)作業(yè)狹窄，無(wú)法通過(guò)移動(dòng)作業(yè)機(jī)器人本體繞開視場(chǎng)內(nèi)障礙對(duì)作業(yè)環(huán)境進(jìn)行空間高覆蓋的感知和作業(yè)目標(biāo)粗定位的問(wèn)題。

這里的研究?jī)?nèi)容主要集中在系統(tǒng)的基本功能實(shí)現(xiàn)，尚未對(duì)從設(shè)備的運(yùn)動(dòng)規(guī)劃和控制、多種目標(biāo)識(shí)別與定位等內(nèi)容開展研究。因此，未來(lái)需做三點(diǎn)研究。（1）引入機(jī)器人的路徑規(guī)劃方法研究，提高從設(shè)備對(duì)主設(shè)備的動(dòng)態(tài)跟蹤性能；（2）將遙操作感知與機(jī)器人自主感知相結(jié)合，研究機(jī)器人自主繞障觀測(cè)目標(biāo)，并指導(dǎo)作業(yè)設(shè)備（作業(yè)臂）的路徑規(guī)劃；（3）研究針對(duì)配網(wǎng)作業(yè)相線路金具戶外環(huán)境識(shí)別與定位方法，提高設(shè)備實(shí)用性；（4）用戶體驗(yàn)上，需增加空氣墻，防止從設(shè)備超出安全工作空間。