一种基于联邦卡尔曼滤波器的多源信息融合定位算法

张 靖，陈鸿跃，陈 雨，刘宇航，孙 谦

0 引 言

现代战争的突发性和复杂性，要求陆基武器系统在公路及铁路长时间、大范围机动下具备精确打击能力。车载定位系统提供的姿态、位置信息是陆基武器系统具备精确打击能力的核心支撑之一。因此对车载定位系统长时间精度保持能力、复杂环境适应能力提出了较高要求。

基于多传感器的组合导航成为解决这一问题的有效方法，一般采用卡尔曼滤波技术最优组合各个传感器的信息。利用卡尔曼滤波技术对组合导航系统进行最优组合有两种途径：a）集中式卡尔曼滤波器，理论上可以得到误差状态的最优估计，但存在状态维数高，计算量大，容错性能差的问题[1]。b）分散化卡尔曼滤波器，其中联邦卡尔曼滤波器[2]（Federated Kalman Filter）具备算法灵活性高、计算量小、容错性能好的特点，可有效融合多种定位信息源。

文献[3]将联邦卡尔曼滤波器运用在全球定位系统/航位推算（GPS/DR）的组合导航中，并进行实地跑车试验，验证了利用联邦卡尔曼滤波器进行组合导航的可行性。文献[4]、文献[5]建立了基于联邦卡尔曼滤波器的捷联惯性导航系统/全球定位系统/里程计（SINS/GPS/OD）组合导航数学模型，但没有对分系统的杆臂误差及安装误差角进行建模。文献[6]改进了误差状态模型，采用集中式卡尔曼滤波器实现了SINS/OD组合导航系统，并对里程计刻度因子和安装误差角进行了估计。文献[7]搭建的GPS/SINS/OD组合导航系统验证了在GPS失效的时间段内，采用联邦卡尔曼滤波器可增加系统的容错性。文献[8]通过改进联邦滤波器对惯性导航系统/全球卫星导航系统/天文导航系统（INS/GNSS/CNS）进行仿真试验，说明应用联邦卡尔曼滤波器的定位系统精度仍有提升空间。

本文采用容错性能高的无重置式联邦卡尔曼滤波器作为多源信息融合的定位算法，建立了包括里程计和卫星定位杆臂误差的组合导航模型。搭建了SINS/GNSS/里程计/高程计四种典型车载定位信息源的融合框架，通过仿真和实车试验验证了所提算法在计算量、容错性、定位精度上的优势。

1 开放式系统架构

选取车载定位领域最常用的信息源SINS、GNSS、里程计、高程计，搭建了一个典型的多信息源组合定位系统框架。如图1所示，以SINS为公共参考系统，分别与 GNSS、里程计、高程计组成子滤波器，通过故障诊断与系统重构决定各子滤波器是否参与融合，可以实现定位信息源的即插即用。系统架构具备扩展能力，融合更多定位信息源时只需对算法框架做较小改动，可以具备开放性。

图1 多源信息融合定位系统算法结构Fig.1 Multi-source Information Fusion System Algorithm Architecture

2 系统误差模型

文中使用的坐标系有：惯性坐标系（i），地球坐标系（e），地理坐标系（g），导航坐标系（n），计算导航坐标系（～n）选取地理坐标系作为导航坐标系，载体坐标系（b），车体坐标系（ ～m）。

2.1 SINS/OD子滤波器

误差状态量选取为

式中 ψ为姿态误差角，；δv为速度误差，δp为位置误差，δp = [ δ L δλ δh ]T；ε为陀螺常值零偏，∇为加表常值零偏，；δk为里程计刻度系数误差；，abδδ分别为里程计的航向、俯仰安装误差角；为里程计杆臂误差，共21维状态变量。

系统误差方程为如下：

式中表示在导航系下导航系相对于惯性系的转动角速度；表示的角速度误差；×表示矢量叉乘；为由b系到n系的方向余弦矩阵。

式中 fb表示比力；表示在导航坐标系下相对于惯性系的地球自转角速度；表示的角速度误差；表示在导航系下导航坐标系相对于地球坐标系的转动角速度；表示的角速度误差；v为捷联惯导相对地球表面的速度在导航坐标的表示。

式中 RM表示捷联惯导所在位置处的子午圈曲率半径； RN表示捷联惯导所在位置处的卯酉圈曲率半径；h表示捷联惯导所在位置的高度。表示速度v在导航坐标系下东、北、天3个方向的速度分量。

将式（2）～（8）记作：

式中 F1( t)为状态转移矩阵， w1为系统噪声向量。

2.2 SINS/GNSS子滤波器

误差状态量选取为

式中为GNSS杆臂误差，共18维状态变量。

系统误差方程同式（2）～（6）、式（8），记作：

2.3 SINS/高程计子滤波器

误差状态量选取为

共15维状态变量。

系统误差方程同式（2）～（6），记作：

3 观测方程

3.1 SINS/OD子滤波器

选择一定时间内 SINS解算的位移增量与SINS/OD航位推算的位移增量差值作为观测量，即：

式中 vI表示SINS真实速度； vod表示里程计在导航坐标系中的真实速度；表示里程计外杆臂产生的速度；v～D表示里程计测得的解算到 SINS载体坐标系的速度；θ表示由车体坐标系到SINS载体坐标系旋转的安装误差角；表示车体坐标系到载体坐标系的方向余弦矩阵；表示在载体坐标系下导航坐标系相对于载体坐标系的转动角速度。将上式记作：

式中1Z为观测矩阵；1H为量测矩阵；1v为量测噪声向量。

3.2 SINS/GNSS子滤波器

选择SINS与GNSS的位置差值和速度差值作为观测量。

3.2.1 位置观测量

式中 pI表示SINS的位置； pGNSS表示卫星在导航坐标系中的位置；表示卫星外杆臂产生的位置偏差，其中：

整理得到：

式中 lGNSS为卫星杆臂；GNSS表示解算得到的外杆臂；δlGNSS表示外杆臂杆臂误差，表示计算导航坐标系到导航坐标系的方向余弦矩阵，表示载体坐标系到计算导航坐标系的方向余弦矩阵。

3.2.2 速度观测量

式中 Zv表示真实的速度观测量； vI表示SINS的真实速度； vGNSS表示卫星接收机在导航坐标系中的真实速度；ngwv表示卫星接收机外杆臂产生的速度。得到实际测得的速度观测量为

式中 v～I表示SINS解算的速度；v～gnw表示测得的卫星接收机外杆臂产生的速度； δ vI表示SINS真实速度与解算速度之间相差的速度误差，卫星速度误差 δ vGNSS为量测噪声。记作：

式中2H为量测矩阵；2v为量测噪声向量。

3.3 SINS/高程计子滤波器

式中表示SINS与高程计的高程观测量； hI表示SINS解算得到的高度；表示高程计测得的高度；δh表示高度误差。

将式（23）记作：

式中3H为量测矩阵；3v为量测噪声向量。

4 联邦卡尔曼滤波算法

系统采用无反馈重置式联邦卡尔曼滤波器进行数据处理，如图2所示，各局部滤波器相互独立，没有反馈重置带来的相互影响，这提供了最高的容错性。

对应的离散化系统误差模型如下：

式中为第i个子滤波器的系统状态向量；为第i个子滤波器的量测向量，上标 i = 1表示SINS/GNSS（卫星导航系统）子滤波器的量测，i=2表示SINS/OD（里程计）子滤波器的量测，i=3表示SINS/Baro-altimeter（高程计）子滤波器的量测；表示时刻k-1至时刻k的一步状态转移矩阵；为量测矩阵；Wk−1为系统噪声向量，为量测噪声向量；Wk−1和是互不相关的高斯白噪声序列；的协方差阵为；的协方差阵为。

图2 无反馈重置式联邦卡尔曼滤波器Fig.2 Feedback-Free Federal Kalman Filter

i个子滤波器独立进行时间更新和量测更新，得到状态估计值和误差协方差阵。则联邦滤波算法如下：

a）子滤波器的时间更新。

式中表示第i个子滤波器在时刻k-1至时刻k的一步预测均方误差；表示第i个子滤波器在时刻k的误差协方差阵；表示第i个子滤波器在时刻k-1至时刻 k的一步预测状态估计，子滤波器的误差状态维数见第 2 节，式（1）、（10）、（12）。

b）子滤波器的量测更新。

式中表示全局的状态估计。

c）故障检测。

利用量测值残差（即新息）kr：

可以较好地检测和隔离某些子系统的突变故障。因为ˆk/k−1包含了k以前的量测值Zi(i ＜ k )的信息，当无故障时， Hk/k−1=Zˆk是对 Zk最好的预报估计，所以 rk应很小（理论上为零均值白噪声）。当kZ发生故障时，kr会发生突变，据此可以检测和隔离子系统故障。

d）最优信息融合。

式中为主滤波器中的协方差矩阵；为融合了主滤波器和子滤波器后的协方差矩阵；为融合后的状态估计。在主滤波器中融合了15维公共误差状态。

e）信息分配。

式中 β(i)为信息分配系数，根据“信息守恒”原理，满足：

在无重置式联邦卡尔曼滤波器中 β(m)=0，

通过时间更新、量测更新、信息融合和信息分配的过程，子滤波器在噪声分配过程中部分信息丢失，滤波结果是次优的，在将各个信息源组成的子滤波器结果融合过程中非最优性被重新合成，得到全局最优估计。

采用集中式卡尔曼滤波器对 SINS/GNSS/里程计/高程计 4种定位信息源进行融合，构建的滤波模型误差状态维数为24维。采用联邦卡尔曼滤波器对相同滤波模型进行估计需要的误差状态维数是21维，相对于集中式卡尔曼滤波器减少了大量矩阵运算的计算量。若增加更多信息源，应用联邦卡尔曼滤波器进行组合的定位系统误差状态维数不再增加。

5 仿真试验

一组试验在各信息源持续有效的情况下，采用集中式卡尔曼滤波器和联邦式卡尔曼滤波器对同一组数据进行解算，对比两种滤波器的定位精度，验证采用联邦卡尔曼滤波器进行组合导航的可行性。

另一组试验在导航过程中分别关闭和开启 GNSS和里程计，验证算法的故障诊断与系统重构功能，提高定位系统在复杂环境下的定位可靠性。

5.1 路径设计

起始点设置为：经度 E113.19458°，纬度N36.31295°，高度909 m。

车辆在进行240 s的初始对准后，静止110 s，之后以 2 m/s2加速 10 s，匀速 200 s，以 0.4 m/s2减速 10 s，匀速200 s，再以0.4 m/s2加速10 s，之后匀速600 s，以0.4 m/s2减速10 s，匀速600 s，以2 m/s2加速10 s，匀速600 s，之后以0.4 m/s2减速10 s，匀速600 s。

全程共计用时3110 s，里程约为57 km。

5.2 误差参数设置

以90型机抖激光陀螺为参考，取陀螺随机常值零偏为0.002 (°)/h，随机游走系数为0.0005 (°)/h；加速度计随机常值零偏为 5 µg，随机游走系数为0.5µ/g ；里程计刻度系数误差为0；GPS经纬度误差为10 m(1)σ，高程误差20 m，GPS速度误差为0.1 m/s；里程计杆臂误差为[0.0149 m，0.1756 m，-1.0232 m]；GPS接收机杆臂误差为[0.0149 m，0.1756 m，1.0232 m]。

5.3 仿真结果

5.3.1 各信息源持续有效

采用联邦卡尔曼滤波器和集中式卡尔曼滤波器对上述数据进行仿真，东向、北向和天向定位误差分别如图3～5的纵坐标所示，单位为m，横坐标为时间，去除初始对准的240 s，共计2870 s。

由图 3～5可知，采用联邦卡尔曼滤波器的东向误差范围为-3～2 m，北向误差范围为-2～3 m，天向误差范围为-6～4 m；采用集中式卡尔曼滤波器的东向误差范围为-3～2 m，北向定位误差范围为-2～3 m，天向误差范围为-6～8 m。采用两种滤波器的水平定位误差走势一致，天向定位误差采用联邦卡尔曼滤波器优于集中式卡尔曼滤波器。所以，在各信息源持续有效时，采用联邦式卡尔曼滤波器可以达到与集中式卡尔曼滤波器相当的定位精度。

图3 东向误差对比Fig.3 Longtitude Error Comparison

图4 北向误差对比Fig.4 Atitude Error Comparison

图5 天向误差对比Fig.5 Hight Error Comparison

5.3.2 故障仿真

对于SINS/OD子滤波器，设置 rk的阈值为[0.5 m，0.5 m，0.5 m]；对于SINS/GNSS子滤波器，设置 rk的阈值为[ 0 .5× 1 0−6°， 0 .5 × 1 0−6°，0.5 m/s，0.5 m/s，0.5 m/s]，此处高程观测量不设阈值；对于SINS/高程计子滤波器，设置 rk的阈值为20 m。当 rk的绝对值超出阈值范围时，对应滤波器不进行数据融合。

5.3.2.1 GNSS故障

假定在 600～1200 s定位过程中 GNSS失效，在1200 s之后GNSS重新有效，对上述数据进行仿真。

GNSS失效期间，定位算法进行了故障诊断，识别出了GNSS失效，关闭了SINS/GNSS子滤波器通道，定位算法主要由SINS/OD（里程计）组合进行导航，如图 6所示，GNSS失效期间定位误差有发散趋势，这是由于 SINS/OD组合误差随时间积累这一特性导致。GNSS重新有效后，系统进行了重构，开启了SINS/GNSS子滤波器通道，恢复了失效前的定位状态。

图6 在行驶过程中关闭GPS的误差曲线Fig.6 Turn Off GPS During Driving Graph

5.3.2.2 里程计故障

假定在 600～1200 s定位过程中里程计失效，在1200 s之后里程计重新有效，对上述数据进行仿真。

里程计失效期间，系统定位误差无明显变化，如图7所示。

图7 在行驶过程中关闭里程计的误差曲线Fig.7 Turn Off Odometer During Driving Graph

此时算法已关闭了 SINS/OD通道，主要由SINS/GNSS组合进行导航。表明各信息源持续有效时，算法辨识出定位精度高的SINS/GNSS子系统，在主滤波器信息融合中占据较大权重，在GNSS有效时，里程计的失效对定位精度的影响较小。

结合图6、图7可知，在GNSS或里程计失效期间，系统通过故障诊断和系统重构，具备了容错能力，保证了定位精度，提高了定位算法的可靠性，保证了载体在复杂环境（地下、山洞、电磁环境等）中行驶的定位需求。

6 实车试验

采用 SINS、GNSS、里程计和高程计构建一套组合定位系统，单机具体性能见表1。

表1 实车试验设备条件Tab.1 Test Equipment Condition

将该组合定位系统安装在某型号车上，跑车地点位于山西省，行车路线如图 8所示，行驶过程中以差分GPS的定位结果为基准，每2 km记录定位结果，进行精度检测。

图8 跑车行驶轨迹Fig.8 Driving Track

行车中采集 SINS、GNSS、里程计、高程计的原始数据，进行离线解算。

一组试验在 2900～3500 s关闭里程计信号，在3500 s之后开启。另一组试验在 2900～3500 s关闭GNSS信号，在3500 s之后开启。分别验证在里程计和GNSS失效后，算法的定位精度和容错性能。

定位误差统计如表2所示，共24个位置点。第1列为定位点序号，2、3列为分别在2900～3500 s关闭GNSS或里程计的纬度（°）、经度（°）、高度（m）定位结果。第4列为差分GPS的定位结果，作为标准点检测上述两种情况的定位精度。5、6列为2、3列分别与第4列比较之后东、北、天3个方向的定位误差。

表2 定位误差统计Tab.2 Positioning Error Statistics

续表2

卫星定位间断故障的定位结果，如图9所示。其中水平误差最大值 3.5 m。天向定位误差最大值14.5 m，水平定位精度为 2.0 m，天向定位精度为12.8 m，定位精度计算见式（32），式中第i个采样点数北向定位误差为ix，m，东向定位误差为iy，m，定位精度为ε。水平定位误差在真值附近波动，定位精度较高，功能稳定。天向定位精度较低是由于高程计和GPS自身高程精度限制。

图9 卫星间断故障定位误差曲线Fig.9 GPS Intermittent Fault Location Error Graph

里程计定位间断故障的定位结果，如图10所示。其中水平误差最大值为8.1 m，天向定位误差最大值为14.4 m，水平定位精度为 2.6 m，天向定位精度为13.5 m，定位精度计算方法同上。关闭里程计期信号间水平定位误差从5 m增加到8 m，由于跑车过程中SINS/里程计组合定位精度较高，在主滤波器融合时占较大权重。在里程计信号恢复后，定位精度恢复到信号关闭之前状态。

通过以上2组实车试验，关闭定位子信息源时，搭建的定位算法通过故障诊断和系统重构保证了定位功能的稳定。因此，通过实车试验验证了本文搭建的基于联邦卡尔曼的组合导航算法框架具备了良好的定位精度和容错性能。

7 结 论

本文基于无重置式联邦卡尔曼滤波器对4个典型信息源进行融合，并和集中式卡尔曼滤波器进行了比较。采用联邦卡尔曼滤波算法降低了计算量，提高了算法的实用性，在保证定位精度的同时，提高了车载定位系统的容错能力；融合算法框架具有良好的开放性，增加其他信息源（如数字磁图、景象匹配等）时可以进行扩展；保证了车载定位系统在复杂环境下的定位精度，提高了陆基机动武器系统的实战化水平。

卡尔曼滤波器的模型参数设置对滤波精度有一定影响，可在算法框架增加多模型自适应滤波算法，在不同应用环境中切换滤波参数，进一步提高定位系统的复杂环境适应能力。