針對狹窄空間作業(yè)的機器人動態(tài)視覺系統(tǒng)研究

王 楊,王 犇,麥曉明

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

1 引言

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

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

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

在遙操作實現(xiàn)方面，異構型遙操作的逆運動學問題是難點之一，目前常用的方法是采用D-H法建立遙操作機器人主從端的運動學模型，通過對模型求逆的方法求解逆運動學問題[7]。

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

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

2 系統(tǒng)設計

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

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

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

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

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

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

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

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

2.2 主從異構型遙操作系統(tǒng)逆運動學求解

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

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

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

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

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

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

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

姿態(tài)映射是主從異構型遙操作系統(tǒng)逆運動學問題求解的關鍵。為保證佩戴者的高臨場感體驗，需保證VR頭戴設備的姿態(tài)與機器人末端相機的姿態(tài)一一映射。由于主從設備的相對坐標系定義不同，首先引入旋轉矩陣實現(xiàn)對VR頭戴設備和機器人末端空間姿態(tài)的唯一表達，即：

其次，在映射關系下，機器人末端相對坐標系的n，o，a軸分別與VR設備的a，o，n對應且在笛卡爾坐標系z-軸上的投影等大反向，即：

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

6自由度串聯(lián)機器人機構圖，如圖4所示。機器人的正運動學模型可描述為：

圖4 機器人坐標系定義Fig.4 Coordinate System Definition of Robot

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

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

2.3 被觀測目標絕對空間位置粗定位

基于剛體的坐標變換原理，觀測目標在相機中的位置與在笛卡爾坐標系中的位置存在以下轉換關系：

式中：pin_eye—觀測目標在相機坐標系中的位置；pin_cartesian—觀測目標在笛卡爾坐標系中的位置。

3 樣機試驗及結果分析

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

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

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

試驗過程圖，如圖5所示。UR3機器人的詳細尺寸參數(shù)，及機器人本身的逆運動學問題求解精度在參考文獻［9］中詳述。

圖5 試驗過程Fig.5 Testing Process

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

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

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

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

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

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

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

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

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

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

3.3 目標多方位觀測粗定位試驗

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

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

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

表4 觀測取樣對應的機器人關節(jié)角度Tab.4 Joint Positions Corresponding to the Samples for Vision Location Testing

表5 觀測取樣對應的小球在相機坐標系中定位Tab.5 Location of the Ball in the Coordination System of Camera Corresponding to the Samples for Vision Location Testing

表6 觀測取樣對應小球在笛卡爾坐標系中定位Tab.6 Location of the Ball in the Cartesian Coordination System Corresponding to the Samples for Vision Location Testing

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

4 結論

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

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