结合视觉里程计的微小型空中机器人SLAM研究

移动机器人的同步定位和地图构建(simultaneous lo-calization and mapping，SLAM)问题研究是指对移动机器人探索未知环境，创建可靠环境地图，并同时利用地图来进行自主定位能力的研究。它是移动机器人实现完全自主性的关键技术，具有重要理论和应用价值，甚至被许多学者称之为移动机器人研究领域的圣杯” 。自从提出SLAM命题以来，众多学者进行了孜孜不倦的研究，取得了许多有益的研究成果，并将这些成果成功地应用于很多室收稿日期 ：2010-12 Received Date：2010-12内、室外的陆地移动机器人导航系统中 。 。

与陆地机器人相比，以智能飞行器为代表的空中机器人运动范围更为广阔，运动模式更为复杂。他们拥有6自由度的运动模式，并且常常需要构建 3维的地图来完成复杂的飞行导航任务，所以它们的 SLAM问题往往被称为6DOF 3DSLAM问题。然而，目前提出的较成熟的适用于陆地机器人的SLAM解决方案尚不能完全解决空中机器人 6DOF 3DSLAM问题≌中机器人具有3D运动特点，使运动模型具有高度非线性，在SLAM算法的鲁棒性、计算复杂性等诸多方面依然面临着重大挑战。特别对于有广阔应用和科研前景的微小型空中机器人而言，更需要考虑负重、体积和飞行476 仪 器 仪 表 学 报 第 3 4卷环境等因素，从而使解决它们 SLAM问题所能选择的传感器也受到很大限制。对于这些困难，虽然国际上有不少科研机构正在从事相关研究，但是-直并未有标志性的科研成果报道，在国内更是鲜有相关研究报道。

近几年 ，Davison A J等人 提出了-种被视为首个成功的纯单目视觉”实时3D SLAM方法，为微小型空中机器人 3D SLAM提供了-个很好的思路；2009年，JorgeArtieda等人0 在此方法基础上提出～·种针对微小型无人直升机的单 目实时6 DOF 3D SLAM方法 ，并通过实际飞行实验进行了验证。这套方法所需硬件成本孝功耗低、传感器结构简单(只需要-个摄像头)，非常适合微小型机器人平台。但是，根据Jorge Artieda等人的研究报道，可以看出这套方法直接应用于空中机器人 3D SLAM还存在着许多问题。尤其是这套基于平滑运动摄像机模型”的方法不适用于飞行器较复杂的飞行动作。

针对这个问题 ，本文提出-种不需要运动摄像机平滑运动”约束的单目视觉 SLAM方法。该方法通过视觉里程计技术来直接获得摄像机的运动信息，并将其嵌入视觉 SLAM算法中。同时，在采用视觉里程计进行位姿估计时，针对可能出现的退化问题，采用特征分类的策略，提高了估计的鲁棒性。最后，通过在-套真实的智能微小型无人直升机上的实验，验证了该算法的有效性。

2 平滑运动摄像机状态模型假设机器人与机载摄像机之间固连或存在可测相对运动，则机器人的运动状态也可通过摄像机的运动状态来表示。根据文献[4]，单目运动摄像机的状态向量可以表示为：rqwcc式中：，. 和v 分别表示在世界坐标系 中摄像机的3维位置和线速度，q 代表在世界坐标系和摄像机坐标系 C下使用四元素法表示的摄像机方向， 。表示摄像机在摄像机坐标下的旋转角速度。在k时刻至k1时刻，运动摄像机的状态变化方程可以表示为：r"L w( )·At (2)l垡 g ×( ： 。)·At (3)l r W1vKwV (4) 2 (5)式中： 时刻到 1时刻时间间隔为 △ ， 和 是这个时间间隔中摄像机由噪声引起的线速度和角速度。在这个方程中，文献[4]假设摄像机的运动是-种平滑”运动，即在每个时间间隔中速度为常量。但是，这个假设显然无法-般性的适合于空中机器人的各种运动模式。

空中机器人的运动灵活，经常在很短的时间内进行加减速飞行，无法保证以上方程中时间问隔中速度为常量。

在文献[5]的研究报道中，也指出采用平滑运动”摄像机模型的SLAM算法仅仅可以适用于空中机器人悬停或匀速漂移运动，却无法适用于他们较复杂的飞行动作。

3 结合视觉里程计的SLAM 算法3.1 基于视觉里程计的位姿估计方法视觉里程计最早在文献[6]中提出，并已广泛应用于各种移动机器人的导航中。与基于视觉信息的 SLAM的部分 目标-致，视觉里程计也期望获得运动摄像机的运动轨迹。但与 SLAM不同的是，视觉里程计并不进行实时构图，同时它的实现机制也不是基于滤波的方法，而是通过序列图像之间多视角几何约束来直接获得运动摄像机相对的位姿变化信息。与 SLAM方法相比，在位姿估计方面，视觉里程计可以利用更多的图像特征点来提高估计的精度，降低噪声的影响，但是由于存在误差积累，它无法用于长时间、大范围的实时估计。

在本文研究中，在单目视觉条件下，运动摄像机的位姿变化信息试图由视觉里程计直接获得，并将这些先验运动信息嵌入 SLAM过程中以适用于摄像机的复杂运动。-方面，用于估计姿态变化的大量图像特征并不全部用于构图，在加强了位姿估计鲁棒性的情况下并不大幅增加 SLAM算法计算复杂性；另-方面，在滤波器机制作用下，在 SLAM过程中出现回路 (1oop-closure)现象时，位姿估计值也会趋于收敛。

图1表示了经典视觉里程计最基本的原理： 为空间中的特征点，，分别为k时刻和 1时刻，摄像机两次观测中，M在机载摄像机相平面上的投影，而R，t分别表示了两次观测时机载摄像机的相对位姿变化中的旋转矩阵和位移向量。利用特征点在两次观测中的匹配关系，在多视图几何模型约束下，估计本质矩阵E，从而获得(R，t)的解 。

，，、、、、i赢i-， -- ，- 图 1 机载运动摄像机对特征点 M的两次观测Fig.1 Twice observations of the featureMfrom a movingcamera fixed on the aerial robot第2期 任沁源 等：结合视觉里程计的微小型空中机器人SLAM研究 477可以看出寻找可靠的图像匹配特征和鲁棒 的估计(R，t)是实现视觉里程计的关键技术。根据以往的研究报道，Harris特征和 SIFT特征是视觉里程计研究中最为常用的两种特征。尤其是SIFT特征对图像的尺度变化、旋转和亮度变换保持不变性，对于视角变化、仿射变换和噪声影响也有很强的稳定性，特别适用于飞行器拍摄低空遥感图像。在文献[5]的研究中，通过对多种图像特征的对比实验，也验证了SIFT特征在微小型空中机器人视觉系统中使用的适用性和实用性。而在位姿估计方面，众多研究者的做法大都将随机抽样-致性算法(radom sample corseflsLls，RANSAC)与估计方法结合起来，降低噪声和误匹配的影响，提高估计鲁棒性。但是，-般应用于地面机器人的鲁棒估计方法在空中机器人平台应用中，依然存在着重大的缺陷。

图2 航拍序列图像 SIFT特征点匹配Fig.2 SIFT match points between two serial aerial photos采用微小型智能无人直升机航拍序列图像特征点匹配情况如图2所示，两幅图之间的连线表示图中SIFT特征的匹配对应关系∩以看到，飞行器在飞行过程中，大多观测地面特征，在-般飞行高度下，众多的特征往往属于同-平面。这种情况在基于 RANSAC的视觉里程计算法中会造成严重的退化问题。文献[8]对这种现象进行了详细的 阐述 和分 析，并 针对 性地 提 出 了 -种 被 称 为QDEGSAC的鲁棒估计算法。但是，这种基于模型选择的算法并不具备有实时性；文献[9]针对这类问题则采用了- 种特征分类的方法，针对不同类采用两步估计的方法来提高位姿估计鲁棒性。但是，这个方法主要是针对地面轮式机器人，在分类过程中，主要是将可能产生退化的无穷远点特征区分出来，所以这个方法并不适用空中机器人视觉里程计。针对退化问题，在本文的研究中，采用的策略与文献[9]的特征分类思想相似，但是却采用完全不同的技术方法。简而言之，就是在估计位姿之前，采用单应性约束，将所有特征分为2类。取满足同-平面单映性约束的最大特征集为-类，剩下的特征集构成另-类。在分类基础上，再进行基于 RANSAC的位姿估计，并增加以下约束：在基于 RANSAC的位姿估计过程中，不能将从同平面特征类中选择所有的特征点构造模型。这个约束将降低采用同平面匹配特征点估计的可能性，减少退化现象出现的风险。特征点具体分类算法如下(记所有的匹配特征数据集为 S)：1)随机地从 S中选择4个数据点构成随机样本并计算单应 日；2)利用步骤 1求出的单应型矩阵约束数据集 s，对每组假设对应计算距离 d；3)取 d

由循环过程中与同-单应性矩阵-致的最多特征构成，剩下的特征构成 5，。

3.2 SLAM 系统状态模型和观测模型将 3.1节中描述的通过视觉里程计得到的摄像机相对姿态变化运动信息 (R，t)代入式(2)、(3)，得到在k时刻至 1时刻，摄像机新的状态变化方程：f， ， W-R · K (6)g： 垡 ×g 十1 (7)式中： 表示摄像机坐标系与世界坐标系之间的旋转矩阵， K和口 分别表示在k时刻至k1时刻，通过二视图几何约束估计到摄像机相对位移和旋转(这里也使用四元素法表示)。在这个方程中，摄像机姿态变化直接由序列图像之间特征匹配信息获得，并不需要将速度值再引入状态变化方程中，从而也不需要摄像机平滑运动”的约束。

在修正的摄像机状态变化方程基础上 ，建立 SLAM系统的状态模型和观测模型。SLAM系统的状态变量由机载摄像机位姿和观测到的静态环境特征点组成。在时刻，系统状态向量表示为：(8)X 为摄像机位姿状态，Y 为特征点 i的空间坐标。在单目视觉下，特征的初始深度信息必须通过摄像机两次以上观测才能得到，并且容易产生较大的误差。为了避免初始非线性误差给基于 EKF的 SLAM算法带来的不利影响，特征点 Y 采用文献[10]提 出的视差深度 (in-verse depth)的3D建模描述方法，每-个点特征由-个 6维矢量描述。同时，由于环境静止不动，系统的状态变化/ f 、方程即为f l。

Y l Yi，K/环境中特征点相对摄像机的位姿关系可以由以下方程得到：h R哪(y7-， ) (9)设特征点在图像中的坐标为(， )，在摄像机凶模型基础上得到的系统观测模型为：478 仪 器 仪 表 学 报 第 3 4卷h ：f1：v/(10)式中： ， 为文献[11]中定义的摄像机内参数矩阵系数。

3.3 基于 EKF的 SLAM 迭代过程基于 EKF的SLAM迭代过程中，每-步都可以按照预测、数据关联、更新、构图与状态增广的顺序来进行。

1)预测状态预测：氟川 ：f ( - 1 (1)Y ，川 F P F G G：式中：耻 幽g( ，)GK diag、Odf v， K，。)KI4)构图与状态增广地图构建在环境特征点的空间坐标基础上构造。由于系统状态变化激励噪声和观测噪声的存在，通过 EKF估计得到的环境特征点空问坐标具有不确定性，它们符合-定的概率分布，并且随着观测次数的增加，逐步收敛到真值。所以，本文采用文献[13]中提出的概率地图构图法构图。同时，新引入的环境特征点Y 按： ]， l Y f (21) LJ形式进行状态增广，并且根据摄像机的位姿状态信息和观测方程按文献[8]中提出的视差深度方法进行初始化。

r 12) 4 实 验(13)(14)(15)Q 为状态变化方程在 K时刻的过程噪声激励协方差阵。

2)数据关联系统采用文献[12]中提出的基于联合相容分枝定界算法(joint compatibility branch and bound，JCBB)的数据关联方法。其中，在基于单目视觉的 SLAM中，根据图像中特征点的选择 ，将计算机视觉中的图像特征匹配技术引入特征点之间的关联判断。根据特征在图像中出现位置的预测方程：五 十Jl h(y Il ，； 十Il ) (16)在特征可能出现位置的邻域附近进行搜索和匹配，可以大大增加匹配的准确性和实时性。

3)更新卡尔曼增益为：KK 户n j (月 户 川 。 ) (17)状态更新：川 川 K (，l -hi，KlK) (18)误差协方差更新：户 (，- ) 川 (19)日 由测量雅克比矩阵：日 I( ， ， Oh ) (20根据文献[6]中的线性化方法得到。R为观测噪声协方差阵，在本算法中设为单位阵。

- 般基于EKF的SLAM算法具有O(n )的计算复杂I生(n为SLAM系统状态量) ，会随着机器人运动空间的扩大，大幅增加计算量，所以如何降低大尺度空问中移动机器人SLAM的计算复杂生依然是-个待解决的难题。但是，由于这并不是本文研究重点，所以在试验中，仅仅考察空中机器人在小尺度空间运动中的情况。为了验证本文提出的方法，在-套自主研发的微小型旋翼型空中机器人--无人直升机上进行了实验，如图3所示。无人直升机具有 6自由度，可以在三维空问内自由运动，是空中机器人最具有-般性的良好代表。该飞行器翼长 1.2 m，负重4.5 kg，最高飞行速度为80 knv'h。整个机载电控系统包括飞控计算机、差分GPS、惯性测量单元(ineiN measurement uint，IMU)、数传电台、以及由图像处理机和数字摄像机构成的视觉系统。其中，图像处理机采用的是主频为 2.3 G的双核微型单板机，而摄像机采用的是罗技的定焦摄像头C600。在室外无遮挡的条件下，该飞行器可以实现基于 GPS和 IMU测姿信息的自主悬停和航线飞行，相关细节可以