[发明专利]基于全局地图与多传感器信息融合的全程位姿估计方法

说明书

本发明提出一种基于全局地图与多传感器信息融合的全程位姿估计方法，涉及导航领域。该方法首先搭建包含各传感器的无人机系统；对传感器进行标定，获得每个传感器对应参数，并对无人机系统进行初始化；利用各传感器获得载体无人机当前所处位姿的量测信息，并利用视觉惯性里程计VIO系统的图像信息构造并维护局部地图；构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机系统对应VIO系统每一当前帧的最优状态变量，并更新当前帧下局部坐标系和全局坐标系间的转换关系，将局部地图转换为全局地图。本发明能够利用全局优化的方式将无人机搭载的所有传感器的量测和全局地图信息进行融合，提升无人机系统位姿估计的准确性和可靠性。

技术领域

本发明涉及导航领域，具体涉及一种基于全局地图与多传感器信息融合的全程位姿估计方法。

背景技术

在自动驾驶、搜救、侦查等领域，对无人系统的需求日益提升，而无人系统的定位问题是其中的基础。目前，存在很多采用相机和激光雷达等传感器进行无人系统的局部位姿估计的技术，并且其中也有和IMU(惯性测量单元)结合形成的组合导航系统，它们在局部区域内可以实现对无人系统精确的位姿估计。例如，慕尼黑工业大学于2014年提出的LSD-SLAM(基于直接法的大范围单目即时定位和地图构建方法)基于直接法实现大规模环境下的位姿确定和地图构建；苏黎世联邦理工学院于2015年提出的ROVIO(鲁棒的视觉惯性里程计)算法，是一种基于卡尔曼滤波算法的单目视觉惯性里程计。但是上述的方式在实际应用中仍然存在一些缺陷，一是没有固定的全局参考，即在相同环境下，从不同的起点出发也可能得到不同的位姿估计结果，不利于之后的重复利用；二是缺少全局度量，局部估计随着时间推移容易产生漂移，使得长时间、大区域的局部估计不够准确。

GPS、磁强计和气压计等传感器不依赖于局部参考系，可以得到系统的全局测量，这些测量是不存在漂移的。但是由于传感器的特性，上述传感器的测量结果是存在噪声的，不能用于精确的控制和导航。例如GPS的测量精度在米级，并且测量频率较低且不连续。因此考虑不同传感器的特点，将传感器信息进行融合的结果会有更好的效果和性能。

而在一些较为恶劣的环境中，例如GPS工作时断时续，磁场或气压不够稳定等条件下，利用已存在的全局地图信息可以结合其他所有传感器的量测结果可以提供更为精确的全局位姿估计。

目前，常用的多传感器信息融合方法采用滤波方式。2013年，苏黎世联邦理工学院自动化系统实验室提出了一种基于扩展卡尔曼滤波算法的多传感器融合框架；2014年，香港科技大学也提出了一种基于无迹卡尔曼滤波的多传感器融合方法。但滤波算法需要相对较为准确的初始值预测，以及滤波算法对时间同步较为敏感，迟到的测量量会使得整个结果不够准确，而采用基于优化的方法会得到更有效准确的结果。2018年，香港科技大学提出一种多传感器融合框架，采用VINS-MONO(一种鲁棒通用的单目视觉惯性状态估计器)的输出和GPS、磁强计等传感器的输出进行融合，利用因子图优化的方式得到无人系统的位姿估计结果，但是该方法在整个过程中没有考虑全局坐标系和局部坐标系之间的对齐问题，并且在某些传感器失效的情况下，结果的准确性并不够可靠。

发明内容

本发明的目的是为克服已有技术的不足之处，提出一种基于全局地图与多传感器信息融合的全程位姿估计方法。本发明能够利用全局优化的方式将无人机搭载的所有传感器的量测和全局地图信息进行融合，而全局地图的存在可以在其他传感器失效的情况下对无人机系统载体进行位姿估计，提升无人机系统位姿估计的准确性和可靠性。

本发明提出一种基于全局地图与多传感器信息融合的全程位姿估计方法，其特征在于，包括以下步骤：

1)搭建包含各传感器的无人机系统；具体方法为：选定一台无人机，在无人机上分别搭载各传感器，包括：视觉惯性里程计VIO系统、GPS、磁强计和气压计；其中，所述VIO系统包含相机和惯性测量单元IMU，相机镜头方向与无人机前进方向一致；