CN117824625A - 基于改进视觉里程计的高坝大库水下环境感知与构图方法 - Google Patents
基于改进视觉里程计的高坝大库水下环境感知与构图方法 Download PDFInfo
- Publication number
- CN117824625A CN117824625A CN202410248924.7A CN202410248924A CN117824625A CN 117824625 A CN117824625 A CN 117824625A CN 202410248924 A CN202410248924 A CN 202410248924A CN 117824625 A CN117824625 A CN 117824625A
- Authority
- CN
- China
- Prior art keywords
- pixel
- imu
- value
- image
- points
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 88
- 230000000007 visual effect Effects 0.000 title claims abstract description 34
- 238000005259 measurement Methods 0.000 claims abstract description 16
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000005096 rolling process Methods 0.000 claims abstract description 15
- 238000000059 patterning Methods 0.000 claims abstract description 11
- 239000011159 matrix material Substances 0.000 claims description 36
- 238000005457 optimization Methods 0.000 claims description 29
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 20
- 206010034719 Personality change Diseases 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000004927 fusion Effects 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 abstract description 6
- 238000000605 extraction Methods 0.000 abstract description 5
- 238000010276 construction Methods 0.000 abstract description 4
- 238000004422 calculation algorithm Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 7
- 238000013507 mapping Methods 0.000 description 7
- 230000011218 segmentation Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 101100091501 Mus musculus Ros1 gene Proteins 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于改进视觉里程计的高坝大库水下环境感知与构图方法。首先，对视觉里程计中图像特征点提取的区域分割，将特征点较少的区域与路边特征分隔开，加快特征点提取过程；其次，保留特征点密集的区域，并利用特征点较少的区域完成直接法跟踪，增加视觉里程计的鲁棒性；最后，对于多扰动的环境，采用滚动时域估计优化惯性测量单元的位姿估计过程，提高同步定位与地图构建的稳定性。本发明提高了视觉里程计的特征点提取效率，减少不必要的像素点遍历，有效地提升视觉里程计中对于视角抖动的跟踪鲁棒性，并解决复杂环境下后端误差较大的问题，高效地完成环境感知与地图构建的任务。
Description
技术领域
本发明涉及自主水下航行器（Autonomous Underwater Vehicle，AUV）的同步定位与地图构建方法，尤其涉及基于改进视觉里程计的高坝大库水下环境感知与构图方法。
背景技术
AUV在水下探测中起着重要作用，例如在高坝大库水下坝面检测、海洋勘探及探测等。精确的导航和定位是水下探测中的关键技术，是限制AUV进行长时间、远距离深潜和水下自主作业的重要因素之一。在高坝大库水下环境中，成像环境更加复杂，受到的影响和干扰更为严重，存在障碍物、水流、浑浊的水质、光线衰减等因素，给AUV应用视觉系统完成导航定位、环境感知的任务带来了较大的挑战。
目前，同步定位与地图构建（Simultaneous Localization and Mapping，SLAM）算法已经有很多种，包括经典的ORB-SLAM2和VINS-Fusion等。上述方法将SLAM技术推广到了更多的应用场景，并实现了更多的功能。然而，这些方法在水下应用时存在一些问题，比如实时性不足、对水下环境干扰的鲁棒性不够等。相对而言，ORB-SLAM3在实时性方面表现较好，基本适用于水下环境。目前对于AUV的水下同步定位与地图构建方法还很缺乏，因此当前亟待解决的问题是如何通过更优化的方法，在提高视觉里程计的实时性的同时，增强SLAM系统框架的鲁棒性，目前相关报道较少。
发明内容
发明目的：为了解决在高坝大库复杂的水下环境中，现有的SLAM技术存在难以满足实时性、对数据处理性能要求高、且易受水下环境干扰造成误差较大等问题，本发明提出了一种基于改进视觉里程计的高坝大库水下环境感知与构图方法。
技术方案：
本发明提供一种基于改进视觉里程计的高坝大库水下环境感知与构图方法，该方法包括以下步骤：
S1.水下摄像头获取的原始图像，用图像直方图统计工具获取原始图像的直方图信息；
S2.在步骤S1得到的原始图像的直方图上确定深水无效区域的像素强度最小值A down和上限值A up；
S3.以深水无效区域的像素强度最小值A down和上限值A up作为当前灰度级的像素点总和占所有像素点的比例hist的最小值和最大值对应像素数量的搜索区间[A down, A up]；将当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量分别定义为Pix max和Pix min；选择从Pix max这个灰度级对应的像素点中寻找种子点，一旦种子点被确定，以该种子点为起始点进行区域生长，以分割深水无效区域，得到局部有效图像；
S4.将步骤S3得到的局部有效图像进行分割，两边各5%的区域用于直接法跟踪；中心区域用于SLAM过程中的特征点提取与匹配；
S5.基于ORB-SLAM3的视觉惯性融合SLAM框架，使用局部有效图像的中心区域作为视觉端的输入，利用IMU提供的角速度信息，将其融入相机姿态的估计中，通过将角速度积分得到相机的姿态变化，采用滚动时域估计优化IMU的测量值，基于后验最大值估计和光束法平差的图优化方法，通过融合局部有效图像的中心区域和采用滚动时域估计优化后的IMU测量值求解非线性优化问题，最终估计出AUV系统的位姿。
进一步地，所述步骤S1中所述直方图信息用于展示图像中各个颜色通道的像素分布情况，横轴表示像素值，纵轴表示具有相应像素值的像素数量；直方图的高峰代表图像中像素值集中的区域，而波峰和波谷则反映图像中的纹理或物体边缘。
进一步地，所述步骤S2具体包括如下子步骤：
S21.确定深水无效区域：摄像机在水下拍摄的图像中，处在图像高度50%及以上区域、在整幅图像的像素值大小位于前20%的像素点被确定为确定深水无效区域；
S22. 确定深水无效区域的像素强度上限值A up：将在直方图右侧第一个数据统计不为零的最大值是深水无效区域的强度上限值，定义为A up，表示如下：
，
其中，i是当前灰度级的归一化值，hist(j)代表当前灰度级的像素点总数占总像素点数的比例，m 1 是深水无效区域的像素强度上限值设定阈值，m 1 设置为0.98；
S23. 深水无效区域的像素强度最小值A down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：
，
其中，m 2 是深水无效区域的像素强度最小值设定阈值，m 2 设置为0.5。
进一步地，所述步骤S3具体包括如下子步骤：
S31.在区间[A down, A up]里搜索当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量Pix max和Pix min：
，
S32.选择从Pix max这个灰度级对应的像素点中寻找种子点，将所有符合生长条件的点的集合定义为H，Avg是当前集合H里所有像素点的平均值，I p (x 1,y 1)是当前像素点的强度值，ΔI是A up与Pix max灰度级对应像素点之间的差值，ΔI=A up-Pix min从直方图中可以分析得到，β为自定义系数，来自适应控制I p (x 1,y 1)- Avg的范围；当I p (x 1,y 1)满足生长条件时，当前像素点将加入集合H，然后考察下一个像素点直到没有满足生长条件的像素点为止；根据图像间的灰度值差异建立的生长条件如下式：
，
S33.最终筛选出所有满足条件的像素点，分离出深水无效区域。
进一步地，所述步骤S5的具体方法为：
S51.建立AUV系统的模型：
，
其中，是IMU测得的系统状态，/>的误差协方差矩阵定义为R，y k 是测量输出，d k 为未知干扰输入，d k 的协方差矩阵定义为过程噪声协方差矩阵Q；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P0，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；
系统初始化输入的初始参数为：初始误差协方差矩阵P0，初始过程噪声协方差矩阵Q0，初始观测噪声协方差矩阵R0，状态变量的初值x 0和滚动时域长度N；
S52.构建优化问题：
，
目标函数为：
，
其中，在T时刻时，w k 为噪声序列，w k =y k -Cx k ，N是滚动时域窗口的长度，为T-N时刻的先验估计状态，P T-N 是与先验估计状态/>和系统状态x T-N 之间的误差相对应的协方差矩阵；
S53.假设在T时刻，测量输出数据为且优化问题存在最优解/>，那么当前时刻的状态估计值通过下式计算：
，
当系统估计干扰的实数矩阵M中的先验估计L k 满足约束条件L k CM-M=0时，未知干扰输入d k 对系统的影响不用考虑，因此目标函数的形式改写为：
，
S54.建立状态估计方程：
，
其中，为矩阵/>的转置，I为单位矩阵，K k+1为k+1时刻的比例增益，由此推导出每一采样时刻的IMU状态估计值/>；
S55.假设在时刻T时，AUV系统内待优化的变量为：
，
其中，表示s帧关键帧，/>为相机到IMU的旋转矩阵，/>表示z个关键帧的特征点；在世界坐标系中IMU的位姿P w 、速度v w 和旋转R w ，以及IMU的加速度偏置b a ，IMU的角速度偏置b g 在第l帧a l 的状态为：
，
其中，；
基于相机模型和IMU模型的先验信息，采用非线性优化方法最小化误差，待优化变量为：
，
其中，表示第l帧a l 到第l+1帧a l+1 之间IMU的残差，/>表示点特征残差，/>为鲁棒柯西核函数；通过高斯牛顿法求解优化问题，得到最终位姿优化的结果；通过建立非线性优化估计器、构建待优化目标函数、计算当前时刻IMU状态估计值、计算下一时刻预测估计IMU状态值；随时间进行滚动更新IMU的测量值。
本发明相比现有技术的有益效果是：
本发明提供了一种基于改进视觉里程计的高坝大库水下环境感知与构图方法，建立了AUV视觉里程计的优化框架。解决了在复杂水下环境中同步定位与建图过程中实时性难以满足的问题，以及对数据处理性能要求较高和AUV容易受水下环境干扰的问题。采用区域生长方法分离出深水无效区域，从而减少了计算冗余，提高了实时性。面对多扰动的水下环境，采用滚动时域估计来优化AUV的位姿，从而提高了SLAM建图的稳定性。增强了视觉里程计的特征点提取效率，并解决了复杂环境下后端误差较大的问题。该发明对于改善AUV的定位与建图性能以及提高对水下环境扰动的抗性具有显著的效果。
附图说明
图1为本发明的总体框图；
图2为区域生长算法的流程图；
图3为改进视觉里程计的SLAM框架流程图；
图4为水下图像的特征点提取与匹配示意图；
图5为AUV的仿真轨迹。
具体实施方式
如图1所示，本实施例的一种基于改进视觉里程计的高坝大库水下环境感知与构图方法，该方法包括以下步骤：
S1.水下摄像头获取的原始图像，用图像直方图统计工具获取原始图像的直方图信息；
所述直方图信息用于展示图像中各个颜色通道的像素分布情况，横轴表示像素值，纵轴表示具有相应像素值的像素数量；直方图的高峰代表图像中像素值较为集中的区域，而波峰和波谷可能反映图像中的纹理或物体边缘。
S2.在步骤S1得到的原始图像的直方图上确定深水无效区域的像素强度最小值A down和上限值A up；具体包括如下子步骤：
S21.确定深水无效区域：摄像机在水下拍摄的图像中，处在约图像高度50%及以上区域、在整幅图像的像素值大小位于前20%的像素点被确定为确定深水无效区域；
S22. 确定深水无效区域的像素强度上限值A up：将在直方图右侧第一个数据统计不为零的最大值是深水无效区域的强度上限值，定义为A up，表示如下：
，
其中，i是当前灰度级的归一化值，hist(j)代表当前灰度级的像素点总数占总像素点数的比例，m 1 是深水无效区域的像素强度上限值设定阈值，m 1 设置为0.98；
S23. 深水无效区域的像素强度最小值A down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：
，
其中，m 2 是深水无效区域的像素强度最小值设定阈值，m 2 设置为0.5。
S3.以深水无效区域的像素强度最小值A down和上限值A up作为当前灰度级的像素点总和占所有像素点的比例hist的最小值和最大值对应像素数量的搜索区间[A down, A up]；将当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量分别定义为Pix max和Pix min；选择从Pix max这个灰度级对应的像素点中寻找种子点，一旦确定了种子点的选择，便以该种子点为起始点进行区域生长，以将深水无效区域逐步分割出来。新的像素点会不断地考察其周围的像素点，判断其是否符合区域生长的条件并进行区域生长。这个过程迭代进行，新的像素点在被加入之后，会再次成为种子点引导下一轮的生长，直到没有更多满足条件的像素点为止。通过区域生长的迭代，逐步扩展并分割深水无效区域。区域生长的条件可根据深水无效区域的特征进行调整，以实现对深水区域的精确分割，得到局部有效图像；区域生长算法具有一定的自适应性，它能够根据图像的实际情况调整分割的策略，因此可以在保持较高分割准确性的同时，有效降低计算复杂度。具体包括如下子步骤：
S31.在区间[A down, A up]里搜索当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量Pix max和Pix min：
，
S32.选择从Pix max这个灰度级对应的像素点中寻找种子点，将所有符合生长条件的点的集合定义为H，Avg是当前集合H里所有像素点的平均值，I p (x 1,y 1)是当前像素点的强度值，ΔI是A up与Pix max灰度级对应像素点之间的差值，ΔI=A up-Pix min从直方图中可以分析得到，β为自定义系数，来自适应控制I p (x 1,y 1)- Avg的范围；当I p (x 1,y 1)满足生长条件时，当前像素点将加入集合H，然后考察下一个像素点直到没有满足生长条件的像素点为止；根据图像间的灰度值差异建立的生长条件如下式：
，
S33.最终筛选出所有满足条件的像素点，分离出深水无效区域。
S4.将步骤S3得到的局部有效图像进行分割，两边各5%的区域用于直接法跟踪；中心区域用于SLAM过程中的特征点提取与匹配；如图2所示，将去除深水无效区域后的图像剩余部分进行分割。其中，中心区域用于SLAM过程中的特征点提取与匹配，而图像两边各5%的边缘区域则用于直接法的跟踪。相对于特征点法，直接法不需要计算描述子，因此节省了大量时间。通过采用直接法处理图像边缘信息，可以提高视觉里程计的跟踪速度与效率。在水下多干扰环境中，这种方法可以为AUV提供更高的抗干扰性。直接法对于光照变化和纹理缺失的适应性更强。边缘区域的信息能够增强系统在特殊环境下的稳定性，使AUV能够更可靠地感知并跟踪环境中的目标。通过在SLAM过程中巧妙地利用不同区域的信息，系统在复杂水下环境中的表现得到了优化和增强。
S5.基于ORB-SLAM3的视觉惯性融合SLAM框架，使用局部有效图像的中心区域作为视觉端的输入，利用IMU提供的角速度信息，将其融入相机姿态的估计中，通过将角速度积分得到相机的姿态变化，采用滚动时域估计优化IMU的测量值，基于后验最大值估计和光束法平差的图优化方法，通过融合局部有效图像的中心区域和采用滚动时域估计优化后的IMU测量值求解非线性优化问题，最终估计出AUV系统的位姿，在实际水下环境中，大多数系统通常具有非线性，甚至是强非线性的特性。同时，某些系统对系统状态和噪声有一定的约束限制，而一些系统的测量也带有一定的不确定性。因此，采用滚动时域估计来优化IMU提供的AUV位姿是合理的选择。结合了预测控制的滚动优化原理，可以通过求解在线滚动优化问题，得到满足期望约束的估计器。这个方法充分考虑了水下环境的复杂性，对系统非线性和约束进行有效处理。通过在时域中滚动地对AUV的状态进行估计和优化，可以更好地适应水下环境的动态变化和不确定性。这种滚动时域估计的方法有助于提高AUV在复杂水下环境中的定位与导航性能，使其更具鲁棒性和可靠性。具体方法为：
S51.建立AUV系统的模型：
，
其中，是IMU测得的系统状态，/>的误差协方差矩阵定义为R，y k 是测量输出，d k 为未知干扰输入，d k 的协方差矩阵定义为过程噪声协方差矩阵Q；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P0，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；
系统初始化输入的初始参数为：初始误差协方差矩阵P0，初始过程噪声协方差矩阵Q0，初始观测噪声协方差矩阵R0，状态变量的初值x 0和滚动时域长度N；
S52.构建优化问题：
，
目标函数为：
，
其中，在T时刻时，w k 为噪声序列，w k =y k -Cx k ，N是滚动时域窗口的长度，为T-N时刻的先验估计状态，P T-N 是与先验估计状态/>和系统状态x T-N 之间的误差相对应的协方差矩阵；
S53.假设在T时刻，测量输出数据为且优化问题存在最优解/>，那么当前时刻的状态估计值可通过下式计算：
，
当系统估计干扰的实数矩阵M中的先验估计L k 满足约束条件L k CM-M=0时，未知干扰输入d k 对系统的影响可以不用考虑，因此目标函数的形式改写为：
，
S54.建立状态估计方程：
，
其中，为矩阵/>的转置，I为单位矩阵，K k+1为k+1时刻的比例增益，由此推导出每一采样时刻的IMU状态估计值/>；
S55.最大后验估计是一种贝叶斯推断的方法，通过考虑先验信息和观测数据，计算后验分布，并找到使后验分布最大化的估计值。在视觉里程计中，先验信息为对相机运动、IMU测量精度等的先验。最大后验估计的优点在于可以灵活地整合不同类型的信息。光束法平差对系统的状态进行全局优化，以最小化相机位姿和场景结构的重投影误差。这时候考虑了视觉和IMU的测量信息，帮助提高定位的准确性。假设在时刻T时，AUV系统内待优化的变量为：
，
其中，表示s帧关键帧，/>为相机到IMU的旋转矩阵，/>表示z个关键帧的特征点；在世界坐标系中IMU的位姿P w 、速度v w 和旋转R w ，以及IMU的加速度偏置b a ，IMU的角速度偏置b g 在第l帧a l 的状态为：
，
其中，；
基于相机模型和IMU模型的先验信息，采用非线性优化方法最小化误差，待优化变量为：
，
其中，表示第l帧a l 到第l+1帧a l+1 之间IMU的残差，/>表示点特征残差，/>为鲁棒柯西核函数；通过高斯牛顿法即可求解优化问题，得到最终位姿优化的结果；通过建立非线性优化估计器、构建待优化目标函数、计算当前时刻IMU状态估计值、计算下一时刻预测估计IMU状态值；随时间进行滚动更新IMU的测量值。本发明基于改进视觉里程计的高坝大库水下环境感知与构图方法的总体扩展流程图如图3所示。基于SLAM的流程中加入了本算法，加入了AUV视觉里程计的优化框架。显著提高同步定位与建图的实时性，同时增强AUV在复杂水下环境中的鲁棒性，使其更好地应对水下环境的各种扰动和干扰。改善了AUV的定位与建图的性能，提高对水下环境扰动的鲁棒性。
基于改进视觉里程计的高坝大库水下环境感知与构图方法实验仿真环境为：GPU-NVIDIA RTX3060，CPU i7-12700H，Ubuntu 20.04，ROS 1 Melodic。
选择公开数据集Aqualoc验证算法，选择harbor数据集。为了验证本算法的性能，选择ORB-SLAM3算法进行比较。表1展示了ORB-SLAM3与本发明方法在绝对轨迹误差ATE的比较，可以看出本发明能够在水下环境中显著的提升AUV系统的鲁棒性，将系统面对未知环境干扰的稳定性提升了20%，精度提升了20%。
表1.ORB-SLAM3与本发明方法在绝对轨迹误差ATE的比较：
注：“X”表示跟丢、数据丢失等情况
由此可见，本发明基于改进视觉里程计的高坝大库水下环境感知与构图方法能够在水下环境中显著的提升AUV系统的鲁棒性与精度。具体实验图可参考图4和图5，其中图4为水下图像的特征点提取与匹配，图5为AUV的仿真轨迹。
Claims (5)
1.一种基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：该方法包括以下步骤：
S1.水下摄像头获取的原始图像，用图像直方图统计工具获取原始图像的直方图信息；
S2.在步骤S1得到的原始图像的直方图上确定深水无效区域的像素强度最小值A down和上限值A up；
S3.以深水无效区域的像素强度最小值A down和上限值A up作为当前灰度级的像素点总和占所有像素点的比例hist的最小值和最大值对应像素数量的搜索区间[A down, A up]；将当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量分别定义为Pix max和Pix min；选择从Pix max这个灰度级对应的像素点中寻找种子点，一旦种子点被确定，以该种子点为起始点进行区域生长，以分割深水无效区域，得到局部有效图像；
S4.将步骤S3得到的局部有效图像进行分割，两边各5%的区域用于直接法跟踪；中心区域用于SLAM过程中的特征点提取与匹配；
S5.基于ORB-SLAM3的视觉惯性融合SLAM框架，使用局部有效图像的中心区域作为视觉端的输入，利用IMU提供的角速度信息，将其融入相机姿态的估计中，通过将角速度积分得到相机的姿态变化，采用滚动时域估计优化IMU的测量值，基于后验最大值估计和光束法平差的图优化方法，通过融合局部有效图像的中心区域和采用滚动时域估计优化后的IMU测量值求解非线性优化问题，最终估计出AUV系统的位姿。
2.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S1中所述直方图信息用于展示图像中各个颜色通道的像素分布情况，横轴表示像素值，纵轴表示具有相应像素值的像素数量；直方图的高峰代表图像中像素值集中的区域，而波峰和波谷则反映图像中的纹理或物体边缘。
3.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S2具体包括如下子步骤：
S21.确定深水无效区域：摄像机在水下拍摄的图像中，处在图像高度50%及以上区域、在整幅图像的像素值大小位于前20%的像素点被确定为确定深水无效区域；
S22. 确定深水无效区域的像素强度上限值A up：将在直方图右侧第一个数据统计不为零的最大值是深水无效区域的强度上限值，定义为A up，表示如下：
，
其中，i是当前灰度级的归一化值，hist(j)代表当前灰度级的像素点总数占总像素点数的比例，m 1 是深水无效区域的像素强度上限值设定阈值，m 1 设置为0.98；
S23. 深水无效区域的像素强度最小值A down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：
，
其中，m 2 是深水无效区域的像素强度最小值设定阈值，m 2 设置为0.5。
4.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S3具体包括如下子步骤：
S31.在区间[A down, A up]里搜索当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量Pix max和Pix min：
，
S32.选择从Pix max这个灰度级对应的像素点中寻找种子点，将所有符合生长条件的点的集合定义为H，Avg是当前集合H里所有像素点的平均值，I p (x 1,y 1)是当前像素点的强度值，ΔI是A up与Pix max灰度级对应像素点之间的差值，ΔI=A up-Pix min从直方图中可以分析得到，β为自定义系数，来自适应控制I p (x 1,y 1)- Avg的范围；当I p (x 1,y 1)满足生长条件时，当前像素点将加入集合H，然后考察下一个像素点直到没有满足生长条件的像素点为止；根据图像间的灰度值差异建立的生长条件如下式：
，
S33.最终筛选出所有满足条件的像素点，分离出深水无效区域。
5.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S5的具体方法为：
S51.建立AUV系统的模型：
，
其中，是IMU测得的系统状态，/>的误差协方差矩阵定义为R，y k 是测量输出，d k 为未知干扰输入，d k 的协方差矩阵定义为过程噪声协方差矩阵Q；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P0，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；
系统初始化输入的初始参数为：初始误差协方差矩阵P0，初始过程噪声协方差矩阵Q0，初始观测噪声协方差矩阵R0，状态变量的初值x 0和滚动时域长度N；
S52.构建优化问题：
，
目标函数为：
，
其中，在T时刻时，w k 为噪声序列，w k =y k -Cx k ，N是滚动时域窗口的长度，为T-N时刻的先验估计状态，P T-N 是与先验估计状态/>和系统状态x T-N 之间的误差相对应的协方差矩阵；
S53.假设在T时刻，测量输出数据为且优化问题存在最优解/>，那么当前时刻的状态估计值通过下式计算：
，
当系统估计干扰的实数矩阵M中的先验估计L k 满足约束条件L k CM-M=0时，未知干扰输入d k 对系统的影响不用考虑，因此目标函数的形式改写为：
，
S54.建立状态估计方程：
，
其中，为矩阵/>的转置，I为单位矩阵，K k+1为k+1时刻的比例增益，由此推导出每一采样时刻的IMU状态估计值/>；
S55.假设在时刻T时，AUV系统内待优化的变量为：
，
其中，表示s帧关键帧，/>为相机到IMU的旋转矩阵，/>表示z个关键帧的特征点；在世界坐标系中IMU的位姿P w 、速度v w 和旋转R w ，以及IMU的加速度偏置b a ，IMU的角速度偏置b g 在第l帧a l 的状态为：
，
其中，；
基于相机模型和IMU模型的先验信息，采用非线性优化方法最小化误差，待优化变量为：
，
其中，表示第l帧a l 到第l+1帧a l+1 之间IMU的残差，/>表示点特征残差，/>为鲁棒柯西核函数；通过高斯牛顿法求解优化问题，得到最终位姿优化的结果；通过建立非线性优化估计器、构建待优化目标函数、计算当前时刻IMU状态估计值、计算下一时刻预测估计IMU状态值；随时间进行滚动更新IMU的测量值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410248924.7A CN117824625B (zh) | 2024-03-05 | 2024-03-05 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410248924.7A CN117824625B (zh) | 2024-03-05 | 2024-03-05 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117824625A true CN117824625A (zh) | 2024-04-05 |
| CN117824625B CN117824625B (zh) | 2024-05-14 |
Family
ID=90519475
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410248924.7A Active CN117824625B (zh) | 2024-03-05 | 2024-03-05 | 基于改进视觉里程计的高坝大库水下环境感知与构图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117824625B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107957727A (zh) * | 2016-10-17 | 2018-04-24 | 江苏舾普泰克自动化科技有限公司 | 水下机器人控制系统及动力定位方法 |
| CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
| US20190242711A1 (en) * | 2018-02-08 | 2019-08-08 | Raytheon Company | Image geo-registration for absolute navigation aiding using uncertainy information from the on-board navigation system |
| CN110866887A (zh) * | 2019-11-04 | 2020-03-06 | 深圳市唯特视科技有限公司 | 一种基于多传感器的目标态势融合感知方法和系统 |
| CN113625774A (zh) * | 2021-09-10 | 2021-11-09 | 天津大学 | 局部地图匹配与端到端测距多无人机协同定位系统和方法 |
| CN114488164A (zh) * | 2022-01-17 | 2022-05-13 | 清华大学深圳国际研究生院 | 水下航行器同步定位与建图方法及水下航行器 |
| WO2022205526A1 (zh) * | 2021-04-01 | 2022-10-06 | 江苏科技大学 | 一种用于水下无人机器人集群的动态定位信息融合方法 |
| WO2023082050A1 (zh) * | 2021-11-09 | 2023-05-19 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
| CN116182837A (zh) * | 2023-03-16 | 2023-05-30 | 天津大学 | 基于视觉激光雷达惯性紧耦合的定位建图方法 |
| CN116380079A (zh) * | 2023-04-12 | 2023-07-04 | 东南大学 | 一种融合前视声呐与orb-slam3的水下slam方法 |
-
2024
- 2024-03-05 CN CN202410248924.7A patent/CN117824625B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107957727A (zh) * | 2016-10-17 | 2018-04-24 | 江苏舾普泰克自动化科技有限公司 | 水下机器人控制系统及动力定位方法 |
| US20190242711A1 (en) * | 2018-02-08 | 2019-08-08 | Raytheon Company | Image geo-registration for absolute navigation aiding using uncertainy information from the on-board navigation system |
| CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
| CN110866887A (zh) * | 2019-11-04 | 2020-03-06 | 深圳市唯特视科技有限公司 | 一种基于多传感器的目标态势融合感知方法和系统 |
| WO2022205526A1 (zh) * | 2021-04-01 | 2022-10-06 | 江苏科技大学 | 一种用于水下无人机器人集群的动态定位信息融合方法 |
| CN113625774A (zh) * | 2021-09-10 | 2021-11-09 | 天津大学 | 局部地图匹配与端到端测距多无人机协同定位系统和方法 |
| WO2023082050A1 (zh) * | 2021-11-09 | 2023-05-19 | 浙江大学 | 一种基于双层滤波框架的高精度里程估计方法 |
| CN114488164A (zh) * | 2022-01-17 | 2022-05-13 | 清华大学深圳国际研究生院 | 水下航行器同步定位与建图方法及水下航行器 |
| CN116182837A (zh) * | 2023-03-16 | 2023-05-30 | 天津大学 | 基于视觉激光雷达惯性紧耦合的定位建图方法 |
| CN116380079A (zh) * | 2023-04-12 | 2023-07-04 | 东南大学 | 一种融合前视声呐与orb-slam3的水下slam方法 |
Non-Patent Citations (2)
| Title |
|---|
| 张易;项志宇;陈舒雅;顾淑霞;: "弱纹理环境下视觉里程计优化算法研究", 光学学报, no. 06, 30 January 2018 (2018-01-30) * |
| 李琦;蒋蓁;: "考虑特征误匹配的双目视觉里程计", 工业控制计算机, no. 08, 25 August 2018 (2018-08-25) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117824625B (zh) | 2024-05-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109949375B (zh) | 一种基于深度图感兴趣区域的移动机器人目标跟踪方法 | |
| CN111462175B (zh) | 时空卷积孪生匹配网络目标跟踪方法、装置、介质及设备 | |
| CN110796010B (zh) | 一种结合光流法和卡尔曼滤波的视频稳像方法 | |
| US20170316569A1 (en) | Robust Anytime Tracking Combining 3D Shape, Color, and Motion with Annealed Dynamic Histograms | |
| CN110569704A (zh) | 一种基于立体视觉的多策略自适应车道线检测方法 | |
| CN106338733B (zh) | 基于蛙眼视觉特性的前视声呐目标跟踪方法 | |
| CN103871039B (zh) | 一种sar图像变化检测差异图生成方法 | |
| CN111242864B (zh) | 一种基于Gabor纹理约束的手指静脉图像修复方法 | |
| CN110246151B (zh) | 一种基于深度学习和单目视觉的水下机器人目标跟踪方法 | |
| CN108198201A (zh) | 一种多目标跟踪方法、终端设备及存储介质 | |
| CN107169972B (zh) | 一种非合作目标快速轮廓跟踪方法 | |
| CN110555868A (zh) | 一种复杂地面背景下运动小目标检测方法 | |
| CN114549549B (zh) | 一种动态环境下基于实例分割的动态目标建模跟踪方法 | |
| US20180247418A1 (en) | Method and apparatus for object tracking and segmentation via background tracking | |
| Kong et al. | A method for learning matching errors for stereo computation. | |
| CN115222884A (zh) | 一种基于人工智能的空间对象分析及建模优化方法 | |
| CN113487631B (zh) | 基于lego-loam的可调式大角度探测感知及控制方法 | |
| CN116758421A (zh) | 一种基于弱监督学习的遥感图像有向目标检测方法 | |
| CN115375733A (zh) | 基于视频和点云数据的雪车雪橇三维滑行轨迹提取方法 | |
| CN111161308A (zh) | 一种基于关键点匹配的双波段融合目标提取方法 | |
| CN101344968A (zh) | 星空背景图像中的运动补偿方法 | |
| CN104200434A (zh) | 一种基于噪声方差估计的非局部均值图像去噪方法 | |
| CN113920254B (zh) | 一种基于单目rgb的室内三维重建方法及其系统 | |
| CN114648547A (zh) | 用于反无人机红外探测系统的弱小目标检测方法和装置 | |
| CN112200831B (zh) | 一种基于动态模板的密集连接孪生神经网络目标跟踪方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |