基于在線重建的遙操作預測顯示系統

胡 歡 孫漢旭 賈慶軒

(北京郵電大學自動化學院,北京100876)

?

基于在線重建的遙操作預測顯示系統

胡 歡 孫漢旭 賈慶軒

(北京郵電大學自動化學院,北京100876)

為了提高遙操作的操作效率,采用一種基于單目視覺的預測顯示方法來解決時延導致的視覺反饋滯后問題.該方法通過基于地圖的相機位姿估計算法來實時跟蹤機器人的狀態,在線構建機器人工作環境的三維幾何結構模型,并結合多紋理映射技術進行渲染,將模型重投影到預測視點下,得到逼真的預測圖像.搭建了一個基于客戶端-服務器端模式的系統平臺,用于未知環境下的遙操作.結果表明,在總長為7.112 m的攝像機運動軌跡中,位姿跟蹤的平均誤差約為0.015 m.該系統不僅能提供預測圖像,而且支持生成任意視點的圖像,有利于操作者從各個角度觀察機器人工作場景.

預測顯示;遙操作;同步定位與地圖構建;機器視覺

機器人已被廣泛應用于遠程環境(如太空、外星球、遠程手術、水下、災后救援等)中,研究對遠程機器人的高效魯棒控制是一項重要任務.其控制方法包括自主控制和遙操作兩大類.在目前智能技術尚未完全成熟的情況下,遙操作仍是一種不可或缺的操作控制方式.機器人實際運動和遙操作控制命令之間存在的時延會導致操作性能下降.預測顯示被認為是一種有效解決遙操作時延的方法,通過將時延排除在本地控制回路之外,消除時延對系統穩定性的影響,能夠大幅提升遙操作的成功率和執行效率[1-2].

傳統的預測顯示技術中,首先利用先驗知識構建環境及機器人虛擬模型,然后根據遠端反饋回來的各種傳感器信息在線修正虛擬模型,操作者通過仿真的虛擬環境來感知真實機器人工作環境.然而,有時機器人的操作環境是無法事先預知的.Xie等[3]提出了一種基于增強現實的遙操作方法,通過疊加虛擬機器人模型到延時視頻流上,操作者可以實時了解虛擬機器人和真實機器人的匹配程度,從而進行在線修正,這在很大程度上提高了操作者的作業能力;然而,該方法僅對機器人的狀態進行了預測,大多應用于攝像機固定的情形,且環境是靜態的.如果攝像機是受操作者控制而運動的,仍需對攝像機視野中環境的改變進行預測.J?gersand[4]提出了一種基于圖像插值的快速預測顯示方法,將其與基于增強現實的遙操作方法有效結合,從而達到同時預測機器人狀態和工作環境的目的.然而,基于圖像插值方法生成的預測圖像逼真度較差.

隨著近年來3D重建技術的飛速發展,在線實時重建逐漸成為可能,將該技術引入到預測顯示遙操作系統中是一種發展趨勢.Kelly等[5]搭建了一個遙操作系統平臺,通過融合深度數據和雙目彩色圖像構建出一個逼真的3D場景模型,從而使操作者能夠高效地遙操作遠程移動車輛；然而,多傳感器的融合所產生的傳感器間標定誤差會影響系統的魯棒性.Rachmielowski等[6]提出了一種基于單目視覺的預測顯示方法,通過對視頻圖像進行同步定位與地圖構建,重建場景的3D模型,并將其重投影到預測視點下,得到預測圖像;然而,這種3D模型是離線構建的,無法實時更新.

本文提出了一種基于在線模型構建的預測顯示方法,在進行跟蹤與地圖構建的同時能夠增量式擴展3D模型.

1 預測顯示系統架構

預測顯示的目標是處理時延帶來的視覺反饋滯后問題,通過在操作端根據已接收到的延時視頻流來構建遠程機器人及其工作環境模型,以預測基于操作者控制輸入的即時視覺反饋,使操作者可以進行連續的遙操作控制,無需等待遠程機器人端反饋的視覺信息.預測顯示遙操作系統是一種典型的客戶端-服務器系統,其架構如圖1所示.

圖1上半框圖中的軟件模塊位于機器人端(客戶端).跟蹤與地圖構建模塊根據視頻圖像估計機器人的當前位姿,同時構建環境地圖.地圖是指基于視覺特征檢測方法獲取的場景3D特征點云模型.將模塊置于機器人端是考慮到遙操作通信鏈路帶寬的限制.傳輸完整視頻會占用大量帶寬,且視頻含有大量冗余信息,將部分關鍵幀圖像傳輸到操作端是一種有效的解決帶寬限制的方法.傳輸關鍵幀圖像來代替完整視頻流,可較大程度地減少數據傳輸量.

圖1 預測顯示系統架構

圖1下半部分位于操作端(服務器端),是操作者操縱遠程機器人的軟件平臺.交互輸入模塊接收控制輸入,并轉化為機器人控制命令,發送給客戶端的機器人控制器模塊,同時還發送控制指令到預測顯示模塊,以得到預測圖像.此外,該模塊也包含了一些遠程模塊交互控制命令,如選擇指定圖像構建地圖、保存地圖等操作.模型構建模塊接收到遠程地圖數據,構建出機器人工作環境下的3D幾何結構模型.3D模型可視化模塊根據接收到的關鍵幀圖像,負責對幾何結構模型進行紋理渲染,實現逼真可視化.預測顯示模塊根據控制命令解算相機當前位姿,渲染3D模型,得到預測圖像,補償視覺反饋滯后,是操作者高效地進行下步決策的重要依據.

2 跟蹤與地圖構建

跟蹤與地圖構建模塊可實現基于單目視覺的相機位姿跟蹤與地圖構建2個核心功能.目前主流的視覺跟蹤方法有幀間跟蹤方法和基于地圖的跟蹤方法.幀間跟蹤方法是指對連續2幀圖像進行匹配并計算相對位姿,累加后便可得到全局位姿;該方法會造成誤差累積,通常還需要通過路徑閉合的方法以優化位姿,但前提是必須能夠檢測到閉合,這在很大程度上限制了其應用范圍.基于地圖的跟蹤方法首先通過2幀圖像構建初始3D地圖,然后根據已有的相機位姿估算相機運動模型,預測下一時刻相機的可能位姿,將所有地圖點重投影到該預測視點下,在當前待跟蹤圖像中查找匹配點,并根據匹配誤差修正預測相機位姿,從而得到最終的位姿估計;該方法建立在一個全局的地圖模型下,通過不斷優化全局地圖來減少誤差,從而有效縮小了誤差的累積.本文采用基于地圖的跟蹤算法,同步實現了跟蹤和地圖構建的功能.

2.1 初始地圖

本文采用立體匹配原理構建初始地圖.在初始階段需要人為選擇2幀圖像作為立體匹配的首要條件.具體過程如下:① 針對這2幀圖像,利用自適應通用加速分割算法(AGAST)[7]來檢測特征點,并采用基于塊的零均值像素灰度差平方和算法查找匹配點對;② 采用五點算法[8]結合隨機抽樣一致算法(RANSAC)來估計基礎矩陣;③ 進行匹配點對的三角化,得到初始地圖.

使用AGAST算法代替FAST特征檢測算法,對并行跟蹤與地圖構建算法(PTAM)[9]進行改進,可使檢測速度提高將近2倍.

2.2 在線跟蹤

在線跟蹤的目標是實時估計機器人的位姿.首先,基于上一時刻相機運動速度來預測當前的相機位姿.利用剛體變換群SE(3)來表示相機位姿,指數和對數運算可實現3×4維位姿變換矩陣和六自由度參數間的轉換.利用六自由度參數向量來表示位姿變換矩陣,可提高計算效率.相機運動速度的計算公式為

(1)

式中,Vt為第t幀圖像的相機速度;Pt為第t幀圖像的相機位姿;dt為第t幀圖像的時間間隔.

預測位姿的計算公式為

(2)

根據重投影誤差更新相機位姿.首先,根據相機的透視投影模型將已有的3D地圖點投影到預測視點下,得到預測投影位置;然后,在預測投影位置鄰域范圍內尋找對應的匹配位置;最后,采用Levenberg-Marquardt算法[10]對重投影誤差進行最小化處理,更新相機的位姿.

2.3 地圖擴展

隨著機器人的不斷探索,初始地圖可能已經在機器人攝像頭視域之外,因此需要不斷地對地圖進行擴展.地圖擴展是指,通過對2個已跟蹤的關鍵幀圖像執行極線搜索匹配點對,進行三角化處理得到3D點,將其加入到已有地圖點云模型中.

關鍵幀的選取標準如下:

1) 時間間隔性.連續幀往往存在大量的冗余信息,因此在算法初始階段,假定2個關鍵幀的時間間隔大于20幀,并基于已有關鍵幀圖像間重合度來修正該間隔值,如果重合度小于30%,則適當減小該值;如果重合度大于80%,則適當增大該值.

2) 空間間隔性.在空間上,2個關鍵幀之間應該保留一定的間隔距離,確保極線搜索有足夠的基線.這樣一方面可盡量減少冗余信息,節省存儲空間;另一方面也減少了在極線匹配和后續的地圖優化過程中執行關鍵幀搜索的用時.關鍵幀的數目在很大程度上影響了算法的速率.間隔距離由當前場景的平均深度決定.間隔距離的計算公式為

(3)

式中,sini為手動選擇的2個關鍵幀間的距離;dref_depth為初始化時得到的場景平均深度;davg_depth為當前幀的平均深度.

3) 位姿準確性.只有跟蹤質量好的圖像幀,才可能成為關鍵幀.首先把所有地圖點投影到當前視點下,得到m個投影點;然后,在當前圖像中搜尋對應的匹配點,假設有n個點匹配成功,則跟蹤質量定義為成功匹配點數占總投影點數的比例,即s=n/m.如果s<0.3,則認為跟蹤質量較差;否則認為跟蹤質量較好.

2.4 地圖優化

地圖優化是指不斷根據新測量數據校準已有數據的過程,是提高系統精度和魯棒性的主要步驟.集束調整是一種魯棒的地圖優化方法,通過最小化重投影誤差迭代修正關鍵幀位姿及地圖點位置,模型化為求解如下目標函數的非線性最小二乘問題:

(4)

本文采用Levenberg-Marquardt算法求解非線性最小二乘問題.其核心問題是求解雅克比矩陣.首先,引入3D點的透視投影變換公式,即

(5)

式中,Pw,Pc分別為點P在世界坐標系和相機坐標系下的三維坐標,且Pc={xc，yc,zc}T;K為相機的內參矩陣;M為估計相機位姿相對于世界坐標系的變換矩陣，依據剛體變換群SE(3)的特性,M=eεA,其中A為相機的真實位姿變換矩陣;?運算表示點相對于相機的投影變換[11].

式中,Rc為相機位姿變換矩陣M中的3×3旋轉子矩陣.

3 模型重建與預測顯示

模型重建的目標是根據散亂點云構建場景的幾何網格模型,采用基于3D Delaunay三角化的自由空間雕刻方法[12],增量式擴展構建網格模型,其輸入來源于跟蹤與地圖構建模塊中不斷更新的地圖數據.本文采用事件關聯機制來實現地圖構建與模型構建的同步,即在地圖事件發生的同時,觸發模型構建模塊的相關事件,其關聯如圖2所示.

當添加關鍵幀到地圖時,觸發以下3個模型構建事件:添加新網格點、記錄新關鍵幀、插入可見視線.添加新網格點的過程為:首先找到該點所在的3D網格,并將其刪除;然后針對所有和刪除網格相關聯的3D點及新插入點,執行3D Delaunay三角化，重新構建新網格.記錄新關鍵幀事件是指添加新關鍵幀到關鍵幀隊列中,其數據包含了圖像(用于可視化模塊的紋理渲染).插入可見視線事件記錄點和關鍵幀的關聯關系,同時判斷此可見視線是否違背自由空間約束(即相交于某已存在的網格面),如果違背則刪除相交的網格面.

圖2 地圖更新與模型更新的關聯

對地圖進行集束調整優化時,模型構建模塊需執行以下幾個事件:點位置調整、關鍵幀位置調整、可見視線刪除、壞點刪除.點位置調整等同于先執行壞點刪除事件,然后執行添加新網格點事件.關鍵幀位置調整是指從關鍵幀隊列中找到該關鍵幀,修改其位姿，如果位姿有較大變化,則遍歷所有關聯的可見視線,執行可見視線插入事件.可見視線刪除是指去除與其相關的點和關鍵幀的關聯屬性.壞點刪除是指先刪除所有相關的關鍵幀關聯屬性,然后刪除所有相關網格,最后對其鄰域內所有點重新執行3D Delaunay三角化,生成新網格.

網格模型的可視化能夠帶給操作者逼真的臨場感.逼真可視化算法是通過將關鍵幀圖像投影到網格模型上來實現的.本文選擇空間上最近的4幀關鍵幀圖像進行多紋理融合映射.紋理融合是通過加權合并來實現的.針對渲染點p,第i(i∈{1,2,3,4})幀圖像對應的權值計算公式為

式中,Vi,Ni分別為根據視線夾角和法線向量計算的權重分量;Si為深度測試值;Pi為第i幀圖像對應的相機位置;Pcr為渲染視點位置;Pn為渲染點p的法線向量;Zr,Zb分別為渲染點p在渲染視點下和深度緩沖區中存儲的深度值,且當Zr>Zb時,渲染點p在第i幀圖像中不可見,故權值為0.當Vi>0時,表示渲染視線與當前相機的觀察視線夾角小于90°,且夾角越大,此值越小;當Vi<0時,表示夾角大于90°,直接設置Wi=0,表示第i幀圖像對當前渲染效果無影響.當Ni>0時,表示點p在第i幀圖像中可見;反之不可見,設置Wi=0.

考慮到高效性的需求,紋理融合是通過OpenGL軟件中的著色語言實現的.預測視點的計算是根據操作者的控制輸入來解算的.預測顯示能夠及時為操作者的下一步決策提供實時依據,無需等待延時的反饋圖像,從而實現高效的遙操作.

4 實驗驗證

搭建了一個用于機器人預測顯示遙操作的客戶端-服務器端系統,通過真實場景數據來驗證預測顯示效果.機器人端平臺運行在一個輕量級的筆記本電腦上,其型號為聯想Thinkpad X230t,處理器為2.4 GHz Intel Core(TM) i3-2370M.操作端運行在一個組裝的臺式電腦上,其處理器型號為3.50 GHz Intel Core(TM) i7-3770K,同時配有NVIDIA GeForce 7800 GT圖形卡.2個電腦上均運行ubuntu 12.04系統.軟件平臺搭建在一個通用機器人軟件開發平臺ROS[13]上,每個子模塊封裝為一個節點,節點和節點之間通過消息進行通信.這種模塊化程序設計有利于應用程序的擴展.

本文將從數據傳輸量、位姿跟蹤的精確度和驗證預測顯示3個方面來驗證系統的可行性.

4.1 數據傳輸量

考慮到遙操作通信鏈路帶寬的限制,大量數據的傳輸會降低系統的效率.實驗中采集了3個室外場景視頻,其分辨率分別為 640×320,640×480,752×480像素,記錄軟件運行過程中傳輸不同類型數據的平均數據傳輸量,結果見表1.平均傳輸數據量等于總數據量除以視頻總幀數;圖像視頻數據的平均傳輸數據量即為每幀圖像大小;關鍵幀數據包括了關鍵幀圖像及位姿數據;地圖數據包括三維點云及相應的地圖事件數據.由表1可知,與傳輸圖像視頻數據相比,僅傳輸關鍵幀數據和地圖數據時的平均數據傳輸量明顯減少.

表1 網絡平均傳輸數據量對比 kB

4.2 位姿跟蹤的精確度

根據操作者的控制輸入及機器人的運動學和動力學理論,可以估算出機器人的位姿.但由于存在傳感器及機構誤差,隨著時間的推移誤差將不斷累積,造成較大位姿偏移.因此,基于視覺對機器人位姿進行估計是必要的,這可在一定程度上補償上述偏移.

在本文實驗中,選用了RGB-D數據集[14]中的一組30 s視頻數據freiburg1_xyz作為實驗數據集,其分辨率為640×480像素,攝像機的運動軌跡總長為7.112 m,工作空間為0.46 m×0.70 m×0.44 m.通過比較絕對軌跡誤差來驗證跟蹤的精確度.如圖3所示,實驗中共計算了736幀圖像對應的相機位姿誤差.由圖可知,本文算法與RGB-D SLAM算法[14]具有相似的精確度.兩者的平均誤差分別約為0.015和0.012 m,誤差中值分別約為0.013和0.011 m,最小誤差分別約為0.001和0.001 m,最大誤差分別約為0.115和0.034 m.本文算法的最大誤差較大的原因在于,圖像模糊有時會造成特征檢測與匹配失敗，從而導致跟蹤失敗；RGB-D SLAM算法通過引入深度測量,避免了跟蹤失敗.然而,本文算法的優勢在于對平臺要求較低,可運行在通用的CPU平臺上,僅需單一攝像機;而RGB-D SLAM算法則必須運行在GPU平臺上,并且需要使用深度測量傳感器.

圖3 2種算法的相機位姿誤差比較

4.3 驗證預測顯示

本實驗通過安裝在無人機底端的一個俯視相機來采集場景視頻,在操作端人為引入4 s時延,即每當接收到關鍵幀數據和地圖事件數據時先將其存儲在等待隊列中,經過4 s時延后再傳送給模型構建模塊進行處理.預測顯示是由3D模型重投影得到的,結果見圖4.由圖可知,延時圖像明顯滯后,而預測圖像則較為接近真實圖像.在實驗中,預測位姿為跟蹤算法估算的實時圖像位姿,預測圖像和真實圖像的差異正是由于位姿估計誤差所造成的.利用本文算法獲取的3D幾何結構模型見圖4(d),該模型是對稀疏的3D點云進行3D Delaunay三角化處理得到的,在程序運行過程中進行在線更新.

(a) 實時圖像

(b) 預測圖像

(c) 延時圖像

(d) 3D模型

為了進一步驗證三維重建的效果,選取了具有豐富紋理的實驗室書架作為對象,重建結果見圖5.圖5(a)和(b)為任意視點渲染的圖像,圖5(c)為場景幾何結構模型,它們分別代表了局部視域和全局視域.操作者可以從任意角度、任意距離觀察場景來高效地遙控機器人.模型的可視化是通過融合距離當前渲染視點最近的4幅紋理圖像來實現的.使用單幅紋理圖像和多幅紋理圖像的模型可視化結果見圖6.由圖可知,多幅紋理圖像更有助于增加可視化區域.

(a) 遠視點

(b) 近視點

(c) 全局幾何結構模型

(a) 單紋理(近視點)

(b) 單紋理(遠視點)

(c) 多紋理(近視點)

5 結語

本文搭建了一個客戶端-服務器端模式的預測顯示系統.該系統能夠對操作者的控制輸入提供即時的視覺反饋,將時延排除在本地控制回路之外.早期的預測顯示系統大多針對結構化環境,需要事先構建場景模型,本文則將其擴展到未知環境下的預測顯示遙操作中,利用單目視頻在線重建方法對3D環境進行構建.在線重建包括了2個核心內容:跟蹤與地圖構建和模型重建與預測顯示.通過多紋理融合映射技術,構建出一個逼真的模型可視化平臺,既可以提供預測圖像,又可以給操作者一個全方位的觀測.實驗結果表明,該預測顯示系統能夠逼真地預測延時圖像,解決了時延造成的視覺反饋滯后的問題.配套軟件平臺可用于遙操作無人機、火星車、移動車輛、機械臂等各種遠程機器人.

References)

[1]王永, 謝圓, 周建亮. 空間機器人大時延遙操作技術研究綜述[J]. 宇航學報, 2010, 31(2): 299-306. Wang Yong, Xie Yuan, Zhou Jianliang. A research survey on teleoperation of space robot through time delay[J].JournalofAstronautics, 2010, 31(2): 299-306. (in Chinese)

[2]孫漢旭, 胡歡, 賈慶軒,等. 遙操作系統中預測顯示技術研究[J]. 宇航學報, 2013, 34(11): 1502-1508. Sun Hanxu, Hu Huan, Jia Qingxuan, et al. Research on predictive display in teleoperation system[J].JournalofAstronautics, 2013, 34(11): 1502-1508. (in Chinese)

[3]Xie T, Xie L J, He L S, et al. A general framework of augmented reality aided teleoperation guidance[J].JournalofInformationandComputationalScience, 2013, 10(5): 1325-1335.

[4]J?gersand M. Image-based predictive display for high d.o.f. uncalibrated tele-manipulation using affine and intensity subspace models[J].AdvancedRobotics, 2001, 14(8): 683-701.

[5]Kelly A, Chan N, Herman H, et al. Real-time photorealistic virtualized reality interface for remote mobile robot control[J].TheInternationalJournalofRoboticsResearch, 2011, 30(3): 384-404.

[6]Rachmielowski A, Birkbeck N, Jagersand M. Performance evaluation of monocular predictive display[C]//IEEEInternationalConferenceonRoboticsandAutomation. Anchorage, Alaska, USA, 2010: 5309-5314.

[7]Mair E, Hager G D, Burschka D, et al. Adaptive and generic corner detection based on the accelerated segment test[C]//11thEuropeanConferenceonComputerVision. Heraklion, Greece, 2010: 183-196.

[8]Wu F C, Hu Z Y. 5-point and 4-point algorithm to determine of the fundamental matrix[J].ActaAutomaticaSinica, 2003, 29(2): 175-180.

[9]Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//6thIEEEandACMInternationalSymposiumonMixedandAugmentedReality. Nara, Japan, 2007: 225-234.

[10]Szeliski R. 計算機視覺——算法與應用[M]. 艾海舟，等譯. 北京:清華大學出版社, 2012: 575-581.

[11]Blanco J L. A tutorial on SE(3) transformations and on-manifold optimization[R]. Seville, Spain: University of Malaga, 2010.

[12]Miller G L, Pav S E, Walkington N. Fully incremental 3D delaunay refinement mesh generation[C]//ProceedingsoftheInternationalMeshingRoundtable(IMR). Ithaca, NY, USA, 2002: 75-86.

[13]Cousins S. Exponential growth of ROS [ROS topics][J].IEEERobotics&AutomationMagazine, 2011, 18(1): 19-20.

[14]Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//2012IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems. Vilamoura, Portugal, 2012: 573-580.

On-line reconstruction-based predictive display system for teleoperation

Hu Huan Sun Hanxu Jia Qingxuan

(Automation School, Beijing University of Posts and Telecommunications, Beijing 100876, China)

To improve the operation efficiency of teleoperation, a predictive display method based on monocular vision is proposed for solving the visual feedback delay problem caused by time delay. In this method, the robot’s poses are tracked in real-time by the map-based camera pose estimation algorithm. The three-dimensional geometry model under robot working environments is reconstructed on-line with rendering by multiple texture mapping technology. Finally, the realistic predicted image is obtained by projecting the model into the predicted view. The system platform based on the client-server architecture is constructed which is suitable for teleoperation under unknown environments. The experimental results show that the average error of pose tracking is about 0.015 m over a camera journey of 7.112 m. The proposed system can not only supply the predicted images, but also support to generate the images of arbitrary views, which benefits the operator observing the working environments of the robot from all angles.

predictive display; teleoperation; simultaneous localization and mapping(SLAM); robot vision

10.3969/j.issn.1001-0505.2015.03.007

2014-12-19. 作者簡介: 胡歡(1986—)，女，博士生；孫漢旭(聯系人)，男，博士，教授，博士生導師，hxsun@bupt.edu.cn.

國家自然科學基金資助項目(61175080)、國家重點基礎研究發展計劃(973計劃)資助項目(2013CB733000).

胡歡，孫漢旭，賈慶軒.基于在線重建的遙操作預測顯示系統[J].東南大學學報:自然科學版,2015,45(3):448-454.

10.3969/j.issn.1001-0505.2015.03.007

TP242

A

1001-0505(2015)03-0448-07