CN114485613A - 一种多信息融合水下机器人定位方法 - Google Patents

Abstract

本发明涉及一种多信息融合水下机器人定位方法。本发明用数值积分方法计算不同角度下双目摄像头拍摄范围重叠区域面积，找到双目摄像头拍摄范围重叠区域面积最大的角度；用棋盘法标定双目摄像头的内参，用拟合直线误差判断标定是否成功，之后标定双目摄像头的外参；位姿解算，获取多传感器信息以待分析，利用传感器信息解算机器人位姿并进行融合以实现优势互补；位姿优化，利用存储的若干关键帧状态进行批量调整；回环检测，当前时刻拍摄到的图像和存储的若干帧图像用词袋法度量相似度，以检测水下机器人是否重新回到某个位置附近。本发明结合多种传感器信息，提高其定位的精度，弥补了单一视觉定位对光照变化敏感、在弱纹理区域效果差的缺点。

Description

一种多信息融合水下机器人定位方法

技术领域

本发明涉及一种多信息融合水下机器人定位方法，属于水下组合定位技术领域。

背景技术

目前水下机器人定位多采用高精度惯导与多普勒测速仪组合的导航系统，设备成本高；或者采用声学定位方法，安装和维护较为复杂，同时信号传输易受到干扰，影响水下机器人作业任务。

因此，研究多信息融合水下机器人定位方法，以保证定位精度、稳定性为目标，同时降低成本，对于民用水下机器人的开发具有重要的研究意义和使用价值。

发明内容

本发明的目的是为了提供一种多信息融合水下机器人定位方法。本发明能够为水下机器人提供精确的位姿信息，有助于水下机器人进行更加复杂的作业。

本发明的目的是这样实现的，水下机器人上安装可以调节角度的左、右两个摄像头构成双目摄像头，能够输出三维磁场强度、姿态角和角速度以及加速度的航姿参考系统，以及测量水下机器人深度的深度计，具体方法包括以下步骤：

步骤一：建立双目摄像头拍摄范围重叠区域的模型，基于该模型用数值积分的方法从给定的双目摄像头的角度和高度求出拍摄范围重叠区域的面积，用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度并调节摄像头角度。

步骤二：用摄像头拍摄棋盘式标定板得到棋盘图像，借助OpenCV标定工具标定双目摄像头的内参，并用拟合直线误差判断标定是否成功；如果内参标定成功，则标定双目摄像头的外参。外参包括双目摄像头的左、右摄像头之间的相对位姿以及双目摄像头与航姿参考系统之间的相对位姿。

步骤三：水下机器人运动过程中，获取水下双目摄像头拍摄的图像，航姿参考系统测量的的三维磁场强度、姿态角、角速度和加速度信息以及深度计测量的深度信息。

用双目摄像头拍摄到的图像解算位姿变换，对航姿参考系统得到的加速度信息和角速度信息进行中值积分得到当前时刻相对于上一时刻的位姿变换，根据深度计的信息获得竖直方向的位置变化，将三种传感器解算的结果输入扩展卡尔曼滤波器解算当前位姿。

步骤四：利用存储的若干关键帧状态进行批量调整，实现各个时刻位姿的优化。

步骤五：将当前时刻拍摄到的图像和存储的附近区域的关键帧图像进行对比以判断水下机器人是否重新回到某个位置附近，即回环检测。检测到回环以后，利用高斯牛顿算法更新存储的关键帧的位姿。

综上，本发明主要用于水下机器人在近底作业中，准确解算位姿。这样的过程包含以下步骤：双目摄像头拍摄角度调节：让双目摄像头拍摄范围重叠区域尽可能大；标定双目摄像头的参数：获取双目摄像头的内参和外参；位姿解算：获取多传感器信息，利用传感器信息解算水下机器人位姿；位姿优化：利用存储的附近区域的若干关键帧状态进行批量调整；回环检测：将当前时刻拍摄到的图像和存储的附近区域的关键帧图像进行对比以判断水下机器人是否重新回到某个位置附近。

本发明与现有技术相比，具有以下优点：

(1)本发明利用数值积分方法，解算双目摄像头拍摄范围重叠区域的面积，并且用遍历的方法计算每个摄像头的最佳安装角度，从而让双目摄像头拍摄范围重叠区域尽可能大，实现摄像头视野范围的有效利用。

(2)本发明结合多种传感器信息，实现水下机器人作业过程中的实时定位，提高其定位的精度，弥补了单一视觉定位对光照变化敏感、在弱纹理区域效果差的缺点。

(3)用拟合直线误差能够有效判断标定是否成功。

(4)传统的水下机器人定位系统多利用高精度惯导、多普勒测速仪等贵重设备，或者采用安装和维护比较复杂的声学定位方法。本发明所用的传感器成本低廉，安装简便。

(5)本发明利用航姿参考系统输出的磁场强度信息来检测周围金属干扰的程度，增强了对环境的鲁棒性。

(6)本发明利用存储的关键帧状态进行批量调整和回环检测两种方法减小定位系统的积分误差，同时本发明将回环检测的待匹配图像的范围限制在一定的范围，减小了回环检测的计算量。

附图说明

图1是本发明一种水下机器人定位系统的传感器安装示意图。图1中，1为能够输出三维磁场强度、姿态角和角速度以及加速度的航姿参考系统，2为测量水下机器人深度的深度计，3、4分别为可以调节角度的左、右摄像头，左、右两个摄像头构成双目摄像头，5为提供照明的多个LED灯。

图2是本发明一种多信息融合水下机器人定位方法的整体流程图。

图3是本发明步骤一中双目摄像头的拍摄角度的示意图。

图4是本发明步骤一中用数值积分的方法从给定的双目摄像头的角度和高度求出拍摄范围重叠区域的面积的流程图。

图5是本发明步骤一中用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度的流程图。

图6是本发明步骤三中位姿解算的流程图。

具体实施方式

下面结合说明书附图对本发明作进一步阐述。

如图1至图6所示，本发明一种多信息融合水下机器人定位方法。

如图1所示，水下机器人上安装可以调节角度的左、右两个摄像头构成双目摄像头，能够输出三维磁场强度、姿态角和角速度以及加速度的航姿参考系统，以及测量水下机器人深度的深度计。

具体方法包括以下步骤：

步骤一：建立双目摄像头拍摄范围重叠区域的模型，基于该模型用数值积分的方法从给定的双目摄像头的角度和高度求出拍摄范围重叠区域的面积，用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度并调节摄像头角度。

对于双目摄像头，其拍摄范围重叠区域越大，能够匹配的特征点数目越多，对于定位的帮助越大。双目摄像头拍摄范围重叠区域的模型的建立方法为：

摄像头的照射范围为圆锥状，设底面点的坐标为(x,y,0)，在底面的照射范围为：

其中，向量为u(x,y)从摄像头到坐标(x,y,0)的向量，而u^T(x,y)为向量u(x,y)的转置，v为摄像头中轴的方向向量，θ为摄像头视野角的一半。

因此给定双目摄像头的角度和距离底面的高度h，就能够计算双目摄像头拍射范围重叠区域。为了最大化双目摄像头拍摄范围重叠区域，并且保持水下机器人配重的平衡，双目摄像头对称布置，坐标为

和

如图3所示，摄像头的安装角度：双目摄像头的左、右摄像头中轴与竖直线的夹角均为α，左摄像头中轴在底面的投影与左方向的夹角为β，右摄像头中轴在底面的投影与右方向的夹角也为β。

双目摄像头的左、右摄像头到底面的照射范围分别为：

其中，u₁(x,y)从左摄像头到坐标(x,y,0)的向量

而

为向量u₁(x,y)的转置，u₂(x,y)从右摄像头到坐标(x,y,0)的向量

而

为向量u₂(x,y)的转置,v₁、v₂分别为双目摄像头的左、右摄像头的中轴的方向向量

v₁＝[-sin(α)cos(β)sin(α)sin(β)-cos(α)]^T

v₂＝[sin(α)cos(β)sin(α)sin(β)-cos(α)]^T

除此之外，由于水下环境对光的传播有较强的衰减作用，所以向量u₁(x,y)、u₂(x,y)的模长不能超过光在水下的最大传播距离L。

综上，双目摄像头拍摄范围重叠区域C的模型为：

基于该模型用数值积分的方法从给定的双目摄像头的角度和高度求出拍摄范围重叠区域的面积，其具体步骤为：

对于不同的安装角度下双目摄像头拍摄范围的重合区域，本发明用数值积分的方法计算，流程如图4所示。为了减小计算量，对于不同的安装角度下双目摄像头拍摄范围重叠区域，设置搜索范围如下：

{(x,y,0)|x∈[-W,W],y∈[-W,W]}

其中，W的定义为：

本发明将该搜索范围划分为0.01m×0.01m的小格，每个小格用一个点代替整体，若该点属于区域C，认为整个小格属于区域C。

区域C的初始面积设为S，从x＝-W,y＝-W开始，按照0.01m的间隔依次改变x和y并进行判断，若点(x,y)属于区域C，那么面积S增加0.0001，直到搜索范围内所有的点都判断完为止。

用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度，其具体为：

寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度的流程如图5所示。图5中，α与β的定义与图3一致，双目摄像头的左、右摄像头的中轴与竖直线的夹角均为α，左摄像头中轴在底面的投影与左方向的夹角为β，右摄像头中轴在底面的投影与右方向的夹角也为β。α和β的初始值为0°，按照1°的间隔依次增加并计算对应安装角度下双目摄像头拍摄范围的重合区域的面积S，最终求得最佳的摄像头角度，并据此调节摄像头拍摄角度。

步骤二：取一个有m×n方格的棋盘作为标定板，用双目摄像头从不同的角度拍摄标定板得到棋盘图像，借助OpenCV开源标定工具标定双目摄像头的内参，并用拟合直线误差判断标定是否成功。使用的用拟合直线误差判断标定是否成功的方法，其具体步骤为：利用标定好的内参矫正拍摄的棋盘图像，用直线拟合矫正后图像上位于同一条直线的棋盘点；计算图像上各点距离该直线的距离的平均值作为拟合直线误差，用拟合直线误差判断标定是否成功，如果矫正图像中拟合直线误差小于两端点距离的千分之一，说明标定成功。如果内参标定成功，则标定双目摄像头的外参。外参包括双目摄像头的左、右摄像头之间的相对位姿以及双目摄像头与航姿参考系统的相对位姿。

步骤三：水下机器人开始运动，导航定位系统位姿解算的流程如图6所示。水下机器人第i时刻的状态定义为

x_i＝[p_i,q_i,v_i]^T

其中，p_i为航姿参考系统在惯性坐标系下的位置；q_i为四元数，表示从惯性坐标系到航姿参考系统坐标系的旋转；v_i为航姿参考系统在惯性坐标系下的速度。

在水下机器人运动的初始位置，首先初始化状态向量和协方差。

水下机器人运动过程中按照固定的频率获取水下双目摄像头拍摄的图像，航姿参考系统测量的三维磁场强度、姿态角、角速度和加速度信息以及深度计测量的深度信息，作为待分析的信息。航姿参考系统第i时刻的观测向量为：

其中，

为航姿参考系统测量的三维磁场强度，

为航姿参考系统测量的姿态角，

为航姿参考系统测量的加速度，

为航姿参考系统测量的角速度。

航姿参考系统能够直接观测水下机器人的姿态角：

航姿参考系统测量的加速度通过中值积分能够得到从i时刻到i+1时刻的速度的变化量：

v_i+1-v_i＝aδt

其中，

为表示从航姿参考系统坐标系到惯性坐标系的旋转的四元数。

同样，航姿参考系统测量的角速度通过中值积分能够得到从i时刻到i+1时刻的姿态角变化量。

深度计第i时刻的观测值为z_depth，观测方程为

其中

为i时刻航姿参考系统在竖直方向的位置。

双目摄像头获得图像进行匹配，获得匹配的特征点，由于双目摄像头的相对位姿事先已知，利用三角测量的原理可以确定特征点的三维坐标。当前时刻获得的特征点与上一时刻获得的特征点再次进行匹配，如果两个时刻获得的图像匹配成功的特征点数目多于20个，则利用两个时刻的图像能够匹配的特征点建立几何约束方程，解算当前时刻相对于上一时刻的位姿变换。对上一时刻与当前时刻之间的航姿参考系统得到的加速度信息和角速度信息进行积分得到当前时刻相对于上一时刻的位姿变换。两个时刻的深度计测得的深度求差得到竖直方向的位置变换。如果两个时刻获得的图像匹配成功的特征点数目多于20个，那么将双目摄像头、航姿参考系统、深度计得到的位姿变换输入扩展卡尔曼滤波器中，得到当前时刻的位姿，并将当前时刻的位姿和特征点位置作为关键帧存储下来；如果两个时刻获得的图像匹配成功的特征点数目不超过20个，那么将航姿参考系统、深度计得到的位置变换输入扩展卡尔曼滤波器中，得到当前时刻的位姿。

水下机器人搭载的航姿参考系统主要利用地磁场计算姿态角，一般地球磁场在一般情况下只有微弱的0.5高斯左右，而如果水下机器人受到金属等带来的磁场干扰，磁场强度的测量值会显著增大，此时航姿参考系统输出的姿态角可靠性差。为了减小磁场干扰的影响，在水下机器人运动的初始位置记录航姿参考系统输出的三维磁场强度矢量和的模长M₀(水下机器人初始位置远离金属物体)，同时，运动过程中记录航姿参考系统输出的三维磁场强度，记录三维磁场强度矢量和的模长，根据该模长M与初始模长M₀的差异大小调节航姿参考系统的协方差矩阵，差异越大，航姿参考系统协方差矩阵中姿态角部分对角线元素的数值越大，取原值的M/M₀倍为新的对角线元素值。此时，由于航姿参考系统协方差矩阵中姿态角部分对角线元素的数值较大，姿态角的获得主要依靠航姿参考系统获得的角速度积分以及双目摄像头图像匹配。

步骤四：利用存储的若干关键帧状态进行批量调整，实现各个时刻位姿的优化。

定位系统利用存储的若干关键帧状态进行批量调整，待优化的系统状态组合为：

χ＝[x_n，x_n+1，…，x_n+N，λ₀，λ_1，…，λ_M]

x_i＝[r_i，q_i，v_i]^T，i∈[n，n+N]

其中，x_i包含i时刻航姿参考系统在惯性坐标系的位置、姿态、速度：p_i为航姿参考系统在惯性坐标系下的位置；q_i为四元数，表示从惯性坐标系到航姿参考系统坐标系的旋转；v_i为航姿参考系统在惯性坐标系下的速度。λ_j代表观测到的第j个特征点在惯性坐标系下的位置。n是待优化的状态组合的起始时刻。状态组合中的关键帧数量为N+1，状态组合中观测到的特征点数目为M+1。以两个时刻之间的时间段内IMU积分量作为测量值，对两个时刻的状态量进行约束，构建残差向量

r＝[r_p,r_q,r_v]^T

以r^Tr为损失函数，利用高斯牛顿算法迭代求解得到系统状态，这样就进行了批量调整，实现若干关键帧状态的优化。

步骤五：如图2所示，将当前时刻拍摄到的图像和存储的附近区域的若干关键帧图像进行对比以检测水下机器人是否重新回到某个位置附近，检测到回环以后，利用高斯牛顿算法更新存储的关键帧的位姿。

在位姿解算步骤中对多个时刻的图像用ORB算子提取特征并存储起来，用k-means聚类方法对存储的特征进行聚类构成特征簇，所有的特征簇构成特征字典。

在水下机器人运动过程中得到新的图像后，用ORB算子提取当前时刻的图像的特征，然后查找字典得到每个特征对应的特征簇，得到图像中各个特征簇的比例构成该图像的直方图向量。

查找附近区域的关键帧图像作为待匹配图像，附近区域的确定方法为：计算当前水下机器人运动的路程，取路程的百分之五为区域半径，若历史图像对应的历史位置与当前解算位置的距离小于区域半径，则作为待匹配图像。用直方图向量计算当前图像与待匹配图像的相似度，若相似度大于某个阈值则表明检测到回环。

上面结合附图对本发明的实施方式做了具体的说明，但是本发明并不仅限于上述的实施方式，在所属领域的技术人员所具备的知识范围内，在不脱离本发明宗旨的前提下可以做出各种变化。

Claims (7)

1.一种多信息融合水下机器人定位方法，其特征在于，所述的水下机器人上安装有可以调节角度的双目摄像头，能够输出三维磁场强度、姿态角、角速度以及加速度的航姿参考系统，能够测量水下机器人深度的深度计，具体包含以下步骤：

步骤一：建立双目摄像头拍摄范围重叠区域的模型，基于该模型用数值积分的方法从给定的双目摄像头的角度和高度求出拍摄范围重叠区域的面积，用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度并调节摄像头角度；

步骤二：用摄像头拍摄棋盘式标定板得到棋盘图像，借助OpenCV标定工具标定双目摄像头的内参，并用拟合直线误差判断标定是否成功；如果内参标定成功，则标定双目摄像头的外参；外参包括双目摄像头的左、右摄像头之间的相对位姿以及双目摄像头与航姿参考系统之间的相对位姿；

步骤三：水下机器人运动过程中，获取水下双目摄像头拍摄的图像，航姿参考系统测量的的三维磁场强度、姿态角、角速度和加速度信息以及深度计测量的深度信息；

用双目摄像头拍摄到的图像解算位姿变换，对航姿参考系统得到的加速度信息和角速度信息进行中值积分得到当前时刻相对于上一时刻的位姿变换，根据深度计的信息获得竖直方向的位置变化，将三种传感器解算的结果输入扩展卡尔曼滤波器解算当前位姿；

步骤四：利用存储的若干关键帧状态进行批量调整，实现各个时刻位姿的优化；

步骤五：将当前时刻拍摄到的图像和存储的附近区域的关键帧图像进行对比以判断水下机器人是否重新回到某个位置附近，即回环检测；检测到回环以后，利用高斯牛顿算法更新存储的关键帧的位姿。

2.根据权利要求1所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤一所述的双目摄像头拍摄范围重叠区域的模型的建立方法为：

摄像头的照射范围为圆锥状，设底面点的坐标为(x,y,0)，则所述双目摄像头拍摄范围重叠区域C的模型为：

其中，θ为摄像头视野角的一半；u₁(x,y)从左摄像头到坐标(x,y,0)的向量，而

为向量u₁(x,y)的转置；u₂(x,y)从右摄像头到坐标(x,y,0)的向量，而

为向量u₂(x,y)的转置；v₁、v₂分别为双目摄像头的左、右摄像头的中轴的方向向量；向量u₁(x,y)、u₂(x,y)的模长不能超过光在水下的最大传播距离L。

3.根据权利要求2所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤一所述的基于该模型用数值积分的方法从给定的双目摄像头的位置和高度求出拍摄范围重叠区域的面积，其具体步骤为：

对于不同的安装角度下双目摄像头拍摄范围重叠区域，设置搜索范围如下：

{(x,y,0)|x∈[-W,W],y∈[-W,W]}

其中，W的定义为：

其中，h为双目摄像头距离底面的高度；

将该搜索范围划分为小格，每个小格用一个点代替整体；

若该点属于重叠区域C，认为整个小格属于重叠区域C，从而通过计算属于区域C的小格数目，计算重叠区域的面积。

4.根据权利要求1所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤一所述的用遍历的方法寻找双目摄像头拍摄范围重叠区域面积最大的摄像头角度，其具体为：以1°的间隔遍历所有可能的摄像头角度，从而找到让双目摄像头拍摄范围重叠区域面积最大的摄像头角度。

5.根据权利要求1所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤二所述用拟合直线误差判断标定是否成功，其具体步骤为：利用标定好的内参矫正拍摄的棋盘图像，用直线拟合矫正后图像上位于同一条直线的棋盘点；计算图像上各点距离该直线的距离的平均值作为拟合直线误差，用拟合直线误差判断标定是否成功，如果矫正图像中拟合直线误差小于两端点距离的千分之一，说明标定成功。

6.根据权利要求1所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤三使用的位姿解算的方法，获取多传感器信息，利用传感器信息解算机器人位姿，其具体方法为：

在水下机器人运动的初始位置，首先初始化状态向量和协方差；水下机器人运动过程中按照固定的频率获取水下双目摄像头拍摄的图像，航姿参考系统测量的三维磁场强度、姿态角、角速度和加速度信息以及深度计测量的深度信息，作为待分析的信息；双目摄像头获得图像进行匹配，获得匹配的特征点，由于双目摄像头的相对位姿事先已知，利用三角测量的原理可以确定特征点的三维坐标；当前时刻获得的特征点与上一时刻获得的特征点再次进行匹配，如果两个时刻获得的图像匹配成功的特征点数目多于20个，则利用两个时刻的图像能够匹配的特征点建立几何约束方程，解算当前时刻相对于上一时刻的位姿变换；对上一时刻与当前时刻之间的航姿参考系统得到的加速度信息和角速度信息进行积分得到当前时刻相对于上一时刻的位姿变换；两个时刻的深度计输出求差得到竖直方向的位置变换；

如果两个时刻获得的图像匹配成功的特征点数目多于20个，那么将双目摄像头、航姿参考系统、深度计得到的位姿变换输入扩展卡尔曼滤波器中，得到当前时刻的位姿，并将当前时刻的位姿和特征点位置作为关键帧存储下来；如果两个时刻获得的图像匹配成功的特征点数目不超过20个，那么将航姿参考系统、深度计得到的位姿变换输入扩展卡尔曼滤波器中，得到当前时刻的位姿；

在水下机器人运动的初始位置记录航姿参考系统输出的三维磁场强度矢量和的模长M₀，同时，运动过程中记录航姿参考系统输出的三维磁场强度，记录磁场强度矢量和的模长M，根据该模长M与初始模长M₀的差异大小调节航姿参考系统的协方差矩阵，取原值的M/M₀倍为新的对角线元素值；此时，由于航姿参考系统协方差矩阵中姿态角部分对角线元素的数值较大，水下机器人姿态角的解算主要依靠航姿参考系统获得的角速度积分以及双目摄像头图像匹配。

7.根据权利要求1所述的一种多信息融合水下机器人定位方法，其特征在于：

步骤五使用的回环检测的方法，将当前时刻拍摄到的图像和存储的附近区域的关键帧图像进行对比以判断水下机器人是否重新回到某个位置附近，其具体方法为：

在位姿解算步骤中对多个时刻的图像用ORB算子提取特征并存储起来，用k-means聚类方法对存储的特征进行聚类构成特征簇，所有的特征簇构成特征字典；

在水下机器人运动过程中得到新的图像后，用ORB算子提取当前时刻的图像的特征，然后查找字典得到每个特征对应的特征簇，得到图像中各个特征簇的比例构成该图像的直方图向量；

查找附近区域的关键帧图像作为待匹配图像，附近区域的确定方法为：计算当前水下机器人运动的路程，取路程的百分之五为区域半径，若历史图像对应的历史位置与当前解算位置的距离小于区域半径，则作为待匹配图像；用直方图向量计算当前图像与待匹配图像的相似度，若相似度大于某个阈值则表明检测到回环。

Images