??在进行管道中心线定位时,惯性导航系统在初始化后通过不断地力学编排更新载体的姿态、速度和位置信息,但是由于传感器是惯性器件,其导航误差会不断累积,这便需要借助外界观测量辅助惯性导航系统,才可以进行精确的定位。外界辅助分为姿态辅助观测、速度辅助观测和位置辅助观测。通常位置辅助观测可由全球卫星定位系统获取,速度辅助观测可以由里程计或多普勒测速仪获取。本文将介绍里程计观测信息结合非完整性约束辅助的组合导航。
??所谓非完整性约束,就是假定管道机器人在管道行驶过程中不发生侧滑或跳跃,即机器人理论上侧向、天向速度为0,若是载体坐标系选为“右前上”,则里程计的输出速度为:
V
O
D
O
b
=
[
0
???
V
O
D
O
??
0
]
V_{ODO}^{b}=[0 \;\: V_{ODO} \:\: 0 ]
VODOb?=[0VODO?0]
??受到管道内部压力、管壁介质、器械磨损等因素的影响,里程计的测量值与真实值会存在一个误差,通常用刻度系数误差对里程计输出速度建模分析。在不考虑里程计安装角误差时,里程计输出的速度在载体坐标系中的形式为:
V
~
O
D
O
b
=
(
1
+
δ
k
)
V
O
D
O
\widetilde{V} _{ODO}^{b}=(1+\delta k) {V} _{ODO}
V
ODOb?=(1+δk)VODO?
将里程计的测量值变换到导航坐标系(n系)下:
V
~
O
D
O
n
=
C
^
b
n
V
~
O
D
O
b
=
C
^
b
n
(
1
+
δ
k
)
V
O
D
O
b
=
C
n
n
′
C
b
n
(
1
+
δ
k
)
V
O
D
O
b
\widetilde{V} _{ODO}^{n}=\widehat{C} _{b}^{n} \widetilde{V} _{ODO}^{b}=\widehat{C} _{b}^{n} (1+\delta k) V_{ODO}^{b}=C_{n}^{n'}C_{b}^{n}(1+\delta k) V_{ODO}^{b}
V
ODOn?=C
bn?V
ODOb?=C
bn?(1+δk)VODOb?=Cnn′?Cbn?(1+δk)VODOb?
将n系到n’系变换矩阵公式(4)代入公式(3),得:
C
n
n
′
=
(
I
3
×
3
?
Φ
×
)
C_{n}^{n'}=(I_{3\times 3} - \Phi \times )
Cnn′?=(I3×3??Φ×)
得n系下里程计得速度:
V
~
O
D
O
n
=
C
n
n
′
(
1
+
δ
k
)
V
O
D
O
n
=
(
I
3
×
3
?
Φ
×
)
(
1
+
δ
k
)
V
O
D
O
n
\widetilde{V} _{ODO}^{n}=C_{n}^{n'} (1+\delta k) V_{ODO}^{n} = (I_{3\times 3} - \Phi \times ) (1+\delta k) V_{ODO}^{n}
V
ODOn?=Cnn′?(1+δk)VODOn?=(I3×3??Φ×)(1+δk)VODOn?
整理并略去二阶误差小量,得:
V
~
O
D
O
n
=
V
O
D
O
n
+
δ
k
V
O
D
O
n
?
Φ
×
V
O
D
O
n
\widetilde{V} _{ODO}^{n}=V_{ODO}^{n} +\delta k V_{ODO}^{n} -\Phi \times V_{ODO}^{n}
V
ODOn?=VODOn?+δkVODOn??Φ×VODOn?
移项,得里程计速度微分方程:
δ
V
O
D
O
n
=
V
~
O
D
O
n
?
V
O
D
O
n
=
δ
k
V
O
D
O
n
?
Φ
×
V
O
D
O
n
\delta V_{ODO}^{n} = \widetilde{V} _{ODO}^{n} - V_{ODO}^{n} = \delta k V_{ODO}^{n} - \Phi \times V_{ODO}^{n}
δVODOn?=V
ODOn??VODOn?=δkVODOn??Φ×VODOn?
设惯导推算得速度为
V
n
V^n
VnINS,速度误差为
δ
\delta
δVn,则里程计的速度观测方程为:
V
I
N
S
n
?
C
b
n
V
O
D
O
b
=
δ
V
n
?
δ
V
O
D
O
n
=
δ
V
n
?
δ
k
V
O
D
O
n
?
V
O
D
O
n
×
Φ
V_{INS}^{n}-C _{b}^{n} V_{ODO}^{b}=\delta V_{}^{n}-\delta V_{ODO}^{n}=\delta V_{}^{n} - \delta k V_{ODO}^{n} - V_{ODO}^{n} \times \Phi
VINSn??Cbn?VODOb?=δVn??δVODOn?=δVn??δkVODOn??VODOn?×Φ
??在INS/ODO的组合导航系统中,系统的状态向量X设置为:
X = [ ? δ v n δ r n b g b a δ k ] T X=[\phi\quad\delta v^{\mathrm{n}}\quad\delta r^{\mathrm{n}}\quad b_{g}\quad b_{a}\quad\delta k]^{\mathrm{T}} X=[?δvnδrnbg?ba?δk]T
??式中,
?
\phi
?为姿态误差向量;
δ
\delta
δvn为惯导速度误差向量;
δ
\delta
δrn为惯导位置误差向量;bg为三轴陀螺零偏向量;ba为三轴加速度计零偏向量;
δ
\delta
δk为里程计刻度系数误差;
??里程计的速度观测方程为:
V
I
N
S
n
?
C
b
n
V
O
D
O
b
=
δ
V
n
?
δ
V
O
D
O
n
=
δ
V
n
?
δ
k
V
O
D
O
n
?
V
O
D
O
n
×
Φ
V_{INS}^{n}-C _{b}^{n} V_{ODO}^{b}=\delta V_{}^{n}-\delta V_{ODO}^{n}=\delta V_{}^{n} - \delta k V_{ODO}^{n} - V_{ODO}^{n} \times \Phi
VINSn??Cbn?VODOb?=δVn??δVODOn?=δVn??δkVODOn??VODOn?×Φ