多传感器融合定位技术
由于 GNSS 定位信息更新频率低，不能满足自动驾驶中实时性的要求，且定位信号会因隧道、建筑群等障碍物的遮挡面而中断。而 INS(Inertial Navigation System) 中配备高频传感器，一定时间内可以提供连续的较高精度的汽车位置、速度和航向信息，但其定位误差会随着系统运行时间积累而剧增。将 GNSS 与 INS 相结合，可以利用 GNSS 提供的不随时间增加的高精度定位来纠正 INS 的累积定位误差。同时，INS 可以解决 GNSS 特定场景易受影响的问题。通过结合这两种系统的优点，就能得到实时和精准的定位。如果再与地图匹配技术相结合，利用高精度地图提供的信息，可进一步提高定位精度。
要实现多个定位系统融合，提高定位精度，设计一种融合多个传感器数据的系统尤为重要。这里将从多传感器融合系统简介、系统原理、误差分析以及融合算法等方面对多传感器融合定位系统进行介绍。
1. 多传感器融合系统简介
多传感器数据融合是 20 世纪 80 年代出现的一门新兴学科，它是将不同传感器对某一目标或环境特征描述的信息融合成统一的特征表达信息及其处理的过程。在多传感器系统中，各种传感器提供的信息可能具有不同的特征，如模糊的与确定的、时变的与非时变的、实时的与非实时的等。多传感器数据融合实际上是模拟人脑综合处理复杂问题的过程，通过对各种传感器及其观测信息的合理支配与使用，将各种传感器在空间和时间上的互补与冗余信息，依据某种优化准则加以组合，产生对观测环境或对象的一致性解释和描述，实现多个传感器共同或联合操作，提高整个传感器系统的有效性。数据融合的目标是利用各种传感器的独立观测信息，对数据进行多级别、多方位和多层次的处理，产生新的有意义的信息，这种信息是最佳协同作用的结果，是任何单一传感器无法获得的。
自动驾驶汽车定位的主要模式有 DR、GNSS、GNSS/DR 组合定位模式。在系统精度要求不高的前提条件下可以单独使用这 3 种定位模式。为了进一步提高定位系统的精度，保障自动驾驶的安全，在上述 3 种定位模式中引入了地图匹配，可组合产生出新的 3 种定位模式：DR/MM、GNSS/MM、GNSS/DR/MM。多传感器融合定位系统可在 6 种模式中自动切换以提高整个系统的定位精度和可靠性。接下来对多传感器融合系统体系结构以及系统分层进行介绍。
1.1 多传感器融合系统体系结构
多传感器融合系统体系结构主要包括松耦合(Loosely Coupled)、紧耦合(Tightly Coupled)以及深耦合(Deep Coupled)等组合结构。
-
松耦合：在松耦合系统里，GNSS 给 INS 提供位置信息，二者硬件上相互独立且随时断开连接，分别输出定位信息与速度信息到融合滤波器，融合滤波器进行优化处理后将结果反馈给惯性导航系统对其修正后进行输出。GNSS/INS 松耦合系统原理图如下所示：
-
紧耦合：紧耦合系统是将由 GNSS 环码与载波跟踪环解算得到的伪距、伪距率与由惯性导航系统结合自身信息和卫星星历进行计算得到的伪距、伪距率做差，得到伪距与伪距率的测量残差，将其作为融合滤波器的输入观测量，得到惯性导航系统计算误差以及传感器偏差以完成对惯性导航系统的矫正并获得位置与速度的最优估计值。GNSS/INS 紧耦合系统原理图如下所示：
-
深耦合：深耦合系统相对于紧耦合系统，增加了 INS 单元对 GNSS 接收机的辅助。利用 INS 单元结合星历信息可以对伪距和载波的多普勒频移进行估计，利用估计结果辅助接收机的捕获与跟踪环路，可以有效地提高 GNSS 接收机跟踪环路的动态性与灵敏度。
1.2 多传感器融合系统分层
如下图所示，按照信息处理的流程，可将多传感器融合系统划分为数据层融合、特征层融合和决策层融合。
- 数据层融合：数据层融合也称像素级融合，首先将传感器的观测数据融合，然后从融合的数据中提取特征向量，并进行判断识别。数据层融合需要传感器是同质的（传感器观测的是同一物理量），如果多个传感器是异质的（传感器观测的不是同一个物理量），那么数据只能在特征层或决策层进行融合。数据层融合不存在数据丢失的问题，得到的结果也是最准确的，但计算量最大，且对系统通信带宽的要求很高。
- 特征层融合：特征层融合属于中间层次，先从每种传感器提供的观测数据中提取有代表性的特征，这些特征融合成单一的特征向量，然后运用模式识别的方法进行处理。这种方法的计算量及对通信带宽的要求相对较低，但部分数据的舍弃使其准确性有所下降。
- 决策层融合：决策层融合指在每个传感器对目标做出识别后，再将多个传感器的识别结果进行融合，属于高层次的融合。决策层融合由于对可能包含误差的传感器数据进行再处理，产生的结果相对而言最不准确，但其计算量及对通信带宽的要求最低。
2. 多传感器融合定位系统原理
多传感器数据融合定位系统的输入主要来自 GNSS-RTK、惯性导航系统和地图匹配定位系统。融合定位系统对其数据进行预处理、数据配准和数据融合等处理后，可输出汽车自身的速度、位置和姿态信息。下图为多传感器数据融合定位流程示意图：
数据预处理可以考虑为传感器初始化及校准，传感器初始化相对于系统坐标独立地校准每一个传感器。一旦完成了传感器初始化，就可以利用各传感器对共同目标采集得到的数据进行数据配准。所谓数据配准，就是把来自一个或多个传感器的观测或点迹数据与已知或已经确认的事件归并到一起，保证每个事件集合所包含的观测与点迹数据来自同一个实体的概率较大（直白点说就是匹配嘛）。具体的说，就是要把每批目标的观测或点迹数据与事件集合中各自的数据配对。在传感器配准过程中，收集足够的数据点来计算系统偏差，计算得到的系统偏差用来调整随后得到的传感器数据。其次，传感器的配准主要包括时间配准和空间配准两个方面。
-
时间配准：时间配准，就是将关于同一目标的各传感器不同步的量测信息同步到同一时刻。由于各传感器对目标的量测是相互独立进行的，且采样周期（如观星测量单元和激光雷达大采样周期）往往不同，所以它们向数据处理中心报告的时刻往往也是不同的。另外，由于通信网络的不同延迟，各传感器和融合处理中心之间传送信息所需的时间也各不相同，因此，各传感器上的数据的发送时间有可能存在时间差，所以融合处理前需将不同步的信息配准到相同的时刻。
时间配准的一般做法是将各传感器数据统一到扫描周期较长的一个传感器数据上，目前，常用的方法包括最小二乘法（Least Squares, LS）和内插外推法。这两种方法都对目标的运动模型做了匀速运动的假设，对于做变加速运动的目标，配准效果往往很差。下面仅对基于最小二乘法的时间配准法做简单介绍：
假设有两类传感器，分别表示为传感器1和传感器2，其采样周期分别为 τ \tau τ 和 T，且两者之比为 τ \tau τ： -
空间配准：空间配准，就是借助多传感器对空间共同目标的量测结果对传感器的偏差进行估计和补偿。对于同一系统内采用不同坐标系的各传感器的量测，定位时必须将它们转换成同一坐标系中的数据，对于多个不同子系统，各子系统采用的坐标系是不同的，所以在融合处理各子系统间信息前，也需要将它们转换到同一量测坐标系中，而处理后还需将结果转换成各子系统坐标系的数据，再传送给各子系统。
如下图左所示，由于传感器1（传感器2）存在斜距和方位角偏差 Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1（ Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2），导致在系统平面上出现两个目标，而实际上只有一个真实目标，所以需要进行空间配准，配准过程如下右图所示：
上图中， r 1 r_1 r1、 θ 1 \theta_1 θ1 分别表示传感器 1 的斜距和方位角量测值； r 2 r_2 r2、 θ 2 \theta_2 θ2 分别表示传感器 2 的斜距和方位角量测值； ( x s 1 , y s 1 ) (x_{s1},y_{s1}) (xs1,ys1) 表示传感器 1 在导航坐标平面上的位置； ( x s 2 , y s 2 ) (x_{s2},y_{s2}) (xs2,ys2) 表示传感器 2 在导航坐标平面上的位置； ( x 1 , y 1 ) (x_1,y_1) (x1,y1) 表示传感器 1 在导航坐标系上的测量值； ( x 2 , y 2 ) (x_2,y_2) (x2,y2) 表示传感器 2 在导航坐标系上的测量值。从上图右可以推导出如下的基本方程：
{ x 1 = x s 1 + r 1 sin θ 1 y 1 = y s 1 + r 1 cos θ 1 x 2 = x s 2 + r 2 sin θ 2 y 2 = y s 2 + r 2 cos θ 2 \left\{\begin{array}{l} x_{1}=x_{\mathrm{s} 1}+r_{1} \sin \theta_{1} \\ y_{1}=y_{\mathrm{s} 1}+r_{1} \cos \theta_{1} \\ x_{2}=x_{\mathrm{s} 2}+r_{2} \sin \theta_{2} \\ y_{2}=y_{\mathrm{s} 2}+r_{2} \cos \theta_{2} \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧x1=xs1+r1sinθ1y1=ys1+r1cosθ1x2=xs2+r2sinθ2y2=ys2+r2cosθ2如果忽略噪声，则有：
{ r 1 = r 1 ′ + Δ r 1 θ 1 = θ 1 ′ + Δ θ 1 r 2 = r 2 ′ + Δ r 2 θ 2 = θ 2 ′ + Δ θ 2 \left\{\begin{array}{l} r_{1}=r_{1}^{\prime}+\Delta r_{1} \\ \theta_{1}=\theta_{1}^{\prime}+\Delta \theta_{1} \\ r_{2}=r_{2}^{\prime}+\Delta r_{2} \\ \theta_{2}=\theta_{2}^{\prime}+\Delta \theta_{2} \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧r1=r1′+Δr1θ1=θ1′+Δθ1r2=r2′+Δr2θ2=θ2′+Δθ2其中， r 1 ′ r'_1 r1′、 θ 1 ′ \theta'_1 θ1′ 分别表示目标相对于传感器 1 的真实斜距和方位角； r 2 ′ r'_2 r2′、 θ 2 ′ \theta'_2 θ2′ 分别表示目标相对于传感器 2 的真实斜距和方位角； Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1 表示传感器 1 的斜距和方位角偏差； Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2 表示传感器 2 的斜距和方位角偏差。将上面两式合并，并且将所得到的方程相对于 Δ r 1 \Delta{r_1} Δr1、 Δ θ 1 \Delta{\theta_1} Δθ1 和 Δ r 2 \Delta{r_2} Δr2、 Δ θ 2 \Delta{\theta_2} Δθ2 进行一阶泰勒级数展开，可得：
{ x 1 − x 2 ≈ sin θ 1 Δ r 1 − sin θ 2 Δ θ 2 + r 1 cos θ 1 Δ θ 1 − r 2 cos θ 2 Δ θ 2 y 1 − y 2 ≈ cos θ 1 Δ r 1 − cos θ 2 Δ r 2 − r 1 sin θ 1 Δ θ 1 + r 2 sin θ 2 Δ θ 2 \left\{\begin{array}{l} x_{1}-x_{2} \approx \sin \theta_{1} \Delta r_{1}-\sin \theta_{2} \Delta \theta_{2}+r_{1} \cos \theta_{1} \Delta \theta_{1}-r_{2} \cos \theta_{2} \Delta \theta_{2} \\ y_{1}-y_{2} \approx \cos \theta_{1} \Delta r_{1}-\cos \theta_{2} \Delta r_{2}-r_{1} \sin \theta_{1} \Delta \theta_{1}+r_{2} \sin \theta_{2} \Delta \theta_{2} \end{array}\right. {
x1−x2≈sinθ1Δr1−sinθ2Δθ2+r1cosθ1Δθ1−r2cosθ2Δθ2y1−y2≈cosθ1Δr1−cosθ2Δr2−r1sinθ1Δθ1+r2sinθ2Δθ2该式对与目标运动航迹无关的偏差估计方法提供了基础。(这里是为啥)
常用的与目标运动航迹无关的偏差估计方法主要有实时质量控制法(Real Time Quality Control,RTQC)、最小二乘法、极大似然法(Maximum Likelihood,ML)和基于卡尔曼滤波器的空间配准算法等。在给出的几种算法中，实时质量控制法和最小二乘法完全忽略了传感器量测噪声的影响，认为公共坐标系中的误差来源于传感器配准误差（传感器偏差，在下一节会讲）。广义最小二乘法(Generalized Least Square,GLS)和基于卡尔曼滤波器的方法虽然考虑了传感器量测噪声的影响，但只有在量测噪声相对小时，才会产生好的性能。为了克服前两种局限性，提出了精确极大似然(Exact Maximum Likelihood,EML)空间配准算法。
尽管前面已经介绍了多种不同的配准算法，但它们都是基于立体投影在一个二维区域平面上实现的。更准确地说，首先通过立体投影技术把传感器量测投影到与地球正切的局部传感器坐标上，然后变换到区域平面，并利用不同传感器量测之间的差异来估计传感器偏差。虽然立体投影能够减轻单个配准算法的计算复杂度，但这一方法还有一些缺点。首先，立体投影给局部传感器和区域平面的量测都引入了误差。尽管更高阶的近似可以将变换的精度保证到几米，但由于地球本身是一个椭圆形球而不是一个圆柱，因此地球非正圆球体造成的误差仍然存在。其次，立体投影扭曲了数据，值得注意的是立体投影的保角性只能保留方位角，而不能保留斜距。由此可以断定系统偏差将会依赖于量测，而不再是时不变的。这样，在区域平面上的二维配准模型就不能正确地表示实际的传感器模型。这时，一种直接在三维空间中对传感器偏差进行估计的基于地心坐标系的空间配准(Earth Centered Earth Fixed,ECEF)算法被提出以解决上述问题。
3. 多传感器融合误差分析
在多传感器融合系统中，来自多个传感器的数据通常要变换到相同的时空参照系中。但由于存在量测误差直接进行变换很难保证精度来发挥多传感器的优越性，因此在对多传感器数据进行处理时需要寻求一些传感器的配准算法，但配准误差也随之而来。
多传感器配准的主要误差来源有：
- 传感器的误差，也就是传感器本身因制造误差带来的偏差；
- 各传感器参考系中量测的方位角、高低角和斜距偏差。通常是因量测系统解算传感器数据时造成的误差；
- 相对于公共坐标系的传感器的位置误差和计时误差。位置误差通常由传感器导航系统的偏差引起，而计时误差由传感器的时钟偏差所致；
- 各传感器采用的定位算法不同，从而引起单系统内局部定位误差；
- 各传感器本身的位置不确定为融合处理而进行坐标转换时产生偏差；
- 坐标转换的精度不够，为了减少系统的计算负担而在投影变换时采用了一些近似方法（如将地球视为标准的球体等）所导致的误差。
由于以上原因，同一个目标由不同传感器定位产生的航迹基友一定的偏差。这种偏差不同于单传感器定位时对目标的随机量测误差，它是一种固定的偏差（至少在较长时间段内不会改变）。对于单传感器来说，目标航迹的固定偏差对各个目标来说都是一样的，只是产生一个固定的偏移，并不会影响整个系统的定位性能。而对于多传感器系统来说，本来是同一个目标的航迹，却由于互相偏差较大而被认为是不同的目标，从而给航迹关联和融合带来了模糊和困难，使融合处理得到的系统航迹的定位精度下降，丧失了多传感器处理本身应有的优点。
4. 多传感器融合算法
实现多传感器融合定位的算法有很多种，下面首先简要介绍一下各种数据融合算法及其优缺点。其中，卡尔曼滤波算法作为一种经典算法，由于其实时性强、融合精度高等优点，在自动驾驶领域中被广泛使用，下面将重点介绍卡尔曼滤波技术。
4.1 数据融合算法概述
目前，融合算法可概括为随机类和人工智能类。随机类多传感器数据融合算法主要有综合估计法、贝叶斯估计法、D-S证据推理、最大似然估计、贝叶斯估计、最优估计、卡尔曼滤波算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法及鲁棒估计等。人工智能类多传感器数据融合算法主要有模糊逻辑法、神经网络算法以及专家系统等。下面简介上述算法：
用某种适当的模型来描述一个实际的物理系统，对分析、研究该物理系统是非常重要的。在导航、信号处理、通信、雷达、声呐等许多实际工程应用中，经常采用动态空间模型来描述其中的许多问题。动态空间模型是一个很重要的统计分析工具，如卡尔曼滤波器采用的高斯-马尔可夫线性模型就是一个很好的例子，它用状态方程（动力学方程）来描述状态随时间演变的过程，而用观测方程来描述与状态有关的噪声变量。同样的，只要将高斯-马尔可夫线性模型写成一般的数学映射，就可以用这两个方程来描述更一般的动态系统了：
状 态 方 程 : X k = f ( X k − 1 , W k ) 观 测 方 程 : L k = h ( X k , V k ) 状态方程: X_{k}=f\left(X_{k-1}, W_{k}\right) \\ 观测方程: L_{k}=h\left(X_{k}, V_{k}\right) 状态方程:Xk=f(Xk−1,Wk)观测方程:Lk=h(Xk,Vk) 上式被称为动态空间模型。其中, X k ∈ R k x X_{k} \in \mathbf{R}^{k_{x}} Xk∈Rkx 为系统在 k k k 时刻的状态, L k ∈ R k x L_{k} \in \mathbf{R}^{k_{x}} Lk∈Rkx 为系统状态 X k X_{k} Xk 的观测值; W k W_{k} Wk 、 V k V_{k} Vk 分别为过程和观测噪声。
- 综合平均法：该算法是把来自多个传感器的众多数据进行综合平均。它适用于用同样的传感器检测同一个目标的情况。如果对一个检测目标进行了 k k k 次检测，其平均值 S ˉ = ∑ i = 1 k W i S i / ∑ i = 1 k W i , W i \bar{S}=\sum_{i=1}^{k} W_{i} S_{i} / \sum_{i=1}^{k} W_{i}, W_{i} Sˉ=∑i=1kWiSi/∑i=1kWi,Wi 为分配给第 i i i 次检测的权值。
- 贝叶斯估计法：贝叶斯估计理论是较经典的统计估计理论,具有更大的优势,逐渐 成为科学界推理的一个重要工具,提供了一种与传统算法不同的概率分布形式的估计。贝 叶斯推理技术主要用来进行决策层融合。贝叶斯估计法通过先验信息和样本信息合成为后 验分布,对检测目标做出推断。因此贝叶斯估计是一个不断预测和更新的过程。这样就包 括了观测值和先验知识在内的所有可以利用的信息,得到的估计误差自然较小。
- D-S 证据推理：D-S 证据推理是目前数据融合技术中比较常用的一种算法,该算法通常用来对检测目标的大小、位置以及存在与否进行推断，采用概率区间和不确定区间决定多证据下假设的似然函数来进行推理。提取的特征参数构成了该理论中的证据，利用这些证据构造相应的基本概率分布函数，对于所有的命题赋予一个信任度。基本概率分布函数及其相应的分辨框合称为一个证据体。因此，每个传感器就相当于一个证据体。而多个 传感器数据融合，实质上就是在同一分辨框下，利用 Dempster 合并规则将各个证据体合并成一个新的证据体，产生新证据体的过程就是 D-S 证据推理数据融合。
- 卡尔曼滤波算法：卡尔曼滤波在控制领域得到广泛应用以后，也逐渐成为多传感器数据融合系统的主要技术手段之一。联合卡尔曼滤波器的设计思想是先分散处理、再全局融合，即在诸多非相似子系统中选择一个信息全面、输出速率高、可靠性绝对保证的子系统作为公共参考系统，与其他子系统两两结合，形成若干子滤波器。各子滤波器并行运行，获得建立在子滤波器局部观测基础上的局部最优估计，这些局部最优估计在主滤波器内按融合算法合成，从而获得建立在所有观测基础上的全局估计。
- 模糊逻辑法：针对数据融合中所检测的目标特征具有某种模糊性的现象,利用模 糊逻辑算法来对检测目标进行识别和分类。建立标准检测目标和待识别检测目标的模糊子 集是此算法的研究基础。
- 神经网络算法：神经网络是一种试图仿效生物神经系统处理信息方式的新型计算模型。一个神经网络由多层处理单元或节点组成，可以用各种方法互联。在指挥和控制多传感器数据融合的系统中,神经网络的输入可能是与一个目标有关的测量参数集，输出可能 是目标身份，也可能是推荐的响应或行动。基于神经网络的融合优于传统的聚类算法，尤其 是当输人数据中带有噪声和数据不完整时。然而，要使神经网络算法在实际的融合系统中得到应用,无论在网络结构设计或是算法规则方面，还有许多基砩工作要做,如网络模型、网络的层次和每层的节点数、网络学习策略、神经网络算法与传统分类算法的关系和综合应用等。
- 专家系统：专家系统是一组计算机程序，它获取专家们在某个特定领域内的知识，然后根据专家的知识或经验导出一组规则，由计算机做出本应由专家做出的结论。目前，专家系统已在军用和民用领域得到了广泛应用。
此外，其他数据融合算法还有品质因数、模板算法、聚合分析、统计决策理论等。各种融合算法的特点比较如下表所示：
| 融合算法 | 运行环境 | 信息类型 | 信息表示 | 不确定性 | 融合技术 | 适用范围 |
|---|---|---|---|---|---|---|
| 综合平均法 | 动态 | 冗余 | 原始读数值 | —— | 加权平均 | 低层融合 |
| 贝叶斯估计法 | 静态 | 冗余 | 概率分布 | 高斯噪声 | 贝叶斯估计 | 高层融合 |
| D-S 证据推理 | 静态 | 冗余互补 | 命题 | —— | 逻辑推理 | 高层融合 |
| 卡尔曼滤波 | 动态 | 冗余 | 概率分布 | 高斯噪声 | 系统模型滤波 | 低层融合 |
| 模糊逻辑法 | 静态 | 冗余互补 | 命题 | 隶属度 | 逻辑推理 | 高层融合 |
| 神经网络算法 | 动、静态 | 冗余互补 | 神经元输入 | 学习误差 | 神经元网络 | 低/高层融合 |
| 专家系统 | 静态 | 冗余互补 | 命题 | 置信因子 | 逻辑推理 | 高层融合 |