江平，楊燦軍，壽志成，陳燕虎，范錦昌，黃政明，魏謙笑

?

圓管作業(yè)機器人視覺慣導(dǎo)組合姿態(tài)定位研究

江平1，楊燦軍1，壽志成2，陳燕虎1，范錦昌1，黃政明1，魏謙笑1

(1. 浙江大學(xué) 流體動力與機電系統(tǒng)國家重點實驗室，浙江 杭州，310027； 2. 中海油能源發(fā)展裝備技術(shù)有限公司 上海分公司，上海，200335)

針對石油平臺導(dǎo)管架水下清洗機器人姿態(tài)定位問題，提出一種新型的視覺慣導(dǎo)組合導(dǎo)航系統(tǒng)。該系統(tǒng)利用Canny算子與Hough變換識別圓管特征直線，并通過建立機器人空間模型，解算機器人空間位置姿態(tài)角。針對視覺慣導(dǎo)系統(tǒng)模型，設(shè)計基于四元數(shù)的擴展卡爾曼濾波，實現(xiàn)視覺與慣導(dǎo)信息融合，減小空間角度的累計誤差，提高組合導(dǎo)航精度。研究結(jié)果表明：本文提出的機器人組合導(dǎo)航系統(tǒng)角度精度高，其精確度及實時性滿足了機器人實際應(yīng)用需求。

姿態(tài)定位；視覺導(dǎo)航；組合導(dǎo)航；圖像識別；擴展卡爾曼濾波

圓管作業(yè)機器人適用于各種圓管外側(cè)檢修、噴漆、清洗等功能需求，適應(yīng)了市場對高效化、安全性作業(yè)的發(fā)展需求，因而成為機器人行業(yè)熱門研究的方向之一。圓管作業(yè)機器人的位姿信息的準(zhǔn)確估算是實現(xiàn)機器人自主導(dǎo)航的必要條件之一[1]?，F(xiàn)常用的機器人定位方案包括無線定位[2]、超聲定位[3]、SLAM三維重構(gòu)定位[4]、視覺圖像定位[5?7]等等。但是無線或超聲定位易受周圍環(huán)境干擾，SLAM三維重構(gòu)的設(shè)備成本較高，針對圓管作業(yè)機器人的作業(yè)環(huán)境，可參考無人機的定位方法?，F(xiàn)無人機定位主要采用慣性導(dǎo)航結(jié)合濾波系統(tǒng)的定位方式。慣性導(dǎo)航系統(tǒng)可分為平臺式與捷聯(lián)式慣性導(dǎo)航系統(tǒng)，但慣性導(dǎo)航系統(tǒng)會隨著時間推移產(chǎn)生累積誤差[8]。通常為了減小累積誤差的影響，導(dǎo)航定位系統(tǒng)采用與其他傳感器如GPS導(dǎo)航儀、磁強度計等結(jié)合形成組合導(dǎo)航系統(tǒng)，對慣性導(dǎo)航系統(tǒng)進行定期修正。組合定位系統(tǒng)是現(xiàn)今導(dǎo)航系統(tǒng)的研究熱點，其中常見的組合導(dǎo)航有GPS/SINS導(dǎo)航系統(tǒng)、慣導(dǎo)地磁導(dǎo)航系統(tǒng)等。近年來，慣導(dǎo)視覺組合導(dǎo)航系統(tǒng)由于圖像信息實時性強且信息量大而受到關(guān)注。利用圖像中的位移關(guān)系[9?12]或角度關(guān)系[13?14]對機器人的航向角進行校正。無人機或機器人定位還可結(jié)合慣導(dǎo)系統(tǒng)的速度信息和雙目視覺的圖像深度信息進行機器人定位。針對石油平臺導(dǎo)管架清洗，本文的位姿算法基于永磁輪式機器人平臺，運動范圍小，不適用慣導(dǎo)地磁導(dǎo)航系統(tǒng)或慣導(dǎo)GPS導(dǎo)航系統(tǒng)。本文作者提出了一種創(chuàng)新的慣導(dǎo)視覺組合姿態(tài)定位算法，通過融合陀螺儀、加速度計與攝像機多種傳感器信息，識別圓管特有信息，結(jié)合數(shù)學(xué)模型計算與擴展卡爾曼濾波，可以得到機器人的絕對位置與相對圓管的俯仰角與航向角，為圓管作業(yè)機器人的控制提供有效可靠的位置信息。

1 姿態(tài)運動模型

1.1 參考坐標(biāo)系

水下環(huán)境復(fù)雜未知，需要實時了解機器人的位置信息，以指導(dǎo)機器人的運動。機器人的運動信息是基于其所在的坐標(biāo)系，包含機器人的速度、姿態(tài)和位置信息。采用不同的坐標(biāo)系描述能夠表現(xiàn)機器人不同的運動狀態(tài)。本文機器人控制是基于導(dǎo)管架的運動控制，主要是基于導(dǎo)航坐標(biāo)系、圓管坐標(biāo)系與機器人載體坐標(biāo)系，見圖1，利用坐標(biāo)系的自身位置信息，協(xié)助機器人的導(dǎo)航控制。

導(dǎo)航坐標(biāo)系是機器人導(dǎo)航中用于確定載體信息的坐標(biāo)系，與水平面、圓管產(chǎn)生聯(lián)系，對載體空間信息進行描述。定義導(dǎo)航坐標(biāo)系，其原點O為傾斜圓管軸上的點，沿重力相反方向為Z軸，圓管軸線方向在水平面的投影為Y軸，X軸遵循右手定則，垂直于OYZ平面向前。該坐標(biāo)系定義方法只適用于傾斜圓管，石油平臺的導(dǎo)管架基本為傾斜圓管，可以使用該定義下的圓管導(dǎo)航坐標(biāo)系。

圖1 機器人坐標(biāo)系模型

圓管坐標(biāo)系是機器人吸附的圓管的空間坐標(biāo)系，用于描述機器人相對于圓管的角度位置信息。定義圓管坐標(biāo)系，其原點O與導(dǎo)航坐標(biāo)系原點O共點，X軸與導(dǎo)航坐標(biāo)系X同軸，Z軸沿圓管軸線向上，Y軸遵循右手定則，垂直于OXZ平面向下。

機器人載體坐標(biāo)系是描述載體運動的坐標(biāo)系，相對于載體靜止不動。定義載體坐標(biāo)系，其原點O為機器人質(zhì)心所處位置，沿機器人軸向向前為X軸，沿機器人橫向向左為Y軸，垂直OXY平面向上為Z軸，載體坐標(biāo)系遵循右手定則。

1.2 姿態(tài)解算模型

應(yīng)用于圓管作業(yè)的水下清洗機器人運動控制依托于機器人實時的位姿信息，而相對圓管的位置信息對機器人的操作具有更好的指導(dǎo)作用。相對圓管的位置信息主要是機器人前進方向與圓管軸線的夾角即機器人相對于圓管的俯仰角，以及機器人離圓管截面最低點的夾角即機器人相對于圓管的航向角。機器人位姿信息可通過坐標(biāo)系之間相互轉(zhuǎn)換得到。

圖1中，為Z軸與Z軸的夾角，即管子傾角。圓管坐標(biāo)系能夠通過導(dǎo)航坐標(biāo)系繞X軸旋轉(zhuǎn)得到。(x，y，z)與(x，y，z)為空間中同一點在導(dǎo)航坐標(biāo)系與圓管坐標(biāo)系中的坐標(biāo)表示。

圖1中，為面OXZ與面OXZ的夾角，稱為機器人載體相對于圓管的俯仰角；為Y軸與Y軸的夾角，稱為機器人載體相對于圓管的航向角。機器人載體坐標(biāo)系能夠通過圓管坐標(biāo)系繞Y軸旋轉(zhuǎn)，再繞Z軸旋轉(zhuǎn)得到。(x，y，z)與(x，y，z)為空間中同一點在圓管坐標(biāo)系與機器人載體坐標(biāo)系中的坐標(biāo)表示。

結(jié)合式(1)與(2)可以得到導(dǎo)航坐標(biāo)系轉(zhuǎn)換為機器人載體坐標(biāo)系的變換關(guān)系：

導(dǎo)航坐標(biāo)系轉(zhuǎn)換為機器人載體坐標(biāo)系還可以按照其他的旋轉(zhuǎn)變換順序獲得。通常，慣性導(dǎo)航模塊的空間信息表示，是按照繞Z軸旋轉(zhuǎn)γ，再繞Y軸旋轉(zhuǎn)β，最后繞X軸旋轉(zhuǎn)，得到載體的空間姿態(tài)。將上述的，和稱為絕對姿態(tài)角，分別為橫滾角、俯仰角、航向角。絕對姿態(tài)角能夠通過慣導(dǎo)模塊求解。其變換關(guān)系式如下：

2種變換組合等效，可得

式中：和通過擴展卡爾曼濾波融合求解；作為圓管傾角可通過圖紙獲得；可通過相機圖像角度識別獲取，視為已知量；未知量和可通過最小二乘法求解得到，即得到機器人的絕對航向角和相對圓管的航向角。

1.3 相對圓管俯仰角識別

機器人上的攝像頭置于機器人的前端，從采集圖像中可以判斷探測前端是否存在障礙物，也提供了圓管的邊線位置。對采集圖像利用Canny算子進行邊緣檢測[15]，Hough變換識別圓管邊線[16]，提取圖像中圓管的直線特征，進而計算機器人相對于圓管的俯仰角。Canny邊緣檢測算法利用高斯一階導(dǎo)數(shù)，計算圖像邊緣幅值與角度，檢測圖像階躍型邊緣，其對受白噪聲影響的階躍型邊緣檢測是最優(yōu)的。對提取出的邊緣進行Hough直線識別，Hough直線識別采用投票機制對圖像中的存在直線進行計算，其中最多投票的即為圖像中的識別直線段。圖2(a)所示為水下作業(yè)拍攝原圖，圖2(b)所示為邊緣提取后識別的直線段，其中直線的傾角即為機器人的相對俯仰角。

(a) 水下原始圖像；(b) 圖像直線識別結(jié)果

2 基于四元數(shù)的擴展卡爾曼濾波

圖像識別角度與陀螺儀、加速度計的實時測量均會產(chǎn)生誤差，長時間則會產(chǎn)生角度累積誤差，將對機器人實時位置確定造成明顯影響[16?17]。為了減小運動中傳感器誤差帶來的影響，針對視覺慣導(dǎo)組合導(dǎo)航模型，設(shè)計相應(yīng)的擴展卡爾曼濾波算法，實現(xiàn)機器人位姿識別與顯示，如圖3所示。

圖3 位姿算法流程圖

2.1 狀態(tài)方程定義

基于四元數(shù)的擴展卡爾曼濾波將四元數(shù)=[0123]作為濾波的狀態(tài)量[18]，

其中：為狀態(tài)轉(zhuǎn)移矩陣；為噪聲驅(qū)動矩陣；為過程噪聲矩陣；，和為陀螺儀測得的三軸角速度；為采樣間隔時間；為時刻系統(tǒng)過程噪聲矩陣，表示系統(tǒng)噪聲。針對確定的陀螺儀與加速度設(shè)備，可將的協(xié)方差矩陣視為恒定矩陣，

2.2 觀測方程定義

定義為系統(tǒng)觀測量[19]，

其中：a()為加速度計測量的三軸加速度；()為姿態(tài)模型解算得到的相對航向角；為觀測噪聲。

系統(tǒng)的觀測方程為=+，代入?yún)?shù)可得：

針對確定的陀螺儀與加速度設(shè)備，可將的協(xié)方差矩陣視為恒定矩陣：

2.3 擴展卡爾曼濾波方程

狀態(tài)量與觀測量初始化后，對其進行擴展卡爾曼濾波迭代，如式(11)所示。通過擴展卡爾曼濾波可減小狀態(tài)量的累積誤差。

其中，為系統(tǒng)輸入量；為輸入量的狀態(tài)轉(zhuǎn)移矩陣；為協(xié)方差陣；為狀態(tài)增益矩陣；?1為?1時刻的狀態(tài)估計值；,k?1為?1時刻至?xí)r刻的狀態(tài)中間預(yù)測值；為時刻的狀態(tài)估計值；其余變量含義依此類推。

將擴展卡爾曼濾波處理的四元數(shù)轉(zhuǎn)換成歐拉 角[20?21]，用于姿態(tài)角的表示與計算。

通過以上狀態(tài)模型建立與迭代卡爾曼濾波可以得到校正后小誤差姿態(tài)角參數(shù)，提供較精準(zhǔn)的水下清洗機器人定位數(shù)據(jù)，為水下清洗機器人的運動控制提供位置信息。

3 實驗結(jié)果與分析

本文設(shè)計的組合導(dǎo)航系統(tǒng)包括加速度計、陀螺儀與網(wǎng)絡(luò)攝像頭，下位機采用Stm32F103系列微處理器進行信號采集與擴展卡爾曼濾波處理，上位機進行圖像處理與機器人位置姿態(tài)顯示。慣導(dǎo)模塊與網(wǎng)絡(luò)攝像頭安裝于密封控制腔中，慣導(dǎo)模塊的安裝坐標(biāo)系朝向與網(wǎng)絡(luò)攝像頭的安裝位置如圖4所示。

本文設(shè)計了慣性視覺組合導(dǎo)航系統(tǒng)靜態(tài)試驗與動態(tài)試驗，以驗證導(dǎo)航算法的可行性與準(zhǔn)確性。擴展卡爾曼濾波算法中，參數(shù)采樣時間為10 ms，過程協(xié)方差為0.1，三軸加速度協(xié)方差分別為0.010 6，0.009 9，0.016 0，相機角度協(xié)方差為2.0。

3.1 慣導(dǎo)視覺組合導(dǎo)航靜態(tài)試驗

將地磁慣導(dǎo)組合導(dǎo)航數(shù)據(jù)作為對比參考，對比兩者的解算角度誤差，以驗證視覺慣導(dǎo)導(dǎo)航系統(tǒng)的靜態(tài)性能。在靜態(tài)試驗中，將2種導(dǎo)航系統(tǒng)分別靜置，計算其載體坐標(biāo)系的航向角。每組試驗采集3 000組數(shù)據(jù)，隨機取1 000組數(shù)據(jù)進行比較與分析。由于2種組合導(dǎo)航中的橫滾角與俯仰角解算采用相同的融合濾波方式，因此，這里只針對航向角進行對比試驗，對于橫滾角、俯仰角僅分析其誤差，不進行對比試驗。2種組合導(dǎo)航的結(jié)果對比如圖5所示，具體精度對比見表1。

通過圖5和表1可以看出：視覺慣導(dǎo)組合導(dǎo)航能夠保證導(dǎo)航角度隨時間的累積誤差收斂，與地磁慣導(dǎo)組合導(dǎo)航的精度相近，因此可以在水下強磁環(huán)境中，代替地磁慣導(dǎo)導(dǎo)航系統(tǒng)，使操作者實時了解機器人的空間位置姿態(tài)。

3.2 慣導(dǎo)視覺組合導(dǎo)航動態(tài)試驗

將地磁慣導(dǎo)組合導(dǎo)航系統(tǒng)數(shù)據(jù)作為對比參數(shù)，對比兩者的運動角度，驗證視覺慣性組合導(dǎo)航的動態(tài)性能。動態(tài)試驗中，使2種導(dǎo)航系統(tǒng)分別繞圓管旋轉(zhuǎn)一定角度，記錄兩者航向角信息。由于運動的啟動與停止對導(dǎo)航系統(tǒng)有一定干擾，故僅選取運動中間階段數(shù)據(jù)進行分析。2種組合導(dǎo)航運動對比如圖6所示。

以上試驗是在實驗室理想環(huán)境中進行，試驗環(huán)境相對理想，但是對于實際海上環(huán)境，由于光照或圓管表面平整度的影響，組合導(dǎo)航系統(tǒng)的解算姿態(tài)角將會受到一定的干擾。組合導(dǎo)航海上試驗結(jié)果如圖7所示。

從圖7可以看出：組合導(dǎo)航解算結(jié)果相對穩(wěn)定，能夠較好地實時展示機器人的空間位置姿態(tài)。但是，由于光照影響，圓管邊緣直線識別受到干擾，相對俯仰角發(fā)生波動，同時圓管表面不平整使相對俯仰角發(fā)生突變，這都將對航向角的計算造成影響。

圖4 傳感器安裝位置

1—視覺/慣導(dǎo)解算角度；2—地磁/慣導(dǎo)解算角度。

表1 組合導(dǎo)航結(jié)果準(zhǔn)確度對比

1—視覺慣導(dǎo)解算角度；2—地磁慣導(dǎo)解算角度。

(a) 相對俯仰角與絕對姿態(tài)角曲線；(b) 相對航向角曲線

4 結(jié)論

1) 針對水下未知環(huán)境的導(dǎo)管架清洗機器人姿態(tài)定位問題，提出了基于圓管特征的視覺慣導(dǎo)組合導(dǎo)航系統(tǒng)。通過Canny算法與Hough算法結(jié)合進行圓管邊緣線識別，計算機器人相對圓管的俯仰角。建立了機器人導(dǎo)航系統(tǒng)的載體坐標(biāo)系與導(dǎo)航坐標(biāo)系，通過坐標(biāo)變換與最小二乘法，進行機器人空間角度求解。并基于本文設(shè)計的組合導(dǎo)航系統(tǒng)模型，設(shè)計了視覺慣導(dǎo)擴展卡爾曼濾波算法。

2) 本文研究的組合導(dǎo)航系統(tǒng)能夠?qū)崿F(xiàn)穩(wěn)定實時的機器人位姿估計，達到地磁慣性導(dǎo)航系統(tǒng)的導(dǎo)航精度，角度精度高，滿足水下清洗機器人的實際使用要求與精度。

3) 本文研究的視覺慣導(dǎo)組合導(dǎo)航系統(tǒng)不僅適用于水下導(dǎo)管架，且適用于所有的直圓管結(jié)構(gòu)，具有廣闊的應(yīng)用前景與較大的使用價值。

[1] 張鈞凱. 水下機器人導(dǎo)航定位技術(shù)研究[D]. 青島: 青島科技大學(xué)控制科學(xué)與工程學(xué)院, 2015: 10?16. ZHANG Junkai. Research of navigation and positioning technology for underwater robot[D]. Qingdao: Qingdao University of Science and Technology. School of Control Science and Engineering, 2015: 10?16.

[2] 馮偉亞. 爬壁機器人的無線定位系統(tǒng)的設(shè)計與研究[D]. 杭州: 浙江大學(xué)機械工程學(xué)院, 2017: 1?14. FENG Weiya. Research on wireless location system of wall climbing robots[D]. Hangzhou: Zhejiang University. School of Mechanical Engineering, 2017: 1?14.

[3] 胡亮. 基于傳感器融合的爬壁機器人感知系統(tǒng)的研究[D]. 哈爾濱: 哈爾濱工業(yè)大學(xué)機電工程學(xué)院, 2011: 8?11. HU Liang. Research on obstacle detection of wall climbing robots based on multi-sensor data fusion[D]. Harbin: Harbin Institute of Technology. School of Mechatronics Engineering, 2011: 8?11.

[4] 徐曙. 基于SLAM的移動機器人導(dǎo)航系統(tǒng)研究[D]. 武漢: 華中科技大學(xué)電氣與電子工程學(xué)院, 2014: 6. XU Shu. The research to the mobile robots navigation system based on SLAM[D]. Wuhan: Huazhong University of Science and Technology. School of Electrical and Electronic Engineering, 2014: 6.

[5] HESCH J A, KOTTAS D G, BOWMAN S L, et al. Consistency analysis and improvement of vision-aided inertial navigation[J]. IEEE Transactions on Robotics, 2014, 30(1): 158?176.

[6] LI Luyang, LIU Yunhui, WANG Kai, et al. A simple algorithm for position and velocity estimation of mobile robots using omnidirectional vision and inertial sensors[C]//IEEE International Conference on Robotics and Biomimetics. Piscataway: IEEE, 2014: 1235?1240.

[7] TAYLOR C N. Fusion of inertial, vision, and air pressure sensors for MAV navigation[C]//IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems. Piscataway: IEEE, 2008: 475?480.

[8] CHDID D, OUEIS R, KHOURY H, et al. Inertial-vision sensor fusion for pedestrian localization[C]//IEEE International Conference on Robotics and Biomimetics. Piscataway: IEEE, 2011: 1695?1701.

[9] SCHNEIDER T, LI M, BURRI M, et al. Visual-inertial self-calibration on informative motion segments[C]//IEEE International Conference on Robotics and Automation (ICRA). Piscataway: IEEE, 2017: 6487?6494.

[10] 于永軍, 徐錦法, 張梁, 等. 慣導(dǎo)/雙目視覺位姿估計算法研究[J]. 儀器儀表學(xué)報, 2014, 35(10): 2170?2176.YU Yongjun, XU Jinfa, ZHANG Liang, et al. Research on SINS/binocular vision integrated position and attitude estimation algorithm[J]. Chinese Journal of Scientific Instrument, 2014, 35(10): 2170?2176.

[11] 路丹暉. 融合視覺與慣性導(dǎo)航的機器人自主定位[D]. 杭州:浙江大學(xué)信息與電子工程學(xué)院, 2012: 1?20.LU Danhui. Research on robot automatic localization by fusion with visual and IMU[D]. Hangzhou: Zhejiang University. School of Information and Electronic Engineering, 2012: 1?20.

[12] 陳明芽, 項志宇, 劉濟林. 單目視覺自然路標(biāo)輔助的移動機器人定位方法[J]. 浙江大學(xué)學(xué)報(工學(xué)版), 2014, 48(2): 285?291. CHEN MinYa, XIANG Zhiyu, LIU Jilin. Assistance localization method for mobile robot based on monocular natural visual landmarks[J]. Journal of Zhejiang University(Engineering Science), 2014, 48(2): 285?291.

[13] TANG Daquan, JIAO Yongkang, CHEN Jie. On automatic landing system for carrier plane based on integration of INS, GPS and Vision[C]//Proceedings of 2016 IEEE Chinese Guidance, Navigation and Control Conference. Piscataway: IEEE, 2016: 2260?2264.

[14] CARRERAS M, RIDAO P, GARCIA R. Vision-based localization of an underwater robot in a structured environment[C]//Proceedings of IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 2003: 971?976.

[15] FERRARI C, CANNY J. Planning optimal grasps[C]// Proceedings of the 1992 IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 1992: 2290?2295.

[16] ILLINGWORTH J, KITTLER J. The adaptive Hough transform[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 1987, PAMI-9(5): 690?698.

[17] ROSARIO M B D, LOVELL N H, REDMOND S J. Quaternion-Based complementary filter forattitude determination of a smartphone[J]. IEEE Sensor Journal, 2016, 16(15): 6008?6017

[18] HSU Y L, WANG J S, CHANG C W. A wearable inertial pedestrian navigation system with quaternion-based extended Kalman filter for pedestrian localization[J]. IEEE Sensors Journal, 2017, 17(10): 3193?3206.

[19] 宋宇, 翁新武, 郭昕剛. 基于四元數(shù)EKF算法的小型無人機姿態(tài)估計[J]. 吉林大學(xué)學(xué)報(理學(xué)版), 2015, 53(3): 511?518. SONG Yu, WEN Xinwu, GUO Xingang. Small UAV attitude estimation based on the algorithm of quaternion extended Kalman filter[J]. Journal of Jilin University(Science Edition), 2015, 53(3): 511?518.

[20] HENDERSON D M. Euler Angles, Quaternions, and Transformation Matrices[R]. Washington: NASA, 1977: 5?20.

[21] 賈瑞才. 基于四元數(shù)EKF的低成本MEMS姿態(tài)估計算法[J]. 傳感技術(shù)學(xué)報, 2014, 27(1): 90?95.JIA Ruicai. Attitude estimation algorithm for low cost MEMS based on quaternion EKF[J]. Chinese Journal of Sensor and Actuators, 2014, 27(1): 90?95.

Research on vision-inertial navigation of an underwater cleaning robot

JIANG Ping1, YANG Canjun1, SHOU Zhicheng2, CHEN Yanhu1,FAN Jinchang1, HUANG Zhengming1, WEI Qianxiao1

(1. State Key Laboratory of Fluid Power and Mechatronic Systems, Zhejiang University, Hangzhou 310027, China; 2. Shanghai Branch of China National Offshore Oil Corporation, Shanghai 200335, China)

For the localization of the underwater cleaning robot’s localization, an innovative vision-inertial navigation system was put forward. By recognizing pipe line in Canny/Hough and establishing robot’s spatial model, the attitude angles were solved. For vision-inertial system model, the extended Kalman filter bases on quaternion was designed to fuse vision/inertial data and reduce accumulative errors, which improves the system accuracy. The results show that this attitude accuracy is high, which indicates that the real-time performance and the precision of the navigation system can meet the practical demand.

attitude and location; vision navigation system; integrated navigation; image recognition; extended Kalman filter

10.11817/j.issn.1672?7207.2018.12.007

TP24

A

1672?7207(2018)12?2946?07

2018?01?18；

2018?05?09

浙江省公益技術(shù)應(yīng)用研究計劃項目(2016C33057)；中國海洋石油總公司科技項目(CNOOC-KJ125 ZDXM 13 LTD NFZB 2015-04)(Project(2016C33057) supported by the Zhejiang Public Welfare Technology Application Research Program; Project(CNOOC-KJ125 ZDXM 13 LTD NFZB 2015-04) supported by the CNOOC Scientific Project)

楊燦軍，博士，教授，從事機器人與海洋研究；E-mail：ycj@zju.edu.cn

(編輯 趙俊)