智能车辆双目视觉与地图组合定位研究

潘 尧，吴 聪，张颖江，刘伶俐，辛 梓

智能车辆双目视觉与地图组合定位研究

潘 尧1，吴 聪1，张颖江1，刘伶俐2，辛 梓2

（1湖北工业大学计算机学院，湖北武汉430068；2武汉光庭信息有限公司，湖北武汉430073）

车身传感器能实时感知车身位置信息，通过构造部分道路信息的驾驶地图进行映射完成初始定位，利用立体视觉实时感知环境进行精确定位，解决智能车辆“在哪里”的问题。在真实驾驶场景中测试了其可行性，初步实现了低成本高精度的定位效果。

视觉定位；高精度地图；视觉里程计；驾驶地图；无人驾驶

智能车辆常见定位技术主要有航位推算定位技术、惯性导航定位技术、全球导航卫星定位技术、磁定位技术和视觉定位技术等。航位推算实施成本低，用当前位置信息结合车身姿态信息推测出下一刻位置，容易产生累积误差；惯导与航位推算原理类似，利用惯性元件来测量车辆的加速度，经过积分和运算得到速度和位置［1］，高精度惯导也就意味着高成本的硬件投入，在某些道路场景中建筑物的遮挡、多径效应等［2］都会影响它的精度；GPS在智能车辆定位中属于主流技术，但是容易受到“城市森林”等障碍物遮挡影响信号，民用GPS精度还达不到智能车辆厘米级定位需求［3］；磁定位技术不受环境干扰，拥有很稳定的定位效果，当应用在智能车辆中，前期对道路环境投入成本过高［4］；视觉定位技术通过相机获取丰富环境信息，对算法性能要求较高，具有较高的灵活性。Levinson和Thrun［5］用64线激光雷达记录三维点云再通过激光束测出物体之间的距离达到定位目的，Moosmann等［6］将激光雷达定位与高精度的全球卫星定位系统相结合，进一步提高了定位精度，也有很强的鲁棒性，但太依赖于激光雷达。Badino［7－8］先用全局图像中特征图像绘制成地图轨迹记录并存储下来，定位时用当前图像来匹配地图信息，再用场景与之前地图轨迹对应得出位置，很明显，稠密路标的选取虽然使准确性大大增加，但是算法效率不高。在同时将定位与地图构建［9－12］合在一起，该方法取决于车辆姿态和地标的全球位置，主要有递归滤波和非线性最小二乘法来更新位置信息，Lategahn等［13－14］则将制图和定位分开研究。Pink［15］提出基于道路特征定位，通过迭代图像特征中最近点来估算单个姿态，用卡尔曼滤波对单一功能之间误差进行平滑，更好地耦合了观测和模型状态［16］。国内方面，张学习等［17］利用全向视觉获取图像，用颜色识别出目标后，利用坐标转换实时构建全局地图，该方法具有一定的局限性；林成建等［18］运用改进粒子群算法与模糊神经网络相结合方法进行视觉定位，计算繁琐，不适合工程应用；仲跻炜等［19］设计了北斗高精度定位与高精度地图组合平台实现了车道级定位；北京交通大学的陈红岩［20］只用视觉里程计在限制区域內剔除运动估计中的误匹配点，无法客观评价其方法的好坏，姜雨菲［21］在双目立体视觉下提出了CUFastSLAM算法，但是在室内小尺度环境下进行等等。因此，利用适合智能驾驶的驾驶地图进行初步定位，通过立体视觉实时感知道路环境进行进一步精确定位，实现一种不依赖高精度传感器的低成本定位方式，并在室外真实驾驶环境中对该方法进行测试。

1 驾驶地图

假设车辆始终行驶在道路上，将车辆当前位置信息与导航系统地图数据进行比较和匹配，获得车辆所在道路的初步位置，再通过车辆点投射到道路上，进一步校正减少定位误差［3］。利用地图匹配技术还能将一些道路场景中难以识别或者识别实时运算量较大的场景（比如红绿灯等）提前采集。厘米级高精度地图在智能驾驶中具有重要作用，它能使车辆对所处环境进行精准预判，提前选择合适的行驶策略。然而厘米级高精度地图里面数据采集信息太丰富，地图匹配时造成时间消耗［16］，因此制作适合定位需求的地图数据库来减少搜索匹配时间代价。在视觉定位时只需要根据具有道路标记信息（包括弯道、十字路口、红绿灯等）的驾驶地图来获取车身当前位置。道路轨迹抽象成黄色线段，交通标志牌的位置与道路标线的位置通过地图采集车采集，用经度、纬度、高度来表示。道路标线属性则用实线、虚线、限制线、停止线来表示。整个道路轨迹由若干路线组成，每个路线包含了该条路线的起点、终点和标线属性（图1）。

图1 驾驶地图示意

2 双目立体视觉

单目视觉算法可以得到相机姿态角的该变量和相对位置矢量方向，但难以得到场景中物体的实际尺寸，而双目立体视觉在有共同视野时可重建出环境的三维信息，并精确地得到6个自由度的位姿增量。所以本文按照一定结构［22］构建双目视觉系统。双目相机就是模拟人眼，利用左右目相机成像视差计算出世界坐标系中的观测点与双目相机模型之间的相对距离。如图2所示，世界坐标系中P（Xw，Yw，Zw）点与左右目相机光心OcL、OcR三点组成的平面为対极平面；PL（x，y）、PR（x’，y’）分别是P点在左右目相机图像平面投影点，其横向坐标之差为视差d；OL（cx，cy）、OR（c＇x，c＇）分别是光心在左右目相机成像平面的投影点，也称主点；主点O与光心OC所在的直线成为相机光轴，之间的距离为相机焦距f；双目相机模型中以左目相机光心为参考原点。

根据三角形相似原理［23］易知，

图2 世界坐标系与图像坐标系之间转换

因此，利用上式可以建立世界坐标系点与图像坐标系点之间的对应关系。采用Matlab camera calibration工具箱进行标定，立体校正得出相机的内外参数，如图3所示部分标定过程。

图3 Matlab标定工具箱部分过程

通过上述过程得出相机的内外参数为：右目相对于左目旋转矩阵

右目相对于左目平移矩阵

左目相机标定参数

左目畸变矩阵

右目相机标定参数

右目畸变矩阵

将这些参数写入OpenCV中stereoRectify函数，利用极限约束得到图4校正结果，（具体校正方法正在申请专利（受理号：201510756481.3）中，不再贽述），将应用此立体相机参数开展下一步工作。

图4 双目校正后结果

3 视觉定位过程及实验结果

在视觉定位时，在车辆中安装IMU（惯性测量单元）来随时感知车辆当前姿态，在车前挡风玻璃处安装双目立体相机进行实时图像处理（图5），利用上节标定参数写入图像获得矫正后的双目图像进行图像分析。

图5 车身传感器安装

车身IMU能随时感知车辆的姿态，可抽象成非线性离散模型，其位置信息

其中，车身的初始位置（xveh，0，yveh，0）由高精度卫星导航系统提供，xveh，0表示经度信息，yveh，0表示纬度，、，信息车身当前速度vφk由IMU获取并采用WGS－84地球坐标系进行单位转换，Δt为两个时刻时间差，其中i∈［0，1，2，…，N］，N为自然数。

图6 定位过程

如图6a所示，圆圈位置点代表地图中车道线的位置采集点，用Mi表示。三角形位置点代表视觉识别出的车道线位置点，将图像像素坐标转换为GPS坐标，用Pci表示，这样Mi作为系统模型，Pci作为观测模型，卡尔曼滤波［2，14，16］生成实时纠正，为正方形所示，为了获取连续稳定的定位信息，利用立体图像进行左右目特征点提取和前后帧特征点提取，通过提取图像序列中一致信息来估计相机的位置Pc。由于车身IMU一般安装在车后备箱位置，相机安装在车前挡风玻璃处，故估计相机位置Pc与车身位姿Pveh利用欧式距离算出P，如图6b中所示，再将车身位姿与路面之间标定得出R，将车身位姿映射到地面R·P与纠正P－i进行GPS坐标换算即可得出定位信息。在实时场景中，道路标线信息与驾驶地图中位置进行比较，图7a利用地图映射技术将车辆在驾驶地图中位置显示完成初步定位，在图7b中利用立体视觉识别出车道线与地图中车道线信息做比较，利用卡尔曼滤波估计出当前最佳点与车身当前姿态进行换算，得出当前最佳估计位置从而完成稳定实时的定位过程。

如图8所示，分别尝试了ORB算法、SIFT算法和SURF算法进行特征点提取，参考表1三种算法处理时延可以看出，使用ORB算法在点特征提取与算法处理时间消耗上取得了较好的效果，因此，本文采用ORB算法进行点特征提取处理。

图8 特征点匹配结果

表1 三种算法处理单帧分析

在图7b中，蓝线为视觉识别车道线位置，通过图7a中车道线利用卡尔曼滤波进行估计，通过交通标志位置信息进行纵向位置纠正，就达到了整个定位的目的。道路测试场景选择光庭厂区某专用测试道路（箭头示意路线，图9）。该路段全长约1.4km，其中设置了6个道路箭头，2个停止线，1条斑马线，2个停车标识牌，2个禁止标识牌，餐厅和厂房位置为高层建筑，具备一定城市道路场景特点。

图9 测试道路场景介绍

整个测试结果见图10，横轴代表经度信息，纵轴代表纬度信息，其中实线轨迹代表使用高精度GPS采集，

图10 测试结果对比

获取了2000多个稠密采集点，将误差精度控制在20cm以内，具备很高的参考精度。虚线代表通过本文方法估计出位姿轨迹点，该轨迹点通过局部获取的道路定位信息进行累加记录，反映出了整个过程的定位结果。从整个过程来看在一些高楼场景和树木林荫场景也获取到了定位结果，初步完成预期目标。很明显，在某些特殊路段中也出现异常值，初步调查结果认为在弯道转弯时，由于相机机身抖动造成获取图像不稳定加上当时所在环境树木林荫过于茂密影响了图像获取，造成了道路标线识别不稳定所致，这个也是使用视觉定位的条件限制。

4 结论

因此，本文给出了基于构建驾驶地图的视觉定位方法。将道路标线、红绿灯、交通标志等信息融入到驾驶地图中，在实时定位时，通过立体视觉相机获取当前道路信息，利用卡尔曼滤波与地图道路标线信息匹配进行位置初步映射，运用ORB算法进行特征点匹配左右目和前后帧提取图像序列中的一致性信息，选取一致性信息在图像序列中的位置变化进行相机运动估计，从而得出相机姿态，实现车辆的精确定位。该方法在实际驾驶环境中测试取得了较好的定位效果，初步实现了基于立体相机和地图搭配的低成本定位。然而地图与视觉信息深度融合在下一阶段需要进行深入研究。

［1］ 秦永元.惯性导航［M］.第二版.北京，科学出版社，2014.

［2］ Julius Ziegler，Henning Lategahn，Markus Schreiber，et al.Video based localization for BERTHA［C］.Intelligent Vehicles Symposium.USA，IEEE，2014.

［3］ 刘兴权，金美含.地图匹配算法综述［J］.科技信息，2014（4）：64－35.

［4］ 张营.智能车辆磁定位技术研究［D］.上海：上海交通大学，2008.

［5］ Levinson J，Thrun S.Robust vehicle localization in urban environments using probabilistic maps［C］／／IEEE International Conference on Robotics and Automation.IEEE，2010：4372－4378.

［6］ Moosmann F，Stiller C.Velodyne SLAM［C］／／Intelligent Vehicles Symposium.IEEE，2011：393－398.

［7］ Badino H，Huber D，Kanade T.Visual topometric localization［C］／／Intelligent Vehicles Symposium.IEEE，2011：794－799.

［8］ Badino H，Huber D，Kanade T.Real－time topometric localization［C］／／IEEE International Conference on Robotics and Automation.IEEE，2012：1635－1642.

［9］ Davison A J，Reid I D，Molton N D，et al.Mono－SLAM：real－time single camera SLAM.［J］.IEEE Transactions on Pattern Analysis ＆Machine Intelligence，2007，29（6）：1052.

［10］Bailey T，Durrant Whyte H.Simultaneous localization and mapping（SLAM）：Part ii［J］，Robotics ＆Automation Magazine，IEEE，2006，13（3）：108–117.

［11］Kummerle R，Grisetti G，Strasdat H，et al.G 2o：A general framework for graph optimization［C］／／IEEE International Conference on Robotics and Automation.IEEE，2011：3607－3613.

［12］Lu F，Milios E.Globally Consistent Range Scan Alignment for Environment Mapping［J］.Autonomous Robots，1997，4（4）：333－349.

［13］Lategahn H，Stiller C.Vision－Only Localization［J］.Intelligent Transportation Systems IEEE Transactions on，2014，15（3）：1246－1257.

［14］Lategahn H，Schreiber M，Ziegler J，et al.Urban localization with camera and inertial measurement unit［C］／／Intelligent Vehicles Symposium.IEEE，2013：719－724.

［15］Pink O.Visual map matching and localization using a global feature map［C］／／Computer Vision and Pattern Recognition Workshops，2008.CVPRW＇08.IEEE Computer Society Conference on.IEEE，2008：1－7.

［16］Schreiber M，Knoppel C，Franke U.LaneLoc：Lane marking based localization using highly accurate maps［C］／／Intelligent Vehicles Symposium.IEEE，2013：449－454.

［17］张学习，杨宜民.基于全向是觉的移动机器人实时全局地图构建［J］.计算机测量与控制.2011，19（3）：643－646.

［18］Lin C J，Wang J G，Lee C Y.Pattern recognition using neural－fuzzy networks based on improved particle swam optimization［J］.Expert Systems with Applications，2009，36（3）：5402－5410.

［19］仲跻炜，陈大吾，朱峰.基于北斗高精度定位和车道级高精度地图的车辆监控平台设计与实现［J］.全球定位系统.2015，40（4）：50－54.

［20］陈红岩.无人车立体视觉里程计研究［D］.北京：北京交通大学，2012.

［21］姜雪菲.双目立体视觉SLAM研究［D］.北京：北京交通大学，2014.

［22］湖北工业大学.基于机器视觉应用的车载相机结构［P］.中国：2015205648519，2015：7.

［23］马颂德，张正友.计算机视觉［M］.北京：科学出版社，1998.

Research on Integrated Location with Stereo Vision and Map for Intelligent Vehicle

PAN Yao1，WU Cong1，ZHANG Yingjiang1，LIU Lingli2，XIN Zi2
（1 School of Computer Science，Hubei Univ.of Tech.，Wuhan 430068，China；2 Wuhan Kotei Informatics CO.，LTD，Wuhan 430073，China）

The sensors of vehicle could sense the body position information of vehicle in real－time，then map the initial location by the driving map which is built with some information of lane attributes，and use stereo vision which apperceives the road environment to accomplish the precise positioning.Those could resolve the intelligent vehicle’s situation of Where I am.The method is tested a good practicability in real driving scenarios.The position effect which consists of low－product sensors has been achieved high precision.

vision location；HD map；visual odometry；driving map；intelligent vehicle

TP391

A

［责任编校：张岩芳］

1003－4684（2017）04－0055－05

2016－05－10

湖北省自然科学基金（2012FFB00701）

潘 尧（1987－），男，湖北黄石人，湖北工业大学硕士研究生，研究方向为图像处理

吴 聪（1982－），男，湖北武汉人，湖北工业大学副教授，研究方向为医学图像处理