一种大型球形储罐爬壁机器人定位导航系统及方法与流程

(一)技术领域：

本发明涉及机器人技术领域，具体涉及一种大型球型储罐爬壁机器人定位导航系统及方法。

(二)

背景技术：

作为复杂环境极限作业的一种自动化装置，爬壁机器人被广泛应用于各种检测任务。随着作业要求的提高和作业危险性的逐步提升，尤其是在石油化工行业中，在大型球形储罐的制造过程中,由于球壳是由多块压制成球面的球瓣以橘瓣式分瓣法、足球式分瓣法或足球橘瓣混合式分瓣法组焊而成，导致球罐存在大量的焊缝，在使用过程中，常在焊缝处出现由于硫化氢(h2s)应力腐蚀开裂的问题，为保证生产的安全性，需要对球罐表面的所有焊缝进行无损探伤，以检测焊缝内部缺陷。探伤爬壁机器人通常需要搭载检测设备在球罐表面按照预定路径进行扫描式检测，因此如何准确检测定位球形储罐问题焊接区域成为提高球形储罐安全可靠性的关键技术之一。

在充满易燃易爆气体且具有很少甚至没有地标以及良好照明条件的大型液化石油气(lpg)球形储罐环境中，可应用的传感器类型受到限制，爬壁机器人定位导航问题变得更具挑战性。传统定位导航方法中的里程计法以及惯性导航法，由于两种方法均基于航位推算，即基于当前传感器数据和先前机器人位姿来进行测距计算，推算定位方法的一个劣势在于误差和不确定性是累积的，在机器人长时间、长距离运行情况下，最终会导致机器人位姿估计值偏离真实值越来越严重，无法满足机器人在球罐表面长时间、长距离作业的任务需求。

针对现有技术的不足，目前亟需一种高精度的爬壁机器人定位导航系统及方法以应对大型球形储罐环境下机器人定位导航困难问题。

(三)

技术实现要素：

本发明的目的在于提供一种大型球形储罐爬壁机器人定位导航系统及方法，它可以克服传统定位方法累积误差严重的问题，是一种结构简单、分析可靠、定位准确且容易实现的定位导航系统及方法。

本发明技术方案：一种大型球形储罐爬壁机器人定位导航系统，其特征在于它包括上位机、扩展卡尔曼滤波器(extendedkalmanfilter，ekf)、导航控制器、车轮编码器、惯性测量单元(inertialmeasurementunit，imu)、imu处理单元、相机传感器和爬壁机器人底层控制单元；其中，所述导航控制器的输入端接收爬壁机器人的期望姿态设置值信号以及扩展卡尔曼滤波器的输出信号，其输出端输出定位导航信息给爬壁机器人的爬壁机器人底层控制单元；所述惯性测量单元的输入端连接爬壁机器人底层控制单元，用于测量爬壁机器人惯性数据，并输出给imu处理单元；所述扩展卡尔曼滤波器的输入端分别接收车轮编码器、相机传感器及imu处理单元的输出信号，其输出端与导航控制器的输入端连接；所述车轮编码器用于测量爬壁机器人的编码器脉冲以及车轮速度反馈信息，其输出端连接扩展卡尔曼滤波器；所述相机传感器用于采集罐体表面图像信息，其输出端连接扩展卡尔曼滤波器；所述爬壁机器人底层控制单元和上位机通过串口方式进行连接通讯。

所述上位机为pc机，采用ros操作系统，系统版本为ubuntu14.04；

所述扩展卡尔曼滤波器带有6d(3dpositionand3dorientation)模型信息，包含在上位机linux系统的robot_pose_ekf功能包中，主要用于linux系统下对机器人的3d位姿进行评估；所述robot_pose_ekf是linux系统下用于评估机器人3d位姿的数据融合功能包；所述扩展卡尔曼滤波器接收来自车轮编码器、惯性测量单元及相机传感器所测爬壁机器人位置数据、速度数据、姿态数据，并进行分析融合校正，获得爬壁机器人准确的定位数据信息，用松耦合方式对各个传感器数据进行融合以实现机器人的位姿估计。

所述导航控制器是上位机linux系统下的ros(robotoperatingsystem，机器人操作系统)导航堆栈，主要用于爬壁机器人完成自主导航任务。

所述车轮编码器用于测量编码器脉冲以及车轮速度反馈，其输出信号输入到扩展卡尔曼滤波器，在上位机上发布话题消息，给定爬壁机器人的每个车轮的直径和脉冲数，便于计算机器人在给定时间内移动的距离。

所述imu处理单元用于根据接收到的惯性数据计算得到爬壁机器人的位置数据、速度数据和姿态数据。

所述相机传感器是ccd(charge-coupleddevice，电荷耦合器件)相机传感器，数量不少于2个，用于采集罐体表面图像信息，通过实时对图像进行二值化处理，突出焊缝特征区域，以实现对焊缝的识别和跟踪；所述两个ccd相机传感器水平向下放置于机器人左右两侧，且与机器人平行，左右相机各自采集球罐表面的图像信息。

所述相机传感器上配备有辅助光源，便于安装在机器人两侧的ccd相机进行图像采集。

所述导航控制器用于根据得到的定位数据信息来完成相应的爬壁机器人自主导航任务。

一种大型球形储罐爬壁机器人定位导航方法，其特征在于它包括以下步骤：

(1)利用车轮编码器获取爬壁机器人的里程计数据；

(2)通过坐标系统变换，将不同坐标系下的定位信息统一转换到同一坐标系下，保证里程计得到正确的测量数据；

传统的通过里程计定位方法使机器人局限于平面下的作业环境，导致机器人里程计无法在球罐这种曲面环境中得到正确的位置信息；机器人借助先前已知位置进行位姿递推定位是基于全局坐标系下进行的，因此，要想得到机器人在球罐表面运行过程中的位姿信息，需要进行坐标系统的转换，即按照一定的转换方法将机器人在球面的位置数据统一转换到空间平面直角坐标系中；

所述步骤(2)中将不同坐标系下的定位信息统一转换到同一坐标系具体是指以下内容：

记x、y、z为空间直角坐标系的三个坐标轴，规定为球罐环境下的全局坐标系，为机器人坐标系，机器人坐标系相对于全局坐标系的角度关系即为爬壁机器人的姿态角，记球罐上机器人的任一位置p，其高度角为α，方位角为β，球罐半径为ρ，区别于平面环境，机器人坐标系需转换到全局坐标系下才可得到机器人正确的里程计测量数据，其中，坐标系可以看作是由坐标系经旋转平移变化而来；记：

p＝[xg,yg,zg]^t

p'＝[xr,yr,zr]^t

则有：p'＝rp+t；

其中r为旋转矩阵，t为平移矩阵，显然，t＝[0,0,-ρ]；

对旋转矩阵r的求导：

对于坐标系绕任意轴旋转存在以下转换关系：

绕x轴旋转α角如式(1)所示：

绕y轴旋转β角如式(2)所示：

绕z轴旋转γ角如式(3)所示：

将坐标系先绕z轴旋转β角，再绕x轴旋转角度，再经过一个平移向量t，即可得到坐标系将其写成如式(4)所示的矩阵形式：

p'＝rx(rzp)＝(rxrz)p(4)

将式(1)、式(2)和式(3)分别带入式(4)，则有：

整理可得到坐标系到坐标系的转换关系，如式(6)所示：

同理可得坐标系到坐标系的转换关系如式(7)所示：

其中，坐标系与间的转换旨在得到机器人正确的位姿信息，以保证里程计测量数据的准确性，同时也是为了根据转换关系在上位机发布正确的里程计脚本节点信息；

(3)利用惯性测量单元获取爬壁机器人的惯性数据，通过对所得惯性数据进行一次积分得到爬壁机器人的位姿数据；

(4)利用安装在机器人两侧的ccd相机进行图像采集，通过设置阈值，将高于阈值的像素灰度值置为255，将低于阈值的像素灰度值置为0，对采集到的图像进行实时二值化处理，将图像转换为二值图，图像上灰度值为255的白色区域则代表焊缝特征区域；

所述步骤(4)中的阈值设置为158～162，保证明显的突出焊缝特征区域。

(5)利用步骤(4)即可识别出焊缝特征区域，并输出相应的检测信号，把二值化处理的图像上的白色区域看作兴趣点，代表焊缝特征区域，以此，可以将机器人相对于焊缝的运动用兴趣点的运动代替，兴趣点周围的区域称为兴趣区域，代表整个范围内的焊缝相机检测；根据当前数据与先前数据之间的差异进行增量计算即可得出机器人相对于焊缝的运动；

(6)在得到机器人相对于焊缝运动的增量信号后利用均值滤波器和低通滤波器进行增量信号滤波以避免噪声的干扰，通过对结果进行二次积分即可获取机器人的相对位置值；

其中，均值滤波器计算公式如式(8)所示：

当p为负数时，可有效滤除白噪声，是正的异常值；p为正数时，可对黑噪声进行过滤，是负的异常值；

低通滤波按公式(9)实现：

y(k)＝α·x(k)+(1-α)·y(k-1)(9)

其中，x(k)为本次采样值；y(k-1)为上次的滤波输出值；α为滤波系数，其值一般远小于1；y(k)为本次滤波输出值；

由公式(9)可以看出，本次滤波的输出值主要取决于上次滤波输出值，且本次采样值对滤波输出影响较小；

(7)由扩展卡尔曼滤波器接收来自车轮编码器、惯性测量单元以及ccd相机传感器的数据信息，利用robot_pose_ekf对三个数据源进行数据融合处理，获得对爬壁机器人位姿的一致性解释与描述，输出爬壁机器人位姿估计结果，以改善机器人定位；

所述步骤(7)中的robot_pose_ekf是linux系统下用于评估机器人3d位姿的数据融合功能包；它使用了来自不同传感器源的位姿测量信息，其基本思路是通过松耦合方式对不同传感器数据进行融合来实现机器人位姿估计；robot_pose_ekf默认监听的话题为：imu_data、odom、vo，其中imu_data为来自imu的爬壁机器人位姿数据，odom为来自里程计，即车轮编码器，的位姿数据，vo为来自ccd相机传感器所测的位姿数据；因此，在发布里程计、imu、ccd相机传感器消息时要注意与话题的名称相对应，否则起不到滤波作用，robot_pose_ekf中的扩展卡尔曼滤波器作为概率滤波器运行，根据每个传感器输入附有的协方差计算给定数据源的有效性，并为该数据源指定权重，输出更好的估计结果。

(8)根据步骤(7)得到的爬壁机器人定位数据信息，将其输入到导航控制器，驱动爬壁机器人完成指定的检测任务。

所述步骤(8)中导航控制器是上位机linux系统下ros导航堆栈组成，其具体工作方法包括以下步骤：

①由switch模块读取一个文件，其中，所述文件的内容包含了爬壁机器人需遵循的轨迹及目标点；将该轨迹及目标点发送到导航控制器；

所述步骤①中switch模块向导航控制器发送目标点具体是指：switch模块根据当前轨迹及目标点设置，向导航控制器发送目标，同时导航控制器读取建图模块映射；所述导航控制器会读取当前里程计数据信息，并将速度信息发送到爬壁机器人自身的基本控制器，将接收到的速度信息转化为电机命令发送给爬壁机器人底层控制单元；

②爬壁机器人的基本控制器接收到导航控制器的信息后，作为逆运动学计算模块，它会根据接收到的速度信息得出每个电机的速度，然后将其发送给爬壁机器人底层控制单元；

③爬壁机器人底层控制单元连接了车轮编码器模块、imu模块以及相机传感器模块，它会从爬壁机器人的加速度计、陀螺仪、车轮编码器、相机传感器收集数据并以节点信息的方式将其发布，由扩展卡尔曼滤波器接收相应的节点信息，对爬壁机器人的位置数据、速度数据、姿态数据进行滤波处理，获得对爬壁机器人位姿的一致性解释与描述，输出更好的爬壁机器人位姿估计结果，得到爬壁机器人准确的定位数据信息，从而完成爬壁机器人指定导航任务。

本发明优越性：通过跟踪球罐表面焊缝相对于机器人的运动来改善机器人定位精度；统一坐标系，保证机器人在球罐表面运行时得到正确的里程计测量数据；通过增量计算相邻时刻检测信号的运动来估计机器人的相对位置；利用多传感器数据融合技术，纠正由于航位推算产生的累积误差，以改善机器人定位；克服单个传感器的不确定性和局限性，纠正累积误差，以此提高系统的可靠性和鲁棒性，最终得出比单一传感器测量值更为精确的结果，从而提高爬壁机器人定位导航精度；该系统结构简单，方法容易实现，解决了由于传感器受限而导致的球形储罐内爬壁机器人定位导航困难问题。

(四)附图说明：

图1为本发明所涉一种大型球形储罐爬壁机器人定位导航系统的整体结构框图。

图2为本发明所涉一种大型球形储罐爬壁机器人定位导航方法的流程示意图。

图3为本发明所涉一种大型球形储罐爬壁机器人定位导航方法中相机检测焊缝示意图(其中，箭头指向焊缝)。

图4为本发明所涉一种大型球形储罐爬壁机器人定位导航方法中图像二值化处理的原理示意图。

图5为本发明所涉一种大型球形储罐爬壁机器人定位导航方法中ccd相机传感器已处理信号及兴趣区域(其中，图5-a为前一时刻信号，图5-b为当前时刻信号)。

图6为本发明所涉一种大型球形储罐爬壁机器人定位导航方法中的爬壁机器人根据焊缝的相对运动而进行的运动估计的示意图。

图7为本发明所涉一种大型球形储罐爬壁机器人定位导航方法中坐标变换系统坐标系位置关系示意图。

(五)具体实施方式：

实施例：一种大型球形储罐爬壁机器人定位导航系统，如图1所示，其特征在于它包括上位机、扩展卡尔曼滤波器、导航控制器、车轮编码器、惯性测量单元、imu处理单元、相机传感器和爬壁机器人底层控制单元；其中，所述导航控制器的输入端接收爬壁机器人的期望姿态设置值信号以及扩展卡尔曼滤波器的输出信号，其输出端输出定位导航信息给爬壁机器人的爬壁机器人底层控制单元；所述惯性测量单元的输入端连接爬壁机器人底层控制单元，用于测量爬壁机器人惯性数据，并输出给imu处理单元；所述扩展卡尔曼滤波器的输入端分别接收车轮编码器、相机传感器及imu处理单元的输出信号，其输出端与导航控制器的输入端连接；所述车轮编码器用于测量爬壁机器人的编码器脉冲以及车轮速度反馈信息，其输出端连接扩展卡尔曼滤波器；所述相机传感器用于采集罐体表面图像信息，其输出端连接扩展卡尔曼滤波器；所述爬壁机器人底层控制单元和上位机通过串口方式进行连接通讯。

所述上位机为pc机，采用ros操作系统，系统版本为ubuntu14.04；

所述扩展卡尔曼滤波器带有6d模型信息，包含在上位机linux系统的robot_pose_ekf功能包中，主要用于linux系统下对机器人的3d位姿进行评估；所述robot_pose_ekf是linux系统下用于评估机器人3d位姿的数据融合功能包；所述扩展卡尔曼滤波器接收来自车轮编码器、惯性测量单元及相机传感器所测爬壁机器人位置数据、速度数据、姿态数据，并进行分析融合校正，获得爬壁机器人准确的定位数据信息，用松耦合方式对各个传感器数据进行融合以实现机器人的位姿估计。

所述导航控制器是上位机linux系统下的ros导航堆栈，主要用于爬壁机器人完成自主导航任务。

所述车轮编码器用于测量编码器脉冲以及车轮速度反馈，其输出信号输入到扩展卡尔曼滤波器，在上位机上发布话题消息，给定爬壁机器人的每个车轮的直径和脉冲数，便于计算机器人在给定时间内移动的距离。

所述imu处理单元用于根据接收到的惯性数据计算得到爬壁机器人的位置数据、速度数据和姿态数据。

所述相机传感器是ccd相机传感器，数量不少于2个，用于采集罐体表面图像信息，通过实时对图像进行二值化处理，突出焊缝特征区域，以实现对焊缝的识别和跟踪；所述两个ccd相机传感器水平向下放置于机器人左右两侧，且与机器人平行，左右相机各自采集球罐表面的图像信息，如图1所示。

所述相机传感器上配备有辅助光源，便于安装在机器人两侧的ccd相机进行图像采集。

所述导航控制器用于根据得到的定位数据信息来完成相应的爬壁机器人自主导航任务。

一种大型球形储罐爬壁机器人定位导航方法，如图1、图2所示，其特征在于它包括以下步骤：

(1)利用车轮编码器获取爬壁机器人的里程计数据；

(2)通过坐标系统变换，将不同坐标系下的定位信息统一转换到同一坐标系下，保证里程计得到正确的测量数据，如图7所示；

传统的通过里程计定位方法使机器人局限于平面下的作业环境，导致机器人里程计无法在球罐这种曲面环境中得到正确的位置信息；机器人借助先前已知位置进行位姿递推定位是基于全局坐标系下进行的，因此，要想得到机器人在球罐表面运行过程中的位姿信息，需要进行坐标系统的转换，即按照一定的转换方法将机器人在球面的位置数据统一转换到空间平面直角坐标系中，如图7所示；

所述步骤(2)中将不同坐标系下的定位信息统一转换到同一坐标系具体是指以下内容：

如图7所示，其中，记x、y、z为空间直角坐标系的三个坐标轴，规定为球罐环境下的全局坐标系，为机器人坐标系，机器人坐标系相对于全局坐标系的角度关系即为爬壁机器人的姿态角，记球罐上机器人的任一位置p，其高度角为α，方位角为β，球罐半径为ρ，区别于平面环境，机器人坐标系需转换到全局坐标系下才可得到机器人正确的里程计测量数据，其中，坐标系可以看作是由坐标系经旋转平移变化而来；记：

p＝[xg,yg,zg]^t

p'＝[xr,yr,zr]^t

则有：p'＝rp+t；

其中r为旋转矩阵，t为平移矩阵，显然，t＝[0,0,-ρ]；

对旋转矩阵r的求导：

对于坐标系绕任意轴旋转存在以下转换关系：

绕x轴旋转α角如式(1)所示：

绕y轴旋转β角如式(2)所示：

绕z轴旋转γ角如式(3)所示：

将坐标系先绕z轴旋转β角，再绕x轴旋转角度，再经过一个平移向量t，即可得到坐标系将其写成如式(4)所示的矩阵形式：

p'＝rx(rzp)＝(rxrz)p(4)

将式(1)、式(2)和式(3)分别带入式(4)，则有：

整理可得到坐标系到坐标系的转换关系，如式(6)所示：

同理可得坐标系到坐标系的转换关系如式(7)所示：

其中，坐标系与间的转换旨在得到机器人正确的位姿信息，以保证里程计测量数据的准确性，同时也是为了根据转换关系在上位机发布正确的里程计脚本节点信息；

(3)利用惯性测量单元获取爬壁机器人的惯性数据，通过对所得惯性数据进行一次积分得到爬壁机器人的位姿数据；

(4)利用安装在机器人两侧的ccd相机进行图像采集，通过设置阈值，将高于阈值的像素灰度值置为255，将低于阈值的像素灰度值置为0，对采集到的图像进行实时二值化处理，将图像转换为二值图，图像上灰度值为255的白色区域则代表焊缝特征区域，如图4所示；

所述步骤(4)中的阈值设置为158～162，保证明显的突出焊缝特征区域。本实施例经过调试得出的一个最优的值选取160。

(5)利用步骤(4)即可识别出焊缝特征区域，并输出相应的检测信号，把二值化处理的图像上的白色区域看作兴趣点，代表焊缝特征区域，以此，可以将机器人相对于焊缝的运动用兴趣点的运动代替，兴趣点周围的区域称为兴趣区域，如图5中的虚线框部分，这些区域代表着整个范围内的焊缝相机检测；根据当前数据与先前数据之间的差异进行增量计算即可得出机器人相对于焊缝的运动；例如检测到焊缝在一个方向上移动了10厘米，则意味着机器人在其反方向上移动了10厘米，如图6所示，δx1、δx2即为相邻时刻的焊缝位移增量；

(6)在得到机器人相对于焊缝运动的增量信号后利用均值滤波器和低通滤波器进行增量信号滤波以避免噪声的干扰，通过对结果进行二次积分即可获取机器人的相对位置值；

其中，均值滤波器计算公式如式(8)所示：

当p为负数时，可有效滤除白噪声，是正的异常值；p为正数时，可对黑噪声进行过滤，是负的异常值；

低通滤波按公式(9)实现：

y(k)＝α·x(k)+(1-α)·y(k-1)(9)

其中，x(k)为本次采样值；y(k-1)为上次的滤波输出值；α为滤波系数，其值一般远小于1；y(k)为本次滤波输出值；

由公式(9)可以看出，本次滤波的输出值主要取决于上次滤波输出值，且本次采样值对滤波输出影响较小；

(7)由扩展卡尔曼滤波器接收来自车轮编码器、惯性测量单元以及ccd相机传感器的数据信息，利用robot_pose_ekf对三个数据源进行数据融合处理，获得对爬壁机器人位姿的一致性解释与描述，输出爬壁机器人位姿估计结果，以改善机器人定位；

所述步骤(7)中的robot_pose_ekf是linux系统下用于评估机器人3d位姿的数据融合功能包；它使用了来自不同传感器源的位姿测量信息，其基本思路是通过松耦合方式对不同传感器数据进行融合来实现机器人位姿估计；robot_pose_ekf默认监听的话题为：imu_data、odom、vo，其中imu_data为来自imu的爬壁机器人位姿数据，odom为来自里程计，即车轮编码器，的位姿数据，vo为来自ccd相机传感器所测的位姿数据；因此，在发布里程计、imu、ccd相机传感器消息时要注意与话题的名称相对应，否则起不到滤波作用，robot_pose_ekf中的扩展卡尔曼滤波器作为概率滤波器运行，根据每个传感器输入附有的协方差计算给定数据源的有效性，并为该数据源指定权重，输出更好的估计结果。

(8)根据步骤(7)得到的爬壁机器人定位数据信息，将其输入到导航控制器，驱动爬壁机器人完成指定的检测任务。

所述步骤(8)中导航控制器是上位机linux系统下ros导航堆栈组成，如图2所示，其具体工作方法包括以下步骤：

①由switch模块读取一个文件，其中，所述文件的内容包含了爬壁机器人需遵循的轨迹及目标点；将该轨迹及目标点发送到导航控制器；

所述步骤①中switch模块向导航控制器发送目标点具体是指：switch模块根据当前轨迹及目标点设置，向导航控制器发送目标，同时导航控制器读取建图模块映射；所述导航控制器会读取当前里程计数据信息，并将速度信息发送到爬壁机器人自身的基本控制器，将接收到的速度信息转化为电机命令发送给爬壁机器人底层控制单元；

②爬壁机器人的基本控制器接收到导航控制器的信息后，作为逆运动学计算模块，它会根据接收到的速度信息得出每个电机的速度，然后将其发送给爬壁机器人底层控制单元；

③爬壁机器人底层控制单元连接了车轮编码器模块、imu模块以及相机传感器模块，它会从爬壁机器人的加速度计、陀螺仪、车轮编码器、相机传感器收集数据并以节点信息的方式将其发布，由扩展卡尔曼滤波器接收相应的节点信息，对爬壁机器人的位置数据、速度数据、姿态数据进行滤波处理，获得对爬壁机器人位姿的一致性解释与描述，输出更好的爬壁机器人位姿估计结果，得到爬壁机器人准确的定位数据信息，从而完成爬壁机器人指定导航任务。

下面结合附图对本发明的技术方案做进一步的详细说明：

图3为爬壁机器人焊缝跟踪定位简单工作示意图，两个ccd相机传感器水平向下放置于机器人左右两侧，且与机器人平行，左右相机会各自采集球罐表面的图像信息。

图4为通过设置阈值(160/255)，实时对采集到的图像进行二值化处理，将高于阈值的像素灰度值置为255，将低于阈值的像素灰度值置为0，将图像转换为二值图，图像上灰度值为255的白色区域作为兴趣点，代表焊缝特征区域。

图5为当系统检测到代表焊缝特征的兴趣点时，输出相应的检测信号，将机器人相对于焊缝的运动用兴趣点的运动代替，兴趣点周围的区域称为兴趣区域，如图中的虚线框部分，这些区域代表着整个范围内的相机焊缝检测。

图6为爬壁机器人根据焊缝的相对运动而进行的运动估计，例如检测到焊缝在一个方向上移动了10厘米，则意味着机器人在其反方向上移动了10厘米，图中δx1、δx2即为相邻时刻的焊缝位移增量。