2000年以来，基于视觉传感器的SLAM (simultaneous localization and mapping)研究逐渐兴起^[1-2]，许多国内外学者借助SIFT(scale-invariant feature transform)或其他图像不变特征，开展了视觉SLAM问题研究^[3-4]。目前研究SLAM的方法主要有扩展卡尔曼滤波^[5]、信息滤波^[6]、压缩滤波^[7]、FastSLAM^[8]方法。其中EKF-SLAM^[9]定位效果好，但随路标过多时计算量较大; 基于信息滤波算法的即时定位与地图构建算法虽然稳定准确，但不能进行数据关联；FastSLAM算法只能在短时间内满足一致性要求，但不适用于解决大范围的SLAM问题。

EKF-SLAM方法是一种最优循环迭代估计，在处理信息不确定性方面有独到之处，但针对其计算量随路标个数增多而增长较快的问题，常需改进处理，然而在室内独立小环境中，以子图划分等思想为基础的快速方法难以适用，此外，EKF-SLAM还存在一个明显的缺点便是数据关联脆弱问题，即一旦数据关联出现错误，会将误差代入估计结果中，而且不可恢复，即通常说的EKF(extended Kalman filter)缺乏自恢复能力。

本文以EKF-SLAM算法为基础，并采用了人工离线建立特征库的方法，克服传统方法存在的计算量过大、关联脆弱等问题，实现了室内复杂环境下实时、可靠地定位。此外，在人工建立离线特征库时，解算过程考虑众多特征的有效融合、特征密度控制、优质特征筛选等问题，为此，本算法引入了特征稀疏性策略、稳健特征点选取、误匹配去除等方法。算法在定位过程中需固化特征库，但系统实际工作环境难免会发生一定变化，为弥补算法适应动态环境能力的不足，算法引入了特征库定期更新策略，保证了算法长期有效性。

1 人工地图库建立 1.1 特征点提取及匹配 1.1.1 SIFT特征点检测算法

选取SIFT算法^[10]提取图像特征，其原理如下：SIFT算法的尺度空间由DOG(different of Gaussian)金字塔与不同尺度的图片卷积得到。对于已经检测到的特征点，已知特征点的尺度值σ，根据这一尺度，得到最接近这一尺度的高斯图像。SIFT算法先进行非极大值抑制，去除低对比度的点，用直方图统计邻域像素的梯度幅值和方向，直方图的峰值代表了该特征点处邻域内图像梯度的主方向，由于可能存在多个最高点的情况，因此特征点方向可能存在多个。最后每个关键点都有三个特征信息：位置(x, y)、特征尺度σ和特征方向θ。SIFT算法将特征点方形区域划分为4×4个部分，统计每个部分内的梯度方向和特征值为8 bin的直方图，得到128维的向量作为特征描述子。

1.1.2 特征点匹配

采用基于Kd-树^[11]的BBF(best bin first)算法^[12]进行快速特征点匹配，该算法的优点在于其能够优先查找与查询数据点更为接近的数据空间。在匹配过程中，引入在极线、视差、方向、尺度上的约束限制，从而去除误匹配：

1) 极线约束：图像经过标定校正后，左右特征点垂直方向上小于1个像素。

2) 视差约束：水平方向上，左图特征点坐标大于右图坐标，且差值小于20个像素。

3) 方向约束：左右特征点方向差值小于20°。

4) 尺度约束：左右特征点尺度差值小于1.5。

5) 唯一匹配约束：如果一个特征点有多个特征点满足约束条件与之相匹配，则该特征点失效，将其丢弃。

使用欧氏距离作为特征点间的相似性度量，两个特征点在特征空间上矢量距离公式为

$ d\left( {\mathit{\boldsymbol{x}}, \mathit{\boldsymbol{y}}} \right) = \sum\limits_{i = 1}^{128} {\left| {{x_i} - {y_i}} \right|} $

(1)

式中x=(x₁, x₂, …, x₁₂₈)、y=(y₁, y₂, …, y₁₂₈)分别为特征点x和特征点y的128维矢量描述符。

表 1统计了离线建图过程图像匹配约束结果，也可以从图 1发现经过匹配约束后，能够有效地去除误匹配的特征点，误匹配的特征点显著减少。

1.2 特征点拍摄及位置解算 1.2.1 图片拍摄过程

如图 2所示，相机系统主要由以下几个部分组成(自下往上的顺序)：三脚架、旋转云台、双目相机、指南针。在室内环境下建立世界坐标系，并选取若干具有代表性的拍摄位置，每个位置在世界坐标系的坐标通过测量获得。在每个位置处，将双目相机固定在三脚架上的旋转云台上，建立以相机左目为原点的机器人坐标系，借助三脚架水平泡和云台水平泡确保旋转云台系统水平转动(即转动过程中相机的俯仰角始终为0°)，并在多个角度拍摄，云台带有角度刻度，可直接读取拍摄角度，拍摄时借助指南针来保证不同位置的角度零位一致。如图 3所示，本实验共选取了5个位置，每个位置选取12个拍摄方向(在360°方向均匀分布)，采集的图像为1 024×768的模式，每次拍摄获得左右两幅图像。

1.2.2 特征点位置解算

利用SIFT特征点检测算法提取并保存特征点的像素坐标，已知双目相机的视差d=12 cm，焦距f=839.478 943。特征点位置以左目坐标系为基准，在右目中寻找与左目相匹配的特征点。已知左目特征点的像素坐标(x_l, y_l)，与左目匹配的右目特征点的像素坐标(x_r, y_r)，根据双目几何计算公式可以解算出左目特征点在摄像机坐标系下的三维坐标(x_i, y_i, z_i)。双目几何计算公式为

$ {z_i} = \frac{{Bf}}{d} $

(2)

$ {x_i} = \frac{{\Delta x{z_i}}}{f} $

(3)

$ {y_i} = \frac{{\Delta y{z_i}}}{f} $

(4)

其中：Δx=x_l-x_r，Δy=y_l-y_r。

由特征点在机器人坐标系下的坐标与相机拍摄位置在世界坐标系下的位置姿态t_j=(t_jx, t_jy, t_jz, θ_j)(j=1, 2, 3, 4, 5)，可以解算出该左目特征点在世界坐标系下的三维坐标(u_i, w_i, v_i):

$ {u_i} = {t_{jx}} + {z_i}\cos {\theta _j} - {x_i}\sin {\theta _j} $

(5)

$ {w_i} = {t_{jy}} + {y_i} $

(6)

$ {v_i} = {t_{jz}} + {z_i}\sin {\theta _j} + {x_i}\cos {\theta _j} $

(7)

1.3 数据关联

建图过程需要将多次拍摄的特征融合，所以每提取新的一帧双目图像中的特征时，需首先将每个特征与已记入库中的特征进行匹配，以排除重复特征，即本节所指的数据关联问题，另外，机器人在实时定位过程中也需进行数据关联。数据关联的正确性直接关系到定位和地图创建的精度，为提高关联准确性，采用双向匹配方法。具体做法如下：设L_V1为当前帧图像观测到的特征点之一，遍历搜索地图库中与该点特征相似性最高的特征点，将该点设为L_D1，计算公式见式(1)，此为正向匹配；然后在当前帧观测特征点集中寻找与L_D1特征最相似的点，将该点设为L_V2，此为逆向匹配，然后判断L_V1和L_V2是否为同一点，是则本次关联成功，匹配点对为L_V1(L_V2)和L_D1，否则本次关联失败。

引入数据关联方法后，可初步建立特征地图库，直接将拍摄到所有双目图像中的特征解算位置后汇总，并利用数据关联方法去除重复点即可，建立后的特征库平面分布如图 4所示，其定位实验数据见第4章定位综合实验中表 3全部特征点实验。

1.4 路标的稀疏性策略

考虑到过于密集的特征点未必能进一步提高定位精度，而且会增加计算量，所以为了合理控制路标的数量和密度均匀性，引入稀疏性限制。

在图 4给出的建图结果中，每次提取一个新特征点时，如未关联成功，则直接作为新增特征点插入特征库，本节对未关联成功的特征点先进行稀疏性检验，满足条件的特征才进行插入操作，具体稀疏性检验策略如下：

如图 5，图中绿色的标记代表地图库中已有的路标，对于地图库中的任一路标L_Di，设其对应的两个空间邻域分别为Ω_A和Ω_B(其中Ω_A⊂Ω_B)，Ω_B以外的区域定义位为Ω_C，则路标的稀疏性限制条件为：

1) 在路标L_Di的Ω_A区域内，不允许存在任何其他路标；

2) 在路标L_Di的Ω_B内Ω_A外的区域中，不允许存在与其相似性较高的路标(为便于说明，图中用圆形标记代表与L_Di相似性较高的路标，用三角形标记代表与L_Di相似性较低的路标)。

其中第1个限制条件可以控制路标位置空间分布密度，第2个限制条件可以控制路标的特征相似性分布，保证空间距离较近的路标都在特征上有明显的差别，可有效防止路标匹配混淆。

对于当前帧图像发现的每个路标，首先判断该路标与地图库中已存在路标的相对位置关系以及特征相似性，以L_Di为例，假设新增路标L_Vm与地图库路标L_Di相似性较低，则该路标位置位于L_Di的Ω_A邻域之外时才符合插入条件，而对于新增路标L_Vn与L_Di相似性较高时，则其位置必须位于L_Di的Ω_B邻域以外才符合插入条件。需要说明的是，图 5仅以路标点L_Di为例来说明插入判断条件，实际操作时必须遍历地图库中路标点，对于所有路标均符合插入条件时，该点才允许被插入，即对于任何新发现路标L_Vj，需满足式(8)、(9)其中之一：

$ {L_{{V_j}}} \in {\mathit{\Omega }_C} $

(8)

$ {L_{{V_j}}} \in {\mathit{\Omega }_B}, {L_{{V_j}}} \notin {\mathit{\Omega }_A}, D\left( {{L_{{D_i}}}, {L_{{V_j}}}} \right) > {d_{{f_0}}} $

(9)

式中：i=1, 2, …, N, N为地图库中路标个数；d_f0为阈值常量。在建图过程中，只要合理分配空间邻域和选择特征空间阈值d_f0，就可以同时控制地图中路标点在位置空间和特征空间的分布密度。

表 2统计了离线建图过程图特征点稀疏性约束结果，图 6为路标经稀疏性策略约束后(取d_f0=2.5)，地图库中路标二维平面分布图。其定位实验数据见表 3稀疏策略后实验，相比较全部特征点实验，在定位精度和实时性上有提升。

图 7为建图过程的双目图像及特征匹配过程，抽取相机左目拍摄的4帧图像。图中“○”表示当前帧图像中新加入全局特征库的特征点(未关联成功的特征)，“┌”表示当前帧图像观测到与全局特征库数据关联成功的特征点，“×”表示当前帧图像中被稀疏掉的特征点。

1.5 稳健特征点筛选策略

在识别场景目标时，人类的视觉系统有一个对目标的显著特征区域进行定位的过程。在计算机视觉研究中，稳健局部不变特征^[13-14]选择技术类似于人类的这种视觉显著区域定位功能，是指在不同位置和角度拍摄时，将被反复观测次数较多的特征点称为“稳健特征点”(或称“稳健点”)。稳健点表征了图像局部的显著特性，选择其进行特征匹配，将进一步提高定位可靠性和精度。本节将引入稳健点筛选机制，建立基于稳健点的全局特征库。

在人工建图过程中，记录每个特征点在整个人工建图过程中被双目相机观望到次数，选取观望次数不小于3次的特征点视为特征点中的“稳健特征点”；而观望次数小于3次的特征点视为特征点中的“一般特征点”(或称“非稳健特征点”)。将所有的稳健特征点数据信息保存到全局特征库。在某次拍摄中在稀疏处理基础上，进一步采用稳健点约束处理后的特征点数量，图 8为特征点平面分布图，最后将去除537个一般特征点，最终仅采用307个稳健特征点定位，这不仅大大简化了计算量，同时也提高了定位精度，其定位实验见表 3稳健点筛选后实验。

2 基于特征库的机器人定位

如图 9，采用履带式移动机器人平台，并采用双目摄像机作为视觉传感器，安装在移动机器人前端，机器人左右履带差动运动，可原地旋转，利用左右轮的编码器进行里程测量，很容易估算出机器人的中心速度和角速度，因此后文设计算法时，直接假设可获得机器人的速度和角速度里程信息。在机器人定位过程中，认为特征库已经固定，定位时只需基于特征库动态推算机器人当前位置和姿态即可，本节首先建立机器人的运动模型和观测模型，然后基于扩展卡尔曼滤波方法实现机器人位置估计。

2.1 移动机器人运动模型

通常情况下机器人运动模型都是非线性的：

$ {\mathit{\boldsymbol{X}}_r}\left( {k + 1} \right) = f\left( {{\mathit{\boldsymbol{X}}_r}\left( k \right), \mathit{\boldsymbol{u}}\left( k \right)} \right) + {\partial _r}\left( k \right) $

(10)

式中：在二维坐标系下，X_r(k+1)=[x  y  θ]^T为机器人的位姿，表示机器人在全局坐标系下的坐标和朝向角；机器人绕O_r原地旋转(如图 10)，u(k)=[v_r w_r]为系统控制输入，表示O_r处的里程速度和角速度。视觉坐标系以左目为基准，并在右目中寻找与左目相匹配的特征点。图 10中，a为左右摄像机距离的一半，b为旋转中心到双目的垂直距离，左目O_s处的离散模型为

$ \begin{array}{l} \;\;\;\;\;\;\;\;\;\;\;\;\;\left[ \begin{array}{l} x\left( {k + 1} \right)\\ y\left( {k + 1} \right)\\ \theta \left( {k + 1} \right) \end{array} \right] = \left[ \begin{array}{l} x\left( k \right)\\ y\left( k \right)\\ \theta \left( k \right) \end{array} \right] + \\ \left[ {\begin{array}{*{20}{c}} {\Delta T{v_r}\cos \theta - \Delta T{w_r}\left( {b\sin \theta + a\cos \theta } \right)}\\ {\Delta T{v_r}\sin \theta + \Delta T{w_r}\left( {b\cos \theta - a\sin \theta } \right)}\\ {\Delta T{w_r}} \end{array}} \right] + {\partial _r}\left( k \right) \end{array} $

(11)

式中：ΔT为时间间隔，∂_r(k)为高斯噪声，其协方差矩阵为Q(k)。

2.2 移动机器人观测模型

观测值z(k)采用极坐标表示，即在视觉坐标系下，环境中某一特征点距离传感器的距离r和方向角β，如图 10所示，极坐标表示形式为

$ \begin{array}{*{20}{c}} {{\mathit{\boldsymbol{z}}_i}\left( k \right) = \mathit{\boldsymbol{h}}\left( {{X_r}} \right) = \left[{\begin{array}{*{20}{c}} r\\ \beta \end{array}} \right] = }\\ {\left[{\begin{array}{*{20}{c}} {\sqrt {{{\left( {{x_i}-x} \right)}^2} + {{\left( {{y_i}-y} \right)}^2}} }\\ {{{\tan }^{-1}}\frac{{{y_i} - y}}{{{x_i} - x}} - \theta + {\rm{ \mathit{ π} /2}}} \end{array}} \right] + {\mathit{\boldsymbol{\gamma }}_h}\left( k \right)} \end{array} $

(12)

式中：(x_i, y_i)为观测到第i个路标的世界坐标，γ_h为观测噪声，其协方差矩阵为R(k)。

2.3 基于扩展卡尔曼位置估计

EKF算法将机器人的位姿随机器人的运动不断更新，X_r(k)为k时刻机器人的位姿，其状态向量为：X_r(k)=[x(k) y(k) θ(k)]^T。人工地图建成后，特征库中路标的坐标信息写入一个状态向量X_l(k)=[u₁ v₁ … u_n v_n]^T，在定位过程中X_l(k)不再更新。算法步骤如下：

1) 根据机器人的里程计信息，可以获得第k步机器人的速度和角速度，根据第k步机器人位姿可以推算出第k+1步的机器人位姿估计，利用扩展卡尔曼误差协方差估计公式解得误差协方差矩阵估计P(k+1)^-。

$ \mathit{\boldsymbol{\hat X}}{\left( {k + 1} \right)^ - } = f\left( {\mathit{\boldsymbol{\hat X}}\left( k \right), \mathit{\boldsymbol{u}}\left( k \right)} \right) $

(13)

$ \mathit{\boldsymbol{P}}{\left( {k + 1} \right)^ - } = F\left( k \right)\mathit{\boldsymbol{P}}\left( k \right){\mathit{\boldsymbol{F}}^{\rm{T}}}\left( k \right) + \mathit{\boldsymbol{Q}}\left( k \right) $

(14)

式中F(k)为f对X(k)的偏导。

2) 提取当前帧双目图像特征，将观测到的每个特征点利用1.2节位置解算公式解算出该特征点在世界坐标系下实际三维坐标，并与全局特征库路标进行数据关联，采用1.3节的数据关联方法，对于每次关联成功的特征点，均采用步骤3和步骤4修正机器人的位置和姿态信息。

3) 针对关联成功的特征，利用该点的实际三维坐标解算出到机器人的距离和水平偏角，得到该特征点的观测真值z(k+1)(z(k)见式(12))；借助全局特征库中关联上的路标，计算出该路标到机器人的距离和水平偏角，得到机器人的观测值$ {\mathit{\boldsymbol{\tilde z}}}$(k+1)，利用该路标的观测值对X_r(k+1)求偏导，求解该路标的雅克比矩阵J_h(k+1)。利用扩展卡尔曼公式求得增益K和新息S(k+1)：

$ \mathit{\boldsymbol{K}}\left( {k + 1} \right) = \mathit{\boldsymbol{P}}\left( {k + 1} \right) - \mathit{\boldsymbol{J}}_h^{\rm{T}}\left( {k + 1} \right)\\{\left( {{\mathit{\boldsymbol{J}}_h}\left( {k + 1} \right)\mathit{\boldsymbol{P}}\left( {k + 1} \right) - {\mathit{\boldsymbol{J}}_h}\left( {k + 1} \right)T + \mathit{\boldsymbol{R}}\left( {k + 1} \right)} \right)^{ - 1}} $

(15)

$ \mathit{\boldsymbol{S}}\left( {k + 1} \right) = \mathit{\boldsymbol{z}}\left( {k + 1} \right) - \mathit{\boldsymbol{\tilde z}}\left( {k + 1} \right) $

(16)

4) 根据以上两个步骤得到的状态估计$ \mathit{\boldsymbol{\hat X}}{\left( {k + 1} \right)^-}$，误差协方差矩阵估计P(k+1)^-，扩展卡尔曼增益K和新息S(k+1)，利用以下公式实现状态更新：

$ \mathit{\boldsymbol{\hat X}}\left( {k + 1} \right) = \mathit{\boldsymbol{\hat X}}{\left( {k + 1} \right)^{ - 1}} + \mathit{\boldsymbol{K}}\left( {k + 1} \right)\mathit{\boldsymbol{S}}\left( {k + 1} \right) $

(17)

$ \mathit{\boldsymbol{P}}\left( {k + 1} \right) = \left( {\mathit{\boldsymbol{I}} - \mathit{\boldsymbol{K}}\left( {k + 1} \right){\mathit{\boldsymbol{J}}_h}\left( {k + 1} \right)} \right)\mathit{\boldsymbol{P}}{\left( {k + 1} \right)^ - } $

(18)

3 特征库的定期更新

本文采用的离线建立特征库建立策略，保证了在线定位的实时性、精度和稳定性，但缺点是地图固化后无法适应环境的后期变化，为了提高算法实用性，本节引入特征库的定期更新策略进行弥补。

如图 11，图中的全局特征库代表已经离线建立的特征库，也是机器人实时定位的基础，在此基础上增加一个动态特征库，记录机器人近期一段时间观测获得的特征，动态特征库不断从机器人当前观测特征中获取数据，区别只是此时机器人位置姿态不再通过人工测量，而是实时通过定位算法获取。动态库更新策略与第1节人工建图过程一致，但主要经过特征提取、数据关联、稀疏性限制等步骤，暂不进行稳健点筛选。

动态特征库会定期地更新至全局特征库，更新过程主要经过数据关联(去除重复点)、稀疏性限制和特征的稳健点筛选步骤，更新完成后即可清除动态特征库，后续定位时再重新记录。该更新过程不需要实时进行，可间隔一段时间更新一次，或者室内环境发生明显变化时再更新，这样可以保证用来定位的全局特征库不会发生快速变化，保证定位过程的稳定性，同时也便于在动态特征库中不断统计特征点的观测次数，以便作出正确的稳健点评价后再更新至全局特征库。

为防止全局特征库中特征持续上升，可定期对其进行特征删除操作，删除条件可检测理论上应位于机器人观测视场之内，当实际观测时并未检测到该特征的情况，设机器人k时刻的位姿[x(k) y(k) θ(k)]^T和机器人的观测角度范围(-β₀, β₀)、距离范围(r_min, r_max)，遍历全局特征库中的每一个路标[u_i v_i]^T，并利用式(12)计算该路标在机器人观测坐标系下的极坐标[r_i, β_i]^T，如果位于有效观测范围内，但数据关联时未匹配该特征，则认为本特征观测失败，为防止误操作，可对特征库中特征观测失败次数进行计数，达到一定次数时再进行删除。

通过上述特征更新策略，即可维持特征库数量的平衡，又可定期换入新的特征，从而不断适应环境的变化，达到长期有效的定位。

4 定位综合实验

定位实验选取的是1 024×768的图像，机器人直线速度约为0.07 m/s，转弯的角速度约为0.23 rad/s，机器人的初始位置(2.36 m，6.79 m)。

令机器人在实验室复杂室内环境下进行定位实验，结果如表 3，为了对比前面建图及定位过程各项策略的效果，表中按照特征库选取全部特征点、稀疏策略后、稳健点筛选后以及定位过程特征库定期更新后分别进行实验，各类实验均为离线建图模式，不同实验最终保留的特征点数量如表中第2列所示。图 12为稳健特征点定位实验过程中，拍取几帧图像，图像中的两种符号(○型符号和×型符号)所代表的是该帧图像所观测到的特征点；在定位过程中，图像中○型符号为与特征库中路标匹配关联成功的特征点，而×型符号为与特征库中路标没有匹配关联的特征点。图 13为本次稳健特征点实验过程中，X方向偏差曲线图、Y方向偏差曲线图、角度偏差曲线图，以及在X-Y二维轨迹上偏差曲线图。表 3中分别给出了各实验过程x方向偏差、y方向偏差、角度偏差以及实时性。对比可见，在全部特征点试验结果基础上，通过引入稀疏策略、稳健点筛选策略，实时性不断提升，但定位精度并未受到明显影响，且有少量提升。另外进行了特征库后期更新试验(表中最后一行结果)，在前期定位实验一周后再次在相同环境下定位实验(环境已有部分变化)，引入特征库定期更新策略后，新增了56个特征点，删除掉了42个特征点，最终实验结束后，特征库共有321个特征点，其定位精度及实时性与前期实验结果接近。

5 结论

1) 针对移动机器人室内环境的双目定位问题开展研究，以EKF方法为基础，但初始特征库采用人工离线测量的方法，特征库一旦建立，在线定位时仅对机器人位姿进行卡尔曼滤波估计，而不再维护包含路标位置在内的大型协方差矩阵，克服了EKF-SLAM方法计算量过大和数据关联脆弱的问题，提高了定位算法的实时性和可靠性。

2) 本方法在建立基于双目视觉系统的特征库时，在房间内选择若干拍摄位置，每个位置选择多个拍摄角度，形成初始图片库，然后根据拍摄位置、角度信息和双目视觉测距公式解算特征点的三维位置等信息，解算并建立离线特征库，解算过程中引入了特征稀疏性策略、稳健特征点选取、误匹配去除等方法，既提高了定位精度和可靠性，又可合理控制特征点数量。

3) 在已建立的离线特征库的基础上，通过EKF方法实现机器人在线定位，且为了适应室内环境变化，算法可定期记录图片库，并实现特征库的定期更新，最终算法可保证机器人在室内环境下长期、有效地定位。

4) 本方法应用过程需要事先拍摄环境，仅适用于特定环境下的定位，不适用于对未知环境的探索过程。