惯性导航---里程计非完整性约束

惯性导航—里程计非完整性约束

1 非完整性约束原理

  在进行管道中心线定位时，惯性导航系统在初始化后通过不断地力学编排更新载体的姿态、速度和位置信息，但是由于传感器是惯性器件，其导航误差会不断累积，这便需要借助外界观测量辅助惯性导航系统，才可以进行精确的定位。外界辅助分为姿态辅助观测、速度辅助观测和位置辅助观测。通常位置辅助观测可由全球卫星定位系统获取，速度辅助观测可以由里程计或多普勒测速仪获取。本文将介绍里程计观测信息结合非完整性约束辅助的组合导航。
  所谓非完整性约束，就是假定管道机器人在管道行驶过程中不发生侧滑或跳跃，即机器人理论上侧向、天向速度为0，若是载体坐标系选为“右前上”，则里程计的输出速度为：
$$V_{ODO}^b=[0V_{ODO}0]$$
  受到管道内部压力、管壁介质、器械磨损等因素的影响，里程计的测量值与真实值会存在一个误差，通常用刻度系数误差对里程计输出速度建模分析。在不考虑里程计安装角误差时，里程计输出的速度在载体坐标系中的形式为：
$$V_{ODO}^b=(1+\delta k)V_{ODO}$$

将里程计的测量值变换到导航坐标系(n系)下：
$$V_{ODO}^n=C_b^n{V}_{ODO}^b=C_b^n(1+\delta k)V_{ODO}^b=C_n^{n^{\prime }}{C}_b^n(1+\delta k)V_{ODO}^b$$
将n系到n’系变换矩阵公式（4）代入公式（3），得：
$$C_n^{n^{\prime }}=(I_{3\times 3}-\Phi \times )$$
得n系下里程计得速度：
$$V_{ODO}^n=C_n^{n^{\prime }}(1+\delta k)V_{ODO}^n=(I_{3\times 3}-\Phi \times )(1+\delta k)V_{ODO}^n$$
整理并略去二阶误差小量，得：
$$V_{ODO}^n=V_{ODO}^n+\delta k{V}_{ODO}^n-\Phi \times V_{ODO}^n$$
移项，得里程计速度微分方程：
$$\delta V_{ODO}^n=V_{ODO}^n-V_{ODO}^n=\delta k{V}_{ODO}^n-\Phi \times V_{ODO}^n$$
设惯导推算得速度为$V^n$_INS,速度误差为$\delta$Vⁿ,则里程计的速度观测方程为：
$$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$$

2 非完整性约束实验

  在INS/ODO的组合导航系统中，系统的状态向量X设置为：

$$X=[\varphi \delta v^{\mathrm{n}}\delta r^{\mathrm{n}}{b}_g{b}_a\delta k{]}^{\mathrm{T}}$$

  式中，$\varphi$为姿态误差向量；$\delta$vⁿ为惯导速度误差向量；$\delta$rⁿ为惯导位置误差向量；b_g为三轴陀螺零偏向量；b_a为三轴加速度计零偏向量；$\delta$k为里程计刻度系数误差；
  里程计的速度观测方程为：
$$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$$