雷鹏宇 周莉玲 李志天 邹旭东

摘  要：隨着无人驾驶，扫地机器人的广泛应用，基于移动机器人技术的定位导航系统研究成为了一项科研热点。移动机器人达到自主导航的目的，需要具备精确的定位功能。视场角在定位系统中起着重要的作用，越大的视场角代表着更多的环境信息获取，而传统的单目相机的视场角小，在纹理较少的环境中，无法进行有效的定位。本文利用鱼眼镜头来替换传统的单目针孔相机，可以增加定位系统的视场角，有效利用冗余观测信息来保证位姿估计的鲁棒性和观测目标特征的长可见性。在本文中，设计了搭载鱼眼镜头的定位系统可以捕获更丰富的环境特征信息，并且引入了相机畸变模型改善图像畸变的问题，设计了基于特征点对图像信息进行处理的系统，实现了导航定位的功能，获得了更鲁棒的位姿跟踪效果。实验表明，基于鱼眼镜头的单目视觉定位系统定位性能提高，实现厘米级定位。

关键词：视觉定位  鱼眼相机  SLAM  EUCM

中图分类号：TP249                           文献标识码：A                  文章编号：1674-098X（2021）05（c）-0092-06

Monocular vision positioning system based on fisheye lens

LEI Pengyu1，2  ZHOU Liling1，2  LI Zhitian1  ZOU Xudong1*

（1.Aerospace Information Research Institute， Chinese Academy of Sciences， Beijing， 100190  China;

2.University of Chinese Academy of Sciences， Beijing， 100049  China）

Abstract： With the wide application of unmanned and sweeping robots， the research of positioning and navigation system based on mobile robot technology has become a scientific research hotspot. To achieve the purpose of autonomous navigation， mobile robot needs to have accurate positioning function. The field of view angle plays an important role in the positioning system. The larger the field of view angle represents more environmental information acquisition， while the traditional monocular camera has a small field of view angle and can not locate effectively in the environment with less texture. In this paper， fish eye lens is used to replace the traditional monocular pinhole camera， which can increase the field angle of view of the positioning system， and effectively use redundant observation information to ensure the robustness of pose estimation and the long visibility of observation target features. In this paper， a positioning system equipped with fish eye lens is designed， which can capture more abundant environmental feature information， and the camera distortion model is introduced to improve the problem of image distortion. A system for processing image information based on feature points is designed， which realizes the function of navigation and positioning and obtains a more robust pose tracking effect. Experiments show that the positioning performance of monocular vision positioning system based on fish eye lens is improved and centimeter level positioning is realized.

Key Words： Visual positioning; Fish eye camera; SLAM; EUCM

这年来，同步定位与建图（Simultaneous Localization and Mapping）技术发展迅速，简称SLAM，它是指移动机器人（如无人驾驶，扫地机器人等）在处于未知环境和未知位置情况下，利用自身传感器对周围环境信息进行获取，构建全局一致的环境地图同时进行定位与导航的技术。由于相机成本低，体积小，获取环境信息丰富等突出优势，在SLAM技术中被广泛研究，移动机器人在获取环境信息的时候，相机以其成本低与采集信息丰富等特点成为了定位技术研究中广泛使用的传感器，对于定位导航有着非常重要的意义。目前相机主要分为单目，双目与深度RGB-D，双目的配置与标定都比较复杂，深度RGB-D只适用于室内定位，单目相机凭借着结构简单，成本低的特点非常受研究者的关注，而普通的单目由于视场角小的缘故会导致采集到的信息并不完整，特别是在具有挑战性的环境中，普通单目可能会定位失败。普通单目针孔相机的视场角一般都在90°以内，得到的图片畸变较小，真实度高，不需要繁琐的标定步骤就可以直接使用。虽然针孔相机得到的图像在算法处理工程中简单且效率高，但是较小的视场角同样使得摄像头拍摄得到的内容较少，在纹理不丰富的场景下或运动模糊等原因会导致定位结果不准确甚至定位失败，这也是所有普通单目相机存在的问题，视场角小导致采集得到的信息并不完整，而鱼眼相机由于其能够通过透镜的不同组合而获取包含周围环境信息更多的图像，对于提升定位的鲁棒性和精度有着重要意义。但鱼眼相机具备视场大的优势同样带来了图像畸变的问题，因此需要对图片进行去畸变处理再进行定位与导航。

针对这些传统定位导航方法中单目相机获取信息的局限，本文通过加入鱼眼镜头来加大定位系统的视场角，进而获取更多的环境信息，并且针对鱼眼相机引入的图像畸变问题，引入了相机畸变模型EUCM（Extended Unified Camera Model），在一定程度上改善了图像畸变导致的缺陷，实验证明，基于鱼眼镜头的单目视觉定位系统能够得到更好的定位结果。

1  鱼眼相机模型

鱼眼镜头属于超广角镜头中的一种，它一般是由十几个不同的透镜组合成的，在实际工程应用中的相机镜头，通常能够达到140°的视角，而在一些视觉导航的应用中，相机的视角能够达到360°，因此记录的图像信息更多，视觉导航系统也就可以在高动态的工况下正常工作[1-3]。在成像的过程中，透镜的作用下导致入射光线存在不同程度的折射，投影到较小的成像平面之后，会使得鱼眼镜头相比普通镜头有着更大的视野范围，但是透镜的加入不仅会使得光的传播发生变化，而且在组装过程中，透镜和成像平面也不可能完全平行，这也会使得光线穿过透镜投影到成像平面时的位置发生变化。前者会产生径向畸变，后者会产生切向畸变，两者的叠加导致图像变形，最终使定位结果不准确[4-5]。由于鱼眼镜头的畸变比普通相机的畸变更大，在进行图像处理的时候，需要对图像进行矫正，即将这种广角下的图片处理为平面图片，所以，本文利用鱼眼相机增加定位系统的视场角，同时引入了相机畸变模型EUCM（Extended Unified Camera Model）用来解决图像的畸变问题。

EUCM模型适用于视场角大于180°的鱼眼镜头，这种视角的镜头由于焦距短，视角大，导致光学原理产生的变形更加强烈，因此需要标定6个参数、、、、、，其中、、、为相机的内参，、为相机的畸变系数，如图1。假设三维坐标中有一点p它的坐标为，在定位过程中需要将点p投影到相机平面上[6-8]。在EUCM模型中，投影分为两部分，第一部分需要将三维坐标中的点投影到归一化平面坐标，得到点的坐标如式（1）。

，其中

（1）

第二部分是将归一化平面坐标利用式（2）投影到像素平面中。

（2）（2）

其中，u，v是像素平面的坐标。除此之外，在定位过程中，2D点到3D点的恢复也尤为重要，所以需要将像素坐标反投影到归一化平面中，该过程也分为2个部分[9]。首先，需要将像素平面的坐标反投影到椭球面坐标中，假设椭球面坐标为xp，它的坐标值可以根据像素坐标u，v得到，如式（3）。

（3）

其中，。

然后，将得到椭球面的坐标进行归一化就可以得到归一化平面的坐标。

2  系统框架

在视觉定位中，根据对图像处理的方式不同可以分为特征点法和直接法[10]。特征点法是提取图像中的特征之后根据一定的策略进行特征点匹配的过程，该方法得到的特征较稳定，并且其稀疏的特征特别适用于通过鱼眼镜头所获得的广角、包含信息更多的图像。特别是本文提出的导航系统具有3个线程，分别是跟踪、建图、闭环线程，均与特征点关联密切。因此，本文在Raul Mur-Artal的ORB-SLAM2框架下，基于鱼眼相机模型搭建了一套在宽视场角等具有挑战性环境下使用的鲁棒定位算法。该算法是基于稀疏特征点的定位方法，在此框架中，跟踪与建图是分别处理的，这样可以大大地提高系统的实时性。

在该框架中，首先进行初始化。在对图像处理之后，得到了去畸变的扩展图像，之后需要进行特征点的提取，ORBSLAM2中使用的是ORB特征点，该特征点由FAST角点与BRIEF描述子组成[11]。当在两幅图像中提取到的特征点的描述子欧式距离小于一定的阈值时就证明两者是同一个路标点。在得到一系列的匹配点之后需要进行位姿的恢复，之后通过得到的位姿进行三角化以此来获取图像中的地图点，以便后续进行位姿跟踪的过程。其中位姿的恢复需要同时计算本质矩阵E与单应矩阵H，本质矩阵适用于特征点都不在同一个平面中的情况，单应矩阵适用于平面的情况，最后从中选取得到最优模型，从矩阵中分解得到旋转与平移。当得到位姿之后，可以对匹配点进行三角化得到三维坐标点。

跟踪线程中，主要分为3种模式：恒速模型、关键帧匹配与重定位模式[12]。恒速模型使用前一帧的位姿，并通过对误差模型的优化来获取当前帧的位姿。关键帧匹配是在兩帧图像匹配特征点不够的情况下，将当前帧与关键帧进行特征点匹配，然后进行位姿估计。重定位模式是在前两者都跟踪失败的情况下与地图进行特征点匹配跟踪，该过程中用到了DBoW3词袋模型，该模型可以加快匹配的速度。

跟踪线程中还包含了关键帧的选取，若要成为关键帧必须要满足以下条件。

（1）从上次的重定位到当前帧，间隔20帧以上。

（2）本地映射处于空闲状态，或者从插入最后一个关键帧到当前间隔20帧以上。

（3）当前帧至少跟踪50个特征点。

（4）当前帧跟踪的特征点比参考关键帧少90%。

在得到关键帧之后，可以进入局部建图线程，该线程管理着局部地图，并进行局部地图中的整体优化。而且它还会创建新的地图点，并且进行关键帧的管理。在建图线程中管理着共视图，共视图指的是以2个关键帧共同观测到的地图点的个数为权重，当它达到一定阈值时，就在2个关键帧之间形成了一条边，该边以关键帧为顶点。共视图的存在可以帮助局部地图线程剔除关键帧，如果2个关键帧之间重复的地图点过多则会将其中一个关键帧进行剔除以此来提高空间的利用率。在局部地图线程中，除了会创建新的地图点还会剔除地图点，这样可以避免误匹配和错误的三角化，以此来保证地图点的质量。当前局部地图线程如果空闲的话则会对关键帧与地图点进行整体的优化，优化的内容包括新加入的关键帧，具有共视关系的关键帧，以及所观测到的地图点。总而言之，局部地图线程在提高定位精度的同时也减少了定位过程中信息的冗余。以上就是框架中跟踪线程与局部建图线程的主要过程。

3  图像的处理

鱼眼相机所获得的图片包含的信息量更多，为了方便后续的图像处理，本文选择了ROI（Region of interest）选取。该方法可以将原始的鱼眼图像转换成没有畸变的图像并且保证图像区域没有减少。处理过程分为两部分：（1）将鱼眼图像中的坐标点根据它原始的内参以及EUCM模型反投影得到归一化平面坐标系上的点;（2）将反投影得到的归一化平面坐标系上的点进行投影，投影得到虚拟平面的像素坐标。该过程通过式（4）表示。

（4）

其中，uc、uv和分别为原始鱼眼图像与虚拟图像的坐标。是从归一化平面坐标投影到虚拟平面的过程，该过程适用的相机内参是虚拟内参，与相机光心到虚拟平面的距离与角度有关。是反投影方程，该过程使用的是EUCM模型的原始参数，目的是获得去畸变之后的三维坐标。是虚拟图像和原始图像之间的旋转矩阵[13-14]。

通过上述的投影与反投影过程，便可以通过处理鱼眼镜头获得的图像得到5幅虚拟图像，在后续的过程中不再需要对图像进行去畸变操作。

在上述描述中，使用了ROI区域的提取，所以得到的虚拟平面的像素坐标并不是真实的像素坐标。在传统的针孔相机模型中，重投影误差定义在一个普遍意义的图像平面，因此本文将相机测量残差定义在单位球面上，该残差适用于所有类型的相机，包括广角、鱼眼和全向相机。将该残差建模为一个单位射线到单位球表面的联系，残差的定义如式（5）与式（6）。

（5）

（6）

其中，式（5）中的b1和b2是任意正交基。式6中的R和t是图像j与图像i之间的旋转矩阵与位移，pj为图像j中对应的归一化平面坐标系中的坐标，pi图像i对应的归一化平面坐标系中的坐标，利用该公式便可计算得到相机的测量残差。

4  实验结果

为了验证本文所提出方案，评估使用鱼眼镜头之后定位系统所能实现的精度，设计了如下实验。该实验在Ubuntu 16.04环境下运行该算法，使用的运行平台是ROS Kinetic。测试使用的数据集为TUM数据集的Room4，通过定位误差评估系统的运行效果。在计算的过程中，使用了Sim（3） Umeyama对准（Sim（3） Umeyama alignment）。并且使用绝对位姿误差（Absolute Pose Error， APE）来评估该框架下的关键帧的轨迹以及其定位精度。由于APE比较的是平移部分的误差，为了准确评估实验的性能，本文同时采用了均方根误差（Root Mean Square Error，RMSE），均值误差（Mean Error），中值误差（Median Error）和标准差（Standard Deviation，Std.）来评估实验结果。

图2是系统输出的轨迹图与真值的对比。其中虚线代表轨迹真值，实线代表系统输出的轨迹，右侧的彩条代表着误差，其颜色的变化代表偏离真值的程度。从图2中可以看出，该系统可以正常地在数据集上运行并且能得到高精度的定位结果。通过轨迹的颜色可以看出大部分轨迹的误差保持在较低的水平，在拐角处的纹理信息特别少的地方，误差较大，超过10cm。

图3给出了系统运行轨迹的前100s内的APE平移部分（绝对轨迹误差），单位为米，结果显示绝对轨迹误差控制在了15cm以内，整体的误差保持在较低的范围，经过计算，在该数据集上的RMSE（均方根误差）达到了0.086m，定位精度达到了厘米级。使用多个参数进行评估定位系统的定位精度，对应的结果如图所示，误差表现良好。

5  结语

本文针对普通单目相机获取的环境信息较少，在一些纹理少的环境中，可能导致定位失败的问题，提出了一种结合鱼眼相机，并使用特征点法的导航定位系统。通过利用鱼眼镜头视场角大的特点，获取包含更多环境信息的图片，并且使用相机畸变模型EUCM，实现鱼眼镜头的高精度校准，保证了从鱼眼镜头图像到相应全景图像的精确转换。设计的基于特征的SLAM系统框架由初始化、特征匹配、帧跟踪和闭环等几个特定的策略和算法组成。该系统具有更大的视场角，能适用于更多的场景，提取更多的特征，从而使系统的运行更鲁棒。最后，设计的实验显示，该系统可在标准CPU中实时运行，并且能够实现高精度的导航定位，定位误差小于10cm，均方根誤差达到了0.086m，实现了厘米级的定位。

参考文献

[1] 赵栋.超广角镜头拍摄影像的畸变纠正分析[J].城市勘测，2021（1）：135-140.

[2] 姚伟，徐骏善，汪惠芬.基于视觉导航AGV的视觉定位系统设计[J].制造业自动化，2020，42（11）：18-22.

[3] 杨宇，赵成星，张晓玲.鱼眼相机的视觉标定及畸变校正[J].激光杂志，2020，41（9）：20-23.

[4] 肖智越，刘力双，吕勇，等.基于相机移动的鱼眼成像系统目标方位标定研究[J].激光杂志，2020，41（9）： 24-28.

[5] 姚奕杰.鱼眼相机的标定与视频增强场景方法的研究[D].南京：南京师范大学，2020.

[6] 陈鹏.基于室内视觉定位的轮式机器人主动建图与导航避障研究[D].成都：电子科技大学，2020.

[7] 吴婕.基于视觉的无人机导航定位系统关键技术研究[D].成都：电子科技大学，2020.

[8] 李传立，尚俊娜，李芳.单目视觉人工路标辅助INS的组合导航定位方法[J].传感技术学报，2020，33（1）： 68-73.

[9] E. Rublee，V. Rabaud，K. Konolige，etal. Bradski. ORB： an efficient alternative to SIFT or SURF[J].IEEE International Conference on Computer Vision （ICCV），2011：2564–2571.

[10]D. G' alvez-L' opez， J. D. Tard' os.Bags of binary words for fast place recognition in image sequences[J].IEEE Transactions on Robotics， 2012，28（5）：1188–1197.

[11]朱佳文.基于視觉的机器人室内定位与导航方法研究[D].大连：大连理工大学，2019.

[12]邵泽明.视觉移动机器人自主导航关键技术研究[D].南京：南京航空航天大学，2009.

[13]R. Mur-Artal， J. M. M. Montiel， J. D. Tardós. ORB-SLAM： A Versatile and Accurate Monocular SLAM System[J].IEEE Transactions on Robotics，2015，31（5）：1147-1163.

[14]J. Heikkila， O. Silven.A four-step camera calibration procedure with implicit image correction[J].Proceedings of IEEE Computer Society Conference on Computer Vision and Pattern Recognition， San Juan，PR，USA，1997：1106-1112.