基于紧耦合图优化的视觉卫星融合高精度定位方法与流程

本发明涉及多传感器融合定位，尤其涉及基于紧耦合图优化的视觉卫星融合高精度定位方法。

背景技术：

1、随着移动机器人、自动驾驶、无人巡检等技术的发展，移动平台在大范围、长时间运行过程中，对定位结果的连续性、精度和全局一致性提出了较高要求。视觉惯性里程计能够融合相机图像和imu数据，输出高频连续的相对位姿，适合卫星信号不足场景下的短时定位，但其本质属于相对定位，易受特征误匹配、imu零偏和积分误差影响，在长时间运行后产生累积漂移，且难以直接提供稳定的全球绝对坐标。

2、rtk-gnss能够在开阔环境下提供厘米级绝对位置观测，不存在相对定位中的长期漂移问题。但在城市峡谷、林荫道路、建筑物遮挡等复杂环境中，rtk-gnss容易受到卫星遮挡、多路径反射和信号中断影响，导致解算状态由固定解退化为浮点解、单点解甚至无效解，进而出现定位精度下降、位置跳变或定位中断的问题，难以单独支撑移动平台连续可靠运行。

3、现有视觉定位与卫星定位融合方案多采用松耦合方式，即分别获得视觉惯性里程计和rtk-gnss的定位结果，再通过滤波算法对二者输出进行融合。该类方法通常只能修正当前时刻的位姿，难以将rtk-gnss绝对位置观测直接作用于视觉惯性里程计的历史关键帧、地图点和坐标变换参数，导致已经形成的累积漂移无法被充分回溯校正，轨迹和地图仍可能存在全局不一致问题。

4、因此，本发明提出了基于紧耦合图优化的视觉卫星融合高精度定位方法。在所述背景技术部分公开的上述信息仅用于加强对本公开的背景的理解，因此它可能包含不属于本领域普通技术人员公知常识的现有技术信息。

技术实现思路

1、本发明的目的是针对现有技术的不足之处，提供基于紧耦合图优化的视觉卫星融合高精度定位方法，解决上述背景技术中所提到的技术问题。

2、为实现上述目的，本发明提供如下技术方案：

3、基于紧耦合图优化的视觉卫星融合高精度定位方法，包括如下步骤：

4、s1、采集视觉图像、imu数据和rtk-gnss观测数据，对视觉图像进行特征提取与跟踪，对imu数据进行预积分，对rtk-gnss观测数据进行坐标统一和解算状态识别，生成视觉惯性关键帧、imu预积分约束和rtk观测序列；

5、s2、根据imu角速度序列和rtk位置差分序列构建运动强度序列，通过互相关运算估计vslam与rtk-gnss的时间系统偏差，校正rtk时间戳，并线性插值得到时间对齐rtk绝对位置观测；

6、s3、根据视觉惯性关键帧局部位置和时间对齐rtk绝对位置观测，采用奇异值分解求解sim(3)相似变换，将vslam局部坐标系对齐至rtk全局坐标系；

7、s4、以关键帧位姿、速度、imu零偏、地图点三维位置和sim(3)相似变换参数为状态变量，构建包含视觉重投影因子、imu预积分因子和rtk绝对位置因子的紧耦合图优化模型；

8、s5、基于三类因子的残差建立非线性最小二乘目标，并通过迭代优化联合校正状态变量；

9、s6、实时监测rtk-gnss解算状态和rtk绝对位置残差一致性，动态添加、降权或剔除rtk绝对位置因子，并在rtk信号恢复后继续加入rtk绝对位置因子；

10、s7、根据优化后的状态变量输出无漂移绝对位姿序列和全局一致三维地图。

11、s1具体包括：同步采集视觉图像、imu数据和rtk-gnss观测数据；对视觉图像进行去畸变、灰度化、特征提取、特征跟踪和误匹配剔除，生成视觉特征观测；对imu数据进行零偏校正、噪声滤除和预积分，生成相邻视觉惯性关键帧之间的imu预积分约束；对rtk-gnss观测数据进行投影坐标转换和解算状态识别，形成包含时间戳、绝对位置和解算状态的rtk观测序列。

12、s2具体包括：根据imu角速度构建第一运动强度序列，根据rtk位置差分构建第二运动强度序列；对第一运动强度序列和第二运动强度序列进行标准化互相关运算，将互相关系数最大时对应的时间偏移量确定为时间系统偏差；利用时间系统偏差校正rtk时间戳，并在每个视觉惯性关键帧时间戳前后搜索相邻rtk观测，通过线性插值得到时间对齐rtk绝对位置观测。

13、s3具体包括：选取解算状态为固定解或浮点解且已完成时间对齐的rtk绝对位置观测，并提取对应视觉惯性关键帧在vslam局部坐标系下的局部位置；分别计算局部位置序列和rtk全局位置序列的质心并进行去中心化处理；根据去中心化结果构建协方差矩阵，通过奇异值分解求解sim(3)相似变换的旋转矩阵、尺度因子和平移向量，将vslam局部坐标系初始对齐至rtk全局坐标系。

14、s4具体包括：将视觉惯性关键帧位姿、速度、imu零偏、地图点三维位置以及sim(3)相似变换参数作为状态变量；根据视觉特征观测构建视觉重投影因子，根据相邻视觉惯性关键帧之间的imu预积分约束构建imu预积分因子，根据时间对齐rtk绝对位置观测构建rtk绝对位置因子；并按照rtk-gnss解算状态为rtk绝对位置因子设置信息矩阵，使固定解权重大于浮点解权重，单点解或无效解不参与构建rtk绝对位置因子。

15、s5具体包括：基于视觉重投影因子、imu预积分因子和rtk绝对位置因子的残差及其信息矩阵建立非线性最小二乘目标；采用迭代优化算法对状态变量进行联合求解，使视觉观测误差、imu运动约束误差和rtk绝对位置误差同时收敛；并采用滑动窗口机制限制参与实时优化的视觉惯性关键帧数量，将移出窗口的历史关键帧约束转换为先验因子，以保留历史全局约束对后续优化的影响。

16、s6和s7具体包括：实时监测rtk-gnss解算状态和rtk绝对位置残差一致性，当rtk-gnss为固定解或浮点解且残差一致性满足条件时添加或保留rtk绝对位置因子，当rtk-gnss为单点解、无效解或残差异常时降权或剔除rtk绝对位置因子，并采用鲁棒核函数抑制粗差影响；根据优化后的状态变量输出包含时间戳、全局三维位置、姿态信息、融合模式和定位可信度的无漂移绝对位姿序列，并输出与rtk全局坐标系一致的三维地图。

17、本发明的有益效果在于：

18、本发明通过将视觉重投影因子、imu预积分因子和rtk绝对位置因子统一纳入紧耦合图优化模型，使rtk-gnss绝对位置观测能够参与关键帧位姿、地图点三维位置和坐标变换参数的联合校正，避免传统松耦合方式仅修正当前位姿的问题，有效抑制视觉惯性里程计的累积漂移。

19、本发明通过对imu角速度序列和rtk位置差分序列进行运动强度互相关运算，估计vslam与rtk-gnss之间的时间系统偏差，并通过线性插值得到时间对齐rtk绝对位置观测，降低不同传感器采样频率和时间基准不一致造成的融合误差。通过采用奇异值分解求解sim(3)相似变换，将vslam局部坐标系初始对齐至rtk全局坐标系，使视觉惯性关键帧、地图点和rtk绝对位置观测在统一空间基准下参与优化，提高定位结果和三维地图的全局一致性。

20、本发明通过根据rtk-gnss解算状态和rtk绝对位置残差一致性，动态添加、降权或剔除rtk绝对位置因子，并采用鲁棒核函数抑制视觉误匹配和rtk多路径粗差影响，提升城市峡谷、林荫道路、建筑物遮挡等复杂环境下的定位鲁棒性。采用滑动窗口优化和历史约束先验保留机制，在限制实时优化变量规模的同时保留历史全局约束对后续定位的影响，使系统能够连续输出无漂移绝对位姿序列和全局一致三维地图，满足移动机器人、自动驾驶和精密农业等大范围作业需求。