[发明专利]一种水下相机-IMU-深度计的空间联合标定方法

权利要求书

1.一种水下相机-IMU-深度计的空间联合标定方法，其特征在于，步骤如下：

步骤(1)：获取多帧水下图像，利用传统的视觉里程计算法对相机进行初始化，获得各帧相机位姿和地图路标点信息；若初始化失败，则删除所有信息，重新执行本步骤；若相机初始化成功，执行下一步骤；

步骤(2)：在规划运动激励下，获取深度计和IMU测量数据；同时依据视觉测量信息将深度计离群点剔除；

步骤(3)：依据有效的相邻关键帧间视觉平移变量补偿深度计未知测量，通过视觉测量信息和深度计补偿信息，建立伪手眼标定方程组；

步骤(4)：利用有效的相邻关键帧间视觉变换信息和IMU预积分信息，构建手眼标定方程组；

步骤(5)：通过有效的相邻关键帧间IMU预积分平移信息补偿深度计未知信息，利用IMU预积分和深度计补偿信息，建立伪手眼标定方程组；

步骤(6)：将标定方程组求解问题转化为优化问题，得到相机-IMU-深度计外参变换矩阵初值；

步骤(7)：在滑动窗口下，构建包含外参的最小二乘残差函数，并引入空间变换约束，通过图优化方法求解，得到变换矩阵终值。

2.根据权利要求1所述的一种水下相机-IMU-深度计的空间联合标定方法，其特征在于，步骤(2)具体包括：

步骤(2.1)：针对深度计测量数据只发生在垂直方向这一特性，在深度计和IMU数据获取过程中，相机需满足垂直下视的要求，采用垂直方向绕圈的模式进行传感器激励，以获得可靠的IMU和深度数据；

步骤(2.2)：在深度计坐标系{d}下，相邻i时刻至i+1时刻两个图像关键帧间，深度计平移变换向量表示为：

式中，(·)_r1、(·)_r2、(·)_r3分别表示向量的第1、2、3行元素，应满足：

否则，剔除深度计i+1时刻的突变数据，同时将下一个有效深度数据定为i+1时刻，ε为视觉观测可信度阈值。

3.根据权利要求1所述的一种水下相机-IMU-深度计的空间联合标定方法，其特征在于，所述步骤(3)具体为：利用有效的两关键帧间的视觉位姿变换量和深度信息变换量，构建伪手眼标定方程：

其中，为i时刻相机坐标系指向深度计坐标系的位姿变换矩阵，即所求的相机-深度计外参矩阵，为3行3列矩阵，表示旋转变换，为3行1列向量，表示平移变换；在相机坐标系{c}下，表示相机i至i+1时刻的位姿变换矩阵；在深度计坐标系{d}下，表示深度计i至i+1时刻的位姿变换矩阵；由于对数据获取过程中的运动约束，采用：

构建伪测量数据，同理也可用此方法获得，其中β为相机旋转比例系数，||·||_F表示取矩阵F范数，I为三阶单位矩阵；

同时相机与深度计作为刚体固连在AUV上，它们的相对位置不随时间发生变化，则因此标定方程等效于：

展开得到：即

4.根据权利要求1所述的一种水下相机-IMU-深度计的空间联合标定方法，其特征在于，所述步骤(4)具体为：，通过两关键帧间的视觉位姿变换量和IMU预积分量，构建手眼标定方程：

即其中，为相机坐标系指向IMU坐标系的位姿变换矩阵，表示旋转变换，表示平移变换，即所求的相机-IMU外参矩阵；在IMU坐标系{b}下，表示IMU在i至i+1时刻的位姿预积分量；将旋转矩阵用四元数q表示：

式中，表示四元数相乘，(·)_L和(·)_R分别表示四元数的左乘和右乘矩阵变换。

5.根据权利要求1所述的一种水下相机-IMU-深度计的空间联合标定方法，其特征在于，所述步骤(5)具体为：利用有效的两关键帧间的IMU预积分量和深度信息变换量，构建伪手眼标定方程：

即

式中，为IMU坐标系指向深度计坐标系的位姿变换矩阵，表示旋转变换，表示平移变换，即所求的IMU-深度计外参矩阵；

构建伪测量数据：

同理也可求得，其中τ为IMU旋转比例系数，I为三阶单位矩阵，同时累积、构建伪手眼标定方程组。