基于單目視覺的水下機器人相對位姿精確控制

王曉鳴, 吳高升

基于單目視覺的水下機器人相對位姿精確控制

王曉鳴, 吳高升

(天津科技大學 機械工程學院, 天津, 300222)

針對水下機器人在實際作業(yè)中需要相對于特定的作業(yè)對象保持相對穩(wěn)定且具有一定抗干擾能力的特點, 提出一種基于單目視覺位姿測量原理的水下機器人穩(wěn)定性控制方法, 該方法的關鍵技術在于通過重投影法對水下機器人的三維位姿進行測量。以“觀海ROV”為實驗載體, 在實驗水池中放置待觀察目標, 利用水下機器人的前視攝像頭作為單目視覺傳感器, 建立實驗驗證系統(tǒng)。從實驗結(jié)果所得到的位置數(shù)據(jù)表明: 采用基于單目視覺位姿測量原理控制方法的水下機器人能夠在恒定水流沖擊下相對于特定對象保持基本穩(wěn)定, 可滿足水下機器人在實際作業(yè)環(huán)境中的需求, 驗證了文中方法的有效性。

水下機器人; 單目視覺; 三維位姿; 穩(wěn)定性

0 引言

隨著人類對海洋環(huán)境探測的不斷深入, 水下機器人已成為開發(fā)和利用海洋資源的強有力工具。根據(jù)實際作業(yè)需要, 水下機器人相對于特定作業(yè)對象需要有一定的穩(wěn)定性和抗干擾能力, 即依靠自身攜帶的動力系統(tǒng)在一定的水流作用下, 相對于作業(yè)對象保持一定位姿的能力。水下機器人保持自身的相對穩(wěn)定性需要2個必要條件, 一是通過自身攜帶的傳感器等元器件測量出機器人本體相對于作業(yè)對象的相對位姿; 二是根據(jù)測量出的相對位姿合理地進行水下機器人的動力分配。

傳統(tǒng)的水下機器人主要用慣性導航系統(tǒng)作為位姿測量系統(tǒng)。慣性導航系統(tǒng)屬于推算導航方式, 即從一已知點的位置根據(jù)連續(xù)測得的運動體的航向角和速度, 推算出其下一點的位置, 因而可連續(xù)計算出運動體的當前位置。但是慣性導航系統(tǒng)的定位誤差會隨時間增大, 長期使用時精度較差, 且每次使用前都需要花費較長時間校準[1]。水下攝像頭作為傳感器的創(chuàng)新產(chǎn)品, 具有體積小、局部定位精確等優(yōu)點, 被廣泛關注。

目前, 基于單目視覺的水下機器人穩(wěn)定性研究已取得一定成果[2-4], 部分技術已通過實驗室驗證。唐旭東等[5]利用已知的管道模型, 以單目攝像頭作為視覺傳感器在實驗室水池中進行了管道跟蹤實驗, 驗證了單目視覺在水下環(huán)境中的可行性和有效性; 王鵬等[6]利用屋頂?shù)慕Y(jié)構(gòu)光點設計了單目視覺位姿結(jié)算系統(tǒng), 并通過實驗驗證了其有效性; 王中宇等[7]針對位姿求解過程中解不唯一的問題, 提出了一種新的單目視覺位姿測量方法; 高勝等[8]利用視覺反饋實現(xiàn)海洋環(huán)境下遠程操作遙控水下航行器(remotely operated vehicle, ROV), 將自主式水下機器人的技術轉(zhuǎn)移到ROV上, 減小了ROV操作人員的工作量; Trslic等[9]利用燈光作為位姿估算的基礎, 將位姿估計技術用于水下機器人與對接站的自主對接中, 在北大西洋成功完成了海上實驗并取得了顯著效果; Shkurti等[10]提出一種綜合計算機視覺和慣性系統(tǒng)的ROV狀態(tài)估計算法, 該算法綜合線速度、角速度、深度信息以及視覺信息估計ROV的六自由度姿態(tài); Negahdaripour等[11]提出了一種基于圖像馬賽克的水下機器人定位方法, 利用光流場建立了視覺伺服系統(tǒng); Wasielewski等[12]提出了一種基于特征的相機運動估計方法, 該方法需要提取和匹配相關特征; Zwaan等[13]采用光流和模板匹配等方法, 完成水下機器人的懸停實驗。

總體來說, 國內(nèi)外有關基于單目視覺的水下機器人相對穩(wěn)定性研究主要包括2個方面: 一方面是模型已知的位姿計算, 該方法計算精度高、系統(tǒng)穩(wěn)定性強, 便于水下機器人的控制, 但是該方法只適用于人工放置目標對象或者目標對象已知的場合, 存在一定應用局限性; 另一方面是模型未知的位姿計算, 該方法適用范圍廣、實用性較強, 但是相比前者精度不足, 只能粗略地進行位姿計算, 而且對于圖像處理算法要求較高, 在實際應用中受到限制。

文中以全自由度小型ROV——“觀海ROV”為載體, 從實際工程角度出發(fā), 進行水下機器人穩(wěn)定性研究。由于在實際工作環(huán)境中目標模型已知, 可以通過人工放置目標模型, 因此文中在傳統(tǒng)算法的基礎上設計了一種基于已知模型的位姿估計算法。該方法與傳統(tǒng)基于已知模型算法不同的是[14]: 該方法選取基于目標對象上的4個特征點作為測量的基礎, 利用其中3個點計算出可能的4個解, 再利用最后1個點分別對4個可能的解做重投影, 選取誤差值最小的解作為最終結(jié)果, 稱為重投影法。該方法基于最小的信息量即可求得位姿的線性解, 同時避免了非線性迭代的過程, 因此更能夠滿足實時性的要求。

1 實驗系統(tǒng)組成

基于單目視覺的水下機器人穩(wěn)定性實驗的工作原理為: 通過人工布置的方式在水箱中放置待觀察對象, 然后通過操作桿控制ROV, 使提前放置好的目標出現(xiàn)在ROV視場中, 以便提取出目標上的特征點。進一步通過計算機視覺的方法計算出ROV和目標之間的相對位姿關系, 并以此作為ROV動力分配的依據(jù), 達到在一定水流作用下使ROV保持穩(wěn)定的目的。

實驗以“觀海ROV”作為平臺, 主要包括ROV本體和上位機系統(tǒng)。ROV本體以傳統(tǒng)的攝像機作為視覺傳感器實時采集水下畫面, 采用八推進器非奇異結(jié)構(gòu)布置形式, 其中水平和豎直方向分別有4個采用矢量形式布置的推進器, 完成機器人的前進、后退、上升、下降、左轉(zhuǎn)、右轉(zhuǎn)、橫滾和俯仰的運動。上位機系統(tǒng)主要完成對ROV的圖像采集、三維位姿計算和動力的分配, 工作流程如圖1所示。圖中可見, ROV實時采集水下圖像并通過浮力纜傳回上位機系統(tǒng), 當觀察目標出現(xiàn)在ROV的視場中時, 上位機采集特征點并計算出本體與目標之間的相對位姿關系, 以此作為控制量形成閉環(huán)系統(tǒng), 進行ROV的動力分配, 使ROV相對目標保持穩(wěn)定。

圖1 實驗系統(tǒng)工作流程

2 三維位姿計算

水下機器人在工作環(huán)境下可被當作具有六自由度的剛體, 將機器人所在坐標系稱為相機坐標系, 將觀察對象所在坐標系作為世界坐標系。三維位姿計算的實質(zhì)是求相機坐標系和世界坐標系之間的位姿轉(zhuǎn)化關系。理論上只需知道3個特征點分別在這2個坐標系下的坐標值, 就可計算出這2個坐標系之間的位姿轉(zhuǎn)化關系。在實際工作環(huán)境中, 由于目標模型已知, 則特征點在世界坐標系下的坐標已知, 問題轉(zhuǎn)化為求這3個控制點在相機坐標系下的坐標。在相機已經(jīng)標定出內(nèi)參的情況下[15], 可以計算出各個特征點與光心連線之間的夾角, 如果能計算出各個特征點到光心的距離, 就可求出特征點在相機坐標系下的坐標。綜上所述, 三維位姿的計算主要包括3個過程:首先求出特征點與攝像機光心之間的距離; 然后根據(jù)求出的距離計算特征點在相機坐標系下的坐標; 再根據(jù)3個特征點分別在相機坐標系和世界坐標系下的坐標求2個坐標系的轉(zhuǎn)換關系。

2.1 特征點與光心的距離

已知空間點的相對位置關系以及與光心連線的夾角, 則求空間點到光心的距離屬于PnP (perspective-number-point)問題[16-17]。建立如圖2所示的數(shù)學模型。圖中,為相機光心,、和為3個控制點。

圖2 數(shù)學模型示意圖

根據(jù)余弦定理可知

式中:,分別代表光心與控制點,,之間的距離;,,分別代表控制點,,連線之間的距離;,,分別為光心與控制點,,連線之間的夾角。

該方程組為非線性方程組, 存在多解性和算法魯棒性[18], 但是所有解必為以下4種情況之一[19]

2.2 特征點在相機坐標系下的坐標

三維位姿計算第2步是求特征點在相機坐標系下的坐標, 為此建立投影模型(見圖3)。

圖3 投影模型示意圖

式中,代表相機焦距。

可得

2.3 三維位姿計算

其中

映射變換可表示為

至此, 已經(jīng)求出從相機坐標系轉(zhuǎn)化到世界坐標系的旋轉(zhuǎn)矩陣和平移矩陣, 得到相機坐標系和世界坐標系之間的位置轉(zhuǎn)換關系, 就可以進行下一步的動力分配和運動控制。

3 控制策略

基于機器視覺的水下機器人相對位姿精確控制研究主要應用的場景是: 用戶輸入機器人與作業(yè)對象之間的位姿關系, 使得機器人相對于這個位姿關系保持穩(wěn)定。當上位機采集到作業(yè)目標上的特征點時, 計算出水下機器人與作業(yè)對象之間的實際位姿, 并與用戶輸入的理想位姿作對比, 使水下機器人按一定的規(guī)律進行動力分配。當兩者之間的差距較遠時, 水下機器人以較快的速度運動; 當距離較近時, 水下機器人以較慢的速度運動。由于在水下環(huán)境中存在水流的影響, 當距離小于某個閾值時, 則認為達到理想位置。故選擇傳統(tǒng)的比例-積分-微分(proportion integration differentiation, PID)控制器作為控制方法, PID以其結(jié)構(gòu)簡單、穩(wěn)定性好、工作可靠及調(diào)試方便成為工業(yè)控制的主要技術之一。

此處, 將理想位姿與實際位姿之間的誤差值作為PID的控制量, 算出電機的動力分配方式

4 實驗驗證

為驗證前文所述方法的有效性, 分別進行了位姿測量和ROV穩(wěn)定性控制實驗。在位姿測量實驗中, 采用傳統(tǒng)的迭代法和EPnP(efficient PnP)算法與前文所述重投影法進行了精度測量和運行速度測量實驗, 并對結(jié)果進行比較; 在ROV穩(wěn)定性測量實驗中, 利用Whale 1214-Ⅱ推進器使水箱中的水處于不同運動狀態(tài), 然后觀察使用此控制算法的ROV在不同轉(zhuǎn)速下的穩(wěn)定情況。

圖4 PID控制流程

4.1 位姿測量實驗

以紅色標志點(見圖6)為觀察對象上的特征點并建立世界坐標系。實驗中, 首先將待觀察對象放至實驗平臺(見圖5), 通過直尺測量相機光心大約在世界坐標系中的坐標約為(10, 30, 400)。待觀察對象完全出現(xiàn)在攝像機視場后, 上位機系統(tǒng)會依次選擇紅色特征點進行位姿計算并輸出位姿計算結(jié)果, 結(jié)果曲線如圖7～9所示。

圖5 位姿測量實驗平臺

圖6 觀察對象

圖7 X方向位姿計算結(jié)果曲線

圖8 Y方向位姿計算結(jié)果曲線

圖9 Z方向位姿計算結(jié)果曲線

由圖中可以看出, 文中所涉及算法與迭代法計算結(jié)果都較為穩(wěn)定, 誤差能控制在一定的范圍內(nèi), 而EPnP算法計算出的數(shù)值波動范圍較大, 但是3種算法的計算結(jié)果都基本滿足ROV在水下環(huán)境中的使用要求。

在運行速度測量實驗中, 首先分別用3種方法進行1000次和10000次位姿計算, 從表1中可以看出, 進行1000次計算時, 迭代法平均花費時間為重投影法的10倍, EPnP算法花費時間與文中算法相近; 當進行10000次實驗時(見表2), 迭代法的平均花費時間更長。為了進一步說明該算法在時間上的優(yōu)勢, 在相同的實驗條件下用2種算法分別進行10次實驗, 并且記錄每次計算所耗時間, 結(jié)果如表3所示。

表1 1 000次實驗計算時間對比

表2 10 000次實驗計算時間對比

通過該實驗可知, 文中的重投影法相比傳統(tǒng)迭代法在滿足相同精度的同時, 具有較快的計算速度; 而文中算法與EPnP算法相比, 計算結(jié)果更加穩(wěn)定。在水下機器人行業(yè)中, 較快的計算速度能更好地滿足機器人工作實時性的要求。

表3 單次實驗計算時間對比

4.2 ROV穩(wěn)定性測量實驗

ROV的穩(wěn)定性測試實驗以“觀海ROV”為載體(見圖10), 在實驗室的水池中進行。在水箱中安裝Whale 1214-Ⅱ推進器來控制水箱中水的流速, 根據(jù)視覺系統(tǒng)測量的位姿信息, 利用PID形成閉環(huán)控制, 實現(xiàn)ROV的懸停實驗。

圖10 觀海ROV

實驗中首先調(diào)節(jié)Whale 1214-Ⅱ推進器至一定的轉(zhuǎn)速, 使水箱中的水保持運動狀態(tài), 然后通過操控器操作ROV使待檢測對象完全進入攝像機視場中(見圖11), 上位機自動識別位于檢測對象上特征點的位置, 并計算ROV與載體之間的位姿關系, 進而發(fā)出控制指令使ROV相對檢測對象保持特定姿態(tài), 觀察ROV的運動情況。

圖11 ROV視場

實驗共3次, 分別為當Whale 1214-Ⅱ推進器轉(zhuǎn)速為800、1 200和1 600 r/min時ROV的運動狀態(tài)。實驗時, 首先調(diào)節(jié)Whale 1214-Ⅱ推進器為既定的轉(zhuǎn)速, 輸入ROV與檢測對象間距(方向)為200 mm, 即觀察對象在相機坐標系中的坐標為(0, 0, 200), 然后輸出兩者之間位置變化情況; 等兩者之間距離基本穩(wěn)定之后, 再輸入兩者方向之間的位置為0, 即ROV始終正對觀察對象, 并輸出兩者方向的位置變化。由于ROV本身攜帶有深度傳感器, 能夠進行深度控制(方向), 所以此處不存在深度方向上的數(shù)據(jù)。

圖12～14分別為Whale 1214-Ⅱ推進器在轉(zhuǎn)速為800、1200、1600 r/min時, ROV的位置變化情況, 其中為距離。從圖中可看出, 隨著Whale 1214-Ⅱ推進器轉(zhuǎn)速的增大, ROV最終穩(wěn)定狀態(tài)的偏移量也越來越大, 當推進器轉(zhuǎn)速達到 1600 r/min時, ROV到達穩(wěn)定狀態(tài)的偏移量最為嚴重, 但最終誤差能夠控制在一定范圍內(nèi)。實驗結(jié)果證明了文中方法用于ROV控制的可靠性和可應用性。

圖12 轉(zhuǎn)速為800 r/min時ROV的位置變化曲線

圖13 轉(zhuǎn)速為1 200 r/min時ROV的位置變化曲線

圖14 轉(zhuǎn)速為1 600 r/min時ROV的位置變化曲線

5 結(jié)束語

文中從水下機器人的實際工程角度出發(fā), 提出了一種針對特征點進行重投影的相機位姿計算方法, 詳細說明了該算法的位姿計算步驟, 從理論上保證了該算法的可行性。位姿測量實驗和ROV的穩(wěn)定性實驗表明: 文中方法除具有與傳統(tǒng)位姿計算方法相同的精度外, 還可使ROV相對于特定的作業(yè)對象保持較強的穩(wěn)定性和抗干擾能力。該方法目前僅在實驗室的環(huán)境下進行了驗證, 要真正應用于實際的作業(yè)環(huán)境中, 還需進一步提高算法的自動化程度和觀察目標成像質(zhì)量等, 從而提高整個系統(tǒng)的穩(wěn)定性。

[1] 厲文濤, 聶曉明, 周健. 基于二維激光多普勒測速儀建立新組合導航系統(tǒng)的方法[J]. 中國激光, 2020, 47(3): 296-274.

Li Wen-tao, Nie Xiao-ming, Zhou Jian. Method for Establishing New Integrated Navigation System Based on Two- Dimensional Laser Doppler Velocimeter[J]. Chinese Jour- nal of Lasers, 2020, 47(3): 296-274.

[2] 魏延輝, 張皓淵. 水下作業(yè)型ROV定深控制系統(tǒng)的設計[J]. 中國科技論文, 2016, 11(8): 898-903.

Wei Yan-hui, Zhang Hao-yuan. Design of the Depth Control System for Underwater Work-class ROV[J]. China Sciencepaper, 2016, 11(8): 898-903.

[3] 徐鳳強, 董鵬, 王輝兵, 等. 基于水下機器人的海產(chǎn)品智能檢測與自主抓取系統(tǒng)[J]. 北京航空航天大學學報, 2019, 45(12): 2393-2402.

Xu Feng-qiang, Dong Peng, Wang Hui-bing, et al. Intelligent Detection and Autonomous Capture System of Seafood Based on Underwater Robot[J]. Journal of Beijing University of Aeronautics and Astronautics, 2019, 45(12): 2393-2402.

[4] 周浩, 姜述強, 黃海, 等. 基于視覺感知的海生物吸納式水下機器人目標捕獲控制[J]. 機器人, 2019, 41(2): 242-249, 275.

Zhou Hao, Jiang Shu-qiang, Huang Hai, et al. Vision Based Target Capture Control for Sea Organism Absorptive Underwater Vehicle[J]. Robot, 2019, 41(2): 242-249, 275.

[5] 唐旭東, 龐永杰, 張赫, 等. 基于單目視覺的水下機器人管道檢測[J]. 機器人, 2010, 32(5): 592-600.

Tang Xu-dong, Pang Yong-jie, Zhang He, et al. Underwater Pipeline Detection by AUV Based on Monocular Vision[J]. Robot, 2010, 32(5): 592-600.

[6] 王鵬, 趙汗青, 王江峰, 等. 基于單目視覺的相機位姿解算[J]. 電子科技, 2017, 30(12): 75-78.

Wang Peng, Zhao Han-qing, Wang Jiang-feng, et al. Based on Monocular Vision Camera Pose Computation[J]. Electronic Science and Technology, 2017, 30(12): 75-78.

[7] 王中宇, 李亞茹, 郝仁杰, 等. 基于點特征的單目視覺位姿測量算法[J]. 紅外與激光工程, 2019, 48(5): 169- 176.

Wang Zhong-yu, Li Ya-ru, Hao Ren-jie, et al. Monocular Vision Pose Measurement Algorithm Based on Points Feature[J]. Infrared and Laser Engineering, 2019, 48(5): 169-176.

[8] 高勝, 陳昆, 張利巍, 等. 開架式觀測型ROV定點懸停控制方法研究[J]. 石油機械, 2019, 47(2): 55-64.

Gao Sheng, Chen Kun, Zhang Li-wei, et al. Control Method for Fixed Point Hovering of Open-shelf Observational ROV[J]. China Petroleum Machinery, 2019, 47(2): 55-64.

[9] Trslic P, Rossi M, Robinson L, et al. Vision Based Autonomous Docking for Work Class ROVs[J]. Ocean Engineering, 2020, 196: 10684.

[10] Shkurti F, Rekleitis I, Scaccia M, et al. State Estimation of an Underwater Robot Using Visual and Inertial Information[C]//IEEE/RSJ International Conference on Intelligent Robots & Systems. SanFrancisco: IEEE, 2011: 5054-5060.

[11] Negahdaripour S, Xu X, Jin L. Direct Estimation of Motion from Sea Floor Images for Automatic Station-keeping of Submersible Platforms[J]. IEEE Journal of Oceanic Engineering, 1999, 24(3): 370-382.

[12] Wasielewski S, Aldon M J. Dynamic Vision for ROV Stabilization[C]//Oceans. USA: IEEE, 1996: 1082-1087.

[13] Zwaan S, Santos-Victor J. An Ocean Odyssey. Conference Proceedings Real-time Vision-based Station Keeping for Underwater Robots[C]//Oceans. USA: IEEE, 2001: 1058- 1065.

[14] Ferreira F. Comparing Region-based and Feature-based Methods for ROV Vision-based Motion Estimation[J]. IFAC Proceedings Volumes, 2012, 45(27): 91-96.

[15] 馬宏偉, 王佐勛. 機械臂視覺相機標定的研究與仿真[J]. 齊魯工業(yè)大學學報, 2019, 33(4): 57-63.

Ma Hong-wei, Wang Zuo-xun. Research and Simulation of Visual Camera Calibration of Manipulator[J]. Journal of Shandong Institute of Light Industry(Natural Science Edition), 2019, 33(4): 57-63.

[16] 郝穎明, 吳清瀟, 周船, 等. 基于單目視覺的水下機器人懸停定位技術與實現(xiàn)[J]. 機器人, 2006, 28(6): 656- 661.

Hao Ying-ming, Wu Qing-xiao, Zhou Chuan, et al. Technique and Implementation of Underwater Vehicle Station Keeping Based on Monocular Vision[J]. Robot, 2006, 28(6): 656-661.

[17] Zhou Jie, Wang D K. Solving the Perspective-Three-Point Problem Using Comprehensive Gr?bner Systems[J]. Journal of Systems Science and Complexity, 2016, 29(5): 1446- 1471.

[18] 王波, 胡浩, 張彩霞, 等. P3P問題多解現(xiàn)象的普遍性[J]. 中國科學: 信息科學, 2017, 47(4): 482-491.

Wang Bo, Hu Hao, Zhang Cai-xia, et al. Generality of the Multi-solution Phenomenon in the P3P Problem[J]. Scientia Sinica: Informations, 2017, 47(4): 482-491.

[19] 周鑫, 朱楓. 關于P3P問題解的唯一性條件的幾點討論[J]. 計算機學報, 2003, 26(12): 1696-1701.

Zhou Xin, Zhu Feng. A Note on Unique Solution Conditions of the P3P Problem[J]. Chinese Journal of Computers, 2003, 26(12): 1696-1701.

Relative Position and Attitude Precise Control of Underwater Robot Based on Monocular Vision

WANG Xiao-ming, WUGao-sheng

(SchoolofMechanicalEngineering, Tianjin University of Science and Technology, Tianjin 300222, China)

Aiming at the characteristics of underwater robots in actual operation, which they need to be relatively stable relative to specific operation objects and have certain anti-interference abilities, and a method for stability control of underwater robots based on monocular visual position and attitude measurement is proposed; the key technique of this method is to measure the three-dimensional position and attitude of the underwater robot using the method of re-projection. The “GuanHai ROV” is adoptedfor experimental verification, and the observation target is placed in the experimental pool. The forward camera of the underwater robot is used as a monocular vision sensor to establish an experimental verification system. The experimental results show that the position and attitude measurement principle based on the monocular vision control method of underwater robots can relatively specific objects under constant water flow remain stable, and can meet the needs of the underwater robot in the actual work environment; therefore, the method proposed in this study is effective for the precise control of underwater robot position and attitude. This research has greatly improved the stability of underwater robots in a working environment.

underwater robot; monocular vision; three-dimensional position and attitude; stability

TJ630.33; TB24

A

2096-3920(2021)03-0299-09

10.11993/j.issn.2096-3920.2021.03.008

王曉鳴, 吳高升. 基于單目視覺的水下機器人相對位姿精確控制[J]. 水下無人系統(tǒng)學報, 2021, 29(3): 299-307.

2020-08-05;

2020-10-21.

天津市教委科研計劃項目(2017KJ022).

王曉鳴(1981-), 男, 博士, 副教授, 主要研究方向為機器人和嵌入式控制系統(tǒng)開發(fā).

(責任編輯: 楊力軍)