CN102967313B - 一种履带车辆用捷联惯性导航***的数据处理方法 - Google Patents

Abstract

一种履带车辆用捷联惯性导航***的数据处理方法，其步骤为：(1)将惯性测量单元与履带车辆的履带固定连接；(2)捷联惯性导航计算；(3)对地静止检测；(4)误差计算；(5)误差修正。本发明将履带车辆的定位问题转换为履带的定位问题，利用履带运动的特殊性，不断地检测与履带固定连接的惯性测量单元是否相对于地面静止，并在其相对地面静止时计算和修正惯性测量信号及捷联惯性导航参数的误差，从而有效抑制了捷联惯性***的定位误差，大大提高了惯性***定位精度。

Description

一种履带车辆用捷联惯性导航***的数据处理方法

技术领域

本发明主要涉及到车辆的自主导航定位方法领域，特指一种主要适用于履带车辆的基于捷联惯性导航***的数据处理方法。

背景技术

目前，各种形式、各种用途的履带车辆被广泛使用在各个领域。在履带车辆的应用过程中，对于未知环境下履带车辆的高精度自主导航与定位，在一些典型应用中具有非常重要的意义。

尽管具有很多种导航定位手段，如卫星导航等应用广泛，但对于完全自主的导航与定位，特别是一些典型应用而言，则主要还是依赖惯性导航***。目前，则主要是捷联惯性导航***，其传感器则主要是陀螺和加速度计两种惯性器件。传统技术中，履带车辆用捷联惯性导航***，是与履带车辆的车体固定连接的，通常安装在履带车辆的车体内部。

捷联惯性导航***是一种典型的航迹推算***，由于传感器误差及初始误差的影响，单纯的惯性导航***，其定位误差是随时间不断增长的。因此，为了满足应用中对导航定位误差有界的要求，通常采用惯性与其他导航手段一起，构成组合导航***，例如常用的包括惯性/卫星组合导航***或者惯性/里程计组合导航***。

其中，惯性/卫星组合导航***可以为载体提供高精度、误差有界的导航定位结果，但由于依赖卫星，因此丧失了其自主性。惯性/里程计组合导航***尽管既保持了***的自主性，同时大大提高了***的导航定位精度，但其定位误差仍然随运行距离的增长而增大，例如通常其误差在行程的5‰左右，即其误差仍然是无界的。

到目前为止，现有技术中还没有有关测量设备和方法，能够不依赖其他导航手段，对履带车辆用捷联惯性导航***的误差进行有效抑制，为履带车辆提供高精度的自主导航与定位。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种能够不依赖其他导航手段，计算惯性测量单元中传感器和导航参数的误差并进行修正、进而可有效抑制捷联惯性***的定位误差、大大提高捷联惯性***定位精度的履带车辆用捷联惯性导航***的数据处理方法。

为解决上述技术问题，本发明采用以下技术方案：

一种履带车辆用捷联惯性导航***的数据处理方法，其步骤为：

（1）将惯性测量单元与履带车辆的履带固定连接；

（2）捷联惯性导航计算：根据惯性测量单元输出的惯性测量信号计算载体的位置、速度和姿态；

（3）对地静止检测：对惯性测量单元安装点处的履带接触地面的过程进行检测；即对惯性测量单元相对于地面的静止过程进行检测；

（4）误差计算：利用惯性测量单元相对于地面静止过程，不断地对惯性测量信号和导航参数的进行误差计算；

（5）误差修正：根据计算出来的惯性测量信号误差和导航参数误差，对惯性测量信号和导航参数进行误差修正。

作为本发明的进一步改进：

所述步骤（3）是利用惯性测量单元中的陀螺或/和加速度计测量得到的信号，判断惯性测量单元是否相对大地静止。所述步骤（3）中设在i时刻加速度计输出的比力矢量或陀螺输出的角速度矢量为则静止检测包括以下几个步骤：

1)计算矢量幅值即计算：

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2)计算一段时间内的矢量幅值序列的均值和方差即

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b$

(2)

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

这里，N是时间窗口的长度；

3)判断：根据陀螺和加速度计的误差特性，选择合适的静止阈值T_v，若则判定第i时刻对地静止；若采用比力矢量和角速度矢量联合判断，则需要分别选择两个阈值，即陀螺静止阈值和加速度计静止阈值，若两个矢量幅度序列的方差均小于对应的静止阈值，则判定该时刻对地静止。

所述步骤（3）是利用辅助的压力传感器信号，判断惯性测量单元是否相对大地静止。

所述步骤（4）中误差计算的具体步骤为：建立下述Kalman滤波器模型，当检测到惯性测量单元相对地面静止时，进行Kalman滤波计算，获得惯性测量信号和导航参数的误差。所述Kalman滤波器模型，包括一个***方程和一个观测方程，Kalman滤波器模型的***方程为：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

这里，

X＝[δv_n  δv_e δv_d δψ_n δψ_e δψ_d b_ax b_ay b_az b_gx b_gy b_gz]^T          (4)

$F=\left[\begin{array}{cccc}{F}_w& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w_ax w_ay w_az w_gx w_gy w_gz]^T                             (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ -\frac{{2v}_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{{-v}_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

其中：Ω表示地球自转角速度；L表示当地纬度；R表示地球半径；h表示当地海拔高度。 $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ 为加速度计测量的比力矢量；0_3*3表示3*3的零矩阵；表示载体系与导航系之间的方向余弦矩阵；v＝[v_n v_e v_d]^T为惯性导航***输出的北东地速度矢量；

δv＝[δv_n δv_e δv_d]^T为***北东地速度误差；δψ＝[δψ_n  δψ_e δψ_d]^T为***姿态误差；

b_a＝[b_ax b_ay b_az]^T为加速度计零偏误差；b_g＝[b_gx b_gy b_gz]^T为陀螺零偏误差；

w_a＝[w_ax w_ay w_az]^T为加速度计白噪声；w_g＝[w_gx w_gy w_gz]^T为陀螺白噪声。T表示转置，

×表示向量的反对称矩阵。

采用对地静止时的惯性导航速度误差，也即对地静止时的惯性导航计算的输出速度作为***观测量，构建Kalman滤波器模型的观测方程：

z＝HX+υ                     (12)

这里，

z＝[δv_n δv_e δv_d]^T

(13)

H＝[I_3*3 0_3*3 0_3*3 0_3*3]

其中，I_3*3为3*3的单位矩阵；υ为速度测量噪声；

步骤（5）中对惯性测量信号进行误差修正的具体步骤：设在i时刻加速度计测量的比力矢量为f^b，加速度计测量的比力矢量误差为δf^b，修正后的比力矢量为陀螺测量的角速度矢量为ω^b，陀螺测量的角速度误差为δω^b，修正后的角速度矢量为则惯性测量信号误差修正按照下式执行：

${\hat{f}}^b=f^b-{\mathrm{\δf}}^b---\left(14\right)$

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

其中，δf^b和δω^b的初始值为：

δf^b＝δω^b＝[0 0 0]^T                  (15)

若i时刻为相对地面静止，则在完成Kalman滤波计算后，从滤波状态中抽取加速度计零偏误差b_a和陀螺零偏误差b_g，首先按照下式计算δf^b和δω^b：

$\mathrm{\δ}{\hat{f}}^b={\mathrm{\δf}}^b+b_a---\left(16\right)$

δω^b＝δω^b+b_g

然后令***状态中的b_a和b_g清零，即：

b_a＝b_g＝[0 0 0]^T                        (17)

若i时刻不是相对地面静止，则δf^b和δω^b不变。

步骤（5）中对导航参数进行误差修正的具体步骤：设i时刻惯性导航***输出位置为P＝[L λ h]^T，位置误差为δP＝[δL δλ δh]^T，修正后的位置为 $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ 惯性导航***输出的速度为v＝[v_n  v_e v_d]^T，速度误差为δv＝[δv_n δv_e δv_d]^T，修正后的速度为 $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ 惯性导航***输出的姿态方向余弦矩阵为姿态误差为δψ＝[δψ_n δψ_e δψ_d]^T，修正后的姿态方向余弦矩阵为

当i时刻为相对地面静止时，在完成Kalman滤波计算后，从滤波状态中抽取速度误差和姿态误差，导航参数的修正按照下式执行：

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δψ×为δψ的反对称矩阵。δP按照下式计算：

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{{\mathrm{\δv}}_n}{R+h}& \frac{{\mathrm{\δv}}_e}{(R+h)\cos L}& {\mathrm{\δv}}_d\end{array}\right]}^T---\left(19\right)$

然后令***状态中的δv和δψ清零，即：

δv＝δψ＝[0 0 0]^T                   (20)

这里，t表示上一个对地静止时刻到当前时刻的时间，I表示3*3单位矩阵。

若i时刻不是相对地面静止，则不进行导航参数误差修正。

与现有技术相比，本发明的优点在于：本发明将对履带车体的定位问题转化为对履带上某点的定位问题。履带的运动具有特殊性，从履带上一个固定点来看，它的运动由两个不同的阶段构成，一个是相对于地球表面静止的阶段，即与地面接触的阶段，另一个是相对于地球表面运动的阶段，这两个阶段的不断交替，构成了履带上该固定点的完整运动过程。利用履带上某一点运动的这种特殊性，将捷联惯性导航***中的惯性测量单元与履带固定连接，在此基础上对惯性测量单元安装点处的履带在接触地面过程中，惯性测量单元相对于地面的静止状态进行对地静止检测，进而根据静止状态时***的输出值与理论值之间的差别，计算惯性测量单元和导航参数的误差，并对其进行修正，有效地抑制了捷联惯性***的定位误差，大大提高了惯性***的定位精度。

附图说明

图1是本发明处理方法的原理示意图。

图2是本发明在具体实施例中惯性测量单元安装在履带车履带上的结构示意图。

图3是本发明具体实施例中的处理流程示意图。

图例说明：

1、惯性测量单元；2、履带。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1和图3所示，本发明的履带车辆用捷联惯性导航***的数据处理方法，其具体步骤为：

（1）将惯性测量单元1与履带车辆的履带1固定连接（参见图2所示），即将惯性测量单元1固定安装在履带车的履带2上，惯性测量单元1随履带2的运动而一起运动。惯性测量单元1一般由多个陀螺和加速度计构成，用于测量载体的角速度和比力矢量，惯性测量单元1的输出称作惯性测量信号。

（2）捷联惯性导航计算：根据惯性测量单元1输出的惯性测量信号计算载体的位置、速度和姿态；

（3）对地静止检测：对惯性测量单元1安装点处的履带2接触地面的过程进行检测；也就是惯性测量单元1相对于地面的静止过程；即，可以是利用惯性测量单元1中传感器测量得到的信息、或安装在履带2上且在惯性测量单元1附近的其它传感器信息，进行处理和判断，以决定惯性测量单元1是否处于相对地面静止的状态。由于惯性测量单元1与履带2某点固定连接，因此也即判断履带2该点是否相对大地静止。

该步骤（3）可以利用惯性测量单元1中的陀螺或/和加速度计测量得到的信号，判断惯性测量单元1是否相对大地静止。

在其他实施例中，该步骤（3）是利用其他传感器信号，例如：辅助的压力传感器信号，来判断惯性测量单元1是否相对大地静止。

（4）误差计算：利用惯性测量单元1相对于地面静止的信息，不断地进行惯性测量信号1和惯性导航参数的误差计算；当检测到惯性测量单元1相对于地面静止时，理论上惯性测量单元1中陀螺测量的是地球相对于惯性坐标系的角速度，加速度计测量的是地球当地重力，相对地面的速度为零，相对地面的加速度为零。而惯性测量单元1和惯性导航计算的实际输出值，由于误差的原因，与对应的理论值不同，对这些差别进行比较、分析和处理，可以得到惯性测量单元1和惯性导航输出的误差。它可以采用“零速更新算法”，即当惯性测量单元1安装点相对于地面静止时，则理论上其相对于地球的速度为零，而惯性导航***计算的速度，由于各种误差因素的影响，并不为零。那么，惯性导航***计算出的速度就是其速度误差，建立Kalman滤波模型，利用Kalman滤波算法，以这个速度误差作为观测量，可以估计惯性导航***的各种误差。具体的Kalman滤波器模型包括***方程和观测方程。

（5）误差修正：根据计算出来的惯性测量信号误差和导航参数误差，对惯性测量信号和导航参数进行误差修正。

（5.1）传感器误差修正：是根据计算出来的惯性测量信号误差，对惯性测量信号的输出进行修正。

（5.2）导航参数误差修正：是根据计算出来的导航参数误差，对惯性导航参数进行修正。

鉴于履带车体和履带2的位置接近，本发明将惯性测量单元1固定安装于履带车的履带2上，把对履带车体的定位问题转化为对履带2上某点的定位问题。将捷联惯性导航***中的惯性测量单元1与履带2固定连接，在此基础上对惯性测量单元1安装点处的履带2接触地面的过程，也即惯性测量单元1相对于地面的静止过程，进行对地静止检测，当检测到静止时，利用惯性测量单元1相对于地面静止的信息，不断地计算惯性测量信号和惯性导航参数的误差，并进行误差修正，提高捷联惯性导航的自主定位精度。

在本实施例中，设i时刻加速度计输出的比力矢量或陀螺输出的角速度矢量为则静止检测包括以下几个步骤：

1)计算矢量幅值即计算

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2)计算一段时间内的矢量幅值序列的均值和方差即：

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b$

(2)

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

这里，N是时间窗口的长度；

3)判断：根据陀螺和加速度计的误差特性，选择合适的静止阈值T_v，若则判定第i时刻对地静止；若采用比力矢量和角速度矢量联合判断，则需要分别选择两个阈值，即陀螺静止阈值和加速度计静止阈值，若两个矢量幅度序列的方差均小于对应的静止阈值，则判定该时刻对地静止。

在本实施例中，误差计算的具体步骤为：建立下述Kalman滤波器模型，当检测到惯性测量单元1相对地面静止时，进行Kalman滤波计算，获得惯性测量信号和导航参数的误差。Kalman滤波器模型，包括一个***方程和一个观测方程，Kalman滤波器模型的***方程为：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

这里，

X＝[δv_n  δv_e δv_d δψ_n δψ_e δψ_d b_ax b_ay b_az b_gx b_gy b_gz]^T           (4)

$F=\left[\begin{array}{cccc}{F}_w& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w_ax w_ay w_az w_gx w_gy w_gz]^T                  (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ -\frac{{2v}_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{{-v}_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

其中：Ω表示地球自转角速度；L表示当地纬度；R表示地球半径；h表示当地海拔高度。 $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ 为加速度计测量的比力矢量；0_3*3表示3*3的零矩阵；表示载体系与导航系之间的方向余弦矩阵；v＝[v_n v_e v_d]^T为惯性导航***输出的北东地速度矢量；

δv＝[δv_n δv_e δv_d]^T为***北东地速度误差；δψ＝[δψ_n δψ_e δψ_d]^T为***姿态误差；

b_a＝[b_ax b_ay b_az]^T为加速度计零偏误差；b_g＝[b_gx b_gy b_gz]^T为陀螺零偏误差；

w_a＝[w_ax w_ay w_az]^T为加速度计白噪声；w_g＝[w_gx w_gy w_gz]^T为陀螺白噪声。T表示转置×表示向量的反对称矩阵。

采用对地静止时的惯性导航速度误差，也即对地静止时的惯性导航计算的输出速度作为***观测量，构建Kalman滤波器模型的观测方程：

z＝HX+υ            (12)

这里，

z＝[δv_n δv_e δv_d]^T

(13)

H＝[I_3*3 0_3*3 0_3*3 0_3*3]

其中，I_3*3为3*3的单位矩阵；υ为速度测量噪声；

本实施例中，所述步骤（5）中对惯性测量信号误差进行修正的具体步骤为：

设在i时刻加速度计测量的比力矢量为f^b，加速度计测量的比力矢量误差为δf^b，修正后的比力矢量为陀螺测量的角速度矢量为ω^b，陀螺测量的角速度误差为δω^b，修正后的角速度矢量为则惯性测量信号误差修正按照下式执行：

${\hat{f}}^b=f^b-{\mathrm{\δf}}^b---\left(14\right)$

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

其中，δf^b和δω^b的初始值为：

δf^b＝δω^b＝[0 0 0]^T                (15)

若i时刻为相对地面静止，则在完成Kalman滤波计算后，从滤波状态中抽取加速度计零偏误差b_a和陀螺零偏误差b_g，首先按照下式计算δf^b和δω^b：

$\mathrm{\δ}{\hat{f}}^b={\mathrm{\δf}}^b+b_a$

(16)

δω^b＝δω^b+b_g

然后令***状态中的b_a和b_g清零，即：

b_a＝b_g＝[0 0 0]^T             (17)

若i时刻不是相对地面静止，则δf^b和δω^b不变。

本实施例中，所述步骤（5）中对导航参数进行误差修正的具体步骤：

设i时刻惯性导航***输出位置为P＝[L λ h]^T，位置误差为δP＝[δL δλ δh]^T，修正后的位置为 $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ 惯性导航***输出的速度为v＝[v_n v_e v_d]^T，速度误差为δv＝[δv_n δv_e δv_d]^T，修正后的速度为 $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ 惯性导航***输出的姿态方向余弦矩阵为姿态误差为δψ＝[δψ_n δψ_e δψ_d]^T，修正后的姿态方向余弦矩阵为

当i时刻为相对地面静止时，在完成Kalman滤波计算后，从滤波状态中抽取速度误差和姿态误差，导航参数的修正按照下式执行：

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δψ×为δψ的反对称矩阵。δP按照下式计算：

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{{\mathrm{\δv}}_n}{R+h}& \frac{{\mathrm{\δv}}_e}{(R+h)\cos L}& {\mathrm{\δv}}_d\end{array}\right]}^T---\left(19\right)$

然后令***状态中的δv和δψ清零，即：

δv＝δψ＝[0 0 0]^T               (20)

这里，t表示上一个对地静止时刻到当前时刻的时间，I表示3*3单位矩阵。

若i时刻不是相对地面静止，则不进行导航参数误差修正。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (8)

1.一种履带车辆用捷联惯性导航***的数据处理方法，其特征在于，步骤为：

(1)将惯性测量单元与履带车辆的履带固定连接；

(2)捷联惯性导航计算：根据惯性测量单元输出的惯性测量信号计算载体的位置、速度和姿态；

(3)对地静止检测：对惯性测量单元安装点处的履带接触地面的过程进行检测；即对惯性测量单元相对于地面的静止过程进行检测；

(4)误差计算：利用惯性测量单元相对于地面的静止过程，不断地对惯性测量信号和导航参数进行误差计算；

(5)误差修正：根据计算出来的惯性测量信号误差和导航参数误差，对惯性测量信号和导航参数进行误差修正。

2.根据权利要求1所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(3)是利用惯性测量单元中的陀螺或/和加速度计测量得到的信号，判断惯性测量单元是否相对大地静止。

3.根据权利要求2所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(3)中设在i时刻加速度计输出的比力矢量或陀螺输出的角速度矢量为则静止检测包括以下几个步骤：

1)计算矢量幅值即计算

$v_i^b=\left|v_i^b\right|---\left(1\right)$

2)计算一段时间内的矢量幅值序列的均值和方差即

${\stackrel{\‾}{v}}_i^b=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{v}_i^b---\left(2\right)$

${\mathrm{\σ}}_{v_i}^2=\frac{1}{2N+1}\underset{j=i-N}{\overset{i+N}{\mathrm{\Σ}}}{(v_i^b-{\stackrel{\‾}{v}}_i^b)}^2$

这里，N是时间窗口的长度；

3)判断：根据陀螺和加速度计的误差特性，选择静止阈值T_v；若则判定第i时刻对地静止；若采用比力矢量和角速度矢量联合判断，则需要分别选择两个阈值，即陀螺静止阈值和加速度计静止阈值，若两个矢量幅度序列的方差均小于对应的静止阈值，则判定该时刻对地静止。

4.根据权利要求1所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(3)是利用辅助的压力传感器信号判断惯性测量单元是否相对大地静止。

5.根据权利要求1～4中任意一项所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(4)中误差计算的具体步骤为：建立Kalman滤波器模型，当检测到惯性测量单元相对地面静止时，进行Kalman滤波计算，获得惯性测量信号和导航参数的误差。

6.根据权利要求5所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述Kalman滤波器模型包括：一个***方程和一个观测方程；

Kalman滤波器模型的***方程为：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(3\right)$

这里，

X＝[δv_n δv_e δv_d δψ_n δψ_e δψ_d b_ax b_ay b_az b_gx b_gy b_gz]^T   (4)

$F=\left[\begin{array}{cccc}{F}_{\mathrm{vv}}& F_{\mathrm{v\ψ}}& C_b^n& 0_{3*3}\\ F_{\mathrm{\ψv}}& F_{\mathrm{\ψ\ψ}}& 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}& 0_{3*3}& 0_{3*3}\end{array}\right]---\left(5\right)$

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3*3}\\ 0_{3*3}& -C_b^n\\ 0_{3*3}& 0_{3*3}\\ 0_{3*3}& 0_{3*3}\end{array}\right]---\left(6\right)$

W＝[w_ax w_ay w_az w_gx w_gy w_gz]^T   (7)

$F_{\mathrm{vv}}=\left[\begin{array}{ccc}\frac{v_d}{R+h}& -2(\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h})& \frac{v_n}{R+h}\\ 2\mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& \frac{v_d+v_n\tan L}{R+h}& 2\mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{-2v_n}{R+h}& -2(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(8\right)$

$F_{\mathrm{v\ψ}}=\left(C_b^n{f}^b\right)\×---\left(9\right)$

$F_{\mathrm{\ψv}}=\left[\begin{array}{ccc}0& \frac{1}{R+h}& 0\\ \frac{-1}{R+h}& 0& 0\\ 0& \frac{-\tan L}{R+h}& 0\end{array}\right]---\left(10\right)$

$F_{\mathrm{\ψ\ψ}}=\left[\begin{array}{ccc}0& -(\mathrm{\Ω}\sin L+\frac{v_e\tan l}{R+h})& \frac{v_n}{R+h}\\ \mathrm{\Ω}\sin L+\frac{v_e\tan L}{R+h}& 0& \mathrm{\Ω}\cos L+\frac{v_e}{R+h}\\ \frac{-v_n}{R+h}& -(\mathrm{\Ω}\cos L+\frac{v_e}{R+h})& 0\end{array}\right]---\left(11\right)$

其中：Ω表示地球自转角速度；L表示当地纬度；R表示地球半径；h表示当地海拔高度； $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ 为加速度计测量的比力矢量；0_3*3表示3*3的零矩阵；表示载体系与导航系之间的方向余弦矩阵；v＝[v_n v_e v_d]^T为惯性导航***输出的北东地速度矢量；δv＝[δv_n δv_e δv_d]^T为***北东地速度误差；δψ＝[δψ_n δψ_e δψ_d]^T为***姿态误差；b_a＝[b_ax b_ay b_az]^T为加速度计零偏误差；b_g＝[b_gx b_gy b_gz]^T为陀螺零偏误差；w_a＝[w_ax w_ay w_az]^T为加速度计白噪声；w_g＝[w_gx w_gy w_gz]^T为陀螺白噪声；T表示转置，×表示向量的反对称矩阵；

Kalman滤波器模型的观测方程为：

z＝HX+υ   (12)

这里，

z＝[δv_n δv_e δv_d]^T   (13)

H＝[I_3*3 0_3*3 0_3*3 0_3*3]

其中，I_3*3为3*3的单位矩阵；υ为速度测量噪声。

7.根据权利要求1～4中任意一项所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(5)中对惯性测量信号进行误差修正的具体步骤：设在i时刻加速度计测量的比力矢量为f^b，加速度计测量的比力矢量误差为δf^b，修正后的比力矢量为陀螺测量的角速度矢量为ω^b，陀螺测量的角速度误差为δω^b，修正后的角速度矢量为则惯性测量信号误差修正按照下式执行：

${\hat{f}}^b=f^b-\mathrm{\δ}{f}^b---\left(14\right)$

${\widehat{\mathrm{\ω}}}^b={\mathrm{\ω}}^b-\mathrm{\δ}{\mathrm{\ω}}^b$

其中，δf^b和δω^b的初始值为：

δf^b＝δω^b＝[0 0 0]^T   (15)

若i时刻为相对地面静止，则在完成Kalman滤波计算后，从滤波状态中抽取加速度计零偏误差b_a和陀螺零偏误差b_g，首先按照下式计算δf^b和δω^b：

$\mathrm{\δ}{\hat{f}}^b=\mathrm{\δ}{f}^b+b_a---\left(16\right)$

δω^b＝δω^b+b_g

然后令***状态中的b_a和b_g清零，即

b_a＝b_g＝[0 0 0]^T   (17)

若i时刻不是相对地面静止，则δf^b和δω^b不变。

8.根据权利要求1～4中任意一项所述的履带车辆用捷联惯性导航***的数据处理方法，其特征在于，所述步骤(5)中对导航参数进行误差修正的具体步骤：设i时刻惯性导航***输出位置为P＝[L λ h]^T，位置误差为δP＝[δL δλ δh]^T，修正后的位置为 $\hat{P}={\left[\begin{array}{ccc}\hat{L}& \widehat{\mathrm{\λ}}& \hat{h}\end{array}\right]}^T;$ 惯性导航***输出的速度为v＝[v_n v_e v_d]^T，速度误差为δv＝[δv_n δv_e δv_d]^T，修正后的速度为 $\hat{v}={\left[\begin{array}{ccc}{\hat{v}}_n& {\hat{v}}_e& {\hat{v}}_d\end{array}\right]}^T;$ 惯性导航***输出的姿态方向余弦矩阵为姿态误差为δψ＝[δψ_n δψ_e δψ_d]^T，修正后的姿态方向余弦矩阵为

当i时刻为相对地面静止时，在完成Kalman滤波计算后，从滤波状态中抽取速度误差和姿态误差，导航参数的修正按照下式执行：

$\hat{P}=P-\mathrm{\δP}$

$\hat{v}=v-\mathrm{\δv}---\left(18\right)$

${\hat{C}}_b^n=C_b^n[I+\mathrm{\δ\ψ}\×]$

δψ×为δψ的反对称矩阵；δP按照下式计算：

$\mathrm{\δP}=\frac{t}{2}{\left[\begin{array}{ccc}\frac{\mathrm{\δ}{v}_n}{R+h}& \frac{\mathrm{\δ}{v}_e}{(R+h)\cos L}& \mathrm{\δ}{v}_d\end{array}\right]}^T---\left(19\right)$

然后令***状态中的δv和δψ清零，即

δv＝δψ＝[0 0 0]^T(20)

这里，t表示上一个对地静止时刻到当前时刻的时间，I表示3*3单位矩阵。