徐伟杰，李 平，韩 波

(浙江大学控制科学与工程系，杭州 310027)

姿态测量在移动机器人运动控制中非常重要，例如旋翼机型无人机实现悬停控制需要精准的姿态信息。传统的航姿测量方法［1－2］主要依赖MEMS惯性传感器(如陀螺仪和加速度计)，近十年来，由于摄像机具有成本低、能耗低和重量轻等优点，以标定过的摄像机作为传感器的视觉姿态测量方法成为当前研究热点。但是多数方法需要移动机器人工作环境中存在特定结构物体如道路、跑道的边缘线［3］，地平线等［4－8］，或者人工图标［9］。非结构化工作环境中不存在特定结构物体和人工图标，无法使用前述方法测量姿态。

针对非结构化工作环境，学者们提出了各种称为“视觉罗盘”［10－12］的姿态测量方法，如 Labrosse 等人［10］使用全景摄像机，通过全景图的快速匹配测量航向角(yaw)；Sturm 等人［11］使用普通摄像机，创建环境的一维柱状颜色模式分布图，通过颜色模式匹配测量航向角；与前两种只能测量航向角的方法不同，Montiel等人［12］提出将基于扩展卡尔曼滤波(Extended Kalman Filter，EKF)的同步定位与地图创建(Simultaneous Localization and Mapping，SLAM)［13］方法应用于姿态测量，以摄像机平滑旋转模型假设和主动搜索匹配技术［14］为核心思想，能够同时测量出航向角、俯仰角(pitch)和横滚角(roll)。

基于EKF-SLAM的视觉罗盘姿态测量方法存在两个问题。第1个问题是由于使用EKF，需要对非线性模型进行线性化，会引入线性化截断误差。可以用无迹卡尔曼滤波(Unscented Kalman Filter，UKF)取代EKF，使测量精度得以提高。第2个问题是在实际应用中摄像机可能受到冲击扰动而旋转不平滑，需要提高平滑旋转模型中系统噪声的先验方差设定值，但是随之导致主动搜索匹配区域急剧扩大，造成匹配计算量增大和易受与路标相似的图像特征影响匹配错误发生率升高，目前仍然没有解决方案。

针对匹配计算量增大和匹配错误发生率升高，本文给出一种改进的视觉罗盘姿态测量方法:首先使用多分辨率路标选取策略初始化新路标，然后使用逐层主动搜索匹配算法减少匹配计算量和降低匹配错误发生率，最后结合平滑转动模型得到的姿态先验信息，使用最小抽样集为2个匹配点的RANSAC算法去除匹配野值，提高姿态测量精确度。实验结果表明，改进的方法与原方法相比具有计算量更小、姿态测量更精确的优点。

1 EKF-SLAM的视觉罗盘

1.1 状态向量

定义系统状态向量x及其协方差矩阵P形式如下:

式中摄像机C的状态向量xC=［qWC，ωC］T由四元数qWC和角速度ωC构成，qWC以世界坐标系W为参考系，ωC以摄像机坐标系C为参考系。地图M的状态向量 xM=［y1，…，yN］T由N个路标yi构成，以世界坐标系W为参考系。

如图1，空间点X到光心OC的距离d远大于摄像机平移距离‖t‖时，可认为X是无穷远的。由于X到OC的方向矢量m由方位角θ和高度角φ唯一确定:

因此令路标参数为 y=［θ，φ］T。

图1 视觉罗盘示意图

1.2 运动模型

定义系统运动模型 f(xk－1，αk)如下:

式中摄像机运动模型fC(xC，k－1，αk)具体形式如下:

角加速度 αk=［αx，αy，αz］T是系统噪声，符合零均值高斯分布N(0，σ2αI3×3)，αα是 αk的先验方差。考虑到摄像机非平滑旋转时实际方差^σα较大，需要提高先验方差σα的设定值。

1.3 观测模型

世界坐标系W到摄像机坐标系C的旋转矩阵为RWC=R(qWC)。路标y所对应方向矢量m(θ，φ)在摄像机坐标系C下坐标为:

若摄像机不存在光学畸变，则mC成像为非畸变的像hu:

结合式(2)、式(5)、式(6)和式(7)，摄像机姿态为时路标yi的观测函数h(，yi，vi，k):

式中观测噪声vi，k～N(0，Ri，k)，Ri，k是观测噪声协方差矩阵。

当图像中可视路标数量小于一定数量时，需要从图像中路标最稀疏的区域里选取图像特征znew，初始化路标 ynew，更新状态向量 xnew和协方差矩阵Pnew。

1.4 EKF-SLAM 流程

EKF-SLAM流程分为状态预测、观测预测、观测匹配和状态更新4步。

(1)状态预测 根据k－1时刻状态和协方差的估计值，由运动模型(3)预测时刻状态、协方差的估计值:

(2)观测预测 预测各路标yi观测值和协方差Si，k的估计值:

式中观测方程Jacobian矩阵Hi，k=［∂h/，0，…，0，∂h/∂yi，0，…，0］，Ri，k是观测噪声协方差矩阵。

(3)观测匹配 在视觉EKF-SLAM中，以路标yi的描述子为11像素×11像素的图像模板L11(yi)为例，使用主动搜索匹配对式(13)预测可见路标yi进行模板匹配。以观测值为中心，残差协方差Si，k确定主动搜索的区域Ω(Si，k)，计算模板L11(yi)与区域内各像素(u，v)处局部图像L11(u，v)的正则化相关系数CC(u，v)。取像素(u*，v*)=maxCC(u，v)＞CCth，作为匹配点=［u*，v*］T，其CCth中为相关系数阈值。把所有构成k时刻观测向量:

(4)状态更新 观测向量≠Ø时，更新状态和协方差矩阵的估计值:

其中Kk为Kalman滤波增益，观测Jacobian矩阵Hk=［…，Hi，k，…］T，观测噪声协方差矩阵 Rk=diag(…，Ri，k，…)。式(17) 得到的中通常必须单位化。

2 改进的视觉罗盘姿态测量方法

针对设定较大的先验系统噪声方差时存在匹配计算量大和匹配错误的问题，本文对原视觉罗盘姿态测量方法进行改进。首先初始化路标时构建高斯图像金字塔，选取各层图像中均为Shi-Tomasi角点的特征为路标；然后按照自顶层图像到底层图像的顺序逐层主动搜索匹配，减少匹配计算量和匹配错误发生率；最后结合由平滑旋转模型得到的姿态先验信息，使用2点RANSAC去除匹配野值。

2.1 多分辨率图像路标选取

构建L层图像金字塔［15］，以输入图像L为基层，逐层降采样至原尺寸的 2－2l，l≤L－1，如图 2 所示。本文中输入图像原始分辨率为像素，构建L=3层图像金字塔，各层图像L0、L1和L2对应分辨率320像素×240像素、160像素×120像素和80像素×60像素。

图2 图像金字塔示意图

Shi-Tomasi角点［16］在图像模板匹配中具有良好的定位性能。其提取过程首先计算自相关函数矩阵M:

式中w(u，v)为高斯核函数加权系数、是图像在x和y方向的偏导。然后计算M的特征值λ1、λ2。若像素min(λ1，λ2)取到局部极大值且大于设定阈值，可确认为Shi-Tomasi角点。

改进的方法选取在L0、L1和L2中同为Shi-Tomasi角点的图像特征作为路标yi，取11像素×11像素的模板(yi)(yi)和(yi)作为路标yi的描述子，用于逐层主动搜索匹配。

2.2 逐层主动搜索匹配

在EKF-SLAM流程中，式(13)和式(14)得到观测值、残差协方差矩阵Si的预测值后进行主动搜索匹配。与原方法具有单一分辨率模板不同，改进的方法具有多分辨率模板，可以在图像金字塔各层图像中进行匹配。由图2可知，在图像金字塔顶层图像LL中，坐标尺度变换使得主动搜索区域内像素个数减少到图像L中的2－2L。在其余各层图像Ll，l＜L中，在 Ll+1中匹配点坐标尺度变换附近主动搜索匹配。自顶层图像到底层图像进行逐层主动搜索匹配可以减少匹配计算量并降低匹配错误发生率。

算法1逐层主动搜索匹配算法

输入:图像 Ll和路标模板{(yi)}，l≤L，预测的观测值、残差协方差矩阵Si。

输出:匹配点首先，在第L层图像中进行主动搜索匹配。进行坐标尺度变换，在以为中心的搜索区域Ω()内计算与(yi)的相关度CC(u，v)。搜索区域Ω()定义为:

取像素(u*，v*)=maxCC(u，v)＞CCth，作为的匹配点=［u*，v*］T。阈值CCth根据经验设定，如0.8～0.9。然后，在第l＜L层图像中进行主动搜索匹配。进行坐标尺度变换→，在以为中心的搜索区域 Ω(r)内计算与(yi)的相关度CC(u，v)。搜索区域Ω(r)定义为:

半径r=3像素。取像素(u*，v*)=maxCC(u，v)＞CCth作为的匹配点=［u*，v*］T。若l=0 停止，作为输出；反之令l=l－1重复进行主动搜索匹配。

2.3 2点RANSAC算法去除匹配野值

逐层主动搜索匹配无法保证零匹配错误率，需要使用RANSAC算法去除匹配野值。由于抽样匹配点数量越大，RANSAC算法的抽样迭代次数越大，因此要减少抽样匹配点数量。考虑到只需2个匹配点对能够唯一确定摄像机姿态，又有EKF-SLAM运动模型提供的姿态先验信息可以利用，本文提出抽样集为2个数据点的2点RANSAC去除匹配野点算法，可以减少抽样迭代次数。

算法2 2点RANSAC算法

输入:匹配点集{}，状态向量和协方差矩阵。

输出:匹配点集内点集。

令=∅；

循环j=1，…，nhyp，nhyp为最大抽样次数:

从匹配点集{}随机抽样集{}，令观测向量

根据更新后的状态向量重新进行预测各路标的观测值zi|(j)=h(x(j))；

循环结束。

用匹配点内值集取代式(15)中的，按照式(16)、式(17)、式(18)更新状态和协方差估计值。

3 实验与结果分析

为验证本文改进的方法的有效性，根据应用背景用四旋翼无人机的机载摄像机在室外环境中采集视频用于实验。摄像机为更换焦距2.5 mm广角镜头后的Logitech QuickCam Pro4000，水平视角约为90°，安装角略偏向下方，通过USB线缆与地面PC相连，采集的视频分辨率320像素×240像素，帧率30 frame/s。视频采集时四旋翼无人机绕机体垂直中轴旋转约一周，位置保持在较小范围内。

图3 实验用的四旋翼无人机

改进的方法的主要目的是减少匹配计算量和匹配错误发生。图4(a)为处理一帧图像改进的方法计算时间t2和原方法计算时间t2，图4(b)为计算时间t1和t2的比值。EKF-SALM滤波收敛后，比值t1/t2始终在0.5左右，因此改进的方法有效减少计算量。图5为视频中第2两种方法路标匹配的结果。路标2附近有相似的图像特征，改进的方法使用逐层主动搜索匹配结果正确，而原方法使用标准主动搜索匹配结果错误，因此使用逐层主动搜索匹配算法有利于避免匹配错误发生。

图4 两种方法处理一帧图像消耗时间及比值

图5 两种方法路标匹配的结果

图6 用姿态测量数据对图像进行单应映射

4 结论

基于EKF-SLAM的视觉罗盘姿态测量方法在系统噪声先验方差设定为大值时存在匹配计算量增大和匹配错误发生率升高的问题，本文给出一种改进的视觉罗盘姿态测量方法。首先使用多分辨率路标选取策略初始化新路标，然后使用逐层主动搜索匹配算法减少匹配计算量和降低匹配错误发生率，最后结合平滑转动模型得到的姿态先验信息，使用最小抽样集为2个匹配点的RANSAC算法去除匹配野值，提高姿态测量精确度。实验结果证明，改进的方法与原方法相比具有计算量小、测量姿态精确的优点。

摄像机在旋转过程中总存在位移，本文实验环境中路标空间点到摄像机的距离较远，可以认为是无穷远的，但是实际环境中某些路标空间点(如障碍物)到摄像机的距离较近，不能认为是无穷远的。在以后的研究中，将讨论如何把2点RANSAC去除匹配野值和近距离路标空间点检测结合进行。

［1］史智宁，韩波，徐玉，等.基于重力场自适应互补滤波的无人直升机水平姿态估计方法［J］.传感技术学报，2009，22(7):993－996.

［2］杜继永，黄国荣，张凤鸣，等.基于低成本MEMS器件的捷联航姿系统设计［J］.传感技术学报，2011，23(11):1662－1666.

［3］Sasa S，Gomi H，Ninomiya T，et al.Position and Attitude Estimation Using Image Processing of Runway［C］//Proceedings of 38th Aerospace Sciences Meeting and Exhibit AIAA，2000:1－10.

［4］Cornall T，Greg E.Measuring Horizon Angle from Video on a Small Unmanned Air Vehicle［C］//International Conference on Autono-mous Robots and Agents，2004:13－15.

［5］McGee G T，Sengupta R，Hdrick K.Obstacle Detection for Small Autonomous Aircraft Using Sky Segmentation［C］//IEEE International Conference on Robotics and Automation，2005:4679－4684.

［6］Ettinger S M，Nechyba M C，Ifju P G，et al.Towards Flight Autonomy:Vision-Based Horizon Detection for Micro Air Vehicles［C］//IEEE/RSJ International Conference on Intelligent Robots and System，2002，3:2134－2140.

［7］Bao Guiqiu，Xiong Shenshu，Zhou Zhaoying.Vision-Based Horizon Extraction for Micro Air Vehicle Flight Control［J］.IEEE Transactions on Instrumentation and Measurement，2005，54(3):1067－1072.

［8］Dusha D，Boles W，Walker R.Attitude Estimation for a Fixed-Wing Aircraft Using Horizon Detection and Optical Flow［C］//9th Biennial Conference of the Australian Pattern Recognition Society on Digital Image Computing Techniques and Applications，2007:485－492.

［9］Sharp C S，Shakernia O，Sastry S S.A Vision System for Landing an Unmanned Aerial Vehicle［C］//IEEE International Conference on，Robotics and Automation，2001，2:1720－1727.

［10］Labrosse F.The Visual Compass:Performance and Limitations of an Appearance-Based Method［J］.Journal of Field Robotics，2006，23(10):913－941.

［11］Sturm J，Visser A.An Appearance-Based Visual Compass for Mobile Robots［J］.Robotics and Autonomous Systems，2009，57(5):536－545.

［12］Montiel J M M，Davison A J.A Visual Compass Based on SLAM［C］//IEEE International Conference on Robotics and Automation，2006:1917－1922.

［13］Dissanayake M W M G，Newman P，Clark S，et al.A Solution to the Simultaneous Localization and Map Building(SLAM)problem［J］.IEEE Transactions on Robotics and Automation，2001，17(3):229－241.

［14］Davison A J.Real-Time Simultaneous Localization and Mapping with a Single Camera［C］//Proceedings of ICCV，2003:1403－1410.

［15］Burt P J.Fast Filter Transforms for Image Processing［C］//Computer Vision，Graphics and Image Processing，1981，16:20－51.

［16］Tomasi C，Kanade T.Detection and Tracking of Point Features［R］.Carnegie Mellon University Technical Report CMU－CS－91－132，1991.

［17］Hartley R，Zisserman A.Multiple View Geometry［M］.Cambridge University Press，2000.