MEMS 惯导管内坐标测量的改进RTS 平滑滤波算法∗

杨 洋李 宾袁 泉杨理践谷 硕

(1.辽宁大学物理学院，辽宁 沈阳110036；2.沈阳工业大学信息科学与工程学院，辽宁 沈阳110870)

随着我国城镇化发展和管道运输的扩张，地下管线的交叉重叠现象日益严重，而随着时间的推移，有些管道的位置信息常常因为丢失而无法获得，因此急需一种可安全、准确提供管道位置的检测技术。将低成本微机电器件(Micro-Electro-Mechanical System，MEMS)搭载移动平台的管道内检测系统可以实现对封闭管道的三维位置检测，其采用惯性测量单元(IMU)采集检测器运动的惯性信息，利用航迹推算(DR)得到检测器在管道中运动的轨迹信息，即管道位置信息。 该方法不受外界条件干扰、不用挖开整段管道，不受管道材质的限制，并且可以实现定期检测，是一种有效的检测方法。 采用MEMS 器件具有低成本、低功耗、轻便灵巧等优点，适合小口径地下管道的检测要求，但其测量精度较低，噪声干扰较大，难以保证长时间的检测精度，需要加入有效的校正措施，论文针对这一问题展开研究工作[1-3]。

Sadovnychiy S 和Jaejong Y 等采用基于战术级的光纤陀螺仪(LN200)对76.2 cm 直径58 km 长度的管道进行定位，该方法亦通过反演计算得到管内行走检测器的轨迹信息，进而推断管道的位置，在离线解算条件下，采用改进的双滤波器固定区间平滑算法，每隔800 m 行走距离对管道位置利用DGPS进行标定，应用里程计对惯导解算(SINS)的误差进行校正，因为传感器精度较高，因此需要考虑地球自转和地表曲率引起的定位误差，对于MEMS 型器件，因其较低的测量精度敏感不到这些信息，所以将该类误差在模型方程中忽略，这造成误差模型与实际不匹配，将有较大概率在双滤波平滑的反向递推中出现奇异阵求逆情况，造成滤波器失效[4-5]。 牛小驥等采用MEMS 器件测量小口径管道，用RTS 平滑(Rauch-Tung-Striebel)代替双滤波平滑，采用惯导/里程计/非完整性约束平滑滤波算法，测量2km直线距离最大误差10 m[6-7]，该方法如果加入额外的测量信息，则将进一步提高测量精度。 对此，Wei Zhao 等发现在铁磁性管道中，某些条件下仍然可以测得地磁信号，这表明管道内检测可以引用地磁信号作为测量信息[8]，但由于缺少约束条件，地磁信号只能对运动检测器三个姿态中的偏航角进行观测。 贾瑞才提出重力/地磁辅助滤波估计算法提升低成本MEMS 惯导系统的姿态测量精度，将重力值近似为常量，利用静态加速度计测量值对检测器的倾角进行观测，与地磁的测得的姿态角互相补充，实现对惯导解算三个姿态角的测量与校正[9]，实验表明通过加入校正措施，姿态角估计的精度得到了提高，如能应用于管道定位中，可为RTS 平滑提供较全面的校正参数。 但RTS 平滑作为一种数据后处理算法，虽然优于常规的Kalman 滤波算法，却仍然严重依赖初始静态对准的精度，康泰钟等提出了基于时间双向解算融合的位姿测量事后处理算法，利用凸组合融合最优原则将时间正向解算结果和时间逆向解算结果进行融合，充分利用整段测量数据进一步提高了测量精度[10]，因为管道测量可以满足初始和终止的静态对准条件，所以适合采用此算法，但进行多次求逆运算降低了方差估计的精度，需要进行有效改进。 论文拟在前面研究者的工作基础上，根据管道定位的测量条件，对现有的测量系统进行改进和提高，进一步增加定位精度，降低测量成本。

论文第一部分介绍了管内检测系统的工作原理；第二部分介绍了DR 算法的基本原理，以及如何通过各种辅助方法得到滤波校正的测量参数；第三部分阐述了改进的RTS 平滑滤波算法:包括建立kalman 滤波的模型方程，对方程进行双向平滑滤波，利用双向解算误差的相干性进行数据融合；第四部分介绍了实验验证过程，实验结果证明了算法的有效性；第五部分对实验结果进行了分析和讨论。

1 管道内检测系统工作原理

1.1 检测装置工作原理

检测器由可以在管道内运动的可移动机械平台(可选择管道清管器)，搭载在平台内的电路系统及电池三部分组成，电池和电路系统用非金属材料密封并固定在平台内，以防止短路和冲击引起的破坏。电路系统由嵌入式系统，MEMS 型的惯性测量单元(IMU)、里程测量装置、电磁测量装置和存储器组成，系统功能框图如图1 所示[11-13]。

图1 检测器结构及系统功能框图

嵌入式系统将各测量装置的检测数据下载到存储器中，待检测器在管道中通过后，将检测数据取出进行离线分析解算。 因为管道内部只有惯性导航可以提供连续且全面的定位信息，所以采用惯性导航为主要定位算法，但是惯导具有误差累积问题，需通过Kalman 滤波器对每步解算的误差进行估计和补偿。 惯导坐标系按如下规定设置:导航坐标系的三维方向为:x-东向；y-北向；z-天向(n系)，在n系上进行管道地理坐标定位。 与载体固联的三维坐标系为载体坐标系(b系)，方向为:y指向前进方向，x水平指向右，z垂直xy平面指向上。 将b系三维坐标轴方向与IMU 三个陀螺仪的旋转轴及三个加速度计的敏感轴重合，在b系上测量检测器运动的惯性信息，即载体运动的三维方向角速率和速度增量信息。

在管道入口和出口处需将检测器静置一段时间(2 min～3 min)，利用静态对准方法，即加速度计和地磁传感器得到初始姿态信息，利用GPS 得到初始位置信息。

2 DR 解算原理及辅助定位方法

2.1 DR 解算原理

MEMS 型加速度计精度较低，若直接采用导航中的捷联惯导算法(SINS)对加速度进行一阶积分，得到速度结果，进行二阶积分得到位移结果，在速度和位置更新环节的累加误差通过时间积分被放大，精度迅速下降，对于精度较低的MEMS 型加速度计，这种问题更加严重。 因为管道内的检测器只有前进方向位移，又因里程增量可通过里程计直接测得，避免了SINS 速度更新积分通路的误差累加现象，同时也可将Kalman 滤波方程的状态噪声误差维数从6 个减少为4 个(见式(12))。

如图1 所示，将里程轮转动方向与b系的y轴正向一致，可测得b系下检测器的速度:

vodo为里程计测得的里程增量。 里程轮用弹性元件张紧在管壁上，避免滑动造成的速度损失，同时保证检测器在管道中良好的通过能力。

用里程增量代替捷联惯导的速度更新即航迹推算(DR)算法，DR 原理如图2 所示。

图2 DR 算法原理图

陀螺仪的采样数据为载体旋转的角速率观测值，由于四元数计算比欧拉角形式的姿态矩阵更简便，所以先利用四元数姿态阵对载体姿态进行更新，再转化为姿态矩阵形式。 将测量值转换为四元数微分方程:

式中:ωt为陀螺仪角速率转换而成的单位旋转四元数。 圆圈符号代表四元数相乘，Qt为姿态更新的四元数。 在姿态更新的周期0 至T内ωt近似恒定条件下，采用四阶龙哥-库塔法递推求解此微分方程:

将更新后的四元数转换为姿态矩阵，速度更新利用姿态矩阵求得n系下速度矢量:

位置更新对速度矢量进行一阶积分即得到采样时间ts内载体在n系的位移Rn:

将所有采样时刻的位移进行累加和排序，即得到载体在管道中运动的轨迹信息，即管道的三维地理坐标。

2.2 辅助定位方法

地磁传感器可测得地理磁场在b系的投影值Bz，通过国家地理信息中心网站确定当地总地磁场值B，通过倾角传感器确定仰俯角r，在初始位置确定相对磁导率u，根据式(10)确定偏航角:

由于地磁测量受环境干扰的影响较大，借鉴Davenport 的Q方法，对姿态解算结果进行筛选，提高测量数据的稳定性和可靠性[14]。

通过IMU 的加速度计测量值可以计算得到检测器横滚角θ和仰俯角r:

式(11)和式(12)将重力加速度g看做常量，但作为滤波校正信息，必须去除加速度计中混有的与检测器运动相关的分量。 文献[15]利用条件判别方程(13)判定和去除这部分分量，只保留与重力加速度相关的测量分量:

式中:ξg为条件判别的阈值，通过实验来确定。 以上辅助方法都采用了修正算法，更新速率远小于DR 的姿态解算，但研究者考虑正常工作条件下，检测器在管道内的姿态角变化不是非常剧烈(弯管最小曲率半径必须符合1.5 倍管道直径，且检测器管内移动速率不超过2 m/s)，因此满足管内检测的条件。

3 滤波校正原理

3.1 建立误差的滤波模型方程

DR 解算根据前一时刻的结果进行姿态和位置更新，误差逐渐累积，必须加入校正环节，否则随着时间的推移，累积误差将使解算结果发散。 主要误差来源中，安装误差，传感器温度漂移和尺度因数误差属于确定性误差，可以通过加入误差补偿函数进行校正；惯性数据的随机游走，DR 算法的累积误差和舍入误差等属于随机性误差，需要通过RTS 平滑去除，采用将累积误差作为状态量的间接校正方式，可以减小滤波解算的最大最小数值范围，提高解算的效率与精度，工作原理如图3 所示。

图3 滤波校正算法原理图

①建立滤波状态方程

将每一步DR 更新的解算误差作为估计的状态量X:

式中:F(t)为状态转移函数，L(t)为噪声转移函数，W为状态噪声，包括三轴陀螺仪的测量误差和里程计测量误差:

②建立滤波观测方程

观测方程为离散形式，观测值只决定于同一时刻的状态值，与前后时刻互不相关:

式中:Z为观测值的矢量形式，Z＝[φx，φy，φz]，H为测量转移矩阵，R为测量噪声矢量。

因为X，W和R同为高斯分布，所以其统计分布规律只由均值和方差所决定，简化了模型方程的求解过程。 在平滑解算的反向递推环节必须保证协方差阵的正定性，避免求逆过程出现奇异阵情况。

3.2 RTS 平滑滤波解算

系统待检测器通过管道后将测量数据全部取出进行离线解算，因为可以根据所有测量值对每一时刻的状态量进行估计，采用固定区间平滑将比Kalman 滤波获得更高的精度。 RTS 平滑包括前向滤波和后向递推两部分，前向滤波即Kalman 滤波，需要计算和存储每一时刻估计的状态量和协方差阵；反向递推利用存储数据进行后向递推平滑，获得最优平滑结果，算法框图如图4 所示[16-18]。

图4 RTS 算法框图

①正向滤波:

(a)将状态方程离散化，得到状态转移方程(k从0 至终止点N):

式中:A表示离散化之后的转移矩阵，P表示状态方差。 设定初始状态值m0和初始噪声方差阵Q0，进行递推解算的预测过程:

(b)观测方程可以直接转为状态转移方程，进行递推解算的更新过程:

K为正向滤波的增益。

②反向递推

当正向滤波结束，利用存储的mk和Pk进行逆时方向的递推解算:

G为平滑增益。 递推中，通过LU分解避免P-k＋1出现奇异阵情况，通过式(24)得到平滑的最优估计结果Psk。

3.3 改进的平滑滤波算法

因为检测器进出管道时都可采用静态的初始对准方法，可利用时间逆向的双向解算滤波平滑进一步提高定位精度。 按式(27)进行惯导解算姿态更新的时间逆向化处理:

式中:角标“-”表示时间递推的方向由后往前进行，速度和位置更新与其类似。 传统RTS 算法只依赖于初始运动时刻的静态对准精度，但实际上在检测器终止运动时刻仍然可以进行静态对准，利用式(27)方法进行时间逆向化求解，可以得到逆向化解算结果，逆向化解算依赖于终止点对准的精度。

时间正向解算过程与RTS 的正向递推相同。因为校正的主要为随机误差，所以采用“凸组合原则”对双向结果进行融合，估计的状态量为:

根据凸组合原理，此时P0为最小方差。 改进的平滑算法(RTS-FC)与RTS 对反向平滑的解算方法不同，其将终止点校正信息加入滤波算法，所以可获得比RTS 更高的精度。

4 实验验证和结果分析

为证明研究的误差校正方法有效性，搭建了实验检测装置进行模拟实验，将除地磁传感器外的测量系统封闭在检测器内，各传感器性能指标如表1所示，检测器及IMU 外观如图5 所示。

表1 各传感器性能指标

图5 检测器、数据记录板卡及IMU

通过登录国家地理信息系统，查找到管道起点的总地磁场强度B，水平磁场强度Bz，与大地水准面仰俯角r，偏航角ψ0，如表2 所示。

表2 实验地点的地磁场信息

通过式(31)求得管内相对磁导率:

为了在实验条件下模拟实际检测过程，采用管外拖曳的方式实现检测器在管道内的运动，在拖曳绳上安装编码器作为里程测量装置，记录检测器的里程增量。 实验管道长度180 m，管道内径0.1 m。检测器以大约1.5 m/s 在管内匀速前进，采用TI 的OMAP-L138 对传感器进行统一采样，避免由于量测时间不同步造成的计算误差。 在测量前还需对IMU 与检测器的安装误差进行校正[19]。 根据原始测量信号的统计分布特性及实验观察，设定初始状态值为:

初始状态噪声值为:

将检测器在管道内通过的同时，记录下所有的测量数据，再进行离线分析解算。 实验结果如图6所示，其中RTS-FC 结果用菱形加实线表示，RTS 用叉号加实线表示，KF 用圆形加实线表示。

为方便分析，分别计算了KF，RTS 平滑和RTSFC 在东向，北向和天向的位置误差，如图7～图9 所示，其中RTS-FC 误差为实线表示，RTS 误差用点划线表示，KF 误差用虚线表示。 通过示意图可以看出，在相同条件下，RTS-FC 在东向和天向的误差最小，北向的最大误差为7.9 m，实验表明，RTS-FC 相较KF 算法和RTS 平滑算法，对误差具有较好的校正效果。

图6 实验管道的地理坐标结果

图7 东向位置误差

图8 北向位置误差

图9 天向位置误差

5 结论

采用MEMS 惯性器件对小口径埋地管道进行管内地理坐标定位具有方便，成本小，污染小，可实现定期检测等优点，但是受器件精度的影响，难以满足实用的需要。 论文将原来的捷联惯导解算改为航迹推算形式，降低了滤波误差的维数，利用里程计的速度测量值代替加速度测量值，降低了测量误差对定位的影响；新引入管内可测的地磁信息和重力信息作为测量数据，利用其对DR 解散的姿态角误差进行估计和补偿；针对系统离线解算的特点，建立RTS 平滑滤波算法对DR 解算的累积误差进行估计和校正；利用起点和终点位置信息可测的特点，采用RTS-FC 进一步提高定位精度。 模拟实际检测环境进行了实验，结果表明，应用RTS-FC 检测180 m 管道，最大误差为8 m，优于KF 和RTS 平滑算法。