針對狹窄空間作業的機器人動態視覺系統研究

王 楊,王 犇,麥曉明

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

1 引言

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

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

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

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

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

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

2 系統設計

2.1 系統組成與工作流程

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

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

圖1 動態視覺系統試驗平臺Fig.1 Experiment Platform of Dynamic Vision System

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

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

基于遙操作的動態視覺系統信號傳遞路徑，如圖2所示。

圖2 系統信號傳遞路徑圖Fig.2 Diagram of Signal Transmission

2.2 主從異構型遙操作系統逆運動學求解

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

圖3 遙操作系統坐標系定義Fig.3 Coordinate System Definition of Teleoperation System

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

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

2.2.1 VR設備與動態視覺系統的位姿映射模型

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

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

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

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

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

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

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

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

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

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

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

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

3 樣機試驗及結果分析

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

3.1 樣機系統參數設定

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

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

圖5 試驗過程Fig.5 Testing Process

3.2 環境多方位動態感知遙操作試驗

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

動態視覺系統在工作空間和關節空間位姿跟蹤試驗結果分別，如圖6、圖7所示。

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

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

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

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

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

表2 末端跟蹤誤差統計Tab.2 Tracking Error of Teleoperation

表3 關節跟蹤誤差統計Tab.3 Error of Robot Joint Position

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

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

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

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

表4 觀測取樣對應的機器人關節角度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頭戴設備、六自由度串聯機器人和雙目相機的動態視覺系統，并提出了針對該系統的異構型主從遙操作逆運動學模型，實現了在笛卡爾坐標系下雙目相機對VR頭戴設備的空間位姿跟蹤。其中，位置跟蹤平均誤差不大于1.7mm，姿態跟蹤平均誤差小于2.36°；另一方面，這里提出了被觀測目標在相機中位置與在笛卡爾坐標系中位置映射模型，實現了相機在不同位姿狀態下對一個固定小球在笛卡爾坐標系下的空間粗定位，定位精度優于1%。這里研究成果可應用于配網帶電作業機器人，以及其他在高危環境下開展作業的機器人系統研發。以應對作業狹窄，無法通過移動作業機器人本體繞開視場內障礙對作業環境進行空間高覆蓋的感知和作業目標粗定位的問題。

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