基于联邦滤波的多传感器管道定位方法

张 强，吴 磊，杨 迪，李 倩

(哈尔滨工程大学智能科学与工程学院，哈尔滨 150001)

0 引言

地下管道是城市基础设施的重要组成部分，承担着城市信息、能源、水暖资源流通和废物排弃等重要功能。由于城市化的不断发展，地下管道数量日益增多，管道分布越来越复杂，这给地下管道使用、维护以及城市地下工程开展带来了难度。因此，有必要针对城市地下管道建立精确的三维地图，以防止开挖和施工等工程对管道造成损坏。

目前，常用的非破坏性管道定位技术，包括探地雷达定位法、电磁定位法以及磁强计定位法都具有局限性，其定位性能受限于管道材料、内容物、管道直径和深度以及上覆土壤的性质等各种因素。与上述技术不同，惯性定位技术在管道内部运行过程中，利用惯性测量装置采集其运行角速度与加速度信息，并通过捷联惯性解算实现对管道的有效定位。由于惯性定位方法不受管道材质以及外界环境影响，属于一种完全自主式定位方法，因此近年来在管道定位领域中占据主要地位。但是，捷联惯性解算存在累积式的原理性误差，通常需要必要的外部量测信息对惯性解算累积误差进行校正，从而保证管道定位误差保持在可接受范围内。在管道定位过程中，通常采用里程轮测速信息作为辅助信息对惯性解算误差进行校正。除此以外，管道上的已知位置点可以对管道定位仪进行位置更新，进一步抑制惯性解算误差发散。非完整性约束是指管道定位仪由于受到管道的限制，在管道内只能前后运动，垂直于纵向平面内的速度为零，可以提供载体坐标系下的速度辅助信息。杨阳等提出了一种改进的容积卡尔曼滤波(Cubature Kalman Filter，CKF)算法，引入地磁和重力信息作为滤波器的量测矢量，并使用陀螺测量值自适应地调节CKF过程噪声阵以保持滤波器稳定。杨理践等提出了以里程轮速度为量测量的正向容积卡尔曼滤波算法和以基准点位置为起点的反向平滑两级滤波，实现了对管道缺陷地理坐标的最优估计。同时，由于管道起点和终点的位置已知，可以对管道定位仪采集数据分别进行正向和逆向定位解算，然后分别将正向解算的前二分之一数据和逆向解算的前二分之一数据作为系统的定位结果，提高了系统的定位精度。

通过以上文献可以发现，现有管道定位方案多以惯导/里程轮组合构建定位系统，同时辅以终点校正或反向平滑，从而进一步提高定位精度。但管道定位仪工作过程中，由于里程轮通过与管壁紧密接触进行测速，管道内壁不平整、管道焊缝等容易使里程轮与管壁脱离接触，出现打滑。当里程轮打滑时，输出的里程轮速度信息作为量测信息会导致定位误差增加。为了解决这个问题，D. Hyun和H. S. Yang等将一种改进的扩展卡尔曼滤波(Extended Kalman Filter, EKF)用于管道定位，并使用光电测速传感器作为管道定位仪的速度量测。光电测速传感器属于无接触测速传感器，不存在打滑现象，但其对传感器与管壁间的测速距离要求严格，实际使用过程中测速信息不稳定。为融合不同特点传感器优势，本文利用联邦卡尔曼滤波(Federated Kalman Filter, FKF)对微惯性测量单元(Micro Inertial Measurement Unit，MIMU)/里程轮/光电测速传感器信息进行融合，并基于残差预测设计了自适应信息分配因子，以减弱子系统发生故障对主滤波精度的影响，从而提高整体定位精度。

1 管道定位系统

1.1 管道定位系统组成及工作原理

管道定位系统由低成本MIMU、里程轮、光电测速传感器、微处理器以及存储卡组成，如图1所示。在定位过程中，微处理器将加速度计、陀螺仪信息、光电测速传感器以及里程轮信息存储到存储卡中。首先，对陀螺仪与加速度信息进行捷联惯性解算，得到惯性解算输出的姿态、速度和位置；然后，利用惯性解算的姿态信息将里程轮测速信息与光电测速传感器信息投影到导航坐标系()；最后，将惯性解算信息、里程轮解算速度与光电测速传感器解算速度用联邦滤波进行最优融合，得到系统定位结果。本文导航坐标系取地理坐标系。

图1 管道定位系统组成Fig.1 Composition of pipeline positioning system

1.2 系统初始化

惯导系统初始对准是确定参考坐标系的一个过程。惯导系统刚上电启动时，其载体坐标系()相对参考坐标系的各轴指向完全未知或不够准确，无法进入导航状态，因此必须确定载体系相对导航坐标系的空间方位。MIMU初始对准是在管道定位系统测量前静置于管口位置，已知当地重力加速度和管线起点经纬度，利用静置期间的MIMU加速度计输出信息完成水平姿态对准，静止状态下初始俯仰角和横滚角与加速度计输出近似有如下关系

(1)

(2)

=sin()

(3)

=tan(-)

(4)

(5)

简化得到

(6)

所以

∈(-π,π)

(7)

其中，为载体纵轴相对于磁北的航向角，包括当地的磁偏角。所以，实际的航向角应减去磁偏角，即

=-

(8)

2 基于联邦滤波的管道定位算法

2.1 联邦滤波器结构

本文采取联邦滤波器对MIMU、里程轮和光电测速传感器进行信息融合，实现对惯导误差状态的最优估计，滤波器结构如图2所示。

图2 联邦滤波器结构Fig.2 Structure of federated filter

(9)

其中，=1,2为滤波器的下标;,(0≤,≤1)表示第个子滤波器的信息分配系数，并满足式(10)所示的信息守恒原则

(10)

在主滤波器融合阶段，主滤波器将各子滤波器输出的状态估计信息按式(11)进行最优融合。融合方程为

(11)

最终获得全局最优解。

2.2 联邦子滤波器

状态变量为

=[δδδδδδ

(12)

状态方程为

(13)

其中，为系统过程噪声输入矩阵；为系统噪声阵；为系统状态转移矩阵,均可根据捷联惯性导航系统误差方程构造。

(1)MIMU/里程轮子滤波器量测方程

在MIMU/里程轮子系统中，量测信息由惯性导航系统解算得到的速度信息与里程轮得到的速度信息在导航坐标系上的投影做差得到，即

(14)

(15)

(16)

量测方程可以进一步表示为

(17)

量测矩阵可以用式(18)描述

(18)

(2)MIMU/光电测速传感器子滤波器量测方程

在该子系统中，量测信息由惯性导航系统解算得到的速度信息与光电测速传感器得到的速度信息在导航坐标系上的投影做差得到，即

(19)

(20)

下面简单给出卡尔曼滤波具体流程：

式(13)、式(14)和式(19)的等效离散化模型为

(21)

-1=+()Δ

(22)

-1=()Δ

(23)

其中，-1表示系统从-1时刻到时刻的状态转移矩阵；为系统状态向量；-1为系统噪声输入矩阵；-1为系统噪声向量；为系统量测向量；为系统量测矩阵；为系统量测噪声向量。

卡尔曼滤波包括时间更新和量测更新2个阶段。卡尔曼滤波时间更新阶段

(24)

(25)

滤波量测更新阶段

(26)

(27)

(28)

其中，是滤波增益；是量测噪声方差阵。

2.3 基于预测残差的自适应信息分配因子

在FKF中，信息分配因子对联邦滤波器的估计精度有很大影响。

根据式(9)可知

(29)

(30)

因此，第个子滤波器的一步预测误差协方差矩阵可以写为

(31)

增益矩阵可以写为

(32)

子滤波器在获得光电测速传感器和里程轮量测信息后，进行量测更新，可以得到

(33)

(34)

将式(34)代入式(11)可得

(35)

各子滤波器状态信息所占比例与信息分配系数成正比。各子滤波器的信息分配因子大小体现了子滤波器结果对主滤波器的贡献，应与其滤波精度成正比。

当子系统量测信息含有故障信息时，在主滤波器信息融合时需减小该子系统贡献。

(36)

由于预测残差向量,可以反映系统运动学模型误差和量测信息质量，因此可以根据预测残差构造自适应信息分配因子。当系统模型准确，量测信息可靠时，,满足正态分布，即

(37)

根据,的分布特性，构造新统计量

(38)

因此，可以利用式(39)对自适应信息分配因子进行构造。

(39)

其中，是常数，一般根据工程经验选取，这里取2.7。

由于,反映了时刻运动学模型扰动情况，,可用于确定此刻子滤波器状态。当|,|≤时，表明子系统状态模型准确且量测信息有效，子滤波器性能良好；当|,,|>时，表明量测信息可能含有故障信息。

为了满足信息守恒原则，对信息分配因子进行归一化处理

(40)

以MIMU/里程轮子系统为例，当里程轮测速正常时，残差,为零均值白噪声;当里程轮出现打滑时，MIMU/里程轮子系统量测信息含有野值，残差,均值不为零，相应的信息分配因子变小，从而减小了该子系统故障对主滤波器的影响。

2.4 正逆向加权校正

管道定位仪实际工作时，可以由实时动态载波相位差分(Real-Time Kinematic，RTK)技术和全站仪配合确定管道起终两点坐标，然后对同一管道进行多次往返测量。在实际定位过程中，为便于对多次解算的轨迹数据进行加权平均处理，需要多次测量得到的管道轨迹数据长度尽量一致，因此管道定位仪通过地下管道应尽量保持匀速，且往返测量时也应保持速度一致。

对管道的正逆向解算结果，最简单的办法是将两段轨迹数据取均值，从而得到最终定位结果。但是，这种简单取平均值的数据处理方法忽略了惯性定位误差随时间增长而逐渐增大的特性，即随着离起始点距离的增大，定位误差逐渐增大。因此，正逆向解算轨迹数据应该根据其与起始点距离而进行加权融合。根据以上原理，将正逆向解算数据加权平均处理方法表示为

Avg()=((-)-+)

(41)

其中，表示测量管线轨迹全长；为距离管线终点的长度；-为距离起点为-长度的正向解算轨迹数据；为距离管线终点的长度为的逆向解算轨迹数据。

通过正逆向加权校正，可以进一步提高管道定位精度。

3 仿真实验结果与分析

3.1 仿真管道轨迹及仿真参数

针对城市地下管道存在的直管道、弯管道和深管道轨迹进行模拟，如图3所示。模拟地下管道轨迹长120m，管道定位仪在管道中运行速度为0.4m/s。同时模拟里程轮和光电测速传感器不同测速误差特性，里程轮正常工作时，测速稳定，但存在打滑现象，在管道定位仪运行100s和200s处分别出现持续10s的打滑；光电测速传感器不存在打滑问题，但测速不稳定，输出速度信息噪声大，如图4所示。

图3 管道仿真轨迹Fig.3 Pipeline simulation track

图4 里程轮和光电测速传感器随时间输出的速度Fig.4 Output speed of odometer and optical speed sensor with time

实验中设置各传感器技术指标如表1所示。

表1 传感器参数

3.2 实验结果与分析

仿真实验中，管道定位仪对管道进行测量前，先静止10s，利用这10s的数据完成系统初始对准。然后对管道定位仪数据使用不同的定位算法进行解算，包括MIMU/里程轮组合卡尔曼滤波、MIMU/里程轮/光电测速传感器组合联邦滤波以及在联邦滤波基础上的正逆向校正算法。图5、图6和图7分别比较了由不同定位算法得到的管线位置在东向、北向和天向上的误差。表2列出了不同定位算法的定位精度比较。

图5 不同定位方法东向位置误差Fig.5 East position error of different positioning methods

图6 不同定位方法北向位置误差Fig.6 North position error of different positioning methods

图7 不同定位方法天向位置误差Fig.7 Up position error of different positioning methods

表2 不同定位方法管道定位精度比较

根据表2可知，采用MIMU/里程轮组合定位方法，当里程轮出现打滑时，东向、北向和天向最大位置误差分别达到0.18m、0.12m和0.19m，均方根误差分别为0.09m、0.08m和0.10m；采用MIMU/里程轮/光电测速传感器组合定位方法，管道东向、北向、天向定位均方根误差分别为0.03m、0.01m和0.03m；此外，在MIMU/里程轮/光电测速传感器组合定位结果的基础上采取正逆向加权校正，管道东向、北向、天向定位均方根误差分别减小到0.01m、0.01m和0.01m。由仿真实验结果可以看出，在管道定位过程中，里程轮打滑会导致MIMU/里程轮组合定位误差过大，尤其天向位置误差。在基于联邦滤波的MIMU/里程轮/光电测速传感器组合定位系统中，由于在里程轮打滑时控制其子滤波器贡献率的信息分配因子会自适应减小，从而可以有效抑制里程轮打滑造成的定位误差。除此之外，正逆向加权校正算法可以进一步减小管道定位误差，是管道定位系统中常用的数据处理方法。

4 结论

本文介绍了一种基于低成本MIMU/里程轮/光电测速传感器组合的管道定位方法。

1)针对管道定位仪定位过程中里程轮打滑、量测信息失效、惯性误差累积导致定位误差增大的问题，利用光电测速传感器无接触测速不存在打滑的特点，使其与里程轮实现测速互补，降低了里程轮打滑引起的定位误差。

2)针对里程轮和光电测速传感器测速特性不同的问题,利用预测残差对联邦滤波器中信息分配因子进行自适应调整，通过实时调整子滤波器对主滤波器贡献大小，有效减小了子系统故障对主滤波器精度的影响，从而提高了定位精度。