基于图的视觉slam方法

基于图的视觉slam方法
【专利摘要】本发明公开了一种基于图的视觉SLAM方法，基于图像的自然特征概率向量表示可以得到图像视觉特征间的匹配关系，利用图像间的空间几何关系计算两帧间的相对位姿。利用连续图像之间的对应关系获得视觉里程计的数据关联，由此得到图像序列中的所有约束。将摄像头相对位姿看作地图中的节点，图像帧间的空间约束关系表示为边，构建基于摄像头相对位姿估计的轨迹地图。最后采用最大似然法优化地图，通过随机梯度下降法得到最优化位姿估计。在实验室环境下对本发明提出的方法进行了相关实验，并展示了机器人的运行轨迹，证明了本发明算法的有效性。
【专利说明】基于图的视觉SLAM方法

【技术领域】
[0001] 本发明涉及一种基于图的视觉SLAM方法。

【背景技术】
[0002] 大多数视觉SLAM算法采用特征跟踪的方法，将视觉特征作为自然路标构建环境 特征地图，并同时通过与之前已创建的环境地图（自然路标库）中的路标进行匹配，对机器 人当前位姿进行估计实现机器人定位。由于地图构建与位姿估计两个过程都有噪声，因此 整个SLAM过程具有不确定性。
[0003] 目前最流行的两大算法EKF-SLAM和PF-SLAM以概率描述信息的不确定性， Siegwart等提出了一种基于单摄像机的EKF-SLAM算法，通过多特征匹配来定位并减少定 位误差，算法的不足之处在于需要建立运动模型和观测模型，计算复杂度很高，并且缺乏 闭环能力，数据一旦关联错误会带入到整个SLAM状态估计中，可能使整个预测过程发散。 Murphy和Russel等提出了一种PF-SLAM算法，它的复杂度随特征数目对数地增长，不像 EKF-SLAM那样平方地增长，并且不需要运动模型线性化。


【发明内容】

[0004] 本发明米用基于图（graph-based)，也称为基于网络（network-based)的SLAM方 法，不同于EKF-SLAM和PF-SLAM，不需要建立严格的运动模型和观测模型，只需要通过所有 的观测信息以及帧间的空间约束关系，最终构建一个完整的机器人位姿路径地图。地图中 的点代表机器人位姿，边代表位姿之间的约束。机器人不同时刻的位姿之间存在关联，关联 其实是一种约束，约束用边来表示形成了点的结构图。
[0005] 本发明将利用连续图像帧间的对应关系解决局部数据关联问题。本发明的环境为 自然特征环境，自然特征通过对获取的图像序列进行视觉特征提取得到。基于图的SLAM方 法不需要明确的摄像机运动模型，只需要批量处理顺序获取的所有图像，利用图像间内在 的多视角几何约束即可同时估计特征位置和摄像机位姿。地图创建部分利用观测信息和帧 间的空间约束关系构建摄像机位姿地图，地图优化部分基于极大似然法最小化约束误差得 到最优位姿估计，本发明通过随机梯度下降法最小化约束误差得到极大似然地图。
[0006] 本发明的技术解决方案是：
[0007] -种基于图的视觉SLAM方法，
[0008] 对摄像头获取的每一帧图像进行视觉特征提取，基于自然视觉单词的概率模型表 示每帧图像，基于图像的概率向量表示得到匹配的特征点对，利用同一个特征点在不同时 刻的图像坐标得到机器人在这两个时刻的相对位姿关系，通过立体视距技术计算每帧图像 代表的相对位姿；
[0009] 机器人不同时刻的位姿之间存在关联，基于图像序列中连续图像之间的位姿转换 得到视觉里程计的数据关联，得到图像帧间的空间约束关系；
[0010] 将摄像头相对位姿看作地图中的节点，图像帧间的空间约束关系表示为边，构建 基于摄像机相对位姿估计的轨迹地图；
[0011] 通过调整图中节点的位置进行最优位姿序列估计，通过随机梯度下降法最小化约 束误差得到最优位姿地图。
[0012] 优选地，为得到当前时刻所有相机的位姿集合Xciim = (Xci, 每个当前位姿需 要计算相对于初始时刻k=0相机局部坐标系的运动估计；
[0013] 由于不同时刻转换矩阵Tk(k = 1…n)之间存在连接关系，当前位姿xn = xn_Jn，xQ 为初始时刻的相机位姿，由此通过计算图像Ilrt与Ik之间的转换关系Tk把所有转换关系连 接起来即可增量得到相机序列运动轨迹X 1 :n。
[0014] 优选地，计算摄像机相对运动Tk :
[0015] 首先估计相对位姿和两帧摄像机的运动方向，然后恢复这两帧中观测到的特征点 的位姿，最后通过路标位姿的匹配计算这些路标的正确尺度，进行估计相机运动。
[0016] 优选地，由本质矩阵E进行分解可以得到旋转矩阵R和平移矢量I, I与实际平移 矢量t相差一个比例系数，最终可以得到k-1时刻摄像头局部坐标系下，k时刻的摄像头位 姿表示为^ 得到每个时刻的相对位姿后，将所有时刻的这些相对 > 转换放到一个结构IL 中，以表示连续图像节点间的关联。
[0017] 优选地，假设第j时刻成功检测到当前图像与第i时刻的图像形成闭环，则两个图 像间的相对位姿表示为言息矩阵为该闭环数据关联加入到连 续帧的数据结构中，形成一个新的结构^
[0018] 优选地，假设节点i和节点j之间已经存在约束薄~,而某一时刻又得到两个节点 间的另一个约束if，如此两个约束可以融入到一个约束中，融合的标准如下：

【权利要求】
1. 一种基于图的视觉SLAM方法，其特征在于： 对摄像头获取的每一帧图像进行视觉特征提取，基于自然视觉单词的概率模型表示每 帧图像，基于图像的概率向量表示得到匹配的特征点对，利用同一个特征点在不同时刻的 图像坐标得到机器人在这两个时刻的相对位姿关系，通过立体视距技术计算每帧图像代表 的相对位姿； 机器人不同时刻的位姿之间存在关联，基于图像序列中连续图像之间的位姿转换得到 视觉里程计的数据关联，得到图像帧间的空间约束关系； 将摄像头相对位姿看作地图中的节点，图像帧间的空间约束关系表示为边，构建基于 摄像机相对位姿估计的轨迹地图； 通过调整图中节点的位置进行最优位姿序列估计，通过随机梯度下降法最小化约束误 差得到最优位姿地图。
2. 如权利要求1所述的基于图的视觉SLAM方法，其特征在于：为得到当前时刻所有相 机的位姿集合X0 :n = {X(I，…，X1J，每个当前位姿需要计算相对于初始时刻k=0相机局部坐 标系的运动估计； 由于不同时刻转换矩阵Tk(k = L···η)之间存在连接关系，当前位姿xn = XlriTn，Xci为 初始时刻的相机位姿，由此通过计算图像Ilrt与Ik之间的转换关系Tk，把所有转换关系连接 起来即可增量得到相机序列运动轨迹X 1 :n。
3. 如权利要求2所述的基于图的视觉SLAM方法，其特征在于，计算摄像机相对运动 Tk = 首先估计相对位姿和两帧摄像机的运动方向，然后恢复这两帧中观测到的特征点的位 姿，最后通过路标位姿的匹配计算这些路标的正确尺度，进行估计相机运动。
4. 如权利要求1所述的基于图的视觉SLAM方法，其特征在于：由本质矩阵E进行分解 得到旋转矩阵R和平移矢量Γ _实际平移矢量t相差一个比例系数，最终可得到k-Ι时 刻摄像头局部坐标系下，k时刻的摄像头位姿表示为：X::_: =Ut: 得到 f 每个时刻的相对位姿后，将所有时刻的这些相对转换放到一个结构=( > 中，以表示连续图像节点间的关联。
5. 如权利要求1所述的基于图的视觉SLAM方法，其特征在于，假设第j时刻成 功检测到当前图像与第i时刻的图像形成闭环，则两个图像间的相对位姿表示为： V=K )-泛J信息矩阵为Ω^.，将该闭环数据关联加入到连续帧的数据结构中，形 成一个新的结构η=?χ;:.α:: > O
6. 如权利要求5所述的基于图的视觉SLAM方法，其特征在于，假设节点i和节点j之 间已经存在约束左而某一时刻又得到两个节点间的另一个约束i;:如此两个约束可以 ， v ， 融入到一个约束中，融合的标准如下：
7. 如权利要求6所述的基于图的视觉SLAM方法，其特征在于，将图像帧看作顶点集V， 图像帧之间的位姿关联作为边的集合E，连接两帧图像的每条路径代表一个相关位姿估计， 由此构建摄像头位姿轨迹地图G = (V，E)。
8. 如权利要求1-7任一项所述的基于图的视觉SLAM方法，其特征在于：采用随机梯 度下降法进行地图优化，通过迭代地选择约束X并移动图中的节点来实现，节点的更新如 下：
其中，X是图中摄像头的位姿变量集合;m为迭代次数Jij.是&的雅可比矩阵， Qij.是观测值的不确定信息矩阵，为约束误差的余；F1是一个先验矩阵，H是系统的 Hessian矩阵,代表约束误差函数的曲率,使用H的近似值
！个可逆 矩阵，
9. 如权利要求8所述的基于图的视觉SLAM方法，其特征在于：找到一个变量X/使代价 函数Fu(X)最小，通过迭代选择初始值X 1计算得到新的节点序列，序列在一定条件下收敛 使Fu(X)最小从而求得解f ;函数Fu(X)对变量X丨:的梯度为，VfTXp为一个负 向量,梯度方向是Fij(X)减少最快的方向；由此得到其中，将Xij定义为 λ ij = I/ O ijin)，m 表示前迭代的次数，参数 K u = diag I (j-i) Ω u I。
10. 如权利要求1-7任一项所述的基于图的视觉SLAM方法，其特征在于：地图优化过 程的迭代步骤为： 步骤1 :选取初始节点X1，给定允许约束误差a > 0, b > 0,并令m = 0 ; 步骤2 :计算梯度下降方向向量. > 步骤3 :判断VJYxP是否满足约束条件Of满足则转向步骤8,否则继 续； 步骤4 :计算最佳学习速率λ u，得到新节点xm+1 ; 步骤5 :所有节点组成一个位姿序列X = (X1, x2,…，xk，xk+1)T ; 步骤6 :判断代价函数是否满足Fu(Xk+1) -Fu(Xk)彡b，满足则转向步骤8,否则继续； 步骤7 :迭代次数加1，令m = m+Ι，转向步骤2 ; 步骤8 :输出结果，结束。
【文档编号】G01C21/32GK104374395SQ201410125956
【公开日】2015年2月25日 申请日期:2014年3月31日 优先权日:2014年3月31日
【发明者】梁志伟, 徐小根 申请人:南京邮电大学