项目编号： ?1824 ??

多源传感器融合定位（相机、GPS、IMU）

Multi-sensor Fusion Positioning Algorithm Research(Camera/GPS/IMU)

项目研制单位：武汉大学、武汉中海庭数据技术有限公司

主要研制人员：牛小骥、张全、陈起金、吴佳豪、李姗姗、唐海亮、彭益堂

陶靖琦、杨贵、董岱、陈林园

主题词：GNSS INS? 相机 车道线融合定位

1、研究内容介绍

随着全球卫星导航定位系统（Global Navigation Satellite System，GNSS）与惯性导航系统（Inertial Navigation System, INS）定位技术的发展，在开阔环境下实现高精度定位已经不再是一个难题，但是在隧道、高架桥、城市峡谷地带等GNSS信号中断的场景下，如何保障系统稳定可靠、高精度的定位是目前研究的难题。针对GNSS信号失锁场景下的定位问题，利用从相机传感器获取的视觉特征信息，结合高精度地图进行特征匹配定位，是一种有效的技术方案。本项目针对GNSS/INS/Odometer/Camera等常用车辆传感器信息特点进行研究，提出了一套相机车道线特征信息辅助GNSS/INS的车载组合系统的定位方案，并进行了大量的车载测试，系统地验证了所设计的车道线信息辅助GNSS/INS系统定位算法的合理性和可靠性。

本项目所设计的算法框架如图1所示，多传感器融合定位算法主要是根据GNSS/INS+车速辅助信息组合导航的定位结果，结合高精度地图，获取当前车身距离左右车道线的距离，作为车身至车道线距离的估计值，再与单目相机提供的车身至车道线距离观测值作差，得到车道线距离差异（即紧组合方案），最后将车道线距离差异作为约束量反馈到Kalman滤波器，进行组合导航算法解算，得到高精度的定位结果。

图 1 多传感器融合算法框架

为了对比观察本项目所设计多传感器融合定位算法的定位精度，首先对GNSS/INS组合定位算法进行测试分析，检验MEMS IMU在中断GNSS信号60s的情况下，自主进行航位推算得到的定位结果的误差。在实验过程中，研发团队进行了多次车载测试，最后统计所有GNSS中断过程下得到的最大导航误差。表1给出了GNSS RTK模式下不同组合模式60秒GNSS中断时导航漂移误差水平统计。从表中可以看出，GNSS/INS组合的水平位置漂移误差大于10m，无法满足车道级定位需求；增加车速信息辅助后其精度水平显著提高，水平位置精度基本控制在1.5m以内，但横向精度略低；增加基于相机提供的车道线信息辅助后其横向位置误差约为0.15m，满足本项目指标要求，可应用于智能驾驶中车辆横向精密控制。

表 1 不同组合模式下60秒GNSS中断时导航漂移误差水平统计（GNSS RTK）

组合模式	位置误差（m）			速度误差(m/s)			姿态误差(deg)
	纵向	横向	垂向	纵向	横向	垂向	横滚	俯仰	航向
GNSS+IMU	10.331	13.242	2.278	0.418	0.459	0.190	0.119	0.184	0.605
GNSS+IMU+ODO	0.732	1.427	1.051	0.077	0.079	0.163	0.110	0.169	0.164
GNSS+IMU+ODO+Camera	0.991	0.152	0.855	0.200	0.105	0.157	0.158	0.152	0.163

同时，为了考察不同GNSS定位模式下的组合导航定位精度水平，表2给出了GNSS SPP模式下不同组合模式60秒GNSS中断时导航漂移误差水平统计。从表中可以看出，GNSS/INS组合的水平位置漂移误差大于30m，无法满足车道级定位需求；增加车速信息辅助后其精度水平显著提高，水平位置精度基本控制在4.0m以内，但横向精度略低；增加基于相机提供的车道线信息辅助后其横向位置误差约为0.15m，满足本项目指标要求，可应用于智能驾驶中车辆横向精密控制。相比于GNSS RTK，GNSS SPP模式下的导航定位精度要差一些，但是增加基于相机提供的车道线信息辅助后其横向位置误差基本相当。

表 2不同组合模式下60秒GNSS中断时导航漂移误差水平统计（GNSS SPP）

组合模式	位置误差（m）			速度误差(m/s)			姿态误差(deg)
	纵向	横向	垂向	纵向	横向	垂向	横滚	俯仰	航向
GNSS+IMU	32.832	31.548	9.967	1.064	1.128	0.305	0.222	0.250	0.704
GNSS+IMU+ODO	3.192	3.669	6.684	0.137	0.142	0.202	0.116	0.175	0.480
GNSS+IMU+ODO+Camera	1.756	0.154	5.058	0.197	0.108	0.237	0.158	0.153	0.174

综上所述，本项目旨在对相机车道线特征信息对GNSS/INS组合系统的辅助定位的理论模型和技术方法进行系统深入的研究，提升GNSS失锁场景下的多传感器融合系统定位能力。

1)?? 基于GNSS/INS组合导航系统常用的扩展卡尔曼滤波算法，设计了一种新的车道线信息辅助GNSS/INS系统的定位算法，利用车道线距离信息对车辆在横向上的定位误差进行约束，实现分米级定位。研发了一套GNSS/INS/相机车道线特征信息多源融合定位系统，并对系统定位能力进行了大量实车测试验证，定量验证了GNSS/INS/相机车道线融合定位算法的有效性和可靠性。导航误差分析表明，车道线特征信息辅助情况下，本项目实现的多传感器融合定位系统在长期GNSS失锁情况下，在车道线质量较好场景下其横向定位精度优于0.25m。

2)?? 为了保障相机车道线信息辅助的效果，并增强系统的定位精度，在不增加传感器的前提下，对车辆CAN总线输出的速度和车辆运动学模型速度约束信息进行了分析和建模，推导实现了三维速度辅助定位算法，显著降低了GNSS失锁场景下融合系统的定位误差发散，保障了相机识别的车道线信息与高精度地图中车道信息的准确匹配。

本项目提出的GNSS/INS/相机车道线多传感器融合定位算法能够有效维持分米级的系统定位精度，为克服隧道、高架桥、城市峡谷等复杂场景的车辆定位误差发散难题提供了解决方案，是挖掘相机信息潜在价值的有效手段之一。同时，该方案具有硬件成本低、精度高、可扩展性强等优点。

2、技术创新点

设计并实现了一套实时的分布式低成本的GNSS/INS+车速+相机车道线多传感器融合定位算法，将车道线信息以原始观测量的形式增加到现有的卡尔曼滤波算法中，算法组合层次为紧组合，可实现GNSS长期失锁情况下的横向分米级车辆定位。

3、专利/获奖情况

正在申请专利：基于相机识别车道线与GNSS/INS的融合位姿估计方法及系统

4、成果应用及经济效益预测

1）洋山港智能驾驶定位

洋山港智能驾驶项目是由上港集团、上汽集团、中国移动合力打造的上海洋山深水港智能重卡示范运营项目。实现在港区特定场景下的L4级自动驾驶，厘米级定位，精确停车（±3cm）,与自动化港机设备的“完美协作”，15s完成货物装卸，并迅速开启自动驾驶，自主规划最优路线，将货物精准运输到指定地点。

目前东海大桥的运输载荷已经接近了每年1000万箱的上限，呈饱和状态。基于高精度定位的多车编队技术，自动驾驶智能重卡行车距离可从150m缩短到50m，未来将减少到10m，将东海大桥通行能力提升1倍以上。

中海庭基于本研究成果为该项目提供了定位模块，利用GNSS/RTK、IMU、轮速、相机/高精度地图等传感器信息，实时计算输出车辆的位置，姿态，速度等信息，满足了自动驾驶车辆在转运场，高速道路，码头等场景所需要的高精度位姿需求。

2）低成本自动驾驶硬件

L2、L3级别的自动驾驶功能目前已经走向落地，在满足安全的前提下降低车辆及运营成本，才能更好的被市场接受。传统高精度定位依赖于RTK定位信号，在开阔场景可达到厘米级定位精度。RTK服务、网络通信终端及通信费用，需要长期的投入与维护，且无法满足半遮挡环境及隧道场景的定位。

基于本研究成果，使用单点GPS的定位结果，结合相机及高精度地图在高速道路，半遮挡环境，短隧道等场景实现车道级定位。

目前该方案已应用于车载高速定位量产项目竞标中，且取得了良好的反馈。