Page 24 - 电力与能源2022年第六期

4 8 6 万炳才, 等: 基于惯性导航原理的电缆排管检测系统设计

现对后续各点坐标的解算。 计算出的速度与里程计实际测得的速度之差作为
根据b 系向n 系转换过程, 通过对惯性测量 观测量Z :
单元 ( InertialMeasurementUnit , 简称 IMU ) 测 é Δ v x ù
ê
得的角速度进行时间的一阶积分, 可以得到姿态 Z =v n -v ln = Δv y ú ú ( 5 )
ê
矩阵, 如式( 1 ) 所示, 姿态矩阵可以表示为 ψ G θγ ê ë Δv z ú û
,,
的关系, 即: 式中 v ln ———里 程 计 实 际 测 得 的 速 度; v n ———
é cos γcos ψ G-sinγsinθsin ψ G -cosθsin ψ G sinγcos ψ G+cosγsinθsin ψ G ù SINS 解算的速度。
ê ú
ê cosγsin ψ G+sin γsinθcos ψ G cos θcos ψ G sin γsin ψ G-cos γsinθcos ψ G ú 由状 态 量 和 观 测 量 可 得 Kalman 滤 波 微 分
ê ú
ë -sin γcosθ sinθ cosγcosθ û 方程:
·
( 1 ) X=F ( t ) X+GW
式 中 θ ———仰 俯 角; γ ———横 滚 角; ———航 { ( 6 )
ψ G Z=HX+V
向角。
式中 F ( t )———系数矩阵; G ———白噪声系数矩
计算 n 系 下 当 前 采 样 时 刻 的 速 度v nt 和 位
]———量测矩阵; W ———
阵; H= [ 0 3×3 I 3×3 0 3×3
:
移 X nt
动态噪声矢 量; V ———测 量 噪 声 矢 量, 且 W 与 V
t
ì 互不相关。
ï v nt =v n0 + a nt dt
ï ∫
0
í t ( 2 ) 为了实现计算机对其数值进行计算, 离散的
ï
ï X nt = X n0 + v nt dt
î ∫ 计算方程如下:
0
, ———载体在上一采样时刻的速度
式中 v n0 X n0 { X k=Φ k , k-1+Γ k , k-1 Wk-1 ( 7 )
和地理坐标, 利用对后续点位的循环计算得到最 Z k=H k X k+V k
终载体的运动轨迹坐标。 式中 X k ———系统在k 时刻的状态变量; Z k 表示
由惯性导航轨迹计算过程可知, 直接利用惯 在k 时刻的观测矩阵; V k ——— k 时刻的量测噪声
性导航模型对采集数据进行解算, 其测量误差会 矢量; Φ k , k-1 ———状态转移矩阵, Γ k , k-1 ———系统噪
随时间的累积和距离的增加而增大, 最终失去定 声矩阵; H k ——— k 时刻的量测矩阵。
位的意义。根据卡尔曼( Kalman ) 滤波原理, 提出 根据基于里程修正的 Kalman 滤波算法和采
了采用基于位移修正的卡尔曼滤波算法, 利用测 集数据预处理的形态学滤波算法, 对基于惯性导
量的里程不断地对惯导计算值进行修正, 提高测 航的地下电缆管道路径三维测绘的算法进行了设

量精度, 实现对长距离轨迹的准确测量。 计, 其数 据 处 理 的 流 程 如 图 2 所 示。 计 算 过 程
本文利用脉冲计数的方式实现对装置运动里 如下。
程的计量, 利用磁粒将里程轮分成n 等分, 利用磁 ( 1 )首先利用形态学滤波算法对采集的数据
开关触发电路形成脉冲, 通过对脉冲的计数实现 进行预处理, 将不合理的数据剔除和修正, 得到合
对里程和平均速度的测量, 其计算公式如下 理的加速度和角速度数据。
( 2 )根据惯性导航的算法对采集的数据进行
S = Nπd / n
{ v 0 =πd / nT ( 3 ) SINS 解算, 得到初步的路径定位信息。
式中 d ———里程轮的直径; T ———相邻两个脉冲 ( 3 )利用里程计测量的数据对位移和速度进

的间隔时间。 行测量, 将实际里程测得速度与 SINS 计算的速
将n 系下 SINS 计算的速度、 姿态以及位置 度之差作为观测量, 利用自适应 Kalman 滤波方

误差作为 Kalman滤波估计的状态量 X : 程 SINS 的解算值进行修正和补偿, 从而得到精
[ , , , n , n , n , , , ] ( 4 ) 确的电缆管道路径信息。
X = φ x φ y φ z δ v δ v δ v δ L δ λ δ h
x y z
n , n , n ———
, , ———姿态角误差; δ v δ v δ v
式中 ϕ x ϕ y ϕ z
x y z 2 地下电缆排管内部缺陷检测技术
, , ———纬度、 经度、 高度
n 系下的速度误差; δ L δ λ δ h
误差。 根据电荷耦合器件( CCD ) 成像原理, 系统采
在系统的量测方程中, 将 n 系下 SINS 解算 用内窥镜 +LED 补光相结合的技术实现对电缆