基于全局地图与多传感器信息融合的全程位姿估计方法与流程

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

背景技术：

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

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

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

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

技术实现要素：

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

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

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

2)在步骤1)搭建完毕的无人机系统中，对各传感器进行标定，分别获得每个传感器对应的参数，包括：相机的内外参矩阵，imu的内参数和测量噪声，相机坐标系和imu坐标系之间的相对位姿关系，磁强计噪声和气压计噪声；

标定完成后，对无人机系统进行初始化，包括：对各传感器信息输出的时间对齐，确定各传感器坐标系和载体无人机坐标系之间的位姿关系，确定无人机系统初始状态下的基帧变换矩阵；其中，全局坐标系为东北天enu坐标系，局部坐标系为vio系统第一帧图像所对应的相机位姿，vio系统的本体坐标系即为无人机系统的本体坐标系；

3)利用各传感器获得载体无人机当前所处位姿的量测信息，并利用vio系统得到的图像信息构造并维护局部地图；

选用局部坐标系下的无人机系统的状态变量为：

其中，χl为局部坐标系下的无人机系统状态变量，n为vio系统的滑动窗口的大小，χt为载体无人机在vio系统第t帧图像时对应的状态变量，为无人机在vio系统第t帧图像时的位置，为无人机在vio系统第t帧图像时的速度，为无人机在vio系统第t帧图像时的姿态，上标l表示状态变量为相对于局部坐标系所得，ba为imu中加速度计的噪声，bg为imu中陀螺仪的噪声；

在相机帧中观测到特征之后，以表示特征点i在局部坐标系中的三维坐标，m为路标点的个数；

最终对vio系统即无人机系统在局部坐标系下的位姿估计为如下非线性最小二乘问题：

其中，rb和rc分别代表惯性测量的误差和视觉测量的误差，wb为惯性测量的协方差，wc为视觉测量的协方差；对该非线性最小二乘问题求解，得到vio系统在局部参考系中6自由度位姿实时估计，将四元数转换为旋转矩阵则vio系统的位姿估计表示为

4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应vio系统当前帧的最优状态变量；具体步骤如下：

4-1)将vio系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量为在全局坐标系下对应vio系统第t帧图像的无人机位姿，其中表示vio系统第t帧图像下无人机在全局坐标系下的位置，表示vio系统第t帧图像下无人机在全局坐标系下的姿态，上标w表示相对于全局坐标系；

4-2)以状态变量xt为因子图变量节点，以无人机系统上搭载的各传感器获得的测量信息和由全局地图获得无人机的位姿信息为因子图因子节点，构建多传感器信息融合框架；

4-3)在所有传感器测量均为独立，且噪声满足高斯分布的条件下，将无人机系统在全局坐标系下的状态估计问题描述为：

其中，为无人机在全局坐标系下的最优状态变量，s为各传感器量测集合，k为不同传感器的量测，z为测量值，为根据状态变量xt得到的估计值，为方差，下标t代表第t帧；

4-4)构建因子图因子节点表达式；

4-4-1)gps因子：

vio系统第t帧图像对应的在全局坐标系下的gps量测为：

其中，代表vio系统第t帧图像对应的gps的量测，为无人机由gps量测得到的在全局坐标系下的三维坐标；

则vio系统第t帧图像对应的gps因子表示为：

4-4-2)磁强计因子：

通过查表得到在全局坐标系下局部区域的磁场强度z^w，根据当前帧对应的磁场量测得到磁强计因子表示为：

其中，为磁强计和本体坐标系之间的坐标转换关系，为当前帧相对于全局坐标系的坐标转换关系，上标m代表磁强计；

4-4-3)气压计因子：

假设同一高度气压恒定，根据当前帧对应的高度测量值将气压计因子表示为：

其中，h代表气压计；

4-4-4)全局地图因子：

全局地图因子为将特征点像素坐标与全局地图点云中对应的3d点按照当前估计的位姿进行投影所得到的位置相比较得到的误差，称为重投影误差，具体表达式为：

其中，为当前帧对应的特征点像素坐标，ui为第i个特征点的像素坐标，π(·)为相机投影函数，为第i个特征点在全局坐标系下的三维坐标，s为特征点个数；

其中，由经过基帧变换矩阵变换得到，利用vio系统第t-1帧图像下基帧变换矩阵将局部地图转换为全局地图，即式中和为和分别对应的齐次坐标；

当t＝1时，基帧变换矩阵为步骤2)得到的的无人机系统初始状态下的基帧变换矩阵；

4-5)将因子图因子节点表达式代入式(1)进行求解，得到无人机系统对应vio系统第t帧图像的最优状态变量，输出在当前帧下对无人机系统的全局位姿估计结果^wt^t；

5)进行基帧变换，得到当前帧下局部坐标系和全局坐标系之间的转换关系，将局部地图转换为全局地图；

根据步骤4)的结果得到vio系统第t帧图像下无人机系统在全局坐标系下的位姿^wt^t，利用vio系统得到无人机系统在局部坐标系下的位姿^lt^t，则第t帧时基帧变换描述为：

^wtt^l＝^wt^t·(^lt^t)^-1

6)当相机获取第t+1帧图像时，令t＝t+1，将该图像作为新的当前帧，然后重新返回步骤3)。

本发明的特点及有益效果：

本发明提出的基于全局地图与多传感器信息融合的全程位姿估计方法，能够利用全局优化的方式将传感器测量量和全局地图信息进行融合，保证无人机系统精确的全局位姿估计。在某些传感器(比如gps)信号较差甚至失效的条件下，全局地图依然能够为无人机系统提供准确的全局位姿。在极端环境或需要频繁往复观测的环境下，比如扫地机器人反复清扫或是战场上强干扰等环境下，系统具有较高的精度和鲁棒性。

附图说明

图1是本发明方法的总体流程图；

图2是本发明一个实施例的因子图示例图；

图3是本发明一个实施例的包含全部变量节点的因子图示例图；

图4是本发明的工作原理图。

具体实施方式

本发明提出一种基于全局地图与多传感器信息融合的全程位姿估计方法，下面结合附图和具体实施例对本发明的技术方案做进一步的说明。

本发明提出一种基于全局地图与多传感器信息融合的全程位姿估计方法，整体流程如图1所示，包括以下步骤：

1)搭建包含各传感器的无人机系统，具体方法为：选定一台无人机(可采用常规型号)，在无人机上分别搭载各传感器，包括：vio(视觉惯性里程计)系统、gps、磁强计和气压计；其中，所述vio系统包含相机和imu(惯性测量单元)，相机镜头方向与无人机前进方向一致，vio系统、gps磁强计、气压计均可以集成在无人机的飞控板上。所述vio系统、gps、磁强计和气压计均可采用常规型号部件，vio系统、gps、磁强计和气压计的重量总和不超过无人机的额定载荷；

2)在步骤1)搭建完毕的无人机系统中，对各传感器进行标定，分别获得每个传感器对应的参数。包括：相机的内外参矩阵，imu的内参数和测量噪声，相机坐标系和imu坐标系之间的相对位姿关系，磁强计和气压计的噪声。其中，相机、imu、gps等已有成熟的标定方法，比如相机的张正友标定法、imu的误差分析标定法等。

标定完成后，对无人机系统进行初始化，包括：对各传感器信息输出的时间对齐，确定各传感器坐标系和载体(即无人机)坐标系之间的位姿关系，确定无人机系统初始状态下的基帧变换矩阵(局部坐标系和全局坐标系之间的变换矩阵)；

具体地，考虑载体为无人机，各传感器已经通过标定获得各项参数，全局坐标系为enu(东北天)坐标系，局部坐标系为vio系统第一帧图像所对应的相机位姿，且认为vio系统的本体坐标系即为无人机系统的本体坐标系。

3)利用各传感器获得载体当前所处位姿的量测信息，并利用vio系统得到的图像信息构造并维护局部地图；

具体地，目前已存在成熟的vio系统可以为载体提供无人机的局部位姿和局部地图。采用苏黎世联邦理工学院提出的okvis(基于关键帧的视觉惯性同步定位和建图方法)，选用局部坐标系下的状态变量为：

其中，χl为局部坐标系下的无人机系统状态变量，n为vio系统的滑动窗口的大小，χt为载体(无人机)在vio系统第t帧图像时对应的状态变量，为为载体在vio系统第t帧图像时的位置，为为载体在vio系统第t帧图像时的速度，为为载体在vio系统第t帧图像时的姿态，上标l表示状态变量为相对于局部坐标系所得，ba为imu中加速度计的噪声，bg为imu中陀螺仪的噪声。一般来说，imu第一次测量所得的相机位姿被设定为局部坐标系。在相机帧中观测到特征之后，以表示特征点i在局部坐标系中的三维坐标，m为路标点(路标点即为二维特征点转化为三维形式的点)的个数。最终对vio系统(即无人机系统)在局部坐标系下的位姿估计可以看做如下非线性最小二乘问题：

其中，rb和rc分别代表惯性测量的误差和视觉测量的误差，wb为惯性测量的协方差，wc为视觉测量的协方差。对该非线性最小二乘问题求解，得到vio系统在局部参考系中6自由度位姿实时估计，将四元数转换为旋转矩阵那么vio系统的位姿估计可以表示为并且vio系统可以实现局部地图的构造和维护。

4)构建基于因子图的多传感器信息融合框架，利用因子图优化得到无人机对应vio系统当前帧的最优状态变量；

具体地，本实施例中因子图构建示例图如图2所示。图中，xt为变量节点，表示无人机系统在全局坐标系下的状态变量；gps、磁强计、气压计的量测信息以及根据全局地图得到的量测信息为因子节点，因子节点和变量节点之间有边相连，从而可构建基于因子图的多传感器信息融合框架。图3所示即为本发明实施例中由全部变量节点构成的因子图示例图。

具体步骤如下：

4-1)将vio系统第t帧图像记为当前帧，选择全局坐标系下无人机系统状态变量为在全局坐标系下对应vio系统第t帧图像的载体位姿，其中表示vio系统第t帧图像下无人机在全局坐标系下的位置，表示vio系统第t帧图像下无人机在全局坐标系下的姿态，上标w表示相对于全局坐标系；

4-2)以状态变量xt为因子图变量节点，以无人机系统上搭载的各传感器获得的测量信息和由全局地图获得无人机的位姿信息为因子图因子节点，构建多传感器信息融合框架；

4-3)在所有传感器测量均为独立，且噪声满足高斯分布的条件下，将无人机系统在全局坐标系下的状态估计问题描述为：

其中，为无人机在全局坐标系下的最优状态变量，式中s为各传感器量测集合，k为不同传感器的量测，z为测量值，为根据状态变量xt得到的估计值，为方差，下标t代表第t帧。

4-4)构建因子图因子节点表达式；

具体地，在步骤4-5)中，各因子图因子节点表达式如下：

4-4-1)gps因子：

以使用enu坐标为例，得到vio系统第t帧图像对应的在enu全局坐标系下的gps量测为：

其中，代表vio系统第t帧图像对应的gps的量测，为载体由gps量测得到的在全局坐标系下的三维坐标；

则vio系统第t帧图像对应的gps因子可以表示为：

4-4-2)磁强计因子：

假设磁强计已经校准且没有误差，通过查表得到在enu坐标系下局部区域的磁场强度z^w并假设为恒定。根据当前帧对应的磁场测量量可以得到磁强计因子可以表示为：

其中，为当前帧对应的特征点像素坐标，ui为第i个特征点的像素坐标，π(·)为相机投影函数，为第i个特征点在全局坐标系下的三维坐标，，s为特征点个数

4-4-3)气压计因子：

假设同一高度气压恒定，根据当前帧对应的高度测量值可以将气压计因子表示为：

其中，h代表气压计。

4-4-4)全局地图因子：

全局地图因子为将特征点像素坐标与全局地图点云中对应的3d点按照当前估计的位姿进行投影所得到的位置相比较得到的误差，称为重投影误差，具体表达式为：

其中，为当前帧对应的特征点像素坐标，ui为第i个特征点的像素坐标，π(·)为相机投影函数，由相机标定过程确定，为第i个特征点在全局坐标系下的三维坐标，n为特征点个数；

具体地，由经过基帧变换矩阵变换得到，在步骤2)中，利用vio系统得到的图像信息构造并维护局部地图，其中局部地图的三维点坐标即为利用上一个周期(即vio系统第t-1帧图像下)步骤5)所求得的基帧变换矩阵可以将局部地图转换为全局地图，即式中和为和分别对应的齐次坐标。

当t＝1时，基帧变换矩阵为步骤2)得到的的无人机系统初始状态下的基帧变换矩阵。

4-5)将因子图因子节点表达式代入式(1)，进行求解，即可求得无人机系统对应vio系统第t帧图像的最优状态变量，即输出在当前帧下对无人机系统的全局位姿估计结果。

具体地，采用googleceressolver库对此因子优化问题进行求解，得到当前帧对应的无人机系统最优位姿估计结果。

5)进行基帧变换，得到当前帧下局部坐标系和全局坐标系之间的转换关系，将局部地图转换为全局地图；

具体地，根据步骤4)的结果，可以得到vio系统第t帧图像下无人机系统在全局坐标系下的位姿^wt^t，利用vio系统得到无人机系统在局部坐标系下的位姿^lt^t，则第t帧时基帧变换可以描述为：

^wtt^l＝^wt^t·(^lt^t)^-1

利用上述关系即可将当前帧下局部地图转换为全局地图。

6)当相机获取新的一帧图像(t+1帧)时，将该新的图像作为新的当前帧(第t帧)，然后重新返回步骤3)；当t大于n时，保留状态变量xt与其之前的n-1个状态变量共同作为滑动窗口中的状态变量，并按照先后顺序重新对保留的n个状态变量更新下标为1到n，此时有t＝n，同理在步骤3)中也应为保留状态变量χt与其之前的n-1个状态变量共同作为滑动窗口中的状态变量，并按照先后顺序重新对保留的n个状态变量更新下标为1到n。

本发明的整体工作原理如图4所示，各传感器和通过全局地图获得的量测信息分别输入至主要vio系统和全局位姿估计器，vio系统输出局部地图和局部位姿，全局位姿估计器输出全局位姿，也是所需求的系统输出，利用全局位姿和局部位姿求得基帧变换矩阵，将局部地图转换为全局地图服务于下一帧的无人机系统的全局位姿估计。

根据本发明提出的基于全局地图与多传感器信息融合的全程位姿估计方法，能够利用全局优化的方式将所有传感器的测量量进行融合，而全局地图的存在可以在其他传感器失效的情况下对载体进行位姿估计，提升位姿估计的准确性和可靠性。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。