[发明专利]一种基于单目ORB-SLAM的自动化地图恢复方法

权利要求书

1.一种基于单目ORB-SLAM的自动化地图恢复方法，其特征在于，包括以下步骤：

步骤1、同步采集图像序列以及实时动态差分RTK数据，进而基于单目ORB-SLAM系统进行地图跟踪与构建，恢复地图信息；

步骤2、若建图跟踪失败，保存当前地图信息，并记录关键帧与地图点序号；

步骤3、重置视觉地图构建系统，利用单目初始化进行地图重建；

步骤4、基于RTK信息辅助，自动化恢复并融合丢失地图信息；

步骤5、若视觉跟踪再次失败，重复步骤2-4直到地图构建结束；

步骤4所述的自动化恢复并融合丢失地图信息是指在新的视觉坐标系下恢复M帧关键帧以及相应地图点信息后，判断旧地图是否存在，若旧地图标志量LastMapFlag为真，自动加载该地图信息，基于RTK地理信息的约束，将旧地图关键帧和地图点坐标转换到当前新视觉坐标系下，并融合到当前地图构建中；M为14到26之间的整数；

所述将旧地图关键帧和地图点坐标转换到当前新视觉坐标系下，并融合到当前地图构建中；具体包括以下步骤：

4.1、建立视觉地图从视觉坐标系到真实地理坐标系下的相似变换模型，以获得视觉信息的缩放、平移以及旋转关系；

X_g＝s_vgR_vgX_v+t_vg

其中，s_vg为尺度因子且满足s_vg＞0，R_vg为三维空间内的旋转矩阵，t_vg＝(t_x,t_y,t_z)^T为平移向量，X_g为地图在真实地理坐标系下的位置坐标，X_v为地图在视觉坐标系下的位置坐标，t_x为x轴在视觉坐标系下的平移量，t_y为y轴在视觉坐标系下的平移量，t_z为z轴在视觉坐标系下的平移量；

4.2、建立目标函数，利用对齐RTK数据以及单目视觉恢复的关键帧轨迹信息，采用优化最小二乘目标函数的形式求解相似变换参数；

其中，所需估计的关键帧集为(c₁,...,c_i,...,c_m)，对齐的RTK数据集为(r₁,...,r_i,...,r_m)，其中，c_i为第i个关键帧在视觉坐标系下的坐标，r_i为第i个关键帧在地理坐标系下的坐标，R、t、s分别为地图从视觉坐标系到地理坐标系下变换的旋转矩阵、平移向量、尺度因子，m为关键帧个数，e²(R_vg,t_vg,s_vg)为相似变换后的视觉估计值与对齐RTK两组数据之间的均方差；

4.3、对旧地图从原视觉坐标系到新视觉坐标系的坐标变换参数进行求解；

根据步骤4.2的目标函数分别求解新旧地图的相似变换参数为(R₁,t₁,s₁)、(R₂,t₂,s₂)，基于真实地理信息对两段地图信息的约束关系，进而将旧地图的三维点信息转换到当前新视觉坐标系下；

X″_vj＝s₂₁R₂₁X′_vj+t₂₁

其中，X′_vj为旧地图中第j个地图点在原视觉坐标系下的位置，X″_vj为第j个地图点在新视觉坐标系下的位置，R₂₁、t₂₁、s₂₁分别为旧地图从原视觉坐标系到新视觉坐标系下变换的旋转矩阵、平移向量、尺度因子，R₁、t₁、s₁分别为新地图从新视觉坐标系到地理坐标系下变换的旋转矩阵、平移向量、尺度因子，R₂、t₂、s₂分别为旧地图从原视觉坐标系到地理坐标系下变换的旋转矩阵、平移向量、尺度因子，上标T为转置；

4.4、根据相似变换关系，将旧地图关键帧的位姿信息转换到当前新视觉坐标系下；

第i个旧关键帧在原视觉坐标系下的位置坐标为则第i个旧关键帧在新视觉坐标系下的位置坐标为其中，为第i个旧关键帧在原视觉坐标系下的旋转矩阵、平移向量，为第i个旧关键帧在新视觉坐标系下的旋转矩阵、平移向量；

4.5、将旧地图信息添加到当前新地图变量中，获得满足新视觉坐标系下位置分布合理、物理尺度与当前新地图一致的关键帧以及地图点数据，同时计算旧关键帧的视觉词袋，并将与旧地图构建相关的索引信息添加到单目ORB-SLAM系统的新地图构建变量中，使其参与当前系统的地图测绘，为跟踪重定位以及回环修正提供更冗余的地图信息。