[发明专利]基于约束相机运动与畸变校正的水下视觉SLAM方法

权利要求书

1.一种基于约束相机运动与畸变校正的水下视觉SLAM方法，其特征在于，包括以下步骤：

步骤1)传感器信息获取；

步骤2)基于单目相机的特征点法的前端视觉里程计；

步骤3)基于约束相机运动与图像畸变校正的后端优化；

步骤3-1)建立误差代价函数和增量方程并求解；

步骤3-2)利用约束拍摄相邻帧数图像以及整体相机运动路径的方法，创建约束局部相邻帧数以及整体水下速度优化相机运动方程；

步骤3-3)在创建SLAM词袋时，通过融合径向畸变、切向畸变以及散射系数的方法，对拍摄到的图像及其特征点进行畸变校正；

步骤4)基于特征元素相似度的回环检测；

步骤5)基于ORB特征匹配的SLAM建图。

2.根据权利要求1所述的基于约束相机运动与畸变校正的水下视觉SLAM方法，其特征在于，所述步骤3-1)具体包括：利用两相机的位置信息以及相对运动信息之间的相对关系，构建最小二乘问题进行优化求解，并在优化后估计相机的运动变化，对相机运动进行反馈；

首先根据两帧相机的位姿得到相对的位姿变换其中η′_ij为计算求得的拍摄两帧图像时相机的相对位移，其中，η_i为i帧的相机位姿信息，η_i^-1为i帧的相机位姿信息的逆矩阵，η_j为j帧的相机位姿信息；另外实际的拍摄两帧图像之间相机的实际相对位移η_ij，根据i，j两帧图像利用对极几何方法计算得到，优化的目标函数o_ij为：

mino_ij＝min(η_ij-η′_ij) (1)

其中，o_ij为目标函数，η_ij为拍摄两帧图像之间相机的实际相对位移；

对目标函数进行求解得到：

其中，为目标函数的泰勒展开，为目标函数的偏导数，δη_i和δη_j为两帧相机位姿的微小扰动。

3.根据权利要求1所述的基于约束相机运动与畸变校正的水下视觉SLAM方法，其特征在于，所述步骤3-2)具体包括：提出一种限制相机局部整体运动方法，记作C_{k,k+1,k+2,k+3,k+4…}为某一帧数的相机位置，C_k,k+1、C_k+1,k+2、C_k+2,k+3为相邻帧数之间相机的位移路径，T为每一帧相隔时间，通过速度优化公式来限制局部与整体速率为：

其中，k为相机运动过程内某一位置点，T为相机拍摄相邻帧数图片的时间，n为总拍摄图像总数-1。

4.根据权利要求1所述的基于约束相机运动与畸变校正的水下视觉SLAM方法，其特征在于，所述步骤3-3)具体包括：采用求解相机的径向畸变与切向畸变，从而解决特征点因为水下环境而发生的空间坐标与像素点坐标的畸变；

相机的坐标系点Q[X,Y,Z]通过四个畸变纠正系数h₁，h₂，l₁，l₂以及散射系数k_υ，获取正确的像素坐标[x_cd,y_cd]，即通过径向畸变和切向畸变的纠正：

其中，整体畸变矫正坐标为[x_cd,y_cd]，畸变坐标为[x,y]，h₁和h₂为中心区域和边缘区域的径向畸变系数，l₁和l₂为x和y轴方向上切向畸变系数，b²的数值为纠正位置的横纵坐标平方和，k_υ为散射系数，d_x、d_y分别为x，y轴方向上的偏移量；

畸变矫正后的图像及特征点，即算法提取的所有回环中是真实回环的概率：

其中，P为回环检测准确率，∑m_tp为算法是回环且事实也是回环的全部事件，∑n_fp为算法是回环但事实不是回环的全部事件。