一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法制造技术

本发明专利技术公开了一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法。GNSS定位技术易受外界观测条件的限制，在高架桥、城市峡谷等遮挡区域，卫星信号接收能力急剧减弱，无法满足定位导航的需求。纯视觉的位置估计不能解决尺度问题，且在弱纹理场景及快速运动场景下无法建立匹配关系，容易跟踪丢失；IMU可以在短时间内(毫秒级)反映动态变化，但长时间运行下(秒级)其累积误差会不断增加。本发明专利技术依据不同传感器数据源之间的互补性，利用本地数据源(视觉及IMU)提高GNSS的全局定位精度，利用GNSS全局数据源消除本地的累积误差，在卫星信号被遮挡时依赖本地视觉、IMU数据保持短期的高精度全局定位，整体上提高了定位导航系统的性能及鲁棒性。性能及鲁棒性。性能及鲁棒性。

【技术实现步骤摘要】
一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法


[0001]本专利技术涉及导航定位领域，尤其涉及基于GNSS、视觉以及IMU的导航定位系统。

技术介绍

[0002]高精度、高鲁棒性的定位技术对于国家安全、人工智能以及物联网等高新产业领域至关重要。经过行业不断的研究发展，相应地产生了多种不同的定位系统，包括GNSS全球导航卫星系统、INS惯性导航系统、VO视觉里程计、CNS天文导航系统等。
[0003]近年来，随着位置服务行业的快速发展，单一定位系统已经无法满足用户对于可靠定位结果的需求。GNSS全球导航卫星系统，可以获取真实世界下的定位轨迹，但其易受外界观测条件的限制，在高架桥、城市峡谷、地下车库以及隧道等遮挡区域，卫星信号接收能力急剧减弱，导致定位精度显著下降或无法解算定位信息。视觉里程计利用相机的每帧图像进行纯视觉的位置估计，但其解算的定位轨迹与真实世界相比存在一个不确定的尺度缩放比例，且在弱纹理场景及快速运动场景下无法建立匹配关系，容易跟踪丢失。视觉惯性里程计使用相机以及IMU进行位置估计，具有较高的精度，但在长期运行中会受到累计误差的影响而导致精度不断下降，且其定位轨迹与真实世界中的轨迹存在一个航向角的偏差。由于单传感器定位方法的局限性以及多传感器之间数据源的互补性，高精度、高鲁棒性的多源融合定位技术已经成为亟需解决的科学问题。

技术实现思路

[0004]本专利技术的目的是提升GNSS导航定位系统的定位性能以及解决复杂场景下GNSS卫星信号丢失的问题。
[0005]本专利技术提出一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法，实现过程包括：
[0006]步骤S1：移动设备上挂载GNSS卫星接收装置、单目相机以及IMU模块，在移动过程中采集GNSS信号、图像数据以及IMU数据，作为整个定位算法的输入；
[0007]步骤S2：对本地的视觉、IMU数据进行初始化对齐；对图像数据进行前端视觉跟踪以及SFM视觉位姿求解，对IMU数据进行预积分，然后将两者的数据对齐从而粗略解算初始时刻的视觉尺度、重力加速度、系统初始速度；
[0008]步骤S3：对本地数据进行非线性优化：对设定滑动窗口中的关键帧，构建视觉代价函数以及IMU代价函数，通过最小化总体代价函数，得到本地数据的定位结果；
[0009]步骤S4：进行本地与全局数据源的松耦合联合初始化：基于非线性优化技术，将本地的定位结果与GNSS基于SPP算法下的全局定位结果进行融合，粗略解算初始时刻的系统全局位置以及本地坐标系与全局坐标系之间的航向角偏差；
[0010]步骤S5：构建GNSS的紧耦合全局代价函数约束定位估计：若此刻接收的卫星数量为0颗，则直接将本地定位结果全局化；若此刻接收的卫星数量大于0颗，则利用GNSS卫星信号的原始数据构建GNSS代价函数对定位估计进行约束；
[0011]步骤S6：根据系统当前运动速度调节多普勒频移代价函数的协方差矩阵，改变定位估计对多普勒频移信息的置信程度；
[0012]步骤S7：进行本地与全局的紧耦合联合非线性优化：通过最小化由视觉约束、IMU约束以及GNSS原始数据约束构成的总体代价函数，解算系统的最终全局定位估计。
[0013]本专利技术的有益效果：
[0014](1)本专利技术通过融合GNSS的全局数据源与高精度的视觉、IMU本地数据源，提升了导航定位系统的定位性能。在GNSS信号接收的室外场景下，基于SPP算法解算接收机在全球坐标系下唯一坐标的定位方法精度不足，无法满足许多场景下定位导航的需求。
[0015](2)本专利技术所提出的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，使得系统在GNSS卫星信号完全丢失的各种场景下，亦能无缝地提供短期高精度的全局定位估计。当卫星信号重新捕获时，无缝切换系统定位方式，利用GNSS全局信息消除本地的累积误差，持续获取高精度全局定位。
[0016](3)本专利技术提出的基于GNSS原始数据(包括伪距、多普勒频移、时钟等)构建代价函数约束定位估计的方法，在接收卫星的数量小于4颗大于0颗的情况下，依旧可以利用GNSS全局信息消除本地累积误差，使定位估计收益。
附图说明
[0017]图1为基于GNSS、视觉以及IMU进行精确定位的数据融合算法的流程图。
[0018]图2为本专利技术中系统速度与多普勒置信的关系示意图。
具体实施方式
[0019]下面将结合说明书附图，对本专利技术做进一步的说明。
[0020]本专利技术公开了一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法。本专利技术的具体实施方式流程图如图1所示：
[0021]步骤S1：移动设备上挂载GNSS卫星信号接收机、单目相机以及IMU模块，在移动过程中采集频率为10Hz的GNSS信号、帧率为30fps的图像数据以及频率为200Hz的IMU数据，作为整个定位算法的输入。其中GNSS的原始数据包括星历、伪距多普勒频移时钟等信息，相机的输入数据是连续序列的图像帧，IMU的输入数据是一连串物体的线性加速度α以及旋转角速度ω信息。
[0022]步骤S2：对本地的视觉、IMU数据进行初始化对齐：经过前端特征点跟踪算法把图像总结为一系列稀疏的特征点，并基于SFM进行一个纯视觉的位姿估计。由于IMU的采样频率高于相机，将IMU的输入数据进行预积分，与相机的每帧图像在时间上对齐。接着将视觉、IMU的数据联合校准，大致解算系统的初始速度、重力加速度(标志着初始姿态)以及视觉尺度。
[0023]步骤S3：对本地数据进行非线性优化：对设定滑动窗口中的关键帧，构建视觉代价函数以及IMU代价函数。对于视觉代价函数，通过两帧之间由光束平差法得到的重投影误差构建，对于IMU代价函数，通过物体在两帧之间的加速度、角速度以及IMU的零偏构建。
[0024]步骤S4：进行本地与全局数据源的松耦合联合初始化：基于非线性优化技术，以松耦合的方式，将本地的定位结果与GNSS基于SPP算法下的全局定位结果进行融合，粗略解算
初始时刻的系统全局位置以及本地坐标系与全局坐标系之间的航向角偏差，如式(1)所示。
[0025][0026]其中，r
L
为本地数据的代价函数：
[0027][0028]r
G
为松耦合的全局数据代价函数：
[0029][0030]其中，下标k表示第k帧，δx表示x方向上的代价，δy表示y方向上的代价，x与y即需要优化的定位坐标。右上角标的local与global分别表示本地数据源与全局数据源各自的定位结果,θ为本地坐标系与全局坐标系的航向角偏差，可通过本地定位轨迹与全局定位轨迹的角度偏差获取。
[0031]步骤S5：构建GNSS全局代价函数约束定位估计：若此刻接收的卫星数量为0颗，则通过弥补本地与全局坐标系之间的航向角偏差，将本地定位结果全局化。若此刻接收的卫星数量大于0颗，则利用GNSS卫星信号的原始数据，包括伪距、多普勒频移以本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，包括如下步骤：步骤S1：移动设备上挂载GNSS卫星接收装置、单目相机以及IMU模块，在移动过程中采集GNSS信号、图像数据以及IMU数据，作为整个定位算法的输入；步骤S2：对本地的视觉、IMU数据进行初始化对齐；对图像数据进行前端视觉跟踪以及SFM视觉位姿求解，对IMU数据进行预积分，然后将两者的数据对齐从而粗略解算初始时刻的视觉尺度、重力加速度、系统初始速度；步骤S3：对本地数据进行非线性优化：对设定滑动窗口中的关键帧，构建视觉代价函数以及IMU代价函数，通过最小化总体代价函数，得到本地数据的定位结果；步骤S4：进行本地与全局数据源的松耦合联合初始化：基于非线性优化技术，将本地的定位结果与GNSS基于SPP算法下的全局定位结果进行融合，粗略解算初始时刻的系统全局位置以及本地坐标系与全局坐标系之间的航向角偏差；步骤S5：构建GNSS的紧耦合全局代价函数约束定位估计：若此刻接收的卫星数量为0颗，则直接将本地定位结果全局化；若此刻接收的卫星数量大于0颗，则利用GNSS卫星信号的原始数据构建GNSS代价函数对定位估计进行约束；步骤S6：根据系统当前运动速度调节多普勒频移代价函数的协方差矩阵，改变定位估计对多普勒频移信息的置信程度；步骤S7：进行本地与全局的紧耦合联合非线性优化：通过最小化由视觉约束、IMU约束以及GNSS原始数据约束构成的总体代价函数，解算系统的最终全局定位估计。2.如权利要求1所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S1，定位算法的输入包括来自GNSS的原始数据、相机的输入数据、IMU的输入数据的数据；GNSS的原始数据包括星历、伪距多普勒频移时钟等信息，相机的输入数据是连续序列的图像帧，IMU的输入数据是一连串物体的线性加速度α以及旋转角速度ω信息。3.如权利要求1所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S1，采集频率为10Hz的GNSS信号、帧率为30fps的图像数据以及频率为200Hz的IMU数据，作为整个定位算法的输入。4.如权利要求1...

【专利技术属性】
技术研发人员：王睿，侯治，
申请(专利权)人：同济大学，
类型：发明
国别省市：