[发明专利]一种单目视觉和GNSS融合的SLAM定位方法及系统

权利要求书

1.一种单目视觉和GNSS融合的SLAM定位方法，其特征在于，包括如下步骤：

S1、获取图像信息；

S2、通过单目纯视觉SLAM系统进行初步定位和建图，获取关键帧数据；

S3、通过GNSS系统获取GNSS信息；

S4、对获取的GNSS信息进行预处理，利用线性预测法计算每一GNSS帧的图像时间戳t_k对应GNSS位置信息p_k：

其中，t_k为第k帧对应的时间戳，t_G1和t_G2为第k帧图像时间戳t_k之前最接近的两帧GNSS帧，P_G1、P_G2为t_G1和t_G2对应的GNSS位置信息；

S5、使用预处理后的GNSS信息和关键帧数据创建融合关键帧；

S6、至少获得30帧融合关键帧后，运行视觉GNSS融合的单目初始化算法中的旋转对齐算法，求解得到坐标系旋转对齐矩阵R_CA；

S7、运行视觉GNSS融合的单目初始化算法中的地图比例恢复算法，求解得到真实比例变换矩阵S；

S8、通过R_CA和S对SLAM系统的所有关键帧相机位置X_c和地图点X_w进行比例缩放，恢复单目SLAM系统的定位和建图信息的真实尺度，得到新的关键帧相机位置X′_c和地图点X'_w；

X′_c＝SX_c 式2

X'_w＝SX_w 式3

S9、运行视觉和GNSS融合定位算法，更新定位位置和地图点位置；

S10、运行全局捆绑调整优化算法，得到全局捆绑调整优化后的定位结果。

2.根据权利要求1所述的方法，GNSS与视觉SLAM坐标系的旋转对齐算法求解R_CA，其特征在于，包括如下步骤：

S61、取时间维度上的最远关键帧和最近关键帧，对系统估计的相对位移进行归一化，得到SLAM系统和GNSS系统的相对位移和

其中：p为NED坐标系向量，x为SLAM系统坐标系向量，k为第k帧，k+n为第k+n帧，为视觉SLAM估计的相机位置，为GNSS位置；

S62、通过公式6和公式7可求解得到消掉地图真实比例变换矩阵的方程：

其中，S表示大小为3×3的SLAM真实地图比例变换矩阵；R_CA表示GNSS的NED坐标系相对视觉相机SLAM坐标系的旋转对齐矩阵；为SLAM系统的相对位移；为GNSS系统的相对位移；

S63、向量和的叉乘求解得旋转轴m：

S64、对向量m进行归一化操作：

S65、向量和的点乘得到旋转角θ：

S66、通过公式11求解旋转矩阵R_CA：

式中单位向量