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

本发明专利技术涉及一种适用于水下近底作业航行器的速度估计方法，属于无人平台的速度估计领域；步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；步骤二、t

【技术实现步骤摘要】
一种适用于水下近底作业航行器的速度估计方法


[0001]本专利技术属于无人平台的速度估计领域，涉及一种适用于水下近底作业航行器的速度估计方法。

技术介绍

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

技术实现思路

[0003]本专利技术解决的技术问题是：克服现有技术的不足，提出一种适用于水下近底作业航行器的速度估计方法，能够在航行器近底作业时，提供鲁棒的速度信息，且具有实时性特点。
[0004]本专利技术解决技术的方案是：
[0005]一种适用于水下近底作业航行器的速度估计方法，包括如下步骤：
[0006]步骤一、将相机镜头竖直向下安装在航行器上，进行相机标定；
[0007]步骤二、t
‑
1时刻，对水底图像进行获取；
[0008]步骤三、图像预处理；
[0009]步骤四、对图像提取稀疏ORB特征；获得t
‑
1时刻图像的第一特征点集A；
[0010]步骤五、t时刻图像获取；
[0011]步骤六、通过稀疏ORB特征对t时刻图像进行光流场提取；
[0012]步骤七、根据光流场进行速度解算。
[0013]在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤一中，通过相机标定；获得相机内部参数矩阵为M，相机焦距为f。
[0014]在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤三中，图像预处理内容包括对水底图像进行滤波处理，减少图像的噪声。
[0015]在上述的一种适用于水下近底作业航行器的速度估计方法，所述步骤六中，光流场提取的具体方法为：
[0016]采用基于梯度的Lucas
‑
Kanade法，将步骤四获得的第一特征点集A中每一个特征点，在步骤五获得的t时刻图像找到对应的特征点，获得第二特征点集B；
[0017]再采用基于梯度的Lucas
‑
Kanade法，将第二特征点集B中每一个特征点，在步骤三获得的t
‑
1时刻图像找到对应的特征点，获得第三特征点集C；
[0018]在图像采集过程中存在噪声，因此第一特征点集A与第三特征点集C并不完全重
合；
[0019]选取误差和相似度两个标准对第一特征点集A与第三特征点集C进行滤波；选取相似度大且误差小的特征点，完成光流场的提取。
[0020]在上述的一种适用于水下近底作业航行器的速度估计方法，所述误差标准为第一特征点集A与第三特征点集C中对应特征点的距离；距离越小误差越小。
[0021]在上述的一种适用于水下近底作业航行器的速度估计方法，所述相似度标准为：
[0022]对第一特征点集A与第三特征点集C中的各组对应特征点，分别选取10像素
×
10像素大小的邻域；对两个邻域进行模板匹配，选择归一化相关系数匹配法，将匹配的结果作为相似度；匹配值越大越相似。
[0023]在上述的一种适用于水下近底作业航行器的速度估计方法，所述速度解算的具体方法为：
[0024]将t
‑
1时刻得到的第一特征点集A中各特征点记为将t时刻得到的第二特征点集B中的各特征点记为k＝1,2,
……
,n；n为特征点个数；
[0025]设定t
‑
1时刻，相机的坐标为t时刻，相机的坐标为相机的成像平面与航行器运动平面平行；建立航行器的体坐标系oxyz；建立相机坐标系o1x1y1z1；获得相机到航行器体坐标系的旋转矩阵相机在t
‑
1时刻至t时刻的移动量用单位矩阵H表示；将第二特征点集B中的各特征点在相机坐标系o1x1y1z1中的投影集合记为对应水底地面的点为设定相机内部参数矩阵为M，相机焦距为f；则各特征点在z1轴上的尺度为Z
c
；
[0026]图像更新频率大于60Hz；认为连续两帧图像之间，相机与水底距离h不发生变化；则t时刻有和
[0027]t
‑
1到t时刻n个特征点的稀疏光流为：
[0028][0029]式中，为t时刻的特征点；
[0030]为t
‑
1时刻的特征点；
[0031]Δt为时间差，Δt＝t
‑
(t
‑
1)；
[0032]光流与航行器的线运动和角运动的关系为：
[0033][0034]式中，V
b
为航行器线速度；
[0035]β为航行器体坐标系的x轴与航行器
‑
目标点之间连线的夹角；
[0036]w为航行器的角速率；
[0037]得到：
[0038][0039]式中，v
bx
为航行器在体坐标系下沿x方向的移动速度；
[0040]v
by
为航行器在体坐标系下沿y方向的移动速度；
[0041]Δu为投影到成像平面的像素移动总速度；
[0042]p为滚转角速率；通过IMU测量得到；
[0043]q为俯仰角速率；通过IMU测量得到；
[0044]h为相机与水底距离，通过水声高度计或声学多普勒计程仪得到；
[0045]光流法直接测量的是航行器在体坐标系下的移动速度V
b
＝(v
bx
,v
by
)；计算航行器的真实移动速度V：
[0046][0047]式中，为转换矩阵；
[0048][0049]式中，ψ为航行器的偏航角；
[0050]θ为航行器的俯仰角；
[0051]φ为航行器的横滚角。
[0052]在上述的一种适用于水下近底作业航行器的速度估计方法，所述航行器的体坐标系oxyz的建立方法为：
[0053]以航行器的质心为原点o，x方向指向航行器水下平移运动方向，z方向竖直向上；y方向由右手定则确定；
[0054]相机坐标系o1x1y1z1的建立方法为：
[0055]以相机光心为原点o1，x1方向与图像坐标系o2x2y2的x2方向平行；y1方向与图像坐标系o2x2y2的y2方向平行；z1由右手定则确定；
[0056]图像坐标系o2x2y2的建立方法为：
[0057]以图像左上角像素点为原点o2，x2轴指向图像像素列方向；y2方向指向图像像素行方向。
[0058]本专利技术与现有技术相比的有益效果是：
[0059](1)本专利技术基于光学图像设备，实现本文档来自技高网...

【技术保护点】

【技术特征摘要】
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为特...

【专利技术属性】
技术研发人员：王亭亭，卢曼曼，吕雪，董权威，岳才谦，田锋，
申请(专利权)人：中国航天空气动力技术研究院，
类型：发明
国别省市：