一种基于视觉-惯性-卫星多源传感器融合的导航定位方法

本技术涉及导航定位，具体而言，涉及一种基于视觉-惯性-卫星多源传感器融合的导航定位方法。

背景技术：

1、随着无人机在物流运输、城市巡检、农业测绘、灾害应急与军事侦察等领域的快速普及，其作业环境呈现高度复杂性和多样性，对无人机导航定位系统的精度、稳定性以及自主决策能力提出了前所未有的高要求。现阶段，无人机自主飞行普遍依赖全球卫星导航系统（gnss）提供的绝对位置信息。然而，在城市高楼“峡谷”、深山峡谷、隧道、森林覆盖区、室内空间以及受到电磁干扰或欺骗攻击的环境中，gnss信号常遭受遮挡、干扰或恶意欺骗，导致导航精度明显下降甚至完全不可用。因此，在gnss不可用或信号极不稳定的情况下实现无人机的持续、高精度定位，已成为无人机自主导航领域亟待攻克的关键科学与工程难题。

2、目前，室外无人机导航通常采用gnss与惯性导航系统（ins）组合的方案。ins能够在短时间内稳定输出位置、速度与姿态信息，并且不依赖任何外部电磁信号，可在gnss短时中断时维持连续导航。然而，该类组合导航在本质上仍依赖gnss对惯导误差进行长期约束。尤其在gnss信号极弱或完全失锁的情况下，惯性器件的加速度与角速度测量误差会累积放大，引起位置与姿态估计随时间快速漂移。对于工程领域广泛采用的低成本工业级惯导器件，这一问题更为突出，使其难以满足长期高精度的自主导航需求。

3、随着计算机视觉技术的快速发展，基于视觉的定位方法逐渐成为gnss/ins组合导航的重要补充甚至替代路线。视觉传感器能够从环境中获取丰富的纹理、边缘和几何结构信息，即便在gnss受限的城市街区、室内空间或林下区域，也能提取稳定特征用于运动估计。视觉里程计可以通过特征提取、匹配与跟踪，有效抑制惯导系统的累计误差。此外，视觉传感器属于无源传感器，不会主动发射电磁信号，在隐蔽性要求较高的军事任务场景中具有天然优势。然而，仅依赖视觉信息的导航方案在复杂场景下仍存在显著局限。现有特征跟踪方法往往基于灰度恒定、小位移等理想假设，难以在光照快速变化、遮挡频繁、动态物体干扰或剧烈视角变化的情况下保持稳健性。尽管基于sift、orb等局部描述子的特征匹配方法提升了光照与视角变化下的鲁棒性，但其计算耗时较长，对无人机高速机动或高帧率视觉里程计而言难以兼顾实时性与精度。

4、为提升无人机导航系统在复杂环境中的适应性，多源传感器融合技术逐渐成为主流方案。典型做法是将视觉、imu、gnss等多源观测在统一框架下进行状态估计（如公开号为cn120121043a的专利文献）。然而，该类方案通常以内置imu状态为主变量，其他传感器作为附加因子参与因子图优化，通过变量消元机制强化imu在估计过程中的主导地位。此策略缺乏针对传感器观测质量的自适应加权机制，无法依据环境变化动态调整各类观测的权重。在实际应用中，各传感器的观测质量常随场景变化而显著波动，例如视觉观测可能因光照不足、遮挡或运动模糊而退化，gnss观测可能受多路径或电离层扰动影响，而imu则可能因温度漂移或机械振动产生偏置变化。如果融合算法不能依据观测的实时质量对其赋予合理权重，则可能出现低质量观测反而主导导航解算的情况，导致整体估计精度下降甚至发生发散，难以实现真正意义上的最优融合。

5、此外，现有多源融合方案在大尺度场景下容易出现累积误差增大和全局一致性不足的问题。无人机任务区域常同时包含gnss可用区与gnss拒止区。在无有效的全局约束和跨场景切换机制的情况下，系统难以在全局范围内维持统一一致的坐标系。例如，在无人机执行大范围巡检任务时，起飞阶段可以利用gnss实现全局定位；进入室内仓库、地下空间或隧道后，则需依赖视觉惯性系统进行相对位姿估计。由于视觉匹配与imu积分均不可避免地存在误差，这些误差会在长航程中逐渐累积，使得估计轨迹偏离真实路径。当无人机重新进入gnss可用区时，若缺乏有效的全局重定位与误差校正机制，系统无法将室内场景中的累积误差与室外绝对坐标对齐，从而导致整个飞行轨迹在全局坐标系下出现不连续、不一致的问题。此类缺陷在长距离巡检、边境巡逻与搜索救援等任务中尤为突出，严重制约了现有导航系统在复杂大尺度环境中的实用性与可靠性。

6、综上所述，现有无人机导航定位技术在gnss拒止环境下的连续高精度定位、复杂场景中的视觉特征稳健跟踪、多源传感器非线性自适应融合以及大尺度场景下的全局一致性维护等方面仍存在明显不足，亟需提出一种面向复杂环境的多源自适应融合的无人机导航定位方法，以实现gnss可用与不可用场景间的无缝切换与高鲁棒连续定位。

技术实现思路

1、本技术的目的在于，针对上述技术中的不足，提供一种基于视觉-惯性-卫星多源传感器融合的导航定位方法。

2、为实现上述目的，本技术采用的技术方案如下：

3、本技术提供一种基于视觉-惯性-卫星多源传感器融合的导航定位方法，包括以下步骤：

4、s1：对无人机的视觉图像构建金字塔尺度空间，在金字塔尺度空间的每个尺度层级提取视觉图像的初始特征点，采用klt光流法对初始特征点进行相邻图像帧之间的光流跟踪，得到跟踪特征点，基于正反向光流一致性验证机制和rbrief描述子匹配依次对跟踪特征点进行筛选，得到用于后端解算的融合特征点；

5、s2：构建滑动窗口局部紧耦合估计器，并在滑动窗口内建立局部系统状态向量，局部系统状态向量包括imu状态向量、相机相对于imu的外参数、融合特征点首次被观测到的逆深度，将融合特征点输入滑动窗口紧耦合状态估计器中，利用基于滑动窗口的图优化方法求解局部系统状态向量，得到无人机在世界坐标系的局部位姿；

6、s3：采用精密单点定位模型处理无人机的gnss原始观测值，得到无人机在ecef坐标系的三维位置，构建导航坐标系，将无人机在ecef坐标系的三维位置转换为无人机在导航坐标系的三维位置；

7、s4：构建全局位姿图优化估计器，并建立全局系统状态向量，将无人机在世界坐标系的局部位姿以及无人机在导航坐标系的三维位置输入全局位姿图优化估计器中，利用非线性最小二乘法求解全局系统状态向量，得到无人机在导航坐标系的全局位姿。

8、进一步地，s1中，对无人机的视觉图像构建金字塔尺度空间前，对视觉图像依次进行畸变校正、尺度归一化与直方图均衡化。

9、进一步地，s1中，采用klt光流法对初始特征点进行相邻图像帧之间的光流跟踪前，获取无人机搭载的imu的角速度与加速度，基于初始特征点在上一图像帧的像素位置，通过imu的角速度与加速度预测初始特征点在当前图像帧的像素位置，将预测结果作为klt光流法的迭代初值。

10、进一步地，s2中，利用基于滑动窗口的图优化方法求解局部系统状态向量的具体步骤如下：

11、构建局部系统状态向量的非线性量测模型和残差项，残差项包括imu残差项和视觉残差项；

12、对窗口中内的最老关键帧进行边缘化处理，将移除的最老关键帧的约束转化为先验约束项；

13、基于imu残差项、视觉残差项和先验约束项构建非线性量测模型的代价函数，求解局部系统状态向量。

14、进一步地，s2中，局部系统状态向量的数学表达式为：

15、

16、

17、

18、其中，为滑动窗口内待求解的局部系统状态向量，为第k个关键帧的imu状态向量，，为滑动窗口内关键帧的帧数，分别为无人机在世界坐标系的位置、速度和姿态，分别为imu中加速度计的零偏参数和imu中陀螺仪的零偏参数，为相机相对于imu的外参数，为从imu坐标系原点到相机坐标系原点的平移向量，为从imu坐标系旋转到相机坐标系的旋转四元数，为第i个融合特征点首次被观测到的逆深度，，为滑动窗口维护的融合特征点个数；

19、局部系统状态向量的非线性量测模型和残差项的数学表达式为：

20、

21、其中，为局部系统状态向量在第k个关键帧的实测值，为由局部系统状态向量映射到观测空间的非线性量测函数，为零均值高斯噪声；

22、

23、其中，为残差项，为采用非线性量测函数迭代得到的局部系统状态向量在第k个关键帧的估计值。

24、进一步地，s2中，imu残差项和视觉残差项的数学表达式为：

25、

26、其中，为imu残差项，为第k个关键帧和第k+1个关键帧之间的imu预积分实测值，分别为无人机的位置、速度和姿态在第k个关键帧和第k+1个关键帧之间的imu预积分观测值，为无人机在世界坐标系的姿态的旋转矩阵，为相邻关键帧之间的时间间隔，为世界坐标系的重力加速度；

27、

28、

29、

30、其中，为视觉残差项，为第个关键帧的像素坐标实测值，为在第个关键帧的像素坐标，为从第个关键帧投影到第个关键帧的重投影结果，第个关键帧为首次观测到滑动窗口内被连续观测的任一融合特征点的关键帧，为首次被观测到的像素坐标，为用相机内参数表示的投影矩阵，为的归一化结果，为在相机坐标系的三维坐标，为第一次被观测到的逆深度，为第个关键帧的图像，为相机相对于imu的外参数，为从imu坐标系旋转到相机坐标系的旋转矩阵，为从imu坐标系原点到相机坐标系原点的平移向量，为第个关键帧无人机在世界坐标系的位置，为第个关键帧无人机在世界坐标系的姿态的旋转矩阵，为第个关键帧无人机在世界坐标系的位置，为第个关键帧无人机在世界坐标系的姿态的旋转矩阵。

31、进一步地，s2中，先验约束项的求解过程为：

32、对残差项进行一阶泰勒展开，得到局部系统状态向量增量的二次型近似并建立高斯-牛顿增量方程，数学表达式为：

33、

34、，

35、其中，为采用非线性量测函数本次迭代待求解的局部系统状态向量增量，为采用非线性量测函数迭代得到的当前局部系统状态向量的信息矩阵，为梯度向量，为所有残差项按行堆叠后的向量，为对局部系统状态向量的一阶雅可比矩阵；

36、将局部系统状态向量增量按照是否需要保留划分为需要边缘化的旧局部系统状态向量增量和需要继续保留在滑动窗口中的当前局部系统状态向量，并对信息矩阵与梯度向量按照相同顺序进行分块，得到对应于旧状态和保留状态的分块方程组，数学表达式为：

37、

38、其中，为的子块矩阵，为需要边缘化的旧局部系统状态向量增量，为需要继续保留在滑动窗口中的当前局部系统状态向量增量，为的对应分块；

39、对分块方程组进行高斯消元运算，得到仅与需要继续保留在滑动窗口中的当前局部系统状态向量增量相关的等效增量方程，数学表达式为：

40、

41、

42、

43、

44、

45、其中，为在中的舒尔补，为的等效梯度。

46、进一步地，s2中，非线性量测模型的代价函数的数学表达式为：

47、

48、

49、其中，为先验约束项，分别为滑动窗口内imu预积分量测的集合和视觉量测的集合，为imu协方差，为鲁棒核函数，为视觉观测噪声方差，为滑动窗口内视觉量测集合中的任一帧，为imu时刻，为相邻imu时刻之间的时间间隔，为局部系统状态向量的转移矩阵，为初始协方差，初始协方差，为噪声驱动阵，为imu观测过程噪声协方差。

50、进一步地，s3中，将无人机在ecef坐标系的三维位置转换为无人机在导航坐标系的三维位置的数学表达式为：

51、

52、

53、其中，为在导航坐标系的坐标，分别为在大地坐标系的经度和纬度，为和在ecef坐标系的坐标差，为gnss解算出的第j个gnss时刻下无人机在ecef坐标系的三维位置，为导航坐标系的原点，为在ecef坐标系的坐标，为在ecef坐标系的坐标。

54、进一步地，s4中，利用非线性最小二乘法求解全局系统状态向量的数学表达式为：

55、

56、

57、

58、

59、其中，为采用非线性量测函数迭代得到的全部系统状态向量，为待估计的全局系统状态向量，，为关键帧的帧数，，任一关键帧的全局系统状态向量，和分别为无人机在导航坐标系的全局位置和全局姿态，为量测的集合，包括来自无人机的三维位置量测和无人机的局部位姿量测，为无人机的三维位置量测和无人机的局部位姿量测的任一种，为全局系统状态向量的观测值，全局系统状态向量映射到观测空间的非线性量测函数，为量测的协方差，为全局残差项，为局部残差项，为gnss残差项，为无人机局部位姿的观测值，为无人机的局部位姿映射到观测空间的非线性量测函数，分别为第t个关键帧和第t-1个关键帧无人机在世界坐标系的局部位置和局部姿态，分别为第t个关键帧和第t-1个关键帧无人机在导航坐标系待估计的全局位置和全局姿态，为无人机三维位置的观测值，为无人机的三维位置映射到观测空间的非线性量测函数，为无人机在导航坐标系的三维位置。

60、本技术的有益效果包括：

61、本技术提供了一种基于视觉-惯性-卫星多源传感器融合的导航定位方法，通过在统一坐标框架下对视觉图像数据、惯性测量单元（imu）数据以及全球卫星导航系统（gnss）原始观测进行联合处理，通过前端鲁棒视觉特征提取与跟踪、局部紧耦合视觉惯性状态估计、精密单点定位处理以及全局位姿图优化四个层级的有机衔接，实现从局部相对位姿到全局绝对位姿的连续、高精度估计。各步骤之间在时间上顺序依赖、在信息流上相互补充，其中视觉与惯性信息主要约束局部短时间尺度的相对运动，gnss信息提供大尺度绝对位置约束，全局图优化对两类信息在统一导航坐标系下进行一致性融合。

62、首先，通过前端采用融合klt光流与rbrief描述子的视觉处理策略，在多尺度图像空间中实现特征检测与光流跟踪，并结合正反向光流一致性检验与描述子匹配双重筛选机制，有效解决复杂光照、局部遮挡和动态干扰场景下视觉特征跟踪不稳定的问题，在保持较高计算效率的同时显著提升特征点数据关联的鲁棒性和精度。其次，后端采用基于图优化框架的视觉–惯性局部紧耦合估计器，通过imu预积分与视觉重投影因子的联合约束以及多次迭代中的重新线性化，减小高度非线性视觉量测带来的线性化误差，并在滑动窗口内根据观测数据质量自适应分配权重，使得估计结果在精度与一致性方面均优于传统滤波式或松耦合方法；同时，惯性观测对短时间内的高频运动进行稳定约束，视觉观测对惯性漂移进行有效抑制。然后，通过在视觉–惯性图优化模型中引入基于ppp模型的高精度gnss约束因子，对全局位姿进行绝对位置与航向角的校正，消除长期飞行任务中不可避免的累积漂移，形成覆盖gnss可用区与拒止区的大尺度视觉惯性导航系统。该系统一方面利用视觉与惯性信息的互补性，在gnss缺失场景中维持连续相对导航；另一方面在gnss可用时通过ppp高精度量测对全局位置信息进行周期性校正，实现gnss可用与不可用场景之间的无缝切换，保证无人机在城市峡谷、室内外混合空间以及长距离巡检任务中的全程连续、高鲁棒定位。

63、整体而言，该方法通过基于多传感器信息质量的自适应加权机制，在优化层面动态平衡视觉、惯性与卫星观测的贡献，实现了在复杂大尺度环境中的高精度、全场景连续导航定位，有效解决背景技术中gnss拒止环境下连续定位困难、多源融合缺乏自适应性及全局一致性不足等关键问题。