[发明专利]一种水下视觉惯导压力定位的紧耦合初始化方法

权利要求书

1.一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，包括如下步骤：

步骤（1）采用基于直接法或特征点法的slam方法求解欠尺度下的单目轨迹，选择数秒内的关键帧进行求解，保存关键帧的位姿和相关地图点信息，并判断当前地图中关键帧数量；

步骤（2）对相邻两帧关键帧之间的IMU信息进行预积分获得两帧关键帧图像之间的预积分伪测量，对关键帧与压力测量之间的IMU信息进行预积分获得图像与压力之间的预积分伪测量；

步骤（3）将所述图像之间的预积分伪测量和关键帧与压力测量之间的预积分伪测量统一到因子图中，建立残差方差，通过非线性优化求解初始化参数；

步骤（4）通过所述初始化参数更新地图：将初始参数代入系统中，更新系统的位姿轨迹，使其具有真实的尺度并将位姿表示在世界坐标系下；

步骤（5）将更新后的系统进行光束平差优化，完成初始化过程。

2.如权利要求1所述的一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，所述步骤（1），具体包括如下步骤：

（1.1）对当前每一帧的图像与之前的图像进行特征匹配，求解当前帧图像位姿，并对匹配到的特征点进行三角化，保存为地图点；

（1.2）对求解完位姿的图像与前一帧关键帧进行视差比较，若视差超过设定的阈值或当前帧追踪的特征点少于设定数量，则将当前帧决定为关键帧进行保存；

（1.3）判断当前地图中关键帧数量，若关键帧数量超过一定数量则进行视觉压力惯导的联合初始化，时间控制在数秒内。

3.如权利要求1所述的一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，所述步骤（2），具体包括如下步骤：

（2.1）提取相邻两帧关键帧图像之间的所有IMU信息进行预积分计算，其中IMU信息包括加速度信息和陀螺仪信息，将预积分初始姿态增量设为单位矩阵，将预积分初始速度增量、位置增量设为零向量，将测量协方差矩阵设为零矩阵，根据连续高频的IMU数据和IMU运动学模型对速度姿态增量、速度增量、位置增量和协方差矩阵进行更新，最后形成预积分伪测量；

（2.2）对相邻关键帧图像与压力测量之间的IMU信息进行预积分计算，该计算直接采用所述相邻两帧关键帧图像之间的所有IMU信息进行预积分计算的过程量。

4.如权利要求3所述的一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，所述步骤（3），具体包括：

保存所有图像之间的IMU信息预积分量和图像与压力测量之间的IMU信息预积分量，作为伪测量加入到后续的因子图中，同时将预积分中协方差作为该伪测量的不确定度，将图像的重映射误差、图像帧之间的IMU预积分误差以及图像与压力测量之间的IMU预积分误差建立统一的因子图，根据因子图建立非线性误差方程，通过两类IMU预积分伪测量与通过初始化参数预测得到的值进行做差得到残差方程，最后利用非线性优化的方法迭代求解，得到初始化参数值。

5.如权利要求4所述的一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，所述初始化参数，包括系统尺度、加速度偏置、陀螺仪偏置以及重力方向，通过尺度参数对地图进行缩放，主要为将摄像头轨迹与地图点坐标乘以尺度，将获得的摄像头轨迹和地图点缩放到真实世界尺度下；通过重力方向矫正摄像头轨迹与地图点方向，使得与重力方向一致；通过加速度偏置和陀螺仪偏置对所述的两类IMU预积分进行重积分。

6.如权利要求1所述的一种水下视觉惯导压力定位的紧耦合初始化方法，其特征在于，所述步骤（5），具体为：以更新后的位姿轨迹作为初值，重新建立残差方程进行光束平差求解，完成初始化过程。