CN117824625A - 基于改进视觉里程计的高坝大库水下环境感知与构图方法 - Google Patents

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 ₁ 是深水无效区域的像素强度上限值设定阈值，m ₁ 设置为0.98；

S23. 深水无效区域的像素强度最小值A _down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：

，

其中，m ₂ 是深水无效区域的像素强度最小值设定阈值，m ₂ 设置为0.5。

进一步地，所述步骤S3具体包括如下子步骤：

S31.在区间[A _down, A _up]里搜索当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量Pix _max和Pix _min：

，

S32.选择从Pix _max这个灰度级对应的像素点中寻找种子点，将所有符合生长条件的点的集合定义为H，Avg是当前集合H里所有像素点的平均值，I _p (x ₁,y ₁)是当前像素点的强度值，ΔI是A _up与Pix _max灰度级对应像素点之间的差值，ΔI=A _up-Pix _min从直方图中可以分析得到，β为自定义系数，来自适应控制I _p (x ₁,y ₁)- Avg的范围；当I _p (x ₁,y ₁)满足生长条件时，当前像素点将加入集合H，然后考察下一个像素点直到没有满足生长条件的像素点为止；根据图像间的灰度值差异建立的生长条件如下式：

，

S33.最终筛选出所有满足条件的像素点，分离出深水无效区域。

进一步地，所述步骤S5的具体方法为：

S51.建立AUV系统的模型：

，

其中，是IMU测得的系统状态，/>的误差协方差矩阵定义为R，y _k 是测量输出，d _k 为未知干扰输入，d _k 的协方差矩阵定义为过程噪声协方差矩阵Q；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P₀，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；

系统初始化输入的初始参数为：初始误差协方差矩阵P₀，初始过程噪声协方差矩阵Q₀，初始观测噪声协方差矩阵R₀，状态变量的初值x ₀和滚动时域长度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 ₁ 是深水无效区域的像素强度上限值设定阈值，m ₁ 设置为0.98；

S23. 深水无效区域的像素强度最小值A _down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：

，

其中，m ₂ 是深水无效区域的像素强度最小值设定阈值，m ₂ 设置为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 ₁,y ₁)是当前像素点的强度值，ΔI是A _up与Pix _max灰度级对应像素点之间的差值，ΔI=A _up-Pix _min从直方图中可以分析得到，β为自定义系数，来自适应控制I _p (x ₁,y ₁)- Avg的范围；当I _p (x ₁,y ₁)满足生长条件时，当前像素点将加入集合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；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P₀，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；

系统初始化输入的初始参数为：初始误差协方差矩阵P₀，初始过程噪声协方差矩阵Q₀，初始观测噪声协方差矩阵R₀，状态变量的初值x ₀和滚动时域长度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 ₁ 是深水无效区域的像素强度上限值设定阈值，m ₁ 设置为0.98；

S23. 深水无效区域的像素强度最小值A _down：深水无效区域的像素强度最小值分布在直方图的右侧，定义如下：

，

其中，m ₂ 是深水无效区域的像素强度最小值设定阈值，m ₂ 设置为0.5。

4.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S3具体包括如下子步骤：

S31.在区间[A _down, A _up]里搜索当前灰度级的像素点总和占所有像素点的比例hist的最大值和最小值对应像素数量Pix _max和Pix _min：

，

S32.选择从Pix _max这个灰度级对应的像素点中寻找种子点，将所有符合生长条件的点的集合定义为H，Avg是当前集合H里所有像素点的平均值，I _p (x ₁,y ₁)是当前像素点的强度值，ΔI是A _up与Pix _max灰度级对应像素点之间的差值，ΔI=A _up-Pix _min从直方图中可以分析得到，β为自定义系数，来自适应控制I _p (x ₁,y ₁)- Avg的范围；当I _p (x ₁,y ₁)满足生长条件时，当前像素点将加入集合H，然后考察下一个像素点直到没有满足生长条件的像素点为止；根据图像间的灰度值差异建立的生长条件如下式：

，

S33.最终筛选出所有满足条件的像素点，分离出深水无效区域。

5.根据权利要求1所述的基于改进视觉里程计的高坝大库水下环境感知与构图方法，其特征在于：所述步骤S5的具体方法为：

S51.建立AUV系统的模型：

，

其中，是IMU测得的系统状态，/>的误差协方差矩阵定义为R，y _k 是测量输出，d _k 为未知干扰输入，d _k 的协方差矩阵定义为过程噪声协方差矩阵Q；/>和/>分别是模型干扰和测量输出噪声， />的协方差矩阵定义为误差协方差矩阵P₀，A是估计系统状态的实数矩阵，M是估计系统干扰的实数矩阵，C是估计系统输出的实数矩阵；

系统初始化输入的初始参数为：初始误差协方差矩阵P₀，初始过程噪声协方差矩阵Q₀，初始观测噪声协方差矩阵R₀，状态变量的初值x ₀和滚动时域长度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的测量值。