有关无人车的定位有两种,一种称之为绝对定位,不依赖任何参照物,直接给出无人车相对地球坐标或者说WGS84坐标系,也就是坐标(B,L,H),其中B为纬度,L为经度,H为大地高即是到WGS-84椭球面的高度, WGS-84坐标系是美国国防部研制确定的大地坐标系,是一种协议地球坐标系。
WGS-84坐标系的定义是:原点是地球的质心,空间直角坐标系的Z轴指向BIH(1984.0)定义的地极(CTP)方向,即国际协议原点CIO,它由IAU和IUGG共同推荐。X轴指向BIH定义的零度子午面和CTP赤道的交点,Y轴和Z,X轴构成右手坐标系。WGS-84椭球采用国际大地测量与地球物理联合会第17届大会测量常数推荐值,采用的两个常用基本几何参数: 长半轴a=6378137m;扁率f=1:298.257223563。坐标系非常复杂,GPS的测量结果与我国的54系或80系坐标相差几十米至一百多米,随区域不同,差别也不同。经粗略统计,我国西部相差70米左右,东北部140米左右,南部75米左右,中部45米左右。在中国,国家出于安全的考虑,在地图发布和出版的时候,对84坐标进行了一次非线性加偏,得到的坐标我们称之为GCJ02坐标系。国内许多地图公司,可能是由于业务的需求,或者是商业竞争的某些原因,在GCJ02坐标的基础上又进行了一次非线性加偏,得到了自己的坐标系统。百度就是这么一家公司。百度在GCJ02的基础上进行了BD-09二次非线性加偏,得到了自己的百度坐标系统。
传统的GPS定位精度只有3-7米,我国城市主干道单一车道宽一般是3.75米,也就是说GPS无法做到车道线级定位。在城市道路或峡谷中,精度会进一步下降。即便是美国及北约国家,民用系统也不可能用GPS获得亚米级定位。GPS卫星广播的信号包括三种信号分量:载波、测距码和数据码。测距码又分为P码(精码)和C/A码,通常也会把C/A码叫做民码,P码叫军码。P码会再分为明码和W码,想要破解是完全不可能的。后来在新一代GPS上老美又提出了专门的M(Military)码,具体细节仍处于高度保密中,只知道速率为5.115 MHz,码长未知。北斗的定位精度目前还不如GPS。
无人车的车道线级绝对定位是个难题,无人车需要更高精度的厘米级定位 。日本为解决这个问题,研发准天顶卫星系统(QZSS),在2010年发射了一颗卫星,2017年又陆续发射了3颗卫星,构成了三颗人造卫星透过时间转移完成全球定位系统区域性功能的卫星扩增系统,今年4月1日将正式商用,配合GPS系统,QZSS可以做到6厘米级定位。QZSS系统的L5信号频点也是采用1176.45MHz,而且采用的码速率与GPS在该频点的码速率一样,都是10.23MHz。意味着芯片厂商在原有支持GPS系统芯片上无需改动硬件,只需在软件处理上作更改就可以实现对Galileo、QZSS系统的兼容,相当于软件实现上需要多搜索几颗导航卫星。几乎不增加成本,而北斗系统是需要改硬件的,非常麻烦。因为日本国土面积小,4颗卫星就够了,如果换成中国或美国,可能需要35颗-40颗卫星。
国内大部分厂家都采用GPS RTK做绝对定位,不过RTK缺点也是很明显的。RTK确定整周模糊度的可靠性为95~99%,在稳定性方面不及全站仪,这是由于RTK较容易受卫星状况、天气状况、数据链传输状况影响的缘故。首先,GPS在中、低纬度地区每天总有两次盲区(中国一般都是在下午),每次20~30分钟,盲区时卫星几何图形结构强度低,RTK测量很难得到固定解。其次,白天中午,受电离层干扰大,共用卫星数少,因而初始化时间长甚至不能初始化,也就无法进行测量。根据实际经验,每天中午12点~13点,RTK测量很难得到固定解。再次,依赖GPS信号,在隧道内和高楼密集区无法使用。
先进的无人车方案肯定不能完全基于RTK,百度的阿波罗系统使用了激光雷达、RTK与IMU融合的方案,多种传感器融合加上一个误差状态卡尔曼滤波器使得定位精度可以达到5-10厘米,且具备高可靠性和鲁棒性,达到了全球顶级水平。市区允许最高时速超过每小时60公里。
上图为Apollo自动驾驶传感器、计算单元、控制前的连接图,计算系统使用台湾Neousys Nuvo-6108GC工控机,这款工控机使用英特尔双至强E5-2658 V3 12核CPU。主要用来处理激光雷达云点和图像数据。
GPS定位和惯性测量单元方面,IMU 为NovAtel IMU-IGM-A1 ,GPS接收机为NovAtelProPak6。激光雷达使用的是Velodyne HDL-64E S3,通过以太网连接工控机,水平视野360°,垂直视野26.9°,水平角度分辨率为0.08°,距离精度小于2cm,可探测到120m的汽车或树木。视觉系统使用Leopard ImagingLI-USB30-AR023ZWDR with 3.0 case,通过USB连接到工控机。使用安森美的200万像素1080p传感器AR0230AT和AP0202 ISP。FOV为广角58度,有效距离大约60-70米。毫米波雷达采用的是Continental ARS408-21,连接至CAN卡。CAN卡为德国ESD CAN-PCIe/402-B4。
上图为百度无人车定位模块框架,融合了IMU、GNSS和激光雷达等传感器(紫色),以及一个预先制作好的定位地图(黄色)。最上层的是SINS系统,就是Strap-down InertialNavigation。捷联惯导系统是在平台式惯导系统基础上发展而来的,它是一种无框架系统,由三个速率陀螺、三个线加速度计和处理器组成。SINS使用IMU测量得到的加速度和角速度积分得到位置、速度、姿态等,在卡尔曼滤波器的传播阶段作为预测模型;相应地,卡尔曼滤波器会对IMU的加速度和角速度进行矫正,位置、速度和姿态等误差也反馈给SINS。RTK定位和激光雷达点云定位结果作为卡尔曼滤波器的量测更新。
上图为百度的激光雷达点云定位地图。首先要提前制作一幅无人车将要行驶地区的激光雷达点云定位地图,包含有激光雷达强度成像图和高度分布图,这张图通常是地图厂家用测绘级激光雷达完成的。目前全球绝大多数厂家包括Waymo、福特、通用等都是如此。b为激光雷达反射强度成像图,c为高度分布图。这张图覆盖范围3.3*3.1平方公里。
预先制作的激光雷达点云定位地图是一个栅格化地图,每个栅格中存储了激光雷达点云的反射强度和高度值数据的统计信息。准确地说,每个栅格中存储了反射强度、高度值的平均值和方差。也就是说使用了两个单高斯模型来描述栅格的反射强度及高度值。
百度无人车的64线激光点云定位提供4个维度的结果,即(x, y, a, h),其中a为高度,h为航向角,x和y是UTM坐标系下的一个二维笛卡尔坐标表示。激光点云定位算法分为两个步骤:航向角优化和水平(x, y)定位。航向角优化首先将实时点云投影到(x, y)平面上,并转化为栅格结构,与地图数据相同。采用了经典图像匹配算法Lucas-Kanada框架,通过最小化实时数据与地图之间的反射强度和高度值误差来优化航向角。水平(x, y)定位使用了直方图滤波器,状态量为(x, y)。直方图滤波器是一个非参数化的方法,它将求解状态空间划分为多个离散区域,并计算每个区域的后验概率,最后将所有区域的后验概率累计为一个概率值。在每个区域的后验概率计算时引入了反射强度和高度值匹配的自适应权重,增加了激光点云定位的鲁棒性。
整个激光点云定位算法描述如下:
激光点云定位与GPS定位取得后进行定位结果融合,也就是卡尔曼滤波。卡尔曼滤波是一种由卡尔曼(Kalman)提出的用于时变线性系统的递归滤波。最常见的卡尔曼滤波器是锁相环,它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。还有扩展卡尔曼滤波器。
严格地说,卡尔曼滤波器不是滤波器,它是一种最优估算法。它能够实时估计系统中的参数(如连续变化的位置、速度等信息)估计量通过一系列受噪音污染的观测量来更新,在给定的时刻,不要求观测量能够唯一确定当时的参数值。卡尔曼滤波通过先验概率和最新观测数据的加权平均来更新状态估计,显然,卡尔曼滤波是贝叶斯的一种具体表现。
定位结果融合使用了误差状态卡尔曼滤波器,滤波器的状态包括位置误差、速度误差、姿态误差、以及加速计和陀螺仪的偏差。误差状态卡尔曼滤波器的运转分为两个步骤:时间更新和量测更新。SINS使用新的IMU的加速度和角速度计算得到位置误差、速度误差和姿态误差作为卡尔曼滤波器的时间更新输入,而激光点云定位结合和GNSS RTK定位结果作为卡尔曼滤波器的量测更新输入。对量测更新的乱序情况,引入了两个滤波器,一个进行时间更新,一个进行量测更新。
表面上看高精度定位很复杂,实际计算中,耗费的运算资源并不算多,基于激光雷达的运算量远低于基于图像的运算量。这套系统在将来也可以用传统的汽车级SoC完成,如瑞萨的R-Car H3,恩智浦的I.MX8或S32A258C。成本有望大幅度降低。64线激光雷达也有望在未来由flash固态激光雷达取代,进而大幅度降低成本。
百度无人车定位团队的一篇关于多传感器融合定位的学术论文“Robust and Precise VehicleLocalization based on Multi-sensor Fusion in Diverse City Scenes”已被机器人顶级会议ICRA 2018录用,初稿可从arXiv上下载。
即将于3月31日在北京举办的Apollo自动驾驶公开课上,百度无人车定位技术负责人、Apollo 核心算法技术负责人万国伟将做主题为《Apollo2.0多传感器融合定位模块》的分享。无人车定位技术的难点在于如何保证高精准度和高鲁棒性。Apollo2.0的多传感器融合定位模块自适应地使用GNSS、LiDAR及IMU等传感器,在许多有挑战的场景(例如市区、隧道等)都达到了厘米级定位。本次演讲将分享定位模块的框架,以及各子模块的算法原理。