未来车辆定位算法的需求,与如今依赖GNSS的定位算法相比

GNSS能够提供车辆的绝对位置、速度等信息，是现今大多数定位算法的主要信息源。该论文的作者认为在未来，定位的重点是车辆自身与环境的相对位置而非绝对位置，通过惯性传感器和地图信息的融合，获得精确的定位结果。

如今大多数导航定位系统都依赖于全球导航卫星系统（GNSS），通过GNSS和里程计、惯性传感器的组合定位可以获得较好的精度和鲁棒性。对于要实现自动驾驶的汽车，定位的精度和可靠性有着更高的要求。一般情况下车辆都是在路面上行驶的，考虑这种情况，我们将里程计选作第一输入，而当轮速无法获取时，则使用惯性传感器的输入。最后，采用粒子滤波和贝叶斯方法融合地图信息，而不是标准的地图匹配。大量试验结果表明，无GNSS的定位系统性能和有GNSS的系统性能相差无几。

1. 简介

现阶段的定位系统的目标还是为人服务的，是为导航系统提供当前位置，或者收集车辆状态数据。我们特定讨论“Localization Algorithm”这个具体的概念出于以下几个原因。首先，“算法”指软件开发，与仅依靠GNSS信息的定位系统相比，目前已经有足够多的可用的传感器或数据信息，定位性能有了很大的飞跃。

其次，定位是许多系统都需要的服务，而非一个单独的系统。第三，我们回避了“导航”这个词，是因为这只是定位算法的一个应用场景。最后，“location”这个概念比“positioning”的概念更准确，因为如果不能保证地图和周边环境有同样的绝对精度，一个所谓真实的经纬度定位是没有意义的。

上图展示了未来车辆的数据流，一个显著的趋势便是车辆的自主化以及先进驾驶辅助系统(ADAS)的应用。因此，当务之急是提高定位及速度估计系统的性能。从汽车的发展来看，执行器的变化（如制动、转向、发动机转矩等）很小，而过去几十年来传感器却在急剧增多，如：

发动机控制以及悬架传感器中的惯性单元（IMU），用于车辆状态估计；

视觉、夜视、雷达、声呐等传感器用于环境感知，保证车辆行驶在正确的车道并保证跟车距离等；

轮速传感器在ABS系统中的应用；

数据库如道路地图信息用于地图匹配等。

前面提到的所有场景都应用了传感器融合技术，也有一些利用传感器融合计算出无法直接测量或识别的状态量或物体的实例，如基于摄像头的障碍物、行人检测；利用轮速传感器进行胎压和路面附着系数的估计等。我们的方法依赖信号数据处理技术、车辆运动学模型和传感器。需要注意的是，道路地图信息是非线性的，不能用线性高斯模型近似，因此粒子滤波（PF）架构要优于卡尔曼滤波（KF）算法。传感器融合架构见下图。

2. 未来的定位算法应用

本节讨论提高定位算法性能的需求，重点关注自动驾驶及地图领域。

2.1 自动驾驶

未来的自动驾驶对定位算法有着极高的依赖性。一方面，在DARPA的推动下，已有的智能车主要基于激光雷达进行定位。但这种方案价格昂贵，且安装在车顶的激光雷达很影响车辆的设计，如空气动力学特性、美观等；另一方面，我们也看到利用差分GPS，包括通过多天线和摄像头信息得到横摆信息估计的无人驾驶车辆在有高精度地图的区域表现出了极高的性能（如奥迪RS7在德国霍根海姆的自动驾驶展示）。

自动驾驶汽车对定位需要很高的可靠性。定位算法必须要能够在隧道、停车、城市街道、峡谷等GNSS信号丢失或不准的时候保持正常工作。如果只是GNSS无法解算位置信息，但可以解算出伪距信息的话，利用多模型滤波和地图信息限制进行定位也是一种解决方案。而对于一般的室内行驶、长隧道等情况，重点应在于无卫星信号的地图辅助定位技术。

2.2 地图

对于定位来说，先验地图信息是已知的，或者它也可以通过传感器数据来得到。通常精确的向量地图对定位都很有帮助，包括一些路标、限速标志等都可以从数据库中获取。

3. 道路地图和地图匹配

地图匹配是一种提高定位性能的方法，由各类传感器估计出的位置投影到最近的道路上。这种方法有两个问题：首先，地图没有考虑实际的地形地势信息，这就有可能导致定位点从一条路跳到另一条路上；其次，车辆的运动动力学和地图信息的组合并不是最优的。本小节的目的是介绍几种不同的方法，我们称为动态地图匹配（dynamic map matching）。这包括了将运动模型、传感器模型、道路模型等在非线性滤波器中进行组合。难点在于将噪声、干扰等不确定性干扰的轨迹与地图相匹配。图4描述了几种原理，从中可以看出，仅通过里程计和地图信息是可以得到较为精确的定位信息的。

4. 航迹推算(Dead-reckoning)原理

航迹推算最基本的原理是对速度或加速度信号进行积分，可以基于车辆模型，也可以不基于车辆模型。比如，它可以利用IMU数据或者轮速数据。我们从以下几个方面分析航迹推算：

里程计，或基于轮速传感器的航迹推算

惯性传感器航迹推算

利用地图匹配

动态滤波

4.1.航迹推算：里程计

基于位置X,Y及航向角ψ的运动学模型，将速度观测和角速度作为输入信号，我们有如下的带有噪声的动态模型

这个模型有如下的形式

4.2. 航迹推算：惯性传感器

通过惯性传感器可以测量得到速度、加速度等信息，理论上这些值都可以通过积分得到位置、速度及航向。使用基于惯性传感器的航迹推算与地图匹配相结合可以修正传感器的一些误差并得到较为精确的定位。

4.3. 航迹推算：地图匹配

如前所述，地图匹配可以通过将估计值与道路最近点进行正交匹配来实现。利用贝叶斯滤波可以实现较好的地图辅助式定位。

4.4. 航迹推算：动态滤波

一般用于定位的非线性滤波包含以下主要步骤：

时间更新：当下一个测量到达时，利用运动模型来预测车辆位置

测量更新：利用当前测量和传感器模型来更新当前位置信息

5. 基于贝叶斯滤波的融合定位

非线性滤波是统计信号处理的一个分支，它关注的是根据基于时间的一组测量来估计状态的问题。它解决的最一般的问题是计算贝叶斯条件后验密度。计算后验密度有几种不同的算法。KF解决的是模型为线性，噪声为高斯噪声的一类滤波问题。对于非线性问题，可以将模型先线性化后再使用KF滤波，也就是扩展卡尔曼滤波(EKF)。如果高斯近似假设是成立的，也有不需要对非线性模型线性化的处理方法，如无迹卡尔曼滤波（UKF）在每一步都假设后验符合高斯概率密度分布。这些方法的一大问题是只有后验密度很符合高斯分布时才能达到很好的效果。

道路的约束实际上会带来多峰后验密度分布（定位点有可能在这条路或地图上附近的路上等等）。因此，概率密度函数的高斯分布假设是不合适的。另一种完全不同的非线性滤波方法是粒子滤波，它通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布。尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。

根据具体模型的不同，也可以将KF和PF组合应用。这种思想是将状态空间分成两部分，如果一部分可以满足高斯分布假设，则可以用KF来处理，而对于另一部分则使用PF。基于PF的滤波定位算法可以参考下表：

6. 惯性传感器的应用

本小节介绍惯性传感器如加速度计、陀螺仪在航迹推算、传感器融合中的应用。

6.1. 航迹推算IMU

纯积分或者说航迹推算对传感器精度的要求很高，而对于量产车来说无法接受如此昂贵的惯性传感器。而低成本传感器的问题在于其传感器的实时零偏和漂移无法获知并且无法去除。例如，在车辆沿纵轴方向，传感器误差和道路坡度都会反映到纵向加速度上，可以认为是时变的未知偏差，积分两次之后得到的位置会产生很大的位置偏差。对于不基于GPS的纯惯性导航定位来说这是很难解决的一件事。

6.2. GPS/IMU融合

对于下面的状态量

为四元数向量，为位置向量，为车身坐标系下的速度向量，为车身坐标系下的加速度向量。假定为车身坐标系转化到惯性坐标系下的旋转矩阵，则对于离散时刻可以得到惯性坐标系下的位置。

下图展示了基于EKF滤波融合由谷歌手机采集的IMU和GPS数据后的结果。

7. 讨论与结论

本文讨论了未来车辆定位算法的需求，指出了与如今依赖GNSS的定位算法相比，未来的车辆定位需要更高的精确性和可靠性。为了达到这个目的，我们提出了基于多传感器融合和统计信号处理方法的定位算法设计路线。由于车辆定位关心的是自身与环境的相对位置，因此位置估计与周边路网的融合就尤为重要，而精确的道路地图是实现精确定位的关键信息源。