帧计算时，初始化世界地图，并在非首帧计算时，根据步骤S7中计算 的变换矩阵更新世界地图中与当前帧成功匹配的特征点的三维坐标与特征描述子，并向世 界地图中加入当前帧新观察到的特征点及其特征描述子。其中，世界地图包含已拍摄区域 提取出的特征点在世界坐标系下的坐标以及特征点的特征描述子。在对当前帧计算时，成 功匹配的特征点的世界坐标将被更新，新观察到的特征点将被加入世界地图。世界地图的 使用与维护，一方面综合了各个帧的历史信息得到特征点在世界坐标系下三维坐标的准确 估计，另一方面通过世界地图匹配步骤成功建立当前帧与历史帧的数据关联，通过误差抵 偿效应有效减小了测量误差和累计误差。此外，所构建的世界地图包含稠密的空间特征，可 用于后续障碍躲避，路径规划工作。
[0076] 具体地，在首帧计算时，初始化所述世界地图，将首帧的图像捕获系统参考系确定 为本次定位的世界坐标系，将首帧观测到的全部特征点的三维坐标与特征描述子加入世界 地图中，作为世界地图的初始化构建。世界坐标系一经确定在后续计算中不再发生改变。
[0077] 进一步地，在当前帧提取的特征点中，一部分与世界地图成功建立了匹配关系，根 据步骤S7中的位姿估计得到坐标变换关系R和T，将特征点在当前相机参考系下的三维坐标 逆投影回世界坐标系，作为特征点的一次新的观察结果CoorGlobal'，转换公式为：
[0078] CoorGlobal/ =inv(R) X (CoorCamera-T) 〇
[0079] 进一步地，将成功匹配的特征点在世界坐标系下的三维坐标更新为多次观察结果 的均值。具体地，世界地图中某个特征点的坐标是第η次观察到该特征点后的均值，在当前 帧第η+1次观察到该特征点并计算得到观察结果CoorGlobal/，更新该点世界地图中三维坐 标的公式为：
[0080]
[0081]具体地，根据中心极限定理，测量误差近似符合以真值为均值的正态分布。通过对 特征点的各次观察结果取算术平均，测量误差通过误差抵偿效应有效减小，世界地图中特 征点的三维坐标更接近真值。
[0082] 进一步地，成功匹配的特征点在世界地图中的特征点描述子由最近一帧图像中提 取到的特征描述子更新。考虑到时间与空间的连续性，最近一帧图像与后续拍摄图像具有 更相近的成像条件，使用最近一次的特征点描述子与后续图像中提取的特征点描述子建立 成功匹配的几率更大。
[0083] 进一步地，当前帧中未与世界地图成功匹配的特征点被认为是新加入的特征点。 其最可能出现在视角变换后新的观察区域，也可能是对重叠观察区域特征点的补充提取。 其在相机参考系下的三维坐标同样被逆投影到世界坐标系，并被加入到世界地图中，其特 征点描述子也被加入世界地图。
[0084] 进一步地，本发明的实施例具有相比扩展卡尔曼滤波同时定位与地图构建算法 (EKF-SLAM)更简单的的计算复杂度。
[0085] 具体地，本发明实施例的方法与基于扩展卡尔曼滤波同时定位与地图构建方法均 包含步骤S5中所述的世界地图匹配，其计算复杂度为:设世界地图包含η个特征点，每个特 征点的特征描述子为Ρ维列向量，当前帧中有m个特征点参与与世界地图的匹配计算，在这 一步骤矩阵乘法的计算复杂度为O(mXnXp)。在本发明的一个具体实施例中，设当前帧中 有个特征点与世界地图建立了匹配关系，步骤S6中的随机抽样一致方法与步骤S7中的位 姿估计的计算复杂度约为〇(3Χη^Χ?〇，其中k代表随机抽样一致方法中的迭代次数。对于 基于扩展卡尔曼滤波同时定位与地图构建方法，其位姿估计步骤需要维护世界地图的协方 差矩阵，该步骤计算复杂度约为0(nXn)。考虑到迭代次数k及当前帧与世界地图匹配的特 征点个数远远小于世界地图中的特征点个数n，因此，本发明实施例方法相比基于扩展卡 尔曼滤波同时定位与地图构建方法具有更简单的计算复杂度，考虑到移动平台的低计算能 力，低功耗特性，本发明方法更适用于小型移动平台的实时定位任务。
[0086]为了验证本发明实施例所提出的基于视觉测程的同时定位与地图构建方法的性 能，本发明的实施例通过双目图像捕获系统拍摄室内真实场景，并在MATLAB R2014b实施了 该方法的软件验证。其中，双目图像捕获系统安装在水平滑轨上以保证移动平台的直线运 动。如表1所示，给出了双目图像捕获系统运动不同的距离、旋转不同的角度时本发明实施 例中的方法给出的位姿估计值，其中X/Y/Z代表双目图像捕获系统在世界坐标系中的位置 坐标，Pitch/Yaw/Rol 1代表双目图像捕获系统在世界坐标系中的俯仰角/偏航角/滚转角。 测量真值是通过水平轨道的刻度尺示数及量角器所得，当双目图像捕获系统处于静止状态 时，正负值表示对运动估计的标准差；当双目图像捕获系统发生运动时，表1中给出的估计 结果是十次运动估计的平均值。从表1中可以看出，在该实施例中，运动估计误差小于两厘 米，这与基于扩展卡尔曼滤波的同时定位与地图构建算法的定位精度相近。进一步地，计算 静止状态下运动估计的误差，这指示了本发明的实施例对运动估计结果的漂移情况：
[0090] 表 1
[0091] 进一步地，如图5所示，给出了本发明一个实施例的室内定位运动轨迹及世界地图 示意图。在该实施例中，双目图像捕获系统被放置在装配滑轮的小车上。推动小车以5cm/s 的速度向前移动约80cm，逆时针旋转约65°，再沿该方向继续向前运动。可以看出，本发明提 出的位姿估计方法给出了准确的位姿估计结果。同时，构建的世界地图中包含大量空间特 征点的三维信息，可用于后续障碍躲避，路径规划等工作。
[0092] 进一步地，如图6所示，给出了本发明一个实施例的室内无偏测试结果示意图。在 该实施例中，双目图像捕获系统在水平滑轨上沿同一段距离往复运动20次，深度方向的距 离估计在多次计算中没有发生单向的漂移。这说明本发明提出的位置估计方法不随时间推 移而发生累计误差进而逐渐漂离真值。通过世界地图建立的数据关联，有效减小了对空间 中闭环运动估计的累计误差。
[0093] 综上，根据本发明实施例的基于视觉测程的同时定位与地图构建方法，通过双目 相机定位系统，可以在无先验知识的室内环境中恢复空间特征点的真实深度;采用视觉测 程方法进行位姿估计，有效的降低了同时定位与地图构建算法的计算复杂度，使得计算速 度与世界地图规模得到提升，适用于实时定位移动平台，且可以工作在更大范围的室内空 间中；同时，通过随机抽样一致方法有效的消除了错误匹配的特征点，避免为位姿估计引入 误差信息;此外，通过建立并更新世界地图有效消除了测量误差的累计，保证了室内定位精 度在厘米级别且不随时间发生漂移;此外，所构建的世界地图包含稠密的空间特征信息，可 用于后续障碍躲避，路径规划工作。
[0094]在本发明的描述中，需要理解的是，术语"中心"、"纵向"、"横向"、"长度"、"宽度"、 "厚度"、"上"、"下"、"前"、"后"、"左"、"右"、"竖直"、"水平"、"顶"、"底" "内"、"外"、"顺时 针"、"逆时针"、"轴向"、"径向"、"周向"等指示的方位或位置关系为基于附图所示的方位或 位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必 须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
[0095]此外，术语"第一"、"第二"仅用于描述目的，而不能理解为指示或暗示相对重要性 或者隐含指明所指示的技术特征的数量。由此，限定有"第一"、"第二"的特征可以明示或者 隐含地包括至少一个该特征。在本发明的描述中，"多个"的含义是至少两个，例如两个，三 个等，除非另有明确具体的限定。
[0096] 在本发明中，除非另有明确的规定和限定，术语"安装"、"相连"、"连接"、"固定"等 术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体;可以是机械连 接，也可以是电连接;可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内 部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员 而言，可以根据具体情况理解上述术语在本发明中的具体含义。
[0097] 在本发明中，除非另有明确的规定和限定，第一特征在第二特征"上"或"下"可以 是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在 第二特征"之上"、"上方"和"上面