[发明专利]一种基于GNSS、视觉以及IMU进行精确定位的数据融合算法

权利要求书

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所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S4，基于松耦合的优化方法，如式(1)所示；

其中，r_L为本地数据的代价函数：

r_G为松耦合的全局数据代价函数：

其中，下标k表示第k帧，δx表示x方向上的代价，δy表示y方向上的代价，x与y即需要优化的定位坐标；右上角标的local与global分别表示本地数据源与全局数据源各自的定位结果,θ为本地坐标系与全局坐标系的航向角偏差，通过本地定位轨迹与全局定位轨迹的角度偏差获取。

5.如权利要求1所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S5，若此刻接收的卫星数量为0颗，则通过弥补本地与全局坐标系之间的航向角偏差，将本地定位结果全局化；若此刻接收的卫星数量大于0颗，则利用GNSS卫星信号的原始数据，包括伪距、多普勒频移以及时钟信息，构建GNSS代价函数对定位估计进行约束。

6.如权利要求1所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S6，当接收机处于低速运动状态下时，多普勒频移的信噪比会下降，导致其约束精度降低；当速度小于v_min时，对多普勒信息的置信程度为0，即完全不考虑卫星提供的多普勒频移信息；速度在v_min到v_max之间时，两者呈线性关系，当速度增大到v_max以上时，置信程度不再改变；v_min与v_max根据具体的传感器套件特性以及测试情况进行调整。

7.如权利要求1所述的基于GNSS、视觉以及IMU进行精确定位的数据融合算法，其特征在于，所述步骤S7，基于紧耦合的非线性优化方法，如式(4)所示；

其中，第一项为边缘化带来的先验信息，r_B为IMU的代价函数，r_C为相机的代价函数，为GNSS的紧耦合代价函数；根据实际需要，决定是否输出系统的姿态以及视觉的点云信息。