CN104374395A - 基于图的视觉slam方法 - Google Patents

Abstract

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

Description

基于图的视觉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时刻的图像形成闭环，则两个图像间的相对位姿表示为：信息矩阵为将该闭环数据关联加入到连续帧的数据结构中，形成一个新的结构

优选地，假设节点i和节点j之间已经存在约束而某一时刻又得到两个节点间的另一个约束如此两个约束可以融入到一个约束中，融合的标准如下： 

优选地，将图像帧看作顶点集V，图像帧之间的位姿关联作为边的集合E，连接两帧图像的每条路径代表一个相关位姿估计，由此构建摄像头位姿轨迹地图G＝(V，E)。 

优选地，采用随机梯度下降法进行地图优化，通过迭代地选择约束并移动图中的节点来实现，节点的更新如下： 

其中，x是图中摄像头的位姿变量集合；m为迭代次数；J_ij是的雅可比矩阵，是观测值的不确定信息矩阵，r_ij为约束误差的余；H^-1是一个先验矩阵，H是***的Hessian矩阵，代表约束误差函数的曲率，使用H的近似值： H个可逆矩阵，

优选地，找到一个变量使代价函数F_ij(x)最小，通过迭代选择初始值x₁计算得到新的节点序列，序列在一定条件下收敛使F_ij(x)最小从而求得解x^*；函数F_ij(x)对变量的梯度为 为一个负向量，梯度方向是F_ij(x)减少最快的方向；由此得到：其中，将定义为m表示前迭代的次数，参数

优选地，地图优化过程的迭代步骤为： 

步骤1：选取初始节点x₁，给定允许约束误差a＞0，b＞０，并令m＝０； 

步骤2：计算梯度下降方向向量

步骤3：判断是否满足约束条件满足则转向步骤8，否则继续； 

步骤4：计算最佳学习速率得到新节点x_m+1； 

步骤5：所有节点组成一个位姿序列x＝(x₁，x₂…，x_k，x_k+1)； 

步骤6：判断代价函数是否满足满足则转向步骤8，否则继续； 

步骤7：迭代次数加1，令m＝m+1，转向步骤2； 

步骤8：输出结果，结束。 

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

附图说明

图1是实施例图形结构的图解说明示意图； 

图2是自然特征环境的单目视觉SLAM算法整体框架示意图； 

图3是摄像机的相对位姿表示为一个旋转矩阵R和一个平移向量t的说明示意图； 

图4是闭环检测的数据关联及位姿调整图； 

图5是对初始节点结构进行优化得到最大似然结构 

图6是实施例的实验室场景示意图； 

图7是机器人运行前两圈后运动轨迹图； 

图8是机器人最终位姿估计地图； 

图9是实验室环境下连续图像帧间的数据关联说明示意图； 

图10是实验室环境闭环检测的数据关联说明示意图； 

图11是走廊环境下机器人运动过程图； 

图12是图11中(a)第369帧运动轨迹的放大示意图； 

图13是图11中(b)第670帧运动轨迹的放大示意图； 

图14是图11中(c)第890帧运动轨迹的放大示意图； 

图15是图11中(d)第1050帧运动轨迹的放大示意图； 

图16是图11中(e)第1550帧运动轨迹的放大示意图； 

图17是图11中(f)第2150帧运动轨迹的放大示意图。 

具体实施方式

下面结合附图详细说明本发明的优选实施例。 

基于图的SLAM方法，Grisetti提出基于图的SLAM方法分为两个部分：图的构建和图的优化。图的构建通过数据关联即图节点间的约束关系而建立，数据关联包括局部数据关联和全局数据关联，局部数据关联是指连续图像帧间的匹配，用以解决相对姿态估计问题，全局数据关联通过闭环检测得到。基于图的SLAM可以被理解为节点和节点间约束的稀疏图形。图形的节点是机器人的位置x₀，x₁，…，x_T地图里n个特征m₀，m₁，…，m_n-1。约束是相继机器人姿态x_i，x_i-1之间的相对位置和机器人位置之间相对位置以及从那些位置观测到的特征。基于图的SLAM算法其中约束是一个软约束，如图1，图形中节点之间的约束为“软”约束。放松这些约束，即可得到机器人路径和环境地图的最好估计。然后使用最大似然估计对所生成的地图进行优化。 

本实施例采用基于图的SLAM方法，设计了一种自然特征环境的单目视觉SLAM算法。整个算法以图像帧为主线，输入为摄像头获取的每帧图像，输出为一个全局摄像头位姿地图，算法整体框架表示如图2所示。 

对摄像头获取的每一帧图像进行视觉特征提取，基于自然视觉单词的概率模型表示每帧图像，基于图像的概率向量表示可以很容易得到匹配的特征点对，利用同一个特征点在不同时刻的图像坐标可以得到机器人在这两个时刻的相对位 姿关系，通过立体视距技术计算每帧图像代表的相对位姿。机器人不同时刻的位姿之间存在关联，关联其实是一种约束，本发明基于图像序列中连续图像之间的位姿转换得到视觉里程计的数据关联，由此得到图像帧间的空间约束关系。将摄像头相对位姿看作地图中的节点，图像帧间的空间约束关系表示为边，构建基于摄像机相对位姿估计的轨迹地图。通过调整图中节点的位置进行最优位姿序列估计，通过随机梯度下降法最小化约束误差得到一个最优位姿地图。 

图像帧间的数据关联 

机器人不同时刻的位姿之间存在关联，制约着位姿之间的转换关系，通过摄像头相对位姿计算获得。在基于图的视觉SLAM算法中，图像间的数据关联包括两类：一类是基于视觉里程计的数据关联，另一类是基于闭环检测的数据关联。 

视觉里程计的数据关联 

在进行摄像头相对位姿计算之前，需要对摄像头进行标定，从而得到摄像头的内部参数，包括焦距、特征比、畸变因子和主点等，为此，首先离线采用张正友的标定方法对摄像头进行标定。具体方法和步骤已经很成熟，这里不再详细描述。 

摄像头相对位姿计算的目的是为了估计相机增量式运动从而得到相机轨迹。假设当前时刻k获取的图像序列为I_0：n＝{I₀，…，I_n}，而且机器人在平面中运动，由此任何两个相邻时刻k-1和k之间的相机相对位姿可表示为一个转换矩阵T_k，k-1∈R^3×3： 

$T_{k,k-1}=\left[\begin{array}{cc}{R}_{k,k-1}& t_{k,k-1}\\ 0& 1\end{array}\right]---\left(1\right)$

其中R_k，k-1为含有一个参数的旋转矩阵，t_k，k-1∈R^2×1为平移向量。集合 包含了所有子序列的相机运动。为了简化表示，下面用T_k代替T_k，k-1。我们最终的目的是得到当前时刻所有相机的位姿集合x_0：n＝{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，本发明采用特征点匹配的方法，利用从图像提取的若干个特征点，以及这些点在图像中的像点，计算得到表示摄像头运动的位姿参数，并实时更新摄像头位姿。算法首先估计相对位姿和两帧摄像机的运动方向，然后恢复这两帧中观测到的特征点的位姿，最后通过路标位姿的匹配计算这些路标的正确尺度，从而估计相机运动。路标的三维坐标必须对两个不同时刻匹配的特征对进行三维重建才能获得，与基于立体视觉的地图相比，精度存在差距。为了提高定位精度，需要充分利用特征的图像坐标，即通过摄像头k时刻观测的特征图像坐标与地图中的特征在某j时刻的图像坐标，可以计算出摄像头在这两个时刻的相对位姿关系，实现相对定位。 

令P＝[u，v]^T为特征点在像平面的齐次坐标，X＝[x，y，z]^T为特征点在摄像机坐标系下的坐标，根据相机投影模型则有： 

其中，M₁为摄像机内部参数矩阵，包括α_x＝f/d_x，α_y＝f/d_y，u₀，v₀四个内部参数，上面已经给出了本章的标定结果。令为特征点的归一化像平面坐标。已知k时刻与k-1时刻间隔内的匹配特征对p_k与p_k-1，可以计算得到两帧图像I_k与I_k-1之间的转换关系T_k，如图3，转换关系即为摄像头相对位姿，包含一个旋转矩阵R_k和平移失向量t_k，这种几何关系被称为本质矩阵E_k。本质矩阵E_k由一个3维矩阵R_k和组成，是一个具有5个自由度、秩为2的矩阵： 

其中t_k＝[t_x，t_y,1]^T为k-1时刻摄像头坐标系C_k-1到k时刻摄像头坐标系C_k的平移矢量，为t_k的反对称矩阵，实质上与真实平移向量相差一个比例系数。 

给定两幅图像中的匹配点对对应点在两幅图像中的归一化像坐标分别为和则本质矩阵E可以通过计算得到。经典的算法是Longuet-Higgins提出的八个点算法，每个匹配对代表了一种极线约束，根据极限约束建立一个方程AE=0，其中 

AE=0是一个含有9个未知数的齐次线性方程组，因此至少需要8个对应匹配点就可以求出带有比例系数的本质矩阵E。 

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

闭环检测的数据关联 

当检测到闭环时，当前位姿与更早一帧的位姿产生全局关联，这种关联称为revisiting约束，在图的构建中表现为增加了一条边。检测到的闭环图像对之间的数据关联的计算方法与连续图像帧间的局部约束相同，在此不再赘述。假设第j时刻成功检测到当前图像与第i时刻的图像形成闭环，则两个图像间的相对位姿表示为：信息矩阵为将该闭环数据关联加入到连续帧的数据结构中，形成一个新的结构

另外，为了避免过多相似约束融入到该结构，设计一种更新标准。假设节点i和节点j之间已经存在约束而某一时刻又得到两个节点间的另一个约束 如此两个约束可以融入到一个约束中，融合的标准如下： 

${\mathrm{\Ω}}_{\mathrm{ij}}={\stackrel{\‾}{\mathrm{\Ω}}}_{\mathrm{ij}}+{\widehat{\mathrm{\Ω}}}_{\mathrm{ij}}---\left(5\right)$

由此结构的约束数量可以维持在一定的范围内，不会随着相同的闭环检测而导致约束数量的增加，这种减少约束的技术能够加速优化算法的收敛速度。 

闭环约束加上相邻图像帧之间的局部约束，这两种约束同时制约着点的结构，如图4所示。基于全局数据关联带来的当前位姿与特定帧位姿之间的约束关系，当前帧位姿得到更新，与当前帧关联的其他帧位姿也随之得到更新。 

在得到了图像帧间的相对空间约束关系后，可以创建一个完整的摄像头位姿轨迹地图。在基于图的形式化表示中，机器人的位姿x_i被看作是随机变量，位姿间的约束则是与随机变量相关的观测。将图像帧看作顶点集V，图像帧之间的位姿关联作为边的集合E，连接两帧图像的每条路径代表一个相关位姿估计，由此构建摄像头位姿轨迹地图G＝(V，E)。 

地图优化 

由于观测噪声及约束误差的存在，仅仅依靠视觉里程计创建的位姿图通常不具备一致性，需要后期对图中节点进行位置调整，使节点能更好地满足约束关系，最终得到一个最优位姿地图。为了能够生成一个全局一致的地图，对每加入一个约束和更新一个约束后的整个结构进行优化。 

地图优化部分不考虑观测信息，只对整个序列位姿作最大似然估计，不需要知道摄像头的绝对位姿，只利用位姿间的相对变换关系最小化约束误差，得到位姿相对变换的最优估计，算法实质是个最大似然估计问题。目前大多数研究通过最大似然方法寻找最优地图，本发明也采用最大似然法进行地图优化，根据已建立的约束关系求解最优的位姿序列，得到一个最大可能的节点结构。 

本实施例采用随机梯度下降法进行地图优化，通过迭代地选择约束并移动图中的节点来实现，移动节点是为了减小所选择的约束产生的误差。与标准随机下降梯度公式不同，这些约束看作是独立的而不是整体得到优化。节点的更新如下： 

其中x是图中摄像头的位姿变量集合；m为迭代次数；H^-1是一个先验矩阵；J_ij是的雅可比矩阵，是观测值的不确定信息矩阵，r_ij为约束误差的余。 

算法的最终目的是找到一个变量使代价函数F_ij(x)最小，通过迭代选择初始值x₁计算得到新的节点序列，序列在一定条件下收敛使F_ij(x)最小从而求得解x^*。函数F_ij(x)对变量的梯度为 为一个负向量，梯度方向是F_ij(x)减少最快的方向。由此可以得到：

H是***的Hessian矩阵，代表约束误差函数的曲率，考虑到H的计算比较复杂，一般使用H的近似值：H个可逆矩阵，可以近似计算为H^-1＝|diag(H)|^-1。 

算法的关键问题是如何计算λ，λ为学习速率，控制着SGD的收敛速度。如果选得太小，则算法收敛速度慢；如果选得太会导致最终结果发散。这里采用Robbins和Monro的方法，将定义为：λ_ij＝1/(κ_ijm)，m表示前迭代的次数，参数

整个优化过程的迭代步骤为： 

步骤1：选取初始节点x₁，给定允许约束误差a＞0，b＞0，并令m＝0； 

步骤2：计算梯度下降方向向量

步骤3：判断是否满足约束条件满足则转向步骤8，否则继续； 

步骤4：计算最佳学习速率得到新节点x_m-1； 

步骤5：所有节点组成一个位姿序列x＝(x₁，x₂，…，x_k，x_k+1)^T； 

步骤6：判断代价函数是否满足满足则转向步骤8，否则继续； 

步骤7：迭代次数加1，令m＝m+1，转向步骤2； 

步骤8：输出结果，结束； 

整个算法在已知约束的情况下通过迭代减少约束误差得到极大似然地图，从而得到点的极大似然结构，构建全局范围内的精确地图，如图5。 

实验结果 

对本实施例提出的自然特征环境的单目视觉SLAM算法通过对实验室和走廊环境进行相应实验。实验室场景如图6所示，实验所用机器人为p3dx自主移动机器人，携带Logiteh QuickCam单目视觉传感器。除了视觉传感器，机器人还配有激光、声纳、红外等传感器。实验中采用无线以太网远程监控的方式来控制机器人的运行轨迹，由机器人上装载的摄像机进行实时图像采集，算法验证采用离线的方式进行，图像处理单元为Intel4核处理器i7-26003.4G，内存4G。每个实验分别就图像帧间的数据关联和摄像头运动轨迹进行实验结果数据分析，从不同环境下验证本发明算法的有效性。 

机器人首先在沿室内环境行走一圈后再继续前行，在第1000帧的位置准确检测到了闭环，如图7所示。机器人在实验室环境中行走两圈之后继续向前行走，由于闭环检测产生全局图像帧间的数据关联，同时进行了潜在的闭环候选，因此得到与当前帧匹配的历史帧后，当前帧的关联帧存在短时记忆内存中，利用这些帧之间的关联可以调整机器当前位姿序列，从而得到最终位姿估计地图，如图8所示。 

接下来在实验室走廊环境中进行实验，由于走廊范围较大且为结构化环境，基于本发明算法对2200多帧图像进行实验得到了较好的结果，整个运动过程如图11所示，具体图示如图12、图13、图14、图15、图16、图17所示。图中， 蓝色箭头表示机器人不同时刻的相对位姿，当前时刻位姿用红色箭头表示，位姿之间的关联用绿色线连接。 

实验中机器人从一个起始点行走了一段距离后继续前行，由于运动速度平稳，没有拐弯时机器人走的基本是直线，从图11(c)、图17中可以看出，机器人在四个拐弯处的运动轨迹也比较平滑，这是由于我们在拐摄像头获取的图像中检测到的特征点足够多，因此有利于准确计算相对位姿。在检测到闭环之前机器人运动轨迹没有闭合，在行走1000帧左右才开始检测到闭环。从图11(d)、图15中不难看出，检测到闭环后机器人得到了全局数据关联，当前位姿与起始处的历史位姿相匹配，之后的运动轨迹又重新沿第一个起始位置的路径开始。第一圈的运动轨迹虽然还没有与真实轨迹相一致，但是各个时刻位姿之间的关联是准确的，有利于之后的位姿调整。在行驶了1200多帧之后轨迹已经开始形成闭环，之后基于闭环检测带来的全局数据关联，机器人序列位姿得到了相应调整，地图越来越接近真实轨迹。在2000多帧之后基于本发明算法得到的机器人运动轨迹已与环境结构基本相一致，机器人在走廊环境中的最终位姿地图如图11(f)、图17所示，整个位姿序列较为平滑，数据关联准确，构建的位姿轨迹地图得到了很好优化。 

由于地图优化的计算复杂度取决于位姿长度而非环境大小，因此有利于得到图像序列间的数据关联。机器人不断向前运行时基于图像帧间的数据关联即约束关系进行地图优化，地图创建与地图优化同时进行。图像帧间的约束关系包括视觉里程计的数据关联和闭环检测的数据关联两部分，分别如图9和图10所示。连续图像帧间的共同特征点代表着图像间的对应关系，用来计算摄像头相对位姿，从而恢复相机的运动过程。当相机运动速度平稳的情况下，重新回到历史位置的时候不用计算新的位姿估计，直到相机运动有效才开始更新位姿，这样使访问到相同地点时保持地图解析，有利于结构化的环境中的长时间导航。图构建后通过最小化约束误差进行地图优化，最终得到了一个全局最优的最大似然地图，如图8所示。实验结果表明准确的数据关联有利于位姿地图创建的完整性与一致性。 

Claims (10)

1.一种基于图的视觉SLAM方法，其特征在于：

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

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

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

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

2.如权利要求1所述的基于图的视觉SLAM方法，其特征在于：为得到当前时刻所有相机的位姿集合x_0：n＝{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。

3.如权利要求2所述的基于图的视觉SLAM方法，其特征在于，计算摄像机相对运动T_k：

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

4.如权利要求1所述的基于图的视觉SLAM方法，其特征在于：由本质矩阵E进行分解得到旋转矩阵R和平移矢量 与实际平移矢量t相差一个比例系数，最终可得到k-1时刻摄像头局部坐标系下，k时刻的摄像头位姿表示为：得到每个时刻的相对位姿后，将所有时刻的这些相对转换放到一个结构中，以表示连续图像节点间的关联。

5.如权利要求1所述的基于图的视觉SLAM方法，其特征在于，假设第j时刻成功检测到当前图像与第i时刻的图像形成闭环，则两个图像间的相对位姿表示为：信息矩阵为Ω_ij.，将该闭环数据关联加入到连续帧的数据结构中，形成一个新的结构

6.如权利要求5所述的基于图的视觉SLAM方法，其特征在于，假设节点i和节点j之间已经存在约束而某一时刻又得到两个节点间的另一个约束如此两个约束可以融入到一个约束中，融合的标准如下：

${\mathrm{\Ω}}_{\mathrm{ij}}={\stackrel{\‾}{\mathrm{\Ω}}}_{\mathrm{ij}}+{\widehat{\mathrm{\Ω}}}_{\mathrm{ij}}---\left(5\right)$

$x_i^j={\mathrm{\Ω}}_{\mathrm{ij}}^{-1}({\stackrel{\‾}{\mathrm{\Ω}}}_{\mathrm{ij}}{\stackrel{\‾}{x}}_i^j+{\widehat{\mathrm{\Ω}}}_{\mathrm{ij}}{\hat{x}}_i^j)---\left(6\right)$

7.如权利要求6所述的基于图的视觉SLAM方法，其特征在于，将图像帧看作顶点集V，图像帧之间的位姿关联作为边的集合E，连接两帧图像的每条路径代表一个相关位姿估计，由此构建摄像头位姿轨迹地图G＝(V，E)。

8.如权利要求1-7任一项所述的基于图的视觉SLAM方法，其特征在于：采用随机梯度下降法进行地图优化，通过迭代地选择约束并移动图中的节点来实现，节点的更新如下：

其中，x是图中摄像头的位姿变量集合；m为迭代次数；J_ij.是f_ij的雅可比矩阵，Ω_ij.是观测值的不确定信息矩阵，r_ij.为约束误差的余；H^-1是一个先验矩阵，H是***的Hessian矩阵，代表约束误差函数的曲率，使用H的近似值： $H=\mathrm{\Σ}{J}_{\mathrm{ij}}{\mathrm{\Ω}}_{\mathrm{ij}}{J}_{\mathrm{ij}}^T;$ H个可逆矩阵，

9.如权利要求8所述的基于图的视觉SLAM方法，其特征在于：找到一个变量使代价函数F_ij(x)最小，通过迭代选择初始值x₁计算得到新的节点序列，序列在一定条件下收敛使F_ij(x)最小从而求得解x^*；函数F_ij(x)对变量的梯度为 为一个负向量，梯度方向是F_ij(x)减少最快的方向；由此得到：其中，将λ_ij定义为λ_ij＝1/(κ_ijm)，m表示前迭代的次数，参数κ_ij＝diag|(j-i)Ω_ij|。

10.如权利要求1-7任一项所述的基于图的视觉SLAM方法，其特征在于：地图优化过程的迭代步骤为：

步骤1：选取初始节点x₁，给定允许约束误差a＞0，b＞0，并令m＝0；

步骤2：计算梯度下降方向向量

步骤3：判断是否满足约束条件满足则转向步骤8，否则继续；

步骤4：计算最佳学习速率λ_ij，得到新节点x_m+1；

步骤5：所有节点组成一个位姿序列x＝(x₁，x₂，…，x_k，x_k+1)^T；

步骤6：判断代价函数是否满足F_ij(x_k+1)-F_ij(x_k)≤b，满足则转向步骤8，否则继续；

步骤7：迭代次数加1，令m＝m+1，转向步骤2；

步骤8：输出结果，结束。