多传感器融合定位技术

Posted 2022-09-02 爱是与世界平行

由于 GNSS定位信息更新频率低，不能满足自动驾驶中实时性的要求，且定位信号会 因隧道、建筑群等障碍物的遮挡而中断。而INS中配备高频传感器，一定时间内可以提供连续的较高精度的汽车位置、速度和航向信息，但其定位误差会随着系统运行时间累积而剧 增。将 GNSS与INS相结合，可以利用 GNSS提供的不随时间增加的高精度定位来纠正 INS的累积定位误差[24]。

同时，INS可以解决 GNSS特定场景易受影响的问题[25]。通过结合这两种系统的优点，就能得到实时和精准的定位。如果再与地图匹配技术相结合，利用高 精度地图提供的信息，可进一步提高定位精度[26]。

要实现多个定位系统融合，提高定位精度，设计一种融合多个传感器数据的系统尤为重要。本节将从多传感器融合系统简介、系统原理、误差分析以及融合算法等方面对多传感器融合定位系统进行介绍。

1 多传感器融合系统简介

多传感器数据融合是20世纪80年代出现的一门新兴学科，它是将不同传感器对某一目标或环境特征描述的信息融合成统一的特征表达信息及其处理的过程。在多传感器系统 中，各种传感器提供的信息可能具有不同的特征，如模糊的与确定的、时变的与非时变的、实时的与非实时的等。

多传感器数据融合实际上是模拟人脑综合处理复杂问题的过程[27]，通过对各种传感器及其观测信息的合理支配与使用，将各种传感器在空间和时间上的互补与冗余信息，依据某种优化准则加以组合，产生对观测环境或对象的一致性解释和描述，实现多个传感器共同或联合操作，提高整个传感器系统的有效性。

数据融合的目标是利用各种传感器的独立观测信息，对数据进行多级别、多方位和多层次的处理，产生新的有意义的信息，这种信息是最佳协同作用的结果，是任何单一传感器无法获得的。

自动驾驶汽车定位的主要模式有 DR、GNSS、GNSS/DR组合定位模式。

在系统精度要求不高的前提条件下可以单独使用这3种定位模式。为了进一步提高定位系统的精度，保障自动驾驶的安全，在上述3种定位模式中引入了地图匹配，可组合产生出新的3种定位模式:DR/MM、GNSS/MM、GNSS/DR/MM。

多传感器融合定位系统可在6种模式中自动切换以提高整个系统的定位精度和可靠性。接下来对多传感器融合系统体系结构以及系统分层进行介绍。

1.1 多传感器融合系统体系结构

多传感器融合系统体系结构主要包括松耦合 (LooselyCoupled)、紧耦合 (Tightly Coupled)以及深耦合(DeepCoupling)等组合结构[28-29]。

1.1.1 松耦合

在松耦合系统里，GNSS给INS提供位置信息，二者硬件上相互独立且可随时断开连 接，分别输出定位信息与速度信息到融合滤波器，融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后进行输出。

GNSS/INS松耦合系统原理图如图3-22所示。

1.1.2 紧耦合

紧耦合系统是将由 GNSS码环与载波跟踪环解算得到的伪距、伪距率与由惯性导航系 统结合自身信息和卫星星历进行计算得到的伪距、伪距率做差，得到伪距与伪距率的测量残 差，将其作为融合滤波器的输入观测量，得到惯性导航系统计算误差以及传感器偏差以完成对惯性导航系统的校正并获得位置与速度的最优估计值。

GNSS/INS紧耦合系统原理图 如图3-23所示。

1.1.3 深耦合

深耦合系统相对于紧耦合系统，增加了INS单元对 GNSS接收机的辅助。利用INS单元结合星历信息可以对伪距与载波的多普勒频移进行估计，利用估计结果辅助接收机的捕 获与跟踪环路，可以有效地提高 GNSS接收机跟踪环路的动态性与灵敏度。

1.2 多传感器融合系统分层

如图3-24所示，按照信息处理的流程，可将多传感器融合系统划分为数据层融合、特征 层融合和决策层融合[30]。

1.2.1 数据层融合

数据层融合也称像素级融合，首先将传感器的观测数据融合，然后从融合的数据中提取特征向量，并进行判断识别。数据层融合需要传感器是同质的(传感器观测的是同一物理量)，如果多个传感器是异质的(传感器观测的不是同一个物理量)，那么数据只能在特征层或决策层进行融合。

数据层融合不存在数据丢失的问题，得到的结果也是最准确的，但计算量大，且对系统通信带宽的要求很高。

1.2.2 特征层融合

特征层融合属于中间层次，先从每种传感器提供的观测数据中提取有代表性的特征，这些特征融合成单一的特征向量，然后运用模式识别的方法进行处理。这种方法的计算量及对通信带宽的要求相对较低，但部分数据的舍弃使其准确性有所下降。

1.2.3 决策层融合

决策层融合指在每个传感器对目标做出识别后，再将多个传感器的识别结果进行融合， 属于高层次的融合。决策层融合由于对可能包含误差的传感器数据进行再处理，产生的结 果相对而言最不准确，但其计算量及对通信带宽的要求最低。

2 多传感器融合定位系统原理

多传感器数据融合定位系统的输入主要来自 GNSS-RTK、惯性导航系统和地图匹配定位系统。融合定位系统对其数据进行预处理、数据配准和数据融合等处理后，可输出汽车自 身的速度、位置和姿态信息。图3-25所示为多传感器数据融合定位流程示意图。

数据预处理可以考虑为传感器初始化及校准，传感器初始化相对于系统坐标独立地校准每一个传感器。一旦完成了传感器初始化，就可以利用各传感器对共同目标采集得到的 数据进行数据配准。

所谓数据配准，就是把来自一个或多个传感器的观测或点迹数据与已 知或已经确认的事件归并到一起，保证每个事件集合所包含的观测与点迹数据来自同一个 实体的概率较大。具体地说，就是要把每批目标的观测或点迹数据与事件集合中各自的数 据配对[31]。

在传感器配准过程中，收集足够的数据点来计算系统偏差，计算得到的系统偏差用来调整随后得到的传感器数据。其中，传感器的配准主要包括时间配准和空间配准两个方面[32]。

2.1 时间配准

时间配准，就是将关于同一目标的各传感器不同步的量测信息同步到同一时刻。由于各传感器对目标的量测是相互独立进行的，且采样周期(如惯性测量单元和激光雷达的采样 周期)往往不同，所以它们向数据处理中心报告的时刻往往也是不同的。另外，由于通信网 络的不同延迟，各传感器和融合处理中心之间传送信息所需的时间也各不相同，因此，各传感器上数据的发送时间有可能存在时间差，所以融合处理前需将不同步的信息配准到相同的时刻。

时间配准的一般做法是将各传感器数据统一到扫描周期较长的一个传感器数据上[33]， 目前，常用的方法包括最小二乘法(LeastSquares，LS)和内插外推法[34]。这两种方法都对 目标的运动模型做了匀速运动的假设，对于做变加速运动的目标，配准效果往往很差。

下面仅对基于最小二乘法的时间配准法做简单介绍[35]。

假设有两类传感器，分别表示为传感器1和传感器2，其采样周期分别为τ 和T，且两 者之比为τ:T=n，如果第一类传感器1对目标状态的最近一次更新时刻为t_k-1，下一次更 新时刻为t_k=t_k-1+nT，这就意味着在传感器1连续两次目标状态更新之间传感器2有n 次 量测值。因此可采用最小二乘法，将传感器2的n 次量测值进行融合，就可以消除由于时间偏 差而引起的对目标状态量测的不同步，从而消除时间偏差对多传感器数据融合造成的影响。

2.2 空间配准

空间配准，就是借助于多传感器对空间共同目标的量测结果对传感器的偏差进行估计和补偿。对于同一系统内采用不同坐标系的各传感器的量测，定位时必须将它们转换成同 一坐标系中的数据，对于多个不同子系统，各子系统采用的坐标系是不同的，所以在融合处 理各子系统间信息前，也需要将它们转换到同一量测坐标系中，而处理后还需将结果转换成各子系统坐标系的数据，再传送给各个子系统[36]。

如图3-26所示，由于传感器1(传感器2)存在斜距和方位角偏差 Δr₁、Δθ₁(Δr₂、Δθ₂)， 导致在系统平面上出现两个目标，而实际上只有一个真实目标，所以需要进行空间配准[37]。 配准过程如图3-27所示。

图3-27中，r₁、θ₁ 分别表示传感器1的斜距和方位角量测值;r₂、θ₂ 分别表示传感器2 的斜距和方位角量测值;(x_s1，y_s1)表示传感器1在导航坐标平面上的位置;(x_s2，y_s2)表示 传感器2在导航坐标平面上的位置;(x₁，y₁)表示传感器1在导航坐标系上的测量值; (x₂，y₂)表示传感器2在导航坐标系上的测量值。从图3-27可以推导出如下基本方程:

式(3-31)对与目标运动航迹无关的偏差估计方法提供了基础。

常用的与目标运动航迹无关的偏差估计方法主要有实时质量控制法 (RealTime QualityControl，RTQC)、最小二乘法、极大似然法(MaximumLikelihood，ML)和基于卡尔曼滤波器的空间配准算法等。

在给出的几种算法中，实时质量控制法和最小二乘法完全忽略了传感器量测噪声的影响，认为公共坐标系中的误差来源于传感器配准误差(传感器偏 差)。

广义最小二乘法(GeneralizedLeastSquare，GLS)和基于卡尔曼滤波器的方法虽然考虑了传感器量测噪声的影响，但只有在量测噪声相对小时，才会产生好的性能。为了克服前两种局限性，提出了精确极大似然(ExactMaximumLikelihood，EML)空间配准算法。 尽管前面已经介绍了多种不同的配准算法，但它们都是基于立体投影在一个二维区域 平面上实现的。更确切地说，首先通过立体投影技术把传感器量测投影到与地球正切的局部传感器坐标上，然后变换到区域平面，并利用不同传感器量测之间的差异来估计传感器偏差。

虽然立体投影能够减轻单个配准算法的计算复杂度，但这一方法还有一些缺点。首先， 立体投影给局部传感器和区域平面的量测都引入了误差。尽管更高阶的近似可以将变换的精度保证到几米，但由于地球本身是一个椭圆形球而不是一个圆柱，因此地球非正圆球体造 成的误差仍然存在。其次，立体投影扭曲了数据，值得注意的是立体投影的保角性只能保留方位角，而不能保留斜距。由此可以断定系统偏差将会依赖于量测，而不再是时不变的。这样，在区域平面上的二维配准模型就不能正确地表示实际的传感器模型。这时，一种直接在三维空间中对传感器偏差进行估计的基于地心坐标系的空间配准(EarthCenteredEarth Fixed，ECEF)算法被提出以解决上述问题。

3 多传感器融合误差分析

在多传感器融合系统中，来自多个传感器的数据通常要变换到相同的时空参照系中[38]。但由于存在量测误差，直接进行变换很难保证精度来发挥多传感器的优越性，因此在对多传感器数据进行处理时需要寻求一些传感器的配准算法，但配准误差也随之而来。

多传感器配准误差的主要来源有[39]:

(1)传感器的误差，也就是传感器本身因制造误差带来的偏差。

(2)各传感器参考坐标中量测的方位角、高低角和斜距偏差。通常是因量测系统解算 传感器数据时造成的误差。

(3)相对于公共坐标系的传感器的位置误差和计时误差。位置误差通常由传感器导航 系统的偏差引起，而计时误差由传感器的时钟偏差所致。

(4)各传感器采用的定位算法不同，从而引起单系统内局部定位误差。

(5)各传感器本身的位置不确定，为融合处理而进行坐标转换时产生偏差。

(6)坐标转换的精度不够，为了减少系统的计算负担而在投影变换时采用了一些近似 方法(如将地球视为标准的球体等)所导致的误差。

由于以上原因，同一个目标由不同传感器定位产生的航迹就有一定的偏差。这种偏差不同于单传感器定位时对目标的随机量测误差，它是一种固定的偏差(至少在较长时间段内 不会改变)。对于单传感器来说，目标航迹的固定偏差对各个目标来说都是一样的，只是产 生一个固定的偏移，并不会影响整个系统的定位性能。而对于多传感器系统来说，本来是同 一个目标的航迹，却由于相互偏差较大而被认为是不同的目标，从而给航迹关联和融合带来 了模糊和困难，使融合处理得到的系统航迹的定位精度下降，丧失了多传感器处理本身应有 的优点。

4 多传感器融合算法

实现多传感器融合定位的算法有很多种[40]，下面首先简要介绍一下各种数据融合算法 及其优缺点。

其中，卡尔曼滤波算法作为一种经典算法，由于其实时性强、融合精度高等优点，在自动驾驶领域中被广泛使用，下面将重点介绍卡尔曼滤波技术。

4.1 数据融合算法概述

目前，融合算法可概括为随机类和人工智能类。

随机类多传感器数据融合算法主要有综合估计法、贝叶斯估计法、D-S证据推理、最大似然估计、贝叶斯估计、最优估计、卡尔曼滤波算法及鲁棒估计等。

人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算 法以及专家系统等。

下面简介上述算法[41]。

用某种适当的模型来描述一个实际的物理系统，对分析、研究该物理系统是非常重要的。在导航、信号处理、通信、雷达、声呐等许多实际工程应用中，经常采用动态空间模型来描述其中的许多问题[42]。动态空间模型是一个很重要的统计分析工具，如卡尔曼滤波器采用的高斯-马尔可夫线性模型就是一个很好的例子，它用状态方程(动力学方程)来描述状态随时间演变的过程，而用观测方程来描述与状态有关的噪声变量[43]。同样地，只要将高斯-马尔可夫线性模型写成一般的数学映射，就可以用这两个方程来描述更一般的动态系统了:

4.1.1 综合平均法

4.1.2 贝叶斯估计法。

贝叶斯估计理论是较经典的统计估计理论，具有更大的优势，逐渐 成为科学界推理的一个重要工具，提供了一种与传统算法不同的概率分布形式的估计。贝 叶斯推理技术主要用来进行决策层融合。贝叶斯估计法通过先验信息和样本信息合成为后 验分布，对检测目标做出推断。因此贝叶斯估计是一个不断预测和更新的过程。这样就包括了观测值和先验知识在内的所有可以利用的信息，得到的估计误差自然较小。

4.1.3 D-S证据推理。

D-S证据推理是目前数据融合技术中比较常用的一种算法，该算法 通常用来对检测目标的大小、位置以及存在与否进行推断，采用概率区间和不确定区间决定 多证据下假设的似然函数来进行推理[44]。提取的特征参数构成了该理论中的证据，利用这 些证据构造相应的基本概率分布函数，对于所有的命题赋予一个信任度。基本概率分布函数及其相应的分辨框合称为一个证据体。因此，每个传感器就相当于一个证据体。而多个传感器数据融合，实质上就是在同一分辨框下，利用 Dempster合并规则将各个证据体合并成一个新的证据体，产生新证据体的过程就是 D-S证据推理数据融合。

4.1.4 卡尔曼滤波算法。

卡尔曼滤波在控制领域得到广泛应用以后，也逐渐成为多传感 器数据融合系统的主要技术手段之一。联合卡尔曼滤波器的设计思想是先分散处理、再全 局融合，即在诸多非相似子系统中选择一个信息全面、输出速率高、可靠性绝对保证的子系 统作为公共参考系统，与其他子系统两两结合，形成若干子滤波器。各子滤波器并行运行， 获得建立在子滤波器局部观测基础上的局部最优估计，这些局部最优估计在主滤波器内按 融合算法合成，从而获得建立在所有观测基础上的全局估计。

4.1.5 模糊逻辑法。

针对数据融合中所检测的目标特征具有某种模糊性的现象，利用模 糊逻辑算法来对检测目标进行识别和分类。建立标准检测目标和待识别检测目标的模糊子 集是此算法的研究基础。

4.1.6 神经网络算法。

神经网络是一种试图仿效生物神经系统处理信息方式的新型计算 模型。一个神经网络由多层处理单元或节点组成，可以用各种方法互联。在指挥和控制多 传感器数据融合的系统中，神经网络的输入可能是与一个目标有关的测量参数集，输出可能 是目标身份，也可能是推荐的响应或行动。基于神经网络的融合优于传统的聚类算法，尤其 是当输入数据中带有噪声和数据不完整时。然而，要使神经网络算法在实际的融合系统中 得到应用，无论在网络结构设计或是算法规则方面，还有许多基础工作要做，如网络模型、网 络的层次和每层的节点数、网络学习策略、神经网络算法与传统分类算法的关系和综合应 用等。

4.1.7 专家系统。

专家系统是一组计算机程序，它获取专家们在某个特定领域内的知识， 然后根据专家的知识或经验导出一组规则，由计算机做出本应由专家做出的结论。目前，专家系统已在军用和民用领域得到了广泛应用。 此外，其他数据融合算法还有品质因数、模板算法、聚合分析、统计决策理论等。各种融 合算法的特点比较如表3-2所示。

4.2 卡尔曼滤波算法

鉴于卡尔曼滤波算法在多传感器融合系统中使用的普遍性，本节将单独就卡尔曼滤波算法及自动驾驶中常用的改进卡尔曼滤波算法进行详细介绍[45]。

首先介绍卡尔曼滤波的 基本方法，接着介绍针对非线性系统改进的扩展卡尔曼滤波，最后介绍卡尔曼滤波在自动驾 驶中常用的联邦卡尔曼滤波。 1960年，卡尔曼第一次发表了介绍卡尔曼滤波算法的论文[46]。而卡尔曼滤波算法的 第一次实际应用则是将惯性导航器与 C5A 军用飞机上的机载雷达集成在一起[47]。卡尔曼滤波算法被称为“导航组合的驮马”[48]，因为其已经成了现代导航系统的必要部分，特别是对于像 GNSS和INS这样完全不同的系统进行组合导航的系统。卡尔曼滤波可分为线性卡尔曼滤波[49]、扩展卡尔曼滤波[50]、级联式和联邦式卡尔曼滤波[51]、无迹卡尔曼滤波[52] 等，下面将详细阐述卡尔曼滤波的原理。

4.2.1 最小方差估计

最小方差估计是指以均方误差最小作为估计准则的估计，即满足式(3-34):

为了求得X 的最小方差估计，必须首先得到X 的条件概率密度，对于工程中的实际应 用而言，系统状态的各种条件概率密度是很不容易得到的，因此这种估计方法的应用受到一 定的限制[53]。

4.2.2 卡尔曼滤波的估计

卡尔曼滤波是一种递推线性最小方差估计，它的估计准则仍是方差最小估计技术。在工程技术中，为了解工程对象(系统)的各个物理量(状态)，或者为了达到控制工程对象的目的，必须采用测量手段对系统的各个状态进行测量，由于观测值可能是系统的部分状态或其 线性组合，且包含随机误差(也称观测噪声)，最优的估计能将仅与部分相关的观测值进行处理，从而得到统计意义上估计误差最小的更多状态的估计。

因此，卡尔曼滤波是一种递推线性最小方差估计，它的估计值是观测值的线性函数，满足式(3-36)。并且，只要包含初始估计值在内的滤波算法初值选择正确，它的估计也是无偏的。在计算方法上，卡尔曼滤波采用了递推模型，即在历史估计值的基础上，根据t时刻的观测值，递推得到t时刻的状态估计由于历史时刻中每一时刻的估计值又是根据其历史时刻的观测值得到，所以，这种递推算法的估计值可以说是综合利用了t时刻和t时刻以前的所有观测信息得到，并且一次仅处理一个时刻的观测值，使计算量大为减少。因为卡尔曼滤波是用状态方程和观测方程来描述系统和观测值的，所以它主要适用于线性动态系统。

4.2.3 卡尔曼滤波方程

虽然工程对象一般都是连续系统，但是卡尔曼滤波常常采用离散化模型来描述系统，以 便于计算机进行处理。离散系统就是用离散化后的差分方程来描述连续系统。假设离散化 后的系统状态方程和观测方程为

4.2.4 离散卡尔曼滤波的计算流程

4.2.5 扩展卡尔曼滤波

传统卡尔曼滤波要求系统的状态方程和观测方程均是线性条件，然而现实中，许多工程系统往往不能简单地用线性系统来描述，如参数估计引入增广状态方程的非线性、结构关系带来的非线性和观测信号的非线性，因此，十分有必要对非线性滤波进行深入的讨论。

一般情况是将非线性方程线性化，而后利用线性卡尔曼滤波基本方程处理线性问题[56]。 当式(3-38)是非线性的，则系统状态方程和观测方程为[57]

其中，W_k 为过程噪声。扩展卡尔曼滤波算法就是将非线性方程线性化的滤波算法，是解决非线性滤波问题常用的一种方法。其时间递推公式和卡尔曼滤波相似，与卡尔曼滤波算法不同的是式(3-44)中的状态一步预测方程中的参数要用非线性函数的一阶偏导数来计算， 不具有像线性滤波那样可以实现离线计算增益和协方差的优点。

4.2.6 联邦卡尔曼滤波

联邦卡尔曼滤波框图如图3-28所示。联邦卡尔曼滤波一般分为两步滤波，即先基于局部传感器进行滤波，然后再进行主滤波。先假设状态向量从k-1时刻的X_k-1 转移到k 时 刻的X_k，其动力学模型为