CN115453599A - 一种多传感器协同的管道机器人精准定位方法 - Google Patents

Abstract

本发明公开了一种多传感器协同的管道机器人精准定位方法，该方法利用惯性传感器IMU、轮式编码器实时获取机器人的加速度、角速度和局部轮速信息，通过卡尔曼滤波将IMU的加速度计输出作为观测值更新角速度信息，融合角速度和轮速信息的预积分实现对管道机器人的初步运动估计，接着标定相机与IMU的外参，并对相机拍摄的每一帧图片进行ORB特征提取和匹配，以紧耦合的方式融合相机、IMU和轮速信息，利用非线性优化方法得到精度更高的机器人位姿信息，最后融合北斗定位传感器返回的三维位置信息进行优化，获取局部准确和全局无漂移的机器人位姿信息，实现管道机器人的精准定位。

Description

一种多传感器协同的管道机器人精准定位方法

技术领域

本发明涉及一种多传感器协同的管道机器人精准定位方法，属于计算机视觉领域。

背景技术

管道作为一种重要的物料运输手段被广泛应用在天然气、石油、化工、核设施、军事装备等领域，然而由于环境因素和人为因素的影响，管道会因重压、腐蚀、磨损等原因出现裂纹、泄漏等现象。因此为防止此类事故的发生并提高管道的使用寿命，需要定期对管道进行检测和维护。此外，大部分管道所处的环境无法被维修人员直接观测和接触，传统的挖掘法检测不仅耗费大量的人力，还易存在安全隐患。因此，现如今大多采用管道机器人完成管道检测，并对损坏和缺陷位置进行定位。

管道机器人的定位有利于工作人员实时了解管道机器人的当前位置并对管道损坏处进行精准定位和维修。现有的管道机器人定位方法大都采用相机对管道内部进行拍摄，利用视觉里程计估算方法定位机器人。但在管道内部，由于环境的重复性较高以及光照不均匀，难以利用相机传感器获取准确的机器人里程计数据。为了提高管道机器人状态估计的准确性和鲁棒性，越来越多的定位方法依赖于来自多个互补传感器的数据。

相机传感器和IMU在位姿估计方面具有明显的互补性，IMU为快速运动提供了较好的位姿估计，相机传感器能在慢速运动中修正IMU数据的漂移。因此视觉与IMU的联合定位方法被应用在了管道机器人定位上。然而当管道机器人在匀速、或沿着弧线运动等情况下，视觉与IMU的联合定位存在尺度未知的问题，并且随着时间的推移，位姿估计会因误差的累积而发生漂移。

基于此，需要一种多传感器协同的管道机器人精准定位方法，解决视觉与IMU联合定位中的尺度不可观察问题和累积漂移，实现不同传感器的优势互补，更加适合复杂多变的管道环境。

发明内容

本发明所要解决的技术问题是：提出一种多传感器协同的管道机器人精准定位方法，融合相机、IMU、轮速信息和北斗定位传感器，获取尺度可观察、局部准确和全局无漂移的管道机器人位姿信息，实现管道机器人的精准定位。

为解决上述技术问题，本发明提供一种多传感器协同的管道机器人精准定位方法，包括以下步骤：

1)利用IMU、轮式编码器、相机和北斗定位传感器实时获取管道机器人的加速度、角速度、局部轮速信息、机器人周围的环境信息和卫星定位信息；

2)利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，并融合角速度和轮速信息的预积分；

3)标定相机与IMU的外参，对相机拍摄的图像序列进行ORB特征点的检测和描述，匹配特征点对；

4)以紧耦合的方式融合步骤2)获得的角速度、轮速积分信息和步骤3)获得的匹配特征点对信息，通过非线性优化得到精度更高的机器人位姿信息；

5)根据北斗定位传感器返回的三维位置信息约束步骤4)中获得的机器人位姿信息，消除机器人位姿估计的累计误差，实现管道机器人的精准定位。

进一步的，在步骤1)中，利用IMU、轮式编码器、相机和北斗定位传感器实时获取管道机器人的加速度、角速度、局部轮速信息、机器人周围的环境信息和卫星定位信息，其具体步骤为：

以管道机器人为载体，机器人质心处安装IMU，机器人头部安装相机和北斗定位传感器，主动轮上搭载轮式编码器，根据主动轮半径r和编码轮返回的脉冲时间间隔d_t计算出管道机器人的运动速度V：

当管道机器人启动时，实时获取各传感器对应信息。

进一步的，在步骤2)中，利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，并融合角速度和轮速信息的预积分，其具体步骤为：

21)卡尔曼滤波消除角速度积分的累计误差；求出当前时刻t和下一时刻t+1管道机器人的角速度均值和加速度均值，作为IMU数据采集间隔Δt时间内的平均角速度和平均加速度；通过角速度积分获取机器人运动过程中的三维角度信息，由于噪声和积分导致的误差累计，利用卡尔曼滤波将加速度计输出作为观测值更新角度信息；以Y轴角速度Gyro_Y在采集间隔内积分得到的角度信息U(k)作为卡尔曼滤波器状态估计量：

俯仰角Y_Angle作为滤波器的观测值，该值由IMU的X轴和Z轴加速度ACCEL_X和ACCEL_Z决定：

Y_Angle＝arctan(ACCEL_X/ACCEL_Z)

建立t时刻和t-1时刻下俯仰角测量模型方程：

其中Y_bias表示当前角速度偏置，dt表示IMU数据更新频率间隔；通过俯仰角的观测值不断修正和平滑IMU获得更准确和稳定的角度值，X轴和Z轴角度更新同Y轴；

22)角速度和轮速信息的预积分融合；通过求得的轮速信息和t时刻、t+1时刻下的角速度w_t初步估计t+1时刻管道机器人的运动信息，利用平均角速度数据w_t′和t时刻的机器人姿态Q_t求得t+1时刻的姿态Q_t+1：

Q_t+1＝Q_t(w_t′Δt)

该姿态包含了管道机器人运动的三维方向信息，其中B_g表示角速度偏置，Δt表示两帧角速度数据之间的时间间隔，初始时刻的机器人姿态定义为单位矩阵，IMU的加速度数据a_t是在自身坐标系b下测得的，需要根据对应时刻机器人的姿态转换到世界坐标系w下：

a_t,w＝Q_t(a_t,b-B_a)-g

a_t+1,w＝Q_t+1(a_t+1,b-B_a)-g

其中B_a和g分别表示加速度偏置和重力加速度；

23)根据机器人的姿态、t时刻的位置P_t和轮速信息V求出管道机器人在当前时刻姿态下的速度V_t,w以及Δt时间内移动的距离P_t+1：

V_t,w＝Q_t(V)

P_t+1＝P_t+V_t,wΔt

获得管道机器人运动的方向和距离，实现管道机器人运动状态的初步估计。

进一步的，在步骤3)中，标定相机与IMU的外参，对相机拍摄的图像序列进行ORB特征点的检测和描述，匹配特征点对，其具体步骤为：

31)相机与IMU外参标定；在标定实验过程中需要估计的时不变参数为相对世界坐标系的重力方向g_w、相机与IMU测量数据间的时间偏移量d、相机与IMU的空间变换T_c,i，待估计的时变参数为6自由度的IMU位姿T_w,i、加速度和角速度的偏置b_a和b_w；下标w、c、i分别表示世界坐标系、相机坐标系和IMU坐标系，T表示各坐标系之间的空间变换，它们之间的关系为：

T_w,i＝T_w,cT_c,i

首先收集一组相机与IMU的测量数据，T＝[t₁,t_K]，相机传感器获取的图片时间戳设置为t_j，其中每个角点的像素坐标为y_mj，m为单帧图片中角点个数；由于标定板的结构已知，每一个角点对应的世界坐标

根据标定板的实际尺寸大小获得，因此角点的像素坐标和在时间t_k处采样得到的加速度和角速度的测量值α_k、

定义为：

α_k＝(T_w,i)_r(a(t_k)-g_w)+b_a(t_k)+n_ak

其中(T_w,i)_r表示IMU位姿的旋转分量，a(t_k)和w(t_k)表示IMU运动过程中产生的加速度和角速度，h表示角点的观测模型，每一个n和加速度、角速度的偏置都假定服从均值为0方差为R的高斯分布并统计独立；接着利用加速度的二次积分和角速度的一次积分获取IMU相邻测量数据帧之间的位姿估计，并结合前一帧的IMU相对于世界坐标系的位姿关系得到当前帧IMU相对于世界坐标系的位姿估计；最后与测量相关的误差项被构造为测量与给定当前状态估计的预测测量之间的差异，待估计变量的误差函数e定义为：

其中下标表示对应变量的误差函数；利用Levenberg-Marquardt算法最小化所有误差项的累加和，通过迭代方式找到所有未知参数的最大似然估计；在迭代过程中，时间偏移量初始化为0，相机与IMU的初始相对位姿根据硬件安装方向设置；

32)ORB特征点提取；ORB特征点由关键点和描述子组成，首先在图像中选择像素点p，已知其亮度为I_p，人为设置一个阈值T；接着以像素点p为中心，半径为3的圆上的16个像素点被选取，然后判断是否连续有9个像素点的亮度大于I_p+T或者小于I_p-T，如果存在，则认为点p为特征点；利用输入图像构造图像金字塔，并在金字塔的每一层进行FAST角点检测来实现特征点尺度不变性；利用灰度质心法实现特征点的旋转不变性：在某个图像块B中定义图像块的矩：

其中I(x,y)为像素点亮度，(x,y)为像素点坐标，通过矩计算得到图像块的质心：

将图像块B的质心C与几何中心O相连，得到向量

则特征点的方向角θ定义为：

θ＝arctan(m₀₁/m₁₀)；

33)ORB描述子计算和特征匹配；使用二进制描述子BRIEF，描述向量由0和1组成，以检测到的ORB关键点p为中心，选取一个大小为d×d的像素区域，通过某一规则选出该区域内的n个像素点对，对每一个像素点对(I_i,I_j)进行如下判断：

将经过判断得到的二进制数据τ(p；i,j)写成N维向量的形式，得到ORB描述子；以描述子之间的汉明距离为度量，利用快速近似最近邻算法进行特征点匹配，并采用渐进采样一致性算法对特征点进行提纯与优化，获取图像之间的准确特征匹配点对。

进一步的，在步骤4)中，以紧耦合的方式融合步骤2)获得的角速度、轮速积分信息和步骤3)获得的匹配特征点对信息，通过非线性优化得到精度更高的机器人位姿信息，其具体步骤为：

41)构建管道机器人的运动状态估计模型；根据传感器融合理论，以紧耦合的方式融合角速度、轮速积分信息和匹配特征点对信息，所组成的动态模型为：

其中x_k表示机器人状态，包括旋转矩阵R和平移矩阵t，y_k表示模型的观测量，即匹配得到的特征点像素坐标，u_k-1表示模型输入，w_k-1表示随机过程噪声，v_k表示随机观测噪声，均假设为均值为0，方差为R_k和Q_k的高斯白噪声：

w_k～N(0,R_k),v_k～N(0,Q_k)

通过带噪声的观测量以及模型的输入推断出待估计的管道机器人的运动状态，即求在已知的传感器输入数据和观测数据的条件下机器人状态的最大后验概率P(x_k|u_k-1,y_k)，根据贝叶斯法则得到：

将机器人状态的最大后验概率问题转换为求解机器人状态x_k的最大似然估计：

x^* _MLE＝argm ax(P(u_k-1,y_k|x_k))；

42)机器人状态优化过程中相机投影模型和目标函数的构建；为了刻画机器人状态优化过程中相机的投影模型，用像素坐标系、图像坐标系、相机坐标系来描述如何将相机坐标系下的物体投影到像素坐标系上，其中O_c为相机光心，f为相机焦距，现实空间中相机坐标系下的点P经过光心O投影到物理成像平面上，在图像坐标系上得到点P′，设点P的坐标为[X,Y,Z]，点P′的坐标为[X′,Y′]，Z表示点P到相机平面X-Y的距离，即深度；根据相机投影模型和相似三角形得出：

通过采样和量化将图像坐标系上的点转换到像素坐标系上，图像坐标系和像素坐标系间的转换经过了一个缩放变换和平移变换，像素坐标系上u轴坐标相对图像坐标系缩放了α倍，v轴坐标相对图像坐标系缩放了β倍，原点o′相对图像坐标系平移了[c_x,c_y]；因此图像坐标系下的点P′和像素坐标[u,v]的关系为：

将αf合并成f_x，βf合并成f_y，得到像素坐标系与相机坐标系的转换关系：

其中K表示相机内参矩阵，通过相机标定实验获得；假设有n对匹配好的特征点(p₁,p₁′),(p₂,p₂′)...(p_n,p_n′)，并已获得了特征点对应的深度值，需要求得相邻图像帧间的旋转矩阵R和平移矩阵t，其中包含了相机与IMU的空间关系以及角速度和轮速信息求解得到的机器人运动状态，定义误差项e_i为：

e_i＝p_i-(Rp_i′+t)

构建误差平方和损失函数作为机器人状态优化过程中的目标函数F：

43)非线性优化求解机器人位姿；目标函数定义为总体意义下的最小二乘问题，利用非线性优化方法和匹配特征点对求得使误差平方和达到极小的R和t；采用Levenberg-Marquardt算法求解，将误差函数进行泰勒展开：

e(x+Δx)＝e(x)+J(x)Δx

其中Δx表示迭代优化过程中代求变量的下降矢量，J为误差函数对代求变量的导数，则目标函数的最小化问题转化为求解使目标函数最小的Δx^*：

令目标函数对Δx的导数为0，求解使目标函数最小的矢量Δx，得到矢量方程：

J(x)^TJ(x)Δx＝-J(x)^Te(x)

机器人状态初值设为单位矩阵，求解每一次迭代过程中的误差和下降矢量，若Δx足够小，则停止迭代，令x_k+1＝x_k+Δx。

进一步的，在步骤5)中，根据北斗定位传感器返回的三维位置信息约束步骤4)中获得的机器人位姿信息，消除机器人位姿估计的累计误差，实现管道机器人的精准定位，其具体步骤为：

51)位姿图优化；步骤4)中经过非线性优化得到的管道机器人状态未考虑之前时刻机器人的位姿约束，位姿之间的相对运动与真实值存在偏差，利用位姿图优化再次修正管道机器人的位姿估计；位姿图由节点和边组成，节点代表管道机器人的位姿信息，两个位姿节点T_i和T_j之间的相对运动ΔT_ij表示位姿图的边，位姿之间的约束关系为ΔT_ij＝T_i ^-1T_j，优化变量为各个顶点的位姿，边来自于位姿观测约束，构建最小二乘误差e_ij：

e_ij＝ln(ΔT_ij ^-1T_i ^-1T_j)^∨

其中∨表示矩阵转换为向量的运算，优化变量为T_i和T_j，误差对优化变量的扰动项δξ导数为：

其中ξ表示特殊欧式群中位姿矩阵对应的李代数，ρ和

表示旋转分量和平移分量，各变量之间的对应关系为：

其中^表示向量到矩阵的运算，I为单位阵；根据构造的误差函数和误差对位姿的导数，利用Levenberg-Marquardt算法获取经过位姿图优化后的位姿信息；

52)融合北斗定位信息，消除机器人位姿估计的累计误差；融合相机、IMU和轮式编码器信息并经过管道机器人的位姿估计会随着机器人轨迹的变长而发生漂移，产生累积误差，根据北斗定位传感器返回的经度lati、纬度longi和高度信息alti将其转换为以地球为参考系的全局三维坐标全局三维坐标(X_g,Y_g,Z_g)：

其中L表示地球半径，将全局三维坐标作为约束条件，结合已获取的管道机器人位姿信息进行全局优化，建立整体误差模型：

其中k表示某一时刻，将机器人初始的三维位置设为北斗定位设备返回的三维坐标，经过位姿图优化后的旋转矩阵R和平移矩阵t作为全局优化初值，利用Levenberg-Marquardt算法迭代优化获取局部准确和全局无漂移的机器人位姿信息，实现管道机器人的精准定位。

与现有技术相比，本发明的有益效果是：

(1)本发明提出利用IMU、轮式编码器、相机进行协同处理获取精度较高的管道机器人定位结果，结合了IMU为快速运动提供了较好的位姿估计，相机传感器能在慢速运动中修正IMU数据漂移的优点，解决了相机传感器在纹理弱的管道环境内难以进行有效位姿估计的问题，并利用轮式编码器返回的速度信息解决视觉与IMU联合定位存在尺度未知的问题。

(2)本发明首先利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，减小角速度测量噪声和积分导致的误差，再融合角速度和轮速信息的预积分，使得后续的机器人位姿估计更加简单，结果更加准确。

(3)本发明在IMU、轮式编码器、相机传感器协同估计机器人位姿的基础上加入了位姿图优化算法，考虑全局的机器人位姿约束，利用位姿图优化再次修正管道机器人的位姿估计。

(4)由于管道机器人的位姿估计存在累积误差，随着时间的推移，管道机器人的定位结果会发生漂移。本发明根据北斗定位传感器返回的三维位置信息约束IMU、轮式编码器、相机传感器协同求得的机器人位姿，消除机器人位姿估计的累计误差，能够实现管道机器人的精准定位。

附图说明

图1为本发明方法流程图；

图2为相机与IMU、标定板的空间关系；

图3为IMU、视觉、轮速信息融合估计机器人位姿框架图；

图4为相机投影模型示意图；

图5为全局位姿图优化算法示意图。

具体实施方式

下面结合附图对本发明多传感器协同的管道机器人精准定位方法的具体实施方式做详细阐述。

本发明的多传感器协同的管道机器人精准定位方法运行流程如图1所示，具体步骤包括：

1)利用IMU、轮式编码器、相机和北斗定位传感器实时获取管道机器人的加速度、角速度、局部轮速信息、机器人周围的环境信息和卫星定位信息，以管道机器人为载体，机器人质心处安装IMU，机器人头部安装相机和北斗定位传感器，相机采集的图像分辨率设置为1280×960，主动轮上搭载轮式编码器，根据主动轮半径r和编码轮返回的脉冲时间间隔d_t计算出管道机器人的运动速度V：

当管道机器人启动时，实时获取各传感器对应信息。

2)利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，并融合角速度和轮速信息的预积分。

21)卡尔曼滤波消除角速度积分的累计误差。求出当前时刻t和下一时刻t+1管道机器人的角速度均值和加速度均值，作为IMU数据采集间隔Δt时间内的平均角速度和平均加速度。通过角速度积分获取机器人运动过程中的三维角度信息，由于噪声和积分导致的误差累计，利用卡尔曼滤波将加速度计输出作为观测值更新角度信息。以Y轴角速度Gyro_Y在采集间隔内积分得到的角度信息U(k)作为卡尔曼滤波器状态估计量：

俯仰角Y_Angle作为滤波器的观测值，该值由IMU的X轴和Z轴加速度ACCEL_X和ACCEL_Z决定：

Y_Angle＝arctan(ACCEL_X/ACCEL_Z) (3)

建立t时刻和t-1时刻下俯仰角测量模型方程：

其中Y_bias表示当前角速度偏置，dt表示IMU数据更新频率间隔。通过俯仰角的观测值不断修正和平滑IMU获得更准确和稳定的角度值。X轴和Z轴角度更新同Y轴。

22)角速度和轮速信息的预积分融合。通过求得的轮速信息和t时刻、t+1时刻下的角速度w_t初步估计t+1时刻管道机器人的运动信息。利用平均角速度数据w_t′和t时刻的机器人姿态Q_t求得t+1时刻的姿态Q_t+1：

Q_t+1＝Q_t(w_t′Δt) (5)

该姿态包含了管道机器人运动的三维方向信息，其中B_g表示角速度偏置，Δt表示两帧角速度数据之间的时间间隔。初始时刻的机器人姿态定义为单位矩阵。IMU的加速度数据a_t是在自身坐标系b下测得的，因此需要根据对应时刻机器人的姿态转换到世界坐标系w下：

a_t,w＝Q_t(a_t,b-B_a)-g (7)

a_t+1,w＝Q_t+1(a_t+1,b-B_a)-g (8)

其中B_a和g分别表示加速度偏置和重力加速度。

23)根据机器人的姿态、t时刻的位置P_t和轮速信息V求出管道机器人在当前时刻姿态下的速度V_t,w以及Δt时间内移动的距离P_t+1：

V_t,w＝Q_t(V) (9)

P_t+1＝P_t+V_t,wΔt (10)

获得管道机器人运动的方向和距离，实现管道机器人运动状态的初步估计。

3)标定相机与IMU的外参，对相机拍摄的图像序列进行ORB特征点的检测和特征点的描述，匹配特征点对。

31)相机与IMU外参标定。在标定实验过程中需要估计的时不变参数为相对世界坐标系的重力方向g_w、相机与IMU测量数据间的时间偏移量d、相机与IMU的空间变换T_c,i，待估计的时变参数为6自由度的IMU位姿T_w,i、加速度和角速度的偏置b_a和b_w。如图2所示，显示了标定实验中各坐标系的空间关系，其中F_w、F_c、F_i分别表示世界坐标系、相机坐标系和IMU坐标系。T表示各坐标系之间的空间变换，它们之间的关系为：

T_w,i＝T_w,cT_c,i (11)

首先在短时间内收集一组相机与IMU的测量数据，T＝[t₁,t_K](通常为1-2分钟)，相机传感器获取的图片时间戳设置为t_j，其中每个角点的像素坐标为y_mj，m为单帧图片中角点个数。由于标定板的结构已知，每一个角点对应的世界坐标

可根据标定板的实际尺寸大小获得。因此角点的像素坐标和在时间t_k处采样得到的加速度和角速度的测量值α_k、

可定义为：

其中(T_w,i)_r表示IMU位姿的旋转分量，a(t_k)和w(t_k)表示IMU运动过程中产生的加速度和角速度，h表示角点的观测模型。每一个n和加速度、角速度的偏置都假定服从均值为0方差为R的高斯分布并统计独立。接着利用加速度的二次积分和角速度的一次积分获取IMU相邻测量数据帧之间的位姿估计，并结合前一帧的IMU相对于世界坐标系的位姿关系得到当前帧IMU相对于世界坐标系的位姿估计。最后与测量相关的误差项被构造为测量与给定当前状态估计的预测测量之间的差异，待估计变量的误差函数e定义为：

其中下标表示对应变量的误差函数。利用Levenberg-Marquardt(LM)算法最小化所有误差项的累加和，通过迭代方式找到所有未知参数的最大似然估计。在迭代过程中，时间偏移量初始化为0，相机与IMU的初始相对位姿可根据硬件安装方向设置。

32)ORB特征点提取。ORB特征点由关键点和描述子组成。首先在图像中选择像素点p，已知其亮度为I_p，人为设置一个阈值T。接着以像素点p为中心，半径为3的圆上的16个像素点被选取，然后判断是否连续有9个像素点的亮度大于I_p+T或者小于I_p-T，如果存在，则认为点p为特征点。利用输入图像构造图像金字塔，并在金字塔的每一层进行FAST角点检测来实现特征点尺度不变性。利用灰度质心法实现特征点的旋转不变性。该方法首先在某个图像块B中定义图像块的矩：

其中I(x,y)为像素点亮度，(x,y)为像素点坐标。通过矩计算得到图像块的质心：

将图像块B的质心C与几何中心O相连，得到向量

则特征点的方向定义为：

θ＝arctan(m₀₁/m₁₀) (16)

33)ORB描述子计算和特征匹配。使用二进制描述子BRIEF，描述向量由0和1组成。以检测到的ORB关键点p为中心，选取一个大小为d×d的像素区域，接着通过某一规则选出该区域内的n个像素点对，对每一个像素点对(I_i,I_j)进行如下判断：

将经过判断得到的二进制数据τ(p；i,j)写成N维向量的形式，得到ORB描述子。接着根以描述子之间的汉明距离为度量，利用快速近似最近邻算法进行特征点匹配，并采用渐进采样一致性(PRSAC)算法对特征点进行提纯与优化，获取图像之间的准确特征匹配点对。

4)以紧耦合的方式融合上述步骤获得的角速度、轮速积分信息和匹配特征点对信息，通过非线性优化得到精度更高的机器人位姿信息。

41)构建管道机器人的运动状态估计模型。视觉、IMU、轮速信息的融合框架如图3所示。根据传感器融合理论，以紧耦合的方式融合角速度、轮速积分信息和匹配特征点对信息，所组成的动态模型为：

其中x_k表示机器人状态，包括旋转矩阵R和平移矩阵t，y_k表示模型的观测量，即匹配得到的特征点像素坐标，u_k-1表示模型输入，w_k-1表示随机过程噪声，v_k表示随机观测噪声，均假设为均值为0、方差为R_k和Q_k的高斯白噪声：

w_k～N(0,R_k),v_k～N(0,Q_k) (19)

接着通过带噪声的观测量以及模型的输入推断出待估计的管道机器人的运动状态，即求在已知的传感器输入数据和观测数据的条件下机器人状态的最大后验概率P(x_k|u_k-1,y_k)，根据贝叶斯法则可得到：

将机器人状态的最大后验概率问题转换为求解机器人状态x_k的最大似然估计：

x^* _MLE＝argm ax(P(u_k-1,y_k|x_k)) (21)

42)机器人状态优化过程中相机投影模型和目标函数的构建。为了刻画机器人状态优化过程中相机的投影模型，一般用像素坐标系、图像坐标系、相机坐标系来描述如何将相机坐标系下的物体投影到像素坐标系上。如图4所示，为相机投影成像过程。其中O_c为相机光心，f为相机焦距，现实空间中相机坐标系下的点P经过光心O投影到物理成像平面上，在图像坐标系上得到点P′。设点P的坐标为[X,Y,Z],点P′的坐标为[X′,Y′]，Z表示点P到相机平面X-Y的距离，即深度。根据相机投影模型和相似三角形得出：

通过采样和量化将图像坐标系上的点转换到像素坐标系上。图像坐标系和像素坐标系间的转换经过了一个缩放变换和平移变换。像素坐标系上u轴坐标相对图像坐标系缩放了α倍，v轴坐标相对图像坐标系缩放了β倍，原点o′相对图像坐标系平移了[c_x,c_y]。因此图像坐标系下的点P′和像素坐标[u,v]的关系为：

代入式(22)和(23)并将αf合并成f_x，βf合并成f_y，得到像素坐标系与相机坐标系的转换关系：

其中K表示相机内参矩阵，可以通过相机标定实验获得。假设有n对匹配好的特征点(p₁,p₁′),(p₂,p₂′)...(p_n,p_n′)，并已获得了特征点对应的深度值，需要求得相邻图像帧间的旋转矩阵R和平移矩阵t，其中包含了相机与IMU的空间关系以及角速度和轮速信息求解得到的机器人运动状态，定义误差项e_i为：

e_i＝p_i-(Rp_i′+t) (25)

接着构建误差平方和损失函数作为机器人状态优化过程中的目标函数：

43)非线性优化求解机器人位姿。上述目标函数可定义为总体意义下的最小二乘问题，利用非线性优化方法和匹配特征点对求得使误差平方和达到极小的R和t。采用Levenberg-Marquardt算法求解该问题，将误差函数进行泰勒展开：

e(x+Δx)＝e(x)+J(x)Δx (27)

其中Δx表示迭代优化过程中代求变量的下降矢量，J为误差函数对代求变量的导数。则目标函数的最小化问题转化为求解使目标函数最小的Δx^*：

令目标函数对Δx的导数为0，求解使目标函数最小的矢量Δx，得到矢量方程：

J(x)^TJ(x)Δx＝-J(x)^Te(x) (29)

机器人状态初值设为单位矩阵，求解每一次迭代过程中的误差和下降矢量，若Δx足够小，则停止迭代，令x_k+1＝x_k+Δx。

5)根据北斗定位传感器返回的三维位置信息约束上述步骤中获得的机器人位姿信息，消除机器人位姿估计的累计误差，实现管道机器人的精准定位。

51)位姿图优化。上述步骤中经过非线性优化得到的管道机器人状态未考虑之前时刻机器人的位姿约束，位姿之间的相对运动与真实值存在偏差，利用位姿图优化再次修正管道机器人的位姿估计。如图5所示，位姿图由节点和边组成，节点代表管道机器人的位姿信息，两个位姿节点T_i和T_j之间的相对运动ΔT_ij表示位姿图的边，位姿之间的约束关系为ΔT_ij＝T_i ^-1T_j，优化变量为各个顶点的位姿，边来自于位姿观测约束，构建最小二乘误差e_ij：

e_ij＝ln(ΔT_ij ^-1T_i ^-1T_j)^∨ (30)

其中∨表示矩阵转换为向量的运算，优化变量为T_i和T_j。误差对优化变量的扰动项δξ导数为：

其中ξ表示特殊欧式群中位姿矩阵对应的李代数，ρ和

表示旋转分量和平移分量，各变量之间的对应关系为：

其中^表示向量到矩阵的运算，I为单位阵。根据构造的误差函数和误差对位姿的导数，利用Levenberg-Marquardt算法获取经过位姿图优化后的位姿信息。

52)融合北斗定位信息，消除机器人位姿估计的累计误差。融合相机、IMU和轮式编码器信息并经过上述步骤得到的管道机器人的位姿估计会随着机器人轨迹的变长而发生漂移，产生累积误差。根据北斗定位传感器返回的经度lati、纬度longi和高度信息alti将其转换为以地球为参考系的全局三维坐标(X_g,Y_g,Z_g)：

其中L表示地球半径，将全局三维坐标作为约束条件，结合已获取的管道机器人位姿信息进行全局优化，建立整体误差模型：

其中k表示某一时刻。将机器人初始的三维位置设为北斗定位设备返回的三维坐标，经过位姿图优化后的旋转矩阵R和平移矩阵t作为全局优化初值，利用Levenberg-Marquardt算法迭代优化获取局部准确和全局无漂移的机器人位姿信息，实现管道机器人的精准定位。

最后应该说明的是，结合上述实施例仅说明本发明的技术方案而非对其限制。所属领域的普通技术人员应当理解到，本领域技术人员可以对本发明的具体实施方式进行修改或者等同替换，但这些修改或变更均在申请待批的权利要求保护范围之中。

Claims (6)

1.一种多传感器协同的管道机器人精准定位方法，其特征在于，包括以下步骤：

1)利用IMU、轮式编码器、相机和北斗定位传感器实时获取管道机器人的加速度、角速度、局部轮速信息、机器人周围的环境信息和卫星定位信息；

2)利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，并融合角速度和轮速信息的预积分；

3)标定相机与IMU的外参，对相机拍摄的图像序列进行ORB特征点的检测和描述，匹配特征点对；

4)以紧耦合的方式融合步骤2)获得的角速度、轮速积分信息和步骤3)获得的匹配特征点对信息，通过非线性优化得到精度更高的机器人位姿信息；

5)根据北斗定位传感器返回的三维位置信息约束步骤4)中获得的机器人位姿信息，消除机器人位姿估计的累计误差，实现管道机器人的精准定位。

2.根据权利要求1所述的多传感器协同的管道机器人精准定位方法，其特征在于，在步骤1)中，利用IMU、轮式编码器、相机和北斗定位传感器实时获取管道机器人的加速度、角速度、局部轮速信息、机器人周围的环境信息和卫星定位信息，其具体步骤为：

以管道机器人为载体，机器人质心处安装IMU，机器人头部安装相机和北斗定位传感器，主动轮上搭载轮式编码器，根据主动轮半径r和编码轮返回的脉冲时间间隔d_t计算出管道机器人的运动速度V：

当管道机器人启动时，实时获取各传感器对应信息。

3.根据权利要求1所述的多传感器协同的管道机器人精准定位方法，其特征在于，在步骤2)中，利用卡尔曼滤波算法将IMU的加速度计输出作为观测值更新角速度信息，并融合角速度和轮速信息的预积分，其具体步骤为：

21)卡尔曼滤波消除角速度积分的累计误差；求出当前时刻t和下一时刻t+1管道机器人的角速度均值和加速度均值，作为IMU数据采集间隔Δt时间内的平均角速度和平均加速度；通过角速度积分获取机器人运动过程中的三维角度信息，由于噪声和积分导致的误差累计，利用卡尔曼滤波将加速度计输出作为观测值更新角度信息；以Y轴角速度Gyro_Y在采集间隔内积分得到的角度信息U(k)作为卡尔曼滤波器状态估计量：

俯仰角Y_Angle作为滤波器的观测值，该值由IMU的X轴和Z轴加速度ACCEL_X和ACCEL_Z决定：

Y_Angle＝arctan(ACCEL_X/ACCEL_Z)

建立t时刻和t-1时刻下俯仰角测量模型方程：

其中Y_bias表示当前角速度偏置，dt表示IMU数据更新频率间隔；通过俯仰角的观测值不断修正和平滑IMU获得更准确和稳定的角度值，X轴和Z轴角度更新同Y轴；

22)角速度和轮速信息的预积分融合；通过求得的轮速信息和t时刻、t+1时刻下的角速度w_t初步估计t+1时刻管道机器人的运动信息，利用平均角速度数据w_t′和t时刻的机器人姿态Q_t求得t+1时刻的姿态Q_t+1：

Q_t+1＝Q_t(w_t′Δt)

该姿态包含了管道机器人运动的三维方向信息，其中B_g表示角速度偏置，Δt表示两帧角速度数据之间的时间间隔，初始时刻的机器人姿态定义为单位矩阵，IMU的加速度数据a_t是在自身坐标系b下测得的，需要根据对应时刻机器人的姿态转换到世界坐标系w下：

a_t,w＝Q_t(a_t,b-B_a)-g

a_t+1,w＝Q_t+1(a_t+1,b-B_a)-g

其中B_a和g分别表示加速度偏置和重力加速度；

23)根据机器人的姿态、t时刻的位置P_t和轮速信息V求出管道机器人在当前时刻姿态下的速度V_t,w以及Δt时间内移动的距离P_t+1：

V_t,w＝Q_t(V)

P_t+1＝P_t+V_t,wΔt

获得管道机器人运动的方向和距离，实现管道机器人运动状态的初步估计。

4.根据权利要求1所述的多传感器协同的管道机器人精准定位方法，其特征在于，在所述步骤3)中，标定相机与IMU的外参，对相机拍摄的图像序列进行ORB特征点的检测和描述，匹配特征点对，其具体步骤为：

31)相机与IMU外参标定；在标定实验过程中需要估计的时不变参数为相对世界坐标系的重力方向g_w、相机与IMU测量数据间的时间偏移量d、相机与IMU的空间变换T_c,i，待估计的时变参数为6自由度的IMU位姿T_w,i、加速度和角速度的偏置b_a和b_w；下标w、c、i分别表示世界坐标系、相机坐标系和IMU坐标系，T表示各坐标系之间的空间变换，它们之间的关系为：

T_w,i＝T_w,cT_c,i

首先收集一组相机与IMU的测量数据，T＝[t₁,t_K]，相机传感器获取的图片时间戳设置为t_j，其中每个角点的像素坐标为y_mj，m为单帧图片中角点个数；由于标定板的结构已知，每一个角点对应的世界坐标

根据标定板的实际尺寸大小获得，因此角点的像素坐标和在时间t_k处采样得到的加速度和角速度的测量值α_k、

定义为：

α_k＝(T_w,i)_r(a(t_k)-g_w)+b_a(t_k)+n_ak

其中(T_w,i)_r表示IMU位姿的旋转分量，a(t_k)和w(t_k)表示IMU运动过程中产生的加速度和角速度，h表示角点的观测模型，每一个n和加速度、角速度的偏置都假定服从均值为0方差为R的高斯分布并统计独立；接着利用加速度的二次积分和角速度的一次积分获取IMU相邻测量数据帧之间的位姿估计，并结合前一帧的IMU相对于世界坐标系的位姿关系得到当前帧IMU相对于世界坐标系的位姿估计；最后与测量相关的误差项被构造为测量与给定当前状态估计的预测测量之间的差异，待估计变量的误差函数e定义为：

其中下标表示对应变量的误差函数；利用Levenberg-Marquardt算法最小化所有误差项的累加和，通过迭代方式找到所有未知参数的最大似然估计；在迭代过程中，时间偏移量初始化为0，相机与IMU的初始相对位姿根据硬件安装方向设置；

32)ORB特征点提取；ORB特征点由关键点和描述子组成，首先在图像中选择像素点p，已知其亮度为I_p，人为设置一个阈值T；接着以像素点p为中心，半径为3的圆上的16个像素点被选取，然后判断是否连续有9个像素点的亮度大于I_p+T或者小于I_p-T，如果存在，则认为点p为特征点；利用输入图像构造图像金字塔，并在金字塔的每一层进行FAST角点检测来实现特征点尺度不变性；利用灰度质心法实现特征点的旋转不变性：在某个图像块B中定义图像块的矩：

其中I(x,y)为像素点亮度，(x,y)为像素点坐标，通过矩计算得到图像块的质心：

将图像块B的质心C与几何中心O相连，得到向量

则特征点的方向角θ定义为：

θ＝arctan(m₀₁/m₁₀)；

33)ORB描述子计算和特征匹配；使用二进制描述子BRIEF，描述向量由0和1组成，以检测到的ORB关键点p为中心，选取一个大小为d×d的像素区域，通过某一规则选出该区域内的n个像素点对，对每一个像素点对(I_i,I_j)进行如下判断：

将经过判断得到的二进制数据τ(p；i,j)写成N维向量的形式，得到ORB描述子；以描述子之间的汉明距离为度量，利用快速近似最近邻算法进行特征点匹配，并采用渐进采样一致性算法对特征点进行提纯与优化，获取图像之间的准确特征匹配点对。

5.根据权利要求1所述的多传感器协同的管道机器人精准定位方法，其特征在于，在所述步骤4)中，以紧耦合的方式融合步骤2)获得的角速度、轮速积分信息和步骤3)获得的匹配特征点对信息，通过非线性优化得到精度更高的机器人位姿信息，其具体步骤为：

41)构建管道机器人的运动状态估计模型；根据传感器融合理论，以紧耦合的方式融合角速度、轮速积分信息和匹配特征点对信息，所组成的动态模型为：

其中x_k表示机器人状态，包括旋转矩阵R和平移矩阵t，y_k表示模型的观测量，即匹配得到的特征点像素坐标，u_k-1表示模型输入，w_k-1表示随机过程噪声，v_k表示随机观测噪声，均假设为均值为0，方差为R_k和Q_k的高斯白噪声：

w_k～N(0,R_k),v_k～N(0,Q_k)

通过带噪声的观测量以及模型的输入推断出待估计的管道机器人的运动状态，即求在已知的传感器输入数据和观测数据的条件下机器人状态的最大后验概率P(x_k|u_k-1,y_k)，根据贝叶斯法则得到：

将机器人状态的最大后验概率问题转换为求解机器人状态x_k的最大似然估计：

x^* _MLE＝argm ax(P(u_k-1,y_k|x_k))；

42)机器人状态优化过程中相机投影模型和目标函数的构建；为了刻画机器人状态优化过程中相机的投影模型，用像素坐标系、图像坐标系、相机坐标系来描述如何将相机坐标系下的物体投影到像素坐标系上，其中O_c为相机光心，f为相机焦距，现实空间中相机坐标系下的点P经过光心O投影到物理成像平面上，在图像坐标系上得到点P′，设点P的坐标为[X,Y,Z]，点P′的坐标为[X′,Y′]，Z表示点P到相机平面X-Y的距离，即深度；根据相机投影模型和相似三角形得出：

通过采样和量化将图像坐标系上的点转换到像素坐标系上，图像坐标系和像素坐标系间的转换经过了一个缩放变换和平移变换，像素坐标系上u轴坐标相对图像坐标系缩放了α倍，v轴坐标相对图像坐标系缩放了β倍，原点o′相对图像坐标系平移了[c_x,c_y]；因此图像坐标系下的点P′和像素坐标[u,v]的关系为：

将αf合并成f_x，βf合并成f_y，得到像素坐标系与相机坐标系的转换关系：

其中K表示相机内参矩阵，通过相机标定实验获得；假设有n对匹配好的特征点(p₁,p₁′),(p₂,p₂′)...(p_n,p_n′)，并已获得了特征点对应的深度值，需要求得相邻图像帧间的旋转矩阵R和平移矩阵t，其中包含了相机与IMU的空间关系以及角速度和轮速信息求解得到的机器人运动状态，定义误差项e_i为：

e_i＝p_i-(Rp_i′+t)

构建误差平方和损失函数作为机器人状态优化过程中的目标函数F：

43)非线性优化求解机器人位姿；目标函数定义为总体意义下的最小二乘问题，利用非线性优化方法和匹配特征点对求得使误差平方和达到极小的R和t；采用Levenberg-Marquardt算法求解，将误差函数进行泰勒展开：

e(x+Δx)＝e(x)+J(x)Δx

其中Δx表示迭代优化过程中代求变量的下降矢量，J为误差函数对代求变量的导数，则目标函数的最小化问题转化为求解使目标函数最小的Δx^*：

令目标函数对Δx的导数为0，求解使目标函数最小的矢量Δx，得到矢量方程：

J(x)^TJ(x)Δx＝-J(x)^Te(x)

机器人状态初值设为单位矩阵，求解每一次迭代过程中的误差和下降矢量，若Δx足够小，则停止迭代，令x_k+1＝x_k+Δx。

6.根据权利要求1所述的多传感器协同的管道机器人精准定位方法，其特征在于，在所述步骤5)中，根据北斗定位传感器返回的三维位置信息约束步骤4)中获得的机器人位姿信息，消除机器人位姿估计的累计误差，实现管道机器人的精准定位，其具体步骤为：

51)位姿图优化；步骤4)中经过非线性优化得到的管道机器人状态未考虑之前时刻机器人的位姿约束，位姿之间的相对运动与真实值存在偏差，利用位姿图优化再次修正管道机器人的位姿估计；位姿图由节点和边组成，节点代表管道机器人的位姿信息，两个位姿节点T_i和T_j之间的相对运动ΔT_ij表示位姿图的边，位姿之间的约束关系为ΔT_ij＝T_i ^-1T_j，优化变量为各个顶点的位姿，边来自于位姿观测约束，构建最小二乘误差e_ij：

e_ij＝ln(ΔT_ij ^-1T_i ^-1T_j)^∨

其中∨表示矩阵转换为向量的运算，优化变量为T_i和T_j，误差对优化变量的扰动项δξ导数为：

其中ξ表示特殊欧式群中位姿矩阵对应的李代数，ρ和

表示旋转分量和平移分量，各变量之间的对应关系为：

exp(ξ^{^})＝T,

其中∧表示向量到矩阵的运算，I为单位阵；根据构造的误差函数和误差对位姿的导数，利用Levenberg-Marquardt算法获取经过位姿图优化后的位姿信息；

52)融合北斗定位信息，消除机器人位姿估计的累计误差；融合相机、IMU和轮式编码器信息并经过管道机器人的位姿估计会随着机器人轨迹的变长而发生漂移，产生累积误差，根据北斗定位传感器返回的经度lati、纬度longi和高度信息alti将其转换为以地球为参考系的全局三维坐标全局三维坐标(X_g,Y_g,Z_g)：

其中L表示地球半径，将全局三维坐标作为约束条件，结合已获取的管道机器人位姿信息进行全局优化，建立整体误差模型：

其中k表示某一时刻，将机器人初始的三维位置设为北斗定位设备返回的三维坐标，经过位姿图优化后的旋转矩阵R和平移矩阵t作为全局优化初值，利用Levenberg-Marquardt算法迭代优化获取局部准确和全局无漂移的机器人位姿信息，实现管道机器人的精准定位。

Images