基于惯性/里程计/气压高度陆用导航系统高度组合方法

技术领域

本发明属于陆用惯性导航技术领域，具体涉及一种基于惯性/里程计/气压 高度的陆用导航系统高度组合方法。

背景技术

现有陆用导航系统的高度通道通常采用惯性/卫星组合、惯性/气压高度组 合以及惯性/里程计组合等形式。惯性/卫星组合的高程精度高，但依赖于国外 GPS、GLOANSS等卫星导航系统，且易受电磁干扰以及地形遮挡等众多因素 的影响；惯性/气压高度组合的高程精度受限于气压高度表的高程精度；惯性 /里程计组合由于存在俯仰角误差，且俯仰角误差会随车辆载荷以及路面的起 伏有所变化，其高程误差随行驶里程累积。

发明内容

本发明要解决的技术问题是现有的陆用导航系统高度组合方法无法同时 满足高精度、抗电磁干扰、不受气压高度表高程精度及俯仰角误差限制的要 求。

本发明的技术方案如下所述：

一种基于惯性/里程计/气压高度的陆用导航系统高度组合方法，该方法以 气压高度与惯性/里程组合高度的差值为观测量，采用最小二乘法对惯性/里程 计高度组合误差模型中的参数进行估计，并利用估计的参数对惯性/里程高度 进行修正；具体包括如下步骤：

步骤1.建立惯性/里程计组合高度误差模型；步骤2.参数估计；步骤3.误 差补偿。

作为优选实施方式，步骤1中惯性/里程计组合高度误差模型为：

h_k＝h_k-1+ΔD_k×sinα_k

Δh_k＝k₁D_k，cosα+k₂D_k，sin2α

D_k，cosα＝D_k-1，cosα+ΔD_k×cosα_k

D_k，sin2α＝D_k-1，sin2α+ΔD_k×sin2α_k

式中：

h_k表示k时刻惯性/里程组合高度，单位：米；

ΔD_k表示k-1时刻至k时刻之间的里程增量，单位：米；

α_k表示k时刻的俯仰角，单位：弧度；

Δh_k表示k时刻惯性/里程组合高度的误差，单位：米；

D_k，cosα表示k时刻，里程计的余弦里程累积量，单位：米；

D_k，sin2α表示k时刻，里程计的正弦里程累积量，单位：米；

k₁，k₂表示待估计误差参数。

步骤2中通过下式进行参数估计：

$P_k=[P_{k-1}-P_{k-1}{H}_k^T{(I+H_k{P}_{k-1}{H}_k^T)}^{-1}{H}_k{P}_{k-1}]$

${\hat{X}}_k={\hat{X}}_{k-1}+P_k{H}_k^T(Z_k-H_k{\hat{X}}_{k-1})$

式中：

表示待估计参数，为[k₁ k₂]^T，X₀＝[0 0]^T；

P_k表示2×2维矩阵，$P_0=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right);$

H_k表示[D_k，cosαD_k，sin2α]；

Z_k表示观测量，即气压高度与惯性/里程组合高度之差；

以Z_k为自变量，以k₁、k₂为应变量，采用最小二乘法确定k₁、k₂。

步骤3中通过下式进行误差补偿：

${\hat{h}}_k=h_k+H_k{\hat{X}}_k$

式中：

h_k表示k时刻惯性/里程组合高度，单位：米；

表示修正后的高度，单位：米。

本发明的有益效果为：

本发明的基于惯性/里程计/气压高度的陆用导航系统高度组合方法通过 采用惯导、里程计和气压高度三种信息融合，实现不同车/路况变化下的高程 误差抑制，能够同时满足高精度、抗电磁干扰、不受气压高度表高程精度及 俯仰角误差限制的要求。

具体实施方式

下面结合实施例对本发明的基于惯性/里程计/气压高度的陆用导航系统 高度组合方法进行详细说明。

本发明的基于惯性/里程计/气压高度的陆用导航系统高度组合方法以气 压高度与惯性/里程组合高度的差值为观测量，采用最小二乘法对惯性/里程计 高度组合误差模型中的参数进行估计，并利用估计的参数对惯性/里程高度进 行修正。具体步骤如下：

步骤1.建立惯性/里程计组合高度误差模型

h_k＝h_k-1+ΔD_k×sinα_k                        (1)

Δh_k＝k₁D_k，cosα+k₂D_k，sin2α               (2)

D_k，cosα＝D_k-1，cosα+ΔD_k×cosα_k          (3)

D_k，sin2α＝D_k-1，sin2α+ΔD_k×sin2α_k       (4)

式中：

h_k表示k时刻惯性/里程组合高度，单位：米；

ΔD_k表示k-1时刻至k时刻之间的里程增量，单位：米；

α_k表示k时刻的俯仰角，单位：弧度；

Δh_k表示k时刻惯性/里程组合高度的误差，单位：米；

D_k，cosα表示k时刻，里程计的余弦里程累积量，单位：米；

D_k,sin2α表示k时刻，里程计的正弦里程累积量，单位：米；

k₁，k₂表示待估计误差参数。

步骤2.参数估计

$P_k=[P_{k-1}-P_{k-1}{H}_k^T{(I+H_k{P}_{k-1}{H}_k^T)}^{-1}{H}_k{P}_{k-1}]$$\left(5\right)$

${\hat{X}}_k={\hat{X}}_{k-1}+P_k{H}_k^T(Z_k-H_k{\hat{X}}_{k-1})$

式中：

表示待估计参数，为[k₁ k₂]^T，X₀＝[0 0]^T；

P_k表示2×2维矩阵，$P_0=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right);$

H_k表示[D_k，cosα D_k，sin2α]；

Z_k表示观测量，即气压高度与惯性/里程组合高度之差。

以Z_k为自变量，以k₁、k₂为应变量，采用最小二乘法确定k₁、k₂

步骤3.误差补偿

${\hat{h}}_k=h_k+H_k{\hat{X}}_k---\left(6\right)$

式中：

h_k表示k时刻惯性/里程组合高度，单位：米；

表示修正后的高度，单位：米。

实施例1

首先进行参数初始化：X₀＝[0 0]^T，$P_0=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right).$

进而进行参数估计：

设1s时，D_0，cosα＝0，D_0，sin2α＝0，由里程计测量得到ΔD₁＝1.194，由惯导测 量得到α₁＝-28.248°，由惯性/里程计组合高度误差模型中式(3)计算得到的 D_1，cosα为1.1518m，由式(4)计算得到的D_1，sin2α为-0.8999m，气压高度与惯性/里 程组合高度之差Z₁为0.0978m，获取气压高度及惯性/里程计组合高度的方法 为本领域技术人员公知常识，则H₁＝[1.1518-0.8999]，Z₁＝[0.9078]。

第1s时，根据式(5)计算可得：

$P_1=[P_0-P_0{H}_1^T{(I+H_1{P}_0{H}_1^T)}^{-1}{H}_1{P}_0]=\left(\begin{array}{cc}0.577& 0.3305\\ 0.3305& 0.7418\end{array}\right)$

${\hat{X}}_1={\hat{X}}_0+P_1{H}_1^T(Z_1-H_1{\hat{X}}_0)=\left(\begin{array}{c}0.3334\\ -0.2604\end{array}\right)$

即1s时k₁＝0.3334，k₂＝-0.2604。

采用最小二乘法进行参数递推估计时，参数估计值随观测数据的增多而 趋于稳态，估计精度随之提高。

设499s时，根据前一拍数据计算得到的各参数为

X₄₉₉＝[0.0004-0.7315]^T，$P_{499}=\left(\begin{array}{cc}0& 0\\ 0& 0.001\end{array}\right),$H₄₉₉＝[3000.0367.706]。

500s时，由里程计测量得到ΔD₅₀₀＝2.3，由惯导测量得到α₅₀₀＝10.1°，由 式(3)和式(4)计算值为H₅₀₀＝[3002.38.5]；根据500s时刻的气压高度及惯性/ 里程组合高度，可以得到二者之差为Z₅₀₀＝[-3.5922]。

则500s时，根据式(5)各参数估计值如下：

$P_{500}=[P_{499}-P_{499}{H}_{500}^T{(I+H_{500}{P}_{499}{H}_{500}^T)}^{-1}{H}_{500}{P}_{499}]=\left(\begin{array}{cc}0& 0\\ 0& 0.001\end{array}\right),$

${\hat{X}}_{500}={\hat{X}}_{499}+P_{500}{H}_{500}^T(Z_{500}-H_{500}{\hat{X}}_{499})=\left(\begin{array}{c}0.0004\\ -0.728\end{array}\right)$

即500s时k₁＝0.0004，k₂＝-0.728。

最后进行误差补偿：

设500s时，惯性/里程组合高度为h₅₀₀＝32.6941，则根据式(6)利用估计出 的参数补偿后的高度为${\hat{h}}_{500}=h_{500}+H_{500}{\hat{X}}_{500}=37.6221.$

本实施例中，利用被普遍认为精度较高的GPS的高度测量值为38m，气 压高度表的测量值为40.1m，惯性/里程计组合高度为42.1m，由此可知，本 发明专利解算出的组合高度相对于气压高度表和惯性/里程计组合方法要更 接近于GPS高度的测量值，提高了测量精度；且其较GPS系统更能抵抗电磁 干扰；气压高度和惯性/里程计组合高度的结合使本方法能够不受气压高度表 高程精度及俯仰角误差限制的要求。