DOI: 10.11817/j.issn.1672-7207.2018.12.007

圆管作业机器人视觉惯导组合姿态定位研究

江平¹，杨灿军¹，寿志成²，陈燕虎¹，范锦昌¹，黄政明¹，魏谦笑¹

(1. 浙江大学 流体动力与机电系统国家重点实验室，浙江 杭州，310027；

2. 中海油能源发展装备技术有限公司 上海分公司，上海，200335)

摘要：针对石油平台导管架水下清洗机器人姿态定位问题，提出一种新型的视觉惯导组合导航系统。该系统利用Canny算子与Hough变换识别圆管特征直线，并通过建立机器人空间模型，解算机器人空间位置姿态角。针对视觉惯导系统模型，设计基于四元数的扩展卡尔曼滤波，实现视觉与惯导信息融合，减小空间角度的累计误差，提高组合导航精度。研究结果表明：本文提出的机器人组合导航系统角度精度高，其精确度及实时性满足了机器人实际应用需求。

关键词：姿态定位；视觉导航；组合导航；图像识别；扩展卡尔曼滤波

中图分类号：TP24             文献标志码：A         文章编号：1672-7207(2018)12-2946-07

Research on vision-inertial navigation of an underwater cleaning robot

JIANG Ping¹, YANG Canjun¹, SHOU Zhicheng², CHEN Yanhu¹,FAN Jinchang¹, HUANG Zhengming¹, WEI Qianxiao¹

(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)

Abstract: 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.

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

圆管作业机器人适用于各种圆管外侧检修、喷漆、清洗等功能需求，适应了市场对高效化、安全性作业的发展需求，因而成为机器人行业热门研究的方向之一。圆管作业机器人的位姿信息的准确估算是实现机器人自主导航的必要条件之一^[1]。现常用的机器人定位方案包括无线定位^[2]、超声定位^[3]、SLAM三维重构定位^[4]、视觉图像定位^[5-7]等等。但是无线或超声定位易受周围环境干扰，SLAM三维重构的设备成本较高，针对圆管作业机器人的作业环境，可参考无人机的定位方法。现无人机定位主要采用惯性导航结合滤波系统的定位方式。惯性导航系统可分为平台式与捷联式惯性导航系统，但惯性导航系统会随着时间推移产生累积误差^[8]。通常为了减小累积误差的影响，导航定位系统采用与其他传感器如GPS导航仪、磁强度计等结合形成组合导航系统，对惯性导航系统进行定期修正。组合定位系统是现今导航系统的研究热点，其中常见的组合导航有GPS/SINS导航系统、惯导地磁导航系统等。近年来，惯导视觉组合导航系统由于图像信息实时性强且信息量大而受到关注。利用图像中的位移关系^[9-12]或角度关系^[13-14]对机器人的航向角进行校正。无人机或机器人定位还可结合惯导系统的速度信息和双目视觉的图像深度信息进行机器人定位。针对石油平台导管架清洗，本文的位姿算法基于永磁轮式机器人平台，运动范围小，不适用惯导地磁导航系统或惯导GPS导航系统。本文作者提出了一种创新的惯导视觉组合姿态定位算法，通过融合陀螺仪、加速度计与摄像机多种传感器信息，识别圆管特有信息，结合数学模型计算与扩展卡尔曼滤波，可以得到机器人的绝对位置与相对圆管的俯仰角与航向角，为圆管作业机器人的控制提供有效可靠的位置信息。

1  姿态运动模型

1.1  参考坐标系

水下环境复杂未知，需要实时了解机器人的位置信息，以指导机器人的运动。机器人的运动信息是基于其所在的坐标系，包含机器人的速度、姿态和位置信息。采用不同的坐标系描述能够表现机器人不同的运动状态。本文机器人控制是基于导管架的运动控制，主要是基于导航坐标系、圆管坐标系与机器人载体坐标系，见图1，利用坐标系的自身位置信息，协助机器人的导航控制。

导航坐标系是机器人导航中用于确定载体信息的坐标系，与水平面、圆管产生联系，对载体空间信息进行描述。定义导航坐标系N，其原点O_n为倾斜圆管轴上的点，沿重力相反方向为Z_n轴，圆管轴线方向在水平面的投影为Y_n轴，X_n轴遵循右手定则，垂直于O_nY_nZ_n平面向前。该坐标系定义方法只适用于倾斜圆管，石油平台的导管架基本为倾斜圆管，可以使用该定义下的圆管导航坐标系。

图1  机器人坐标系模型

Fig. 1  Coordinate system model of robot

圆管坐标系是机器人吸附的圆管的空间坐标系，用于描述机器人相对于圆管的角度位置信息。定义圆管坐标系P，其原点O_p与导航坐标系原点O_n共点，X_p轴与导航坐标系X_n同轴，Z_p轴沿圆管轴线向上，Y_p轴遵循右手定则，垂直于O_pX_pZ_p平面向下。

机器人载体坐标系是描述载体运动的坐标系，相对于载体静止不动。定义载体坐标系C，其原点O_c为机器人质心所处位置，沿机器人轴向向前为X_c轴，沿机器人横向向左为Y_c轴，垂直O_cX_cY_c平面向上为Z_c轴，载体坐标系遵循右手定则。

1.2  姿态解算模型

应用于圆管作业的水下清洗机器人运动控制依托于机器人实时的位姿信息，而相对圆管的位置信息对机器人的操作具有更好的指导作用。相对圆管的位置信息主要是机器人前进方向与圆管轴线的夹角即机器人相对于圆管的俯仰角，以及机器人离圆管截面最低点的夹角即机器人相对于圆管的航向角。机器人位姿信息可通过坐标系之间相互转换得到。

图1中，α_r为Z_n轴与Z_p轴的夹角，即管子倾角。圆管坐标系能够通过导航坐标系绕X_n轴旋转α_r得到。(x_n，y_n，z_n)与(x_p，y_p，z_p)为空间中同一点在导航坐标系与圆管坐标系中的坐标表示。

     (1)

图1中，β_r为面O_pX_pZ_p与面O_cX_cZ_c的夹角，称为机器人载体相对于圆管的俯仰角；γ_r为Y_p轴与Y_c轴的夹角，称为机器人载体相对于圆管的航向角。机器人载体坐标系能够通过圆管坐标系绕Y_p轴旋转β_r，再绕Z_p轴旋转γ_r得到。(x_p，y_p，z_p)与(x_c，y_c，z_c)为空间中同一点在圆管坐标系与机器人载体坐标系中的坐标 表示。

   (2)

结合式(1)与(2)可以得到导航坐标系转换为机器人载体坐标系的变换关系：

 (3)

导航坐标系转换为机器人载体坐标系还可以按照其他的旋转变换顺序获得。通常，惯性导航模块的空间信息表示，是按照绕Z_n轴旋转γ_a，再绕Y_n轴旋转β_a，最后绕X_n轴旋转α_a，得到载体的空间姿态。将上述的α_a，β_a和γ_a称为绝对姿态角，分别为横滚角、俯仰角、航向角。绝对姿态角能够通过惯导模块求解。其变换关系式如下：

 (4)

2种变换组合等效，可得

        (5)

式中：α_a和β_a通过扩展卡尔曼滤波融合求解；α_r作为圆管倾角可通过图纸获得；β_r可通过相机图像角度识别获取，视为已知量；未知量γ_a和γ_r可通过最小二乘法求解得到，即得到机器人的绝对航向角和相对圆管的航向角。

1.3  相对圆管俯仰角识别

机器人上的摄像头置于机器人的前端，从采集图像中可以判断探测前端是否存在障碍物，也提供了圆管的边线位置。对采集图像利用Canny算子进行边缘检测^[15]，Hough变换识别圆管边线^[16]，提取图像中圆管的直线特征，进而计算机器人相对于圆管的俯仰角。Canny边缘检测算法利用高斯一阶导数，计算图像边缘幅值与角度，检测图像阶跃型边缘，其对受白噪声影响的阶跃型边缘检测是最优的。对提取出的边缘进行Hough直线识别，Hough直线识别采用投票机制对图像中的存在直线进行计算，其中最多投票的即为图像中的识别直线段。图2(a)所示为水下作业拍摄原图，图2(b)所示为边缘提取后识别的直线段，其中直线的倾角即为机器人的相对俯仰角。

图2  相对圆管俯仰角识别

Fig. 2  Relative pitch angle calculation

2  基于四元数的扩展卡尔曼滤波

图像识别角度与陀螺仪、加速度计的实时测量均会产生误差，长时间则会产生角度累积误差，将对机器人实时位置确定造成明显影响^[16-17]。为了减小运动中传感器误差带来的影响，针对视觉惯导组合导航模型，设计相应的扩展卡尔曼滤波算法，实现机器人位姿识别与显示，如图3所示。

图3  位姿算法流程图

Fig. 3  Flow chart of attitude algorithm

2.1  状态方程定义

基于四元数的扩展卡尔曼滤波将四元数X=[q₀ q₁ q₂ q₃]作为滤波的状态量^[18]，

           (6)

其中：Φ为状态转移矩阵；Γ为噪声驱动矩阵；W为过程噪声矩阵；ω_x，ω_y和ω_z为陀螺仪测得的三轴角速度；T为采样间隔时间；W_k为k时刻系统过程噪声矩阵，表示系统噪声。针对确定的陀螺仪与加速度设备，可将W_k的协方差矩阵Q_k视为恒定矩阵，

                (7)

其中：为过程噪声协方差。

2.2  观测方程定义

定义Z_k为系统观测量^[19]，

       (8)

其中：a_b(k)为加速度计测量的三轴加速度；ψ(k)为姿态模型解算得到的相对航向角；V_k为观测噪声。

系统的观测方程为Z_k=HX_k+V_k，代入参数可得：

(9)

其中，D₁=q₁q₂+q₀q₃；；H为观测矩阵。

针对确定的陀螺仪与加速度设备，可将V_k的协方差矩阵R_k视为恒定矩阵：

          (10)

其中：为观测加速度噪声协方差；巡航角噪声协方差。

2.3  扩展卡尔曼滤波方程

状态量与观测量初始化后，对其进行扩展卡尔曼滤波迭代，如式(11)所示。通过扩展卡尔曼滤波可减小状态量的累积误差。

   (11)

其中，U为系统输入量；B为输入量U的状态转移矩阵；P为协方差阵；K为状态增益矩阵；X_k-1为k-1时刻的状态估计值；X_k,k-1为k-1时刻至k时刻的状态中间预测值；X_k为k时刻的状态估计值；其余变量含义依此类推。

将扩展卡尔曼滤波处理的四元数转换成欧拉角^[20-21]，用于姿态角的表示与计算。

通过以上状态模型建立与迭代卡尔曼滤波可以得到校正后小误差姿态角参数，提供较精准的水下清洗机器人定位数据，为水下清洗机器人的运动控制提供位置信息。

3  实验结果与分析

本文设计的组合导航系统包括加速度计、陀螺仪与网络摄像头，下位机采用Stm32F103系列微处理器进行信号采集与扩展卡尔曼滤波处理，上位机进行图像处理与机器人位置姿态显示。惯导模块与网络摄像头安装于密封控制腔中，惯导模块的安装坐标系朝向与网络摄像头的安装位置如图4所示。

本文设计了惯性视觉组合导航系统静态试验与动态试验，以验证导航算法的可行性与准确性。扩展卡尔曼滤波算法中，参数采样时间为10 ms，过程协方差为0.1，三轴加速度协方差分别为0.010 6，0.009 9，0.016 0，相机角度协方差为2.0。

3.1  惯导视觉组合导航静态试验

将地磁惯导组合导航数据作为对比参考，对比两者的解算角度误差，以验证视觉惯导导航系统的静态性能。在静态试验中，将2种导航系统分别静置，计算其载体坐标系的航向角。每组试验采集3 000组数据，随机取1 000组数据进行比较与分析。由于2种组合导航中的横滚角与俯仰角解算采用相同的融合滤波方式，因此，这里只针对航向角进行对比试验，对于横滚角、俯仰角仅分析其误差，不进行对比试验。2种组合导航的结果对比如图5所示，具体精度对比见表1。

通过图5和表1可以看出：视觉惯导组合导航能够保证导航角度随时间的累积误差收敛，与地磁惯导组合导航的精度相近，因此可以在水下强磁环境中，代替地磁惯导导航系统，使操作者实时了解机器人的空间位置姿态。

3.2  惯导视觉组合导航动态试验

将地磁惯导组合导航系统数据作为对比参数，对比两者的运动角度，验证视觉惯性组合导航的动态性能。动态试验中，使2种导航系统分别绕圆管旋转一定角度，记录两者航向角信息。由于运动的启动与停止对导航系统有一定干扰，故仅选取运动中间阶段数据进行分析。2种组合导航运动对比如图6所示。

以上试验是在实验室理想环境中进行，试验环境相对理想，但是对于实际海上环境，由于光照或圆管表面平整度的影响，组合导航系统的解算姿态角将会受到一定的干扰。组合导航海上试验结果如图7所示。

从图7可以看出：组合导航解算结果相对稳定，能够较好地实时展示机器人的空间位置姿态。但是，由于光照影响，圆管边缘直线识别受到干扰，相对俯仰角发生波动，同时圆管表面不平整使相对俯仰角发生突变，这都将对航向角的计算造成影响。

图4  传感器安装位置

Fig. 4  Sensors’ location

图5  组合导航静态试验结果对比

Fig. 5  Comparison of integrated navigation static experimental results

表1  组合导航结果准确度对比

Table 1  Accuracy comparison of integrated navigation

图6  组合导航动态试验结果对比

Fig. 6  Comparison of integrated navigation dynamic experimental results

图7  组合导航海上平台试验结果

Fig. 7  Result of integrated navigation offshore experiments

4  结论

1) 针对水下未知环境的导管架清洗机器人姿态定位问题，提出了基于圆管特征的视觉惯导组合导航系统。通过Canny算法与Hough算法结合进行圆管边缘线识别，计算机器人相对圆管的俯仰角。建立了机器人导航系统的载体坐标系与导航坐标系，通过坐标变换与最小二乘法，进行机器人空间角度求解。并基于本文设计的组合导航系统模型，设计了视觉惯导扩展卡尔曼滤波算法。

2) 本文研究的组合导航系统能够实现稳定实时的机器人位姿估计，达到地磁惯性导航系统的导航精度，角度精度高，满足水下清洗机器人的实际使用要求与精度。

3) 本文研究的视觉惯导组合导航系统不仅适用于水下导管架，且适用于所有的直圆管结构，具有广阔的应用前景与较大的使用价值。

参考文献：

[1] 张钧凯. 水下机器人导航定位技术研究[D]. 青岛: 青岛科技大学控制科学与工程学院, 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] 冯伟亚. 爬壁机器人的无线定位系统的设计与研究[D]. 杭州: 浙江大学机械工程学院, 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] 胡亮. 基于传感器融合的爬壁机器人感知系统的研究[D]. 哈尔滨: 哈尔滨工业大学机电工程学院, 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]. 武汉: 华中科技大学电气与电子工程学院, 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] 于永军, 徐锦法, 张梁, 等. 惯导/双目视觉位姿估计算法研究[J]. 仪器仪表学报, 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]. 杭州:浙江大学信息与电子工程学院, 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] 陈明芽, 项志宇, 刘济林. 单目视觉自然路标辅助的移动机器人定位方法[J]. 浙江大学学报(工学版), 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 for attitude 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] 宋宇, 翁新武, 郭昕刚. 基于四元数EKF算法的小型无人机姿态估计[J]. 吉林大学学报(理学版), 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] 贾瑞才. 基于四元数EKF的低成本MEMS姿态估计算法[J]. 传感技术学报, 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.

(编辑  赵俊)

收稿日期：2018-01-18；修回日期：2018-05-09

基金项目(Foundation item)：浙江省公益技术应用研究计划项目(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