CN112924708A - 一种适用于水下近底作业航行器的速度估计方法 - Google Patents

Abstract

本发明涉及一种适用于水下近底作业航行器的速度估计方法，属于无人平台的速度估计领域；步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；步骤二、t‑1时刻，对水底图像进行获取；步骤三、图像预处理；步骤四、对图像提取稀疏ORB特征；获得t‑1时刻图像的第一特征点集A；步骤五、t时刻图像获取；步骤六、通过稀疏ORB特征对t时刻图像进行光流场提取；步骤七、根据光流场进行速度解算；本发明能够在航行器近底作业时，提供鲁棒的速度信息，且具有实时性特点。

Description

一种适用于水下近底作业航行器的速度估计方法

技术领域

本发明属于无人平台的速度估计领域，涉及一种适用于水下近底作业航行器的速度估计方法。

背景技术

水下导航问题仍然是水下移动平台所面临的主要技术挑战之一。导航系统必须提供远距离及长时间范围内的精确定位、速度及姿态信息，受体积、质量、能源及水声介质的特殊性、隐蔽性等因素的影响，实现水下移动平台的精确导航是一项艰难的任务。当前水下导航主要是通过卫星、惯导、水声多普勒速度仪的组合导航，然而，当平台在水下工作时，卫星导航是不可用的，因此主要采用惯导、水声多普勒速度仪的组合导航。然而当平台近底或坐底作业时，水声多普勒速度仪也往往失效。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提出一种适用于水下近底作业航行器的速度估计方法，能够在航行器近底作业时，提供鲁棒的速度信息，且具有实时性特点。

本发明解决技术的方案是：

一种适用于水下近底作业航行器的速度估计方法，包括如下步骤：

步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；

步骤二、t-1时刻，对水底图像进行获取；

步骤三、图像预处理；

步骤四、对图像提取稀疏ORB特征；获得t-1时刻图像的第一特征点集A；

步骤五、t时刻图像获取；

步骤六、通过稀疏ORB特征对t时刻图像进行光流场提取；

步骤七、根据光流场进行速度解算。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤一中，通过相机标定；获得相机内部参数矩阵为M，相机焦距为f。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤三中，图像预处理内容包括对水底图像进行滤波处理，减少图像的噪声。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤六中，光流场提取的具体方法为：

采用基于梯度的Lucas-Kanade法，将步骤四获得的第一特征点集A中每一个特征点，在步骤五获得的t时刻图像找到对应的特征点，获得第二特征点集B；

再采用基于梯度的Lucas-Kanade法，将第二特征点集B中每一个特征点，在步骤三获得的t-1时刻图像找到对应的特征点，获得第三特征点集C；

在图像采集过程中存在噪声，因此第一特征点集A与第三特征点集C并不完全重合；

选取误差和相似度两个标准对第一特征点集A与第三特征点集C进行滤波；选取相似度大且误差小的特征点，完成光流场的提取。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述误差标准为第一特征点集A与第三特征点集C中对应特征点的距离；距离越小误差越小。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述相似度标准为：

对第一特征点集A与第三特征点集C中的各组对应特征点，分别选取10像素×10像素大小的邻域；对两个邻域进行模板匹配，选择归一化相关系数匹配法，将匹配的结果作为相似度；匹配值越大越相似。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述速度解算的具体方法为：

将t-1时刻得到的第一特征点集A中各特征点记为

将t时刻得到的第二特征点集B中的各特征点记为

k＝1,2,……,n；n为特征点个数；

设定t-1时刻，相机的坐标为

t时刻，相机的坐标为

相机的成像平面与航行器运动平面平行；建立航行器的体坐标系oxyz；建立相机坐标系o1x1y1z1；获得相机到航行器体坐标系的旋转矩阵

相机在t-1时刻至t时刻的移动量用单位矩阵H表示；将第二特征点集B中的各特征点

在相机坐标系o1x1y1z1中的投影集合记为

对应水底地面的点为

设定相机内部参数矩阵为M，相机焦距为f；则各特征点在z1轴上的尺度为Z_c；

图像更新频率大于60Hz；认为连续两帧图像之间，相机与水底距离h不发生变化；则t时刻有

和

t-1到t时刻n个特征点的稀疏光流

为：

式中，

为t时刻的特征点；

为t-1时刻的特征点；

Δt为时间差，Δt＝t-(t-1)；

光流与航行器的线运动和角运动的关系为：

式中，V_b为航行器线速度；

β为航行器体坐标系的x轴与航行器-目标点之间连线的夹角；

w为航行器的角速率；

得到：

式中，v_bx为航行器在体坐标系下沿x方向的移动速度；

v_by为航行器在体坐标系下沿y方向的移动速度；

Δu为投影到成像平面的像素移动总速度；

p为滚转角速率；通过IMU测量得到；

q为俯仰角速率；通过IMU测量得到；

h为相机与水底距离，通过水声高度计或声学多普勒计程仪得到；

光流法直接测量的是航行器在体坐标系下的移动速度V_b＝(v_bx,v_by)；计算航行器的真实移动速度V：

式中，

为转换矩阵；

式中，ψ为航行器的偏航角；

θ为航行器的俯仰角；

φ为航行器的横滚角。

在上述的一种适用于水下近底作业航行器的速度估计方法，所述航行器的体坐标系oxyz的建立方法为：

以航行器的质心为原点o，x方向指向航行器水下平移运动方向，z方向竖直向上；y方向由右手定则确定；

相机坐标系o1x1y1z1的建立方法为：

以相机光心为原点o1，x1方向与图像坐标系o2x2y2的x2方向平行；y1方向与图像坐标系o2x2y2的y2方向平行；z1由右手定则确定；

图像坐标系o2x2y2的建立方法为：

以图像左上角像素点为原点o2，x2轴指向图像像素列方向；y2方向指向图像像素行方向。

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

(1)本发明基于光学图像设备，实现了对平台水平速度信息的估计和发布，该速度信息可以有效提升导航系统的鲁棒性，为近底或坐底作业的水下航行器提供了一种较好的速度估计方案；

(2)本发明在稀疏特征点提取中采用了ORB特征、在光流场提取中引入了前后双向追踪的方法，在速度解算时融合了惯导角速率，这些方法有效提高了速度估计的实时性、鲁棒性和准确性。

附图说明

图1为本发明速度估计流程图；

图2为本发明前后双向追踪方法示意图。

具体实施方式

下面结合实施例对本发明作进一步阐述。

本发明提供了一种适用于水下近底作业航行器的速度估计方法，包括水底图像获取、图像预处理、特征点提取、特征点匹配、随机采样一致滤波、图像光流场提取、速度解算。本发明能够实现当水下近底作业航行器在距离水底较近，声学测速设备失效时，仍能利用光学实现速度估计，进而辅助导航。实现了在航行器近底作业时，提供鲁棒的速度信息，且具有实时性特点。

适用于水下近底作业航行器的速度估计方法，如图1所示，具体包括如下步骤：

步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；通过相机标定；获得相机内部参数矩阵为M，相机焦距为f。

步骤二、t-1时刻，对水底图像进行获取。

步骤三、图像预处理；图像预处理内容包括对水底图像进行滤波处理，减少图像的噪声。

步骤四、对图像提取稀疏ORB特征；获得t-1时刻图像的第一特征点集A；采用基于ORB的特征提取法，时间开销更小，对仿射变换和旋转变换鲁棒性更高。ORB是一种实时性较高的特征提取与特征描述方法，它融合了FAST和BRIEF两种描述子。

在FAST中加入方向因子，提高针对特征尺度变化的鲁棒性，也即：

把具有方向因子的FAST特征记为oFAST。定义一个图像块各个点的矩如下式所示：

其中p、q为矩的阶次，I(x,y)为图像像素点的灰度表达式。利用

来计算质心的坐标，即建立一个从图像块的中心到质心的向量代表方向因子，向量的方向可以简化为θ＝arctan(m₀₁,m₁₀)。

在BRIEF中加入旋转因子，并选取31*31的图像区域进行对比，提高了抗噪性，也即：

对特征点P选取一个m*m的图像块，并对图像块做高斯滤波。在图像块内随机选取N个点对。分别比较这N个点对的两个点之间的亮度值大小。如果p(x)<p(y)则赋值1，否则为0。这样就得到一串二进制字符串，如式

所示。根据式

将得到的带有N个字符的二进制码串组成一个N维向量。从上述可以看出，图像旋转会导致二进制串变化，这不符合对特征点旋转不变的要求。为解决该问题，采用方向因子驱动法实现二进制字符串向量的旋转。定义一个2*N的矩阵：

根据旋转角θ和基础旋转矩阵R_θ，控制S_θ变化为S，S_θ＝R_θS，进而获得BRIEF操作子g_n(p,θ):＝f_n(p)|(x_i,y_i)∈S_θ，这样，只要θ确定了，便可快速获得描述子。

步骤五、t时刻图像获取。

步骤六、通过稀疏ORB特征对t时刻图像进行光流场提取；采用了基于梯度法的稀疏光流，并引入前后双向追踪的方法，提升光流场提取精度。光流场提取采用了梯度法。考虑实时性需求，采用了稀疏光流，在经典的Lucas-Kanade光流算法基础上进行了改进，也即加入了前后双向追踪的方法，如图2所示，光流场提取的具体方法为：

采用基于梯度的Lucas-Kanade法，将步骤四获得的第一特征点集A中每一个特征点，在步骤五获得的t时刻图像找到对应的特征点，获得第二特征点集B；

再采用基于梯度的Lucas-Kanade法，将第二特征点集B中每一个特征点，在步骤三获得的t-1时刻图像找到对应的特征点，获得第三特征点集C；

在图像采集过程中存在噪声，因此第一特征点集A与第三特征点集C并不完全重合；

选取误差和相似度两个标准对第一特征点集A与第三特征点集C进行滤波；选取相似度大且误差小的特征点，完成光流场的提取。所述误差标准为第一特征点集A与第三特征点集C中对应特征点的距离；距离越小误差越小。所述相似度标准为：对第一特征点集A与第三特征点集C中的各组对应特征点，分别选取10像素×10像素大小的邻域；对两个邻域进行模板匹配，选择归一化相关系数匹配法，将匹配的结果作为相似度；匹配值越大越相似。

步骤七、根据光流场进行速度解算。速度解算充分考虑了姿态带来的误差，引入了融合惯导的速度矫正，进一步提高速度解算的精度。速度解算的具体方法为：

将t-1时刻得到的第一特征点集A中各特征点记为

将t时刻得到的第二特征点集B中的各特征点记为

k＝1,2,……,n；n为特征点个数；

设定t-1时刻，相机的坐标为

t时刻，相机的坐标为

相机的成像平面与航行器运动平面平行；建立航行器的体坐标系oxyz；建立相机坐标系o1x1y1z1；航行器的体坐标系oxyz的建立方法为：

以航行器的质心为原点o，x方向指向航行器水下平移运动方向，z方向竖直向上；y方向由右手定则确定；

相机坐标系o1x1y1z1的建立方法为：

以相机光心为原点o1，x1方向与图像坐标系o2x2y2的x2方向平行；y1方向与图像坐标系o2x2y2的y2方向平行；z1由右手定则确定；

图像坐标系o2x2y2的建立方法为：

以图像左上角像素点为原点o2，x2轴指向图像像素列方向；y2方向指向图像像素行方向。

获得相机到航行器体坐标系的旋转矩阵

相机在t-1时刻至t时刻的移动量用单位矩阵H表示；将第二特征点集B中的各特征点

在相机坐标系o1x1y1z1中的投影集合记为

对应水底地面的点为

设定相机内部参数矩阵为M，相机焦距为f；则各特征点在z1轴上的尺度为Z_c；

图像更新频率大于60Hz；认为连续两帧图像之间，相机与水底距离h不发生变化；则t时刻有

和

t-1到t时刻n个特征点的稀疏光流

为：

式中，

为t时刻的特征点；

为t-1时刻的特征点；

Δt为时间差，Δt＝t-(t-1)；

光流与航行器的线运动和角运动的关系为：

式中，V_b为航行器线速度；

β为航行器体坐标系的x轴与航行器-目标点之间连线的夹角；

w为航行器的角速率；

得到：

式中，v_bx为航行器在体坐标系下沿x方向的移动速度；

v_by为航行器在体坐标系下沿y方向的移动速度；

Δu为投影到成像平面的像素移动总速度；

p为滚转角速率；通过IMU测量得到；

q为俯仰角速率；通过IMU测量得到；

h为相机与水底距离，通过水声高度计或声学多普勒计程仪得到；

光流法直接测量的是航行器在体坐标系下的移动速度V_b＝(v_bx,v_by)；计算航行器的真实移动速度V：

式中，

为转换矩阵；

式中，ψ为航行器的偏航角；

θ为航行器的俯仰角；

φ为航行器的横滚角。

自此获得的速度信息可用于后续组合导航的计算。

本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

Claims (8)

1.一种适用于水下近底作业航行器的速度估计方法，其特征在于：包括如下步骤：

步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；

步骤二、t-1时刻，对水底图像进行获取；

步骤三、图像预处理；

步骤四、对图像提取稀疏ORB特征；获得t-1时刻图像的第一特征点集A；

步骤五、t时刻图像获取；

步骤六、通过稀疏ORB特征对t时刻图像进行光流场提取；

步骤七、根据光流场进行速度解算。

2.根据权利要求1所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述步骤一中，通过相机标定；获得相机内部参数矩阵为M，相机焦距为f。

3.根据权利要求2所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述步骤三中，图像预处理内容包括对水底图像进行滤波处理，减少图像的噪声。

4.根据权利要求3所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述步骤六中，光流场提取的具体方法为：

采用基于梯度的Lucas-Kanade法，将步骤四获得的第一特征点集A中每一个特征点，在步骤五获得的t时刻图像找到对应的特征点，获得第二特征点集B；

再采用基于梯度的Lucas-Kanade法，将第二特征点集B中每一个特征点，在步骤三获得的t-1时刻图像找到对应的特征点，获得第三特征点集C；

在图像采集过程中存在噪声，因此第一特征点集A与第三特征点集C并不完全重合；

选取误差和相似度两个标准对第一特征点集A与第三特征点集C进行滤波；选取相似度大且误差小的特征点，完成光流场的提取。

5.根据权利要求4所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述误差标准为第一特征点集A与第三特征点集C中对应特征点的距离；距离越小误差越小。

6.根据权利要求5所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述相似度标准为：

对第一特征点集A与第三特征点集C中的各组对应特征点，分别选取10像素×10像素大小的邻域；对两个邻域进行模板匹配，选择归一化相关系数匹配法，将匹配的结果作为相似度；匹配值越大越相似。

7.根据权利要求6所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述速度解算的具体方法为：

将t-1时刻得到的第一特征点集A中各特征点记为

将t时刻得到的第二特征点集B中的各特征点记为

k＝1,2,……,n；n为特征点个数；

设定t-1时刻，相机的坐标为

t时刻，相机的坐标为

相机的成像平面与航行器运动平面平行；建立航行器的体坐标系oxyz；建立相机坐标系o1x1y1z1；获得相机到航行器体坐标系的旋转矩阵

相机在t-1时刻至t时刻的移动量用单位矩阵H表示；将第二特征点集B中的各特征点

在相机坐标系o1x1y1z1中的投影集合记为

对应水底地面的点为

设定相机内部参数矩阵为M，相机焦距为f；则各特征点在z1轴上的尺度为Z_c；

图像更新频率大于60Hz；认为连续两帧图像之间，相机与水底距离h不发生变化；则t时刻有

和

t-1到t时刻n个特征点的稀疏光流

为：

式中，

为t时刻的特征点；

为t-1时刻的特征点；

Δt为时间差，Δt＝t-(t-1)；

光流与航行器的线运动和角运动的关系为：

式中，V_b为航行器线速度；

β为航行器体坐标系的x轴与航行器-目标点之间连线的夹角；

w为航行器的角速率；

得到：

式中，v_bx为航行器在体坐标系下沿x方向的移动速度；

v_by为航行器在体坐标系下沿y方向的移动速度；

Δu为投影到成像平面的像素移动总速度；

p为滚转角速率；通过IMU测量得到；

q为俯仰角速率；通过IMU测量得到；

h为相机与水底距离，通过水声高度计或声学多普勒计程仪得到；

光流法直接测量的是航行器在体坐标系下的移动速度V_b＝(v_bx,v_by)；计算航行器的真实移动速度V：

式中，

为转换矩阵；

式中，ψ为航行器的偏航角；

θ为航行器的俯仰角；

φ为航行器的横滚角。

8.根据权利要求7所述的一种适用于水下近底作业航行器的速度估计方法，其特征在于：所述航行器的体坐标系oxyz的建立方法为：

以航行器的质心为原点o，x方向指向航行器水下平移运动方向，z方向竖直向上；y方向由右手定则确定；

相机坐标系o1x1y1z1的建立方法为：

以相机光心为原点o1，x1方向与图像坐标系o2x2y2的x2方向平行；y1方向与图像坐标系o2x2y2的y2方向平行；z1由右手定则确定；

图像坐标系o2x2y2的建立方法为：

以图像左上角像素点为原点o2，x2轴指向图像像素列方向；y2方向指向图像像素行方向。

Images