[发明专利]一种基于惯导复用的视觉、惯导、卫导融合定位方法

说明书

本发明公开了一种基于惯导复用的视觉、惯导、卫导融合定位方法。首先将惯导、双目相机、卫星天线三种传感器刚性固连于同一载体，标定惯导零偏、相机内参和三种传感器间的变换矩阵，接着通过时间戳匹配惯导和视觉数据，基于图优化的视觉SLAM框架对惯导和视觉数据进行融合，得到与视觉数据同频率的视觉‑惯导一级定位结果，通过时间戳匹配惯导和卫导数据，基于扩展卡尔曼滤波对惯导和卫导数据进行融合，得到与惯导数据同频率的惯导‑卫导一级定位结果，然后使用图优化的方式将两种一级定位结果进行二级融合，得到最终的位姿结果。本发明能够通过复用惯导数据提升卫导的位姿输出频率，减小惯导、视觉、卫导数据的时间匹配误差，提高位姿精度。

技术领域

本发明属于多源融合定位技术领域，具体涉及一种基于惯导复用的视觉、惯导、卫导融合定位方法。

背景技术

传统的视觉、惯导、卫导多源融合定位技术是先将视觉与惯导数据进行融合，得到定位结果，再将定位结果与卫导数据进行融合，输出最终的定位结果。在已知惯导和卫导数据的情况下，也可先通过惯导与卫导数据融合，提升定位输出频率，再参与后续融合。现有的视觉、惯导、卫导多源融合定位方法至少存在如下技术问题：

一是，多源数据频率不同，卫导数据频率较低，在多源数据匹配过程中时间误差较大；

二是，卫星信号受外界环境影响大，不同环境定位精度不同，直接参与多源融合定位对最终定位精度影响大；

三是，对多源数据中的卫导数据仅作为全局优化中的一个残差项，未充分利用卫导数据进行定位解算。

由此可知，现有技术方法中存在多源不同频数据匹配时间误差大，环境对卫导定位精度的影响亦会影响最终定位精度，未充分利用多源数据的技术问题。

发明内容

针对现有技术不足，本发明提供一种基于惯导复用的视觉、惯导、卫导融合定位方法，用以解决现有技术方法中存在多源不同频数据匹配时间误差大、环境影响卫导定位精度进而影响最终定位精度、未充分利用多源数据的技术问题。

为了达到上述目的，本发明提供的技术方案是一种基于惯导复用的视觉、惯导、卫导融合定位方法，包括以下步骤：

步骤1，获取惯导零偏、双目相机的内参矩阵以及惯导、双目相机、卫星天线三种传感器之间的变换矩阵；

步骤2，获得惯导的三轴加速度和三轴角速度数据、双目视觉数据、，通过时间戳对惯导的加速度、角速度与双目视觉数据进行匹配，基于图优化的视觉SLAM框架对惯导和视觉数据进行融合，计算得到与双目视觉数据同频率的视觉-惯导一级定位结果；

步骤2.1，根据惯导的三轴加速度和三轴角速度数据计算第 k帧和第 k+1帧之间的积分结果，得到 k+1帧的位置、速度和姿态；

步骤2.2，根据步骤2.1计算出的预积分信息，实现视觉、惯导相互约束，完成初始化；

步骤2.3，构建目标函数，目标函数包含边缘化先验信息、惯性测量残差和视觉重投影残差三部分，通过优化各帧状态向量，使目标函数达到最小值，解算出各帧最终的位姿结果；

步骤3，获得惯导的三轴加速度和三轴角速度数据、卫导数据，通过时间戳对惯导的角速度、加速度和卫导数据进行匹配，基于扩展卡尔曼滤波对惯导和卫导数据进行融合，计算得到与惯导数据同频率的惯导-卫导一级定位结果；

步骤4，利用时间戳对上述计算得出的两个一级定位结果和进行匹配，使用两个一级定位结果构建目标函数，进行全局优化，得到最终的定位结果。