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

说明书

本发明属于机器人定位领域，尤其涉及一种水下视觉惯导压力定位的紧耦合初始化方法，通过传统单目SLAM的方法获得欠尺度的机器人位姿和地图特征点，将相邻图像之间的IMU数据进行预积分建立图像之间的预积分残差，将相邻图像与压力测量之间的IMU数据进行预积分建立图像与压力测量之间的预积分残差，通过非线性优化的方法求解系统的初始化参数，利用初始化参数更新地图，将更新后的系统进行光束平差优化，完成初始化过程，获得更精确的结果，以此推动系统的运行。本发明利用高频的IMU信息耦合不同时间步下的图像信息和压力信息，加强了各初始化参数之间的耦合度，提高了初始化算法的求解精度。

技术领域

本发明属于机器人定位领域，尤其涉及一种水下视觉惯导压力定位的紧耦合初始化方法。

背景技术

在水下机器人定位中，基于图优化的多传感器融合定位是常用的方法，此类方法往往需要进行合适的初始化来推动系统的正常运行。对于视觉惯导压力三者融合来说，需要确定系统初始尺度、IMU偏置和初始姿态等参数。由于摄像头、IMU和压力传感器时间戳往往不同步，并且摄像头和压力传感器的频率较低，常用的方法为对两帧图像之间的IMU进行预积分建立预积分残差因子，并将压力传感器信息关联到邻近的图像帧上，以此建立因子图进行优化求解。显然，由于时间戳的不同步，与图像信息关联的压力测量并非是该时刻最准确的测量值，因此该方法求解的是次优解，导致最后求解的初始化参数会带来较大误差，而初始化参数的误差很大程度上会影响后续的机器人定位精度。对于需要依赖机器人定位进行后续高精度导航的机器人应用，该种误差是不被允许的。

发明内容

为了解决现有技术中存在的上述技术问题，本发明提出了一种水下视觉惯导压力定位的紧耦合初始化方法，通过对连续图像之间的IMU信息进行预积分，对图像和相邻压力测量之间的IMU信息进行预积分，并结合重映射误差建立因子图进行优化求解。利用高频的IMU信息耦合不同时间步下的图像信息和压力信息，加强了各初始化参数之间的耦合度，提高了初始化算法的求解精度，其具体技术方案如下：

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

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

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

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

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

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

进一步的，所述步骤（1），具体包括如下步骤：

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

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

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

进一步的，所述步骤（2），具体包括如下步骤：