[发明专利]基于图的视觉SLAM方法

说明书

技术领域

本发明涉及一种基于图的视觉SLAM方法。 

背景技术

大多数视觉SLAM算法采用特征跟踪的方法，将视觉特征作为自然路标构建环境特征地图，并同时通过与之前已创建的环境地图（自然路标库）中的路标进行匹配，对机器人当前位姿进行估计实现机器人定位。由于地图构建与位姿估计两个过程都有噪声，因此整个SLAM过程具有不确定性。 

目前最流行的两大算法EKF-SLAM和PF-SLAM以概率描述信息的不确定性，Siegwart等提出了一种基于单摄像机的EKF-SLAM算法，通过多特征匹配来定位并减少定位误差，算法的不足之处在于需要建立运动模型和观测模型，计算复杂度很高，并且缺乏闭环能力，数据一旦关联错误会带入到整个SLAM状态估计中，可能使整个预测过程发散。Murphy和Russel等提出了一种PF-SLAM算法，它的复杂度随特征数目对数地增长，不像EKF-SLAM那样平方地增长，并且不需要运动模型线性化。 

发明内容

本发明采用基于图(graph-based)，也称为基于网络(network-based)的SLAM方法，不同于EKF-SLAM和PF-SLAM，不需要建立严格的运动模型和观测模型，只需要通过所有的观测信息以及帧间的空间约束关系，最终构建一个完整的机器人位姿路径地图。地图中的点代表机器人位姿，边代表位姿之间的约束。机器人不同时刻的位姿之间存在关联，关联其实是一种约束，约束用边来表示形成了点的结构图。 

本发明将利用连续图像帧间的对应关系解决局部数据关联问题。本发明的环境为自然特征环境，自然特征通过对获取的图像序列进行视觉特征提取得到。基于图的SLAM方法不需要明确的摄像机运动模型，只需要批量处理顺序获取的所有图像，利用图像间内在的多视角几何约束即可同时估计特征位置和摄像机位姿。地图创建部分利用观测信息和帧间的空间约束关系构建摄像机位姿地图，地图优化部分基于极大似然法最小化约束误差得到最优位姿估计，本发明通过随机梯度下降法最小化约束误差得到极大似然地图。 

本发明的技术解决方案是： 

一种基于图的视觉SLAM方法， 

对摄像头获取的每一帧图像进行视觉特征提取，基于自然视觉单词的概率模型表示每帧图像，基于图像的概率向量表示得到匹配的特征点对，利用同一个特征点在不同时刻的图像坐标得到机器人在这两个时刻的相对位姿关系，通过立体视距技术计算每帧图像代表的相对位姿； 

机器人不同时刻的位姿之间存在关联，基于图像序列中连续图像之间的位姿转换得到视觉里程计的数据关联，得到图像帧间的空间约束关系； 

将摄像头相对位姿看作地图中的节点，图像帧间的空间约束关系表示为边，构建基于摄像机相对位姿估计的轨迹地图； 

通过调整图中节点的位置进行最优位姿序列估计，通过随机梯度下降法最小化约束误差得到最优位姿地图。 

优选地，为得到当前时刻所有相机的位姿集合x_0：m＝{x₀，…，x_n}每个当前位姿需要计算相对于初始时刻k=0相机局部坐标系的运动估计； 

由于不同时刻转换矩阵T_k(k＝1…n)之间存在连接关系，当前位姿x_n＝x_n-1T_n，x₀为初始时刻的相机位姿，由此通过计算图像I_k-1与I_k之间的转换关系T_k把所有转换关系连接起来即可增量得到相机序列运动轨迹X_1：n。 

优选地，计算摄像机相对运动T_k： 

首先估计相对位姿和两帧摄像机的运动方向，然后恢复这两帧中观测到的特征点的位姿，最后通过路标位姿的匹配计算这些路标的正确尺度，进行估计相机运动。 

优选地，由本质矩阵E进行分解可以得到旋转矩阵R和平移矢量与实际平移矢量t相差一个比例系数，最终可以得到k-1时刻摄像头局部坐标系下，k时刻的摄像头位姿表示为：得到每个时刻的相对位姿后，将所有时刻的这些相对转换放到一个结构中，以表示连续图像节点间的关联。 

优选地，假设第j时刻成功检测到当前图像与第i时刻的图像形成闭环，则两个图像间的相对位姿表示为：信息矩阵为将该闭环数据关联加入到连续帧的数据结构中，形成一个新的结构