厚势按：针对室外环境提出了一种基于低精度 GPS 的视觉导航方法。采用多传感器融合的方法解决道路和路口的导航问题。对于路口环境，通过对道路标志（如人行横道线）的检测，实现路口的初定位；基于全局地图信息，采用扩展卡尔曼滤波融合 GPS 和里程计信息，进行路口导航。真实环境下的实验结果表明该方法，具有良好的有效性和实时性。
本文来自 年 2 月 15 日出版的《 上海交通大学学报 》，作者是上海交通大学自动化系的梁昆、杨明教授和王冰高工以及上海交通大学机器人研究所的王春香副教授。
智能车常用的导航方法有基于 GPS、磁传感器、机器视觉、激光雷达等的方法。在 DARPA（The Defense Advanced Research Projects Agency）城市挑战赛中参赛队伍一般都采用了基于高精度载波相位 GPS 的方案[1]。但高精度 GPS 等昂贵的传感器制约着智能车的发展和普及。例如，卡内基梅隆大学 BOSS 系统上装备传感器的价格已经远高于智能车本体的价格 [2]。视觉导航相对于其他几种导航方式，成本低、传感器信息丰富以及最接近人类的环境感知方式。但在一般室外环境中，路口环境无法用一个统一的数学模型来描述，因此给视觉导航方法带来了困难。
近年来，基于SIFT（Scale-Invariant Feature Transform）特征的视觉SLAM（Simultaneous Localization and Mapping）方法[3]已成功地应用于室内移动机器人导航中，但运算复杂度将随着环境的加大急剧增加。因此，在智能车室外导航中极少采用 SLAM 的方法。同时，一些学者提出基于地图匹配的导航方法 [4]，但它对地图信息量要求大、精度要求高，并且当面对较大的环境时，地图数据量将难以承受。而针对有一定结构的环境，则更倾向于利用其道路结构的特点。典型代表为面向完全结构化道路的车道线检测方法。
本文提出了一种基于 GPS 的智能车视觉导航方法，建立了基于扩展卡尔曼滤波（Extended Kalman Filter，EKF）的融合框架。对道路和路口 2 种环境，分别将航位推算（Dead Reckoning，DR）与视觉定位结果、GPS 信息融合进行实时定位：
对于一般道路，给出了一种基于兴趣区域的道路检测算法；
对于路口问题，提出了基于道路标线、GPS 信息和全局地图的路口解决方法。
并且，通过实际环境下的实验验证了所提方法的有效性。
1. 系统框架
1.1 全局地图表示形式
要使智能车通过自主导航自主行驶到目的地，全局地图必不可少。本文的全局地图包括节点和链接 2 种数据结构，如图 1 所示。
图 1 地图表示形式
节点包括：
路口，如图 1 中的 N2；
常规目的地，如图 1 中的建筑物 N4；
针对某些特殊路况所设的节点。
连接各个节点间的道路为链接，如图 1 中的 L12。
全局地图包含 GPS 位置信息经高斯投影后得到的节点、链接端点的坐标、道路宽度以及路口标志类型。
1.2 整体流程
道路和路口分别具有各自的特点：
对于道路，存在明显的车道线或者道路的边沿；
而路口中的路面无任何固定的标记，路口区域规模并不大。
并且两者对导航要求也不同：
道路上，要求车辆时时处于本车道的中央；
路口要求车辆以一定的目标位姿进入下一段道路，路口中具体的行驶轨迹无特别要求。
本文根据具体的环境特点和导航要求使用不同的导航方法：
道路上，通过基于视觉的道路检测，计算车辆相对于本车道的实时位姿。同时为了提高定位的可靠性，本文将视觉定位结果和 DR 结果进行融合；
路口中，车辆基本处于开环状态，无法通过视觉信息来定位。在路口中的实时位姿通过EKF融合 DR 及 GPS 信息得到。
图 2 整体流程
本文中位姿为路口局部坐标系下的位姿，在车辆入路口时利用路面标志（斑马线）实现局部定位，建立局部坐标系，得到初始位姿。同时，由全局地图信息计算出目标位姿。整体流程如图 2 所示。
1.3 基于 EKF 的数据融合
为了提高系统的可靠性和精度，本文使用 EKF [5] 进行信息融合（对应图 2 中视觉 /DR 以及 GPS/DR 模块）。EKF 具有精确性高、效率高等特点，其步骤为时间更新和测量更新。时间更新通过系统模型提供预测值，而测量更新需要通过实际观测值来校正预测值。路口处利用 GPS 信息作为观测值，道路上利用道路线的检测结果提供观测值。
由四轮车辆的运动特性可知其运动过程，在 i 时刻的位姿可以推得智能车在 i+1 时刻的位姿，其关系为：
EKF 过程方程为：
输入误差协方差：
由于在道路上和路口处，车辆位姿的描述方法和观测方式并不相同，因此需要分别给出过程方程和测量方程。
道路上的过程方程为：
路口处的过程方程为：
EKF 测量方程为：
视觉 /DR 的 EKF 融合道路的观测方程为：
GPS/DR 的 EKF 融合，路口的观测方程为：
得到过程方程和测量方程后，重复时间更新和测量更新步骤即构成 EKF 的基本步骤。
2. 道路环境
视觉的信息量丰富，但同时也会面临诸如阴影、绿化带等大量的干扰。如何从复杂的环境中准确提取出道路是视觉导航中的难点之一。
图 3 道路检测流程
由于是非高速环境，前瞻距离不需要太大，在一定前瞻范围内的道路都可以认为是直线，同时道路上的干扰大多是无规则的，为此，本文提出了一种基于随机 Hough 变换和兴趣区域的道路检测方法。检测流程如图 3 所示。
图 4 道路检测结果
本文通过 2 组主动兴趣框 ABOI（Active Box of Interest）实现兴趣区域的选取，如图 4 所示。用以追踪道路右边缘的 ABOI 组由下式迭代得到：
预处理部分包括高斯平滑、绿化带干扰抑制；约束条件包括斜率约束、帧间连续性约束等。检测结果如图 4 所示，更多演示和介绍参见文献 [6]。
图 5 路口导航
对检测结果进行逆透视变换 [7]，将道路信息由图像坐标系转化为实际车辆坐标系下的位置。进而确定车辆相对于车道中线的横向位置 x 和航向角 θ。其中，车辆坐标系的定义：原点 O_c 在前轮轴的中心，x_c 轴方向沿着前轮轴向右，y_c 轴方向沿着智能车轴线向前，如图 5 所示。图中，θ_0 为初始航向角。
3. 路口环境
3.1 路口标志检测
低精度的 GPS 在道路上不参与导航，但由其全局定位信息可知车辆是否已临近某路口。车辆一旦临近路口，即开始标志（如斑马线）检测。这是宏观上兴趣区域检测的思想，只在路口附近进行路口标志检测，避免全程大多数时间的无谓检测。
检测路口可以有多种手段，本文使用斑马线检测作为路口检测方法。如果使用其他标志（比如停车线）替代它，只需在系统框架内增加对应标志的检测算法及地图信息。
斑马线的特征较明显，为相互平行的一簇直线段，检测步骤如下：
（1）由随机 Hough 变换得到视野中所有的线段。在得到的线段里粗选出图像坐标系下满足一定角度和长度约束的线段，组成新的线段集合。
（2）对集合内的所有线段进行逆透视变换，并计算所有线段在车辆坐标系下的角度 φ。设置平行线计数器 Count = 1。
（3）斑马线线段集为空则结束，否则随机抽取一条线段 l_t′与其他各条线段对比，若 | φ_t - φ_t" | ，则 Count = Count + 1。其中，φ_max 为设定的最大角度差阈值。
（4）若 Count >Count_min（设定的最小计数阈值），则线段集只保留相互平行的线段，并转步骤（5），否则删掉线段 lt 转步骤（3）。
（5）去孤立线段。如果某条线段与其他线段的距离大于某阈值，则此线段为孤立线段，该线段可能为干扰，并令 Count = Count - 1。重复步骤（5）直至遍历所有线段。如果最终计数器 Count 值大于所设阈值，即相互平行的线段满足一定数目，斑马线检测成功。
3.2 局部坐标系建立及目标位姿计算
当检测到斑马线时，表明已进入节点某个节点区域。此时，建立局部坐标系，并确定其在局部坐标系下的位姿，同时计算目标位姿。选择最临近右道路边缘的斑马线建立局部坐标系：原点 O_l 为斑马线近视野的端点，y_l 轴方向与道路平行，x_l 轴方向与道路垂直，如图 5 所示。
由此，任一点在局部坐标系下的坐标为：
车辆的目标位姿应与道路平行，并且从车道正中的位置进入下一段链接，如图 5 中 E 点所示，车辆目标位姿为：
至此，路口初始位姿、实时位姿（EKF 结果）、目标位姿都已获得。需要指出，由于传感器、控制、地图等误差，很难在最终恰好以目标位姿驶出节点。但当摄像机视野中重新出现道路线或边缘时，可再次进行位姿校正，因此一定的偏差是可以允许的。
4. 实验结果与分析
图 6 实验场地及测试车辆
实验场地如图 6 所示，车辆从 A 楼驶往 B 楼。由 Dijstra 最短路径算法得到路径序列：
S = { N14，N12，N9，N8，N7，N6，N11 }
场地中对某些地段地形复杂、导航难度大的路径，建地图时认为其链接不存在，如路径 N16 与 N17 之间区域 G。因此，在建立地图时认为 N16 与 N17 间无直通链接。
图 7 前轮转角
以上海交通大学自主研制的 CyberC3 智能车作为实验平台。摄像机分辨率为 320 × 240 像素；处理器主频 2.0 GHz；使用 RTK-GPS（精度1~2 cm）建立全局地图。路线全程长约 890 m，车辆的平均速度 9.3km/h，算法处理每帧图像的平均耗时不超过 25 ms（测试环境大小有限，所提方法不会因为环境增大而增加算法复杂度）。前轮转角如图 7 所示。
全程共经过 10 次斑马线，漏检和误检率为 0。其中转角图中两处明显凹谷为智能车经过节点 N9、N6 的情况。实验表明，本文充分利用了环境的特点，用低成本的方式有效地实现了室外环境的自主导航，比 SLAM 方法运算复杂度更小，比地图匹配方法对地图的要求更低。
由此可见，GPS 的使用不仅能有效辅助路口定位（EKF 融合），更能提高全程可靠性、减少算法运算时间。根据 GPS 实时定位信息判断是否临近节点，决定是否进行路口检测。否则，全程的道路不能仅对图像兴趣区域处理，需对整幅图像分析，时时进行路口检测，增加软件额外的开销，同时将不得不面临各种复杂的干扰。
5. 结语
本文提出了一种面向城市室外环境基于低精度 GPS 的低成本视觉导航方法：
首先提出了基于扩展卡尔曼滤波的多信息融合框架；
然后针对道路，提出了基于兴趣区域的道路检测算法；
针对路口问题，提出了基于全局地图、融合视觉、GPS、里程计信息的路口检测与定位方法。
本方法的特点有：
成本低，仅使用了视觉和低精度 GPS；
地图信息量要求少、构建方便，只存储了少量点的全局坐标及路口信息等；
可靠性高、实时性较好，道路检测是基于兴趣区域的，并且仅在路口检测区域内才进行路口检测。
该方法对于覆盖范围较大的路口导航问题尚需利用其他传感器的辅助，对此将做进一步的研究和实验。
参考文献
[1] Thrun S, M ontemerlo M, Dahlkamp H, et al .Stanley: The robot that won the DARPA grand challenge[J] .Journal of Field Robotics, , 23(9):661-692
[2] Dar ms M S, Rybski P E, Bake r C, et al.O bstacle detection and t racking for the urban cha lleng e [J]. IEEE Transactions on Intelligent Transportation Systems,, 10(3):475-483
[3] Cummins M, New man P.FA B-M AP: Probabilistic Localization and mapping in the space of appearance[ J] .International Journal of Robotics Research, ,27(6):647-665
[4] Mattern N , Schubert R, Wanielik G. Image landmark Based po sitioning in r oad safety a pplica tions using high Accurate maps[ J] .IEEE/ ION Position, Location and Navigation Symposium, , 5(8):1008-1013
[5] W elch G, Bishop G .An introduction to the kalman filter[ EB/O L] .(-03-08)[ -05-26].www.cs.Unc.edu/~welch/kalman/kalmanIntro.html
[6] Cy berC3 G roup.T he algo rithm demo nstr ation of road Detection[ EB/ O L] .(-05-18)[ -05-27 ] .http://v.youku .com/vshow/idXMTczOTI0MzY4.html
[ 7] 李 颢. 基于视觉的智能车辆自主导航方法研究[ D] .上海：上海交通大学电子信息与电气工程学院，
编辑整理：厚势分析师拉里佩
转载请注明来自厚势和厚势公号：iHoushi
