CN106052688A - 基于地形轮廓匹配的惯性导航系统速度累积误差修正方法 - Google Patents

Abstract

本发明涉及基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，主要解决现有地形轮廓匹配方法不能修正惯性导航系统速度误差的问题。其实现步骤是：1)根据惯性导航系统指示在地形高程图中找到航行器的位置；2)根据航行器的位置，获取实测高程序列的起始坐标；3)根据实测高程序列的起始坐标，获取实测地形高程序列，并构造地形匹配起始点搜索区；4)根据地形匹配起始点搜索区的范围读取搜索区高程序列，计算搜索区各高程序列与实测高程序列的平方差，得到地形匹配位置坐标；6)根据实测高程序列起始坐标和地形匹配位置坐标对惯性导航系统进行速度修正。本发明减小了惯性导航系统的速度误差和地形匹配位置误差，可用于地形匹配辅助导航。

Description

基于地形轮廓匹配的惯性导航系统速度累积误差修正方法

技术领域

本发明属于数据处理技术领域，特别涉及一种惯性导航系统速度累积误差修正方法，可用于组合导航。

背景技术

随着科学技术的飞速发展，国内外已先后研制出了惯性导航系统和GPS全球定位导航系统等。GPS全球定位导航系统是一种高精度全球三维实时卫星导航系统，具有精度高，可用性好的优点，但也存在动态环境中可靠性差，导航定位是非自主式，且GPS技术受美国控制，因此关键技术受制于他人；惯性导航系统INS，具有自主性、隐蔽性、抗干扰性强，短时间内导航定位精度高，实时连续的提供导航信息等优点，但也存在导航位置和速度信息随着时间累积而发生漂移产生较大误差的缺点。由于惯性导航系统定位误差随时间积累，必须进行补偿和修正，解决这个问题的主要途径之一是发展组合导航技术。因此出现了地形辅助导航TAN技术，它被用来修正惯性导航系统的累积误差。

所谓地形辅助导航，实质上是由惯性导航系统、雷达高度表、气压高度表和数字地图构成的组合导航系统，该系统的最大特征就是能够修正惯性导航系统的导航信息，消除惯性导航系统的累积误差，提高导航精度。

现有的地形辅助导航方法是地形轮廓匹配TERCOM辅助导航方法。地形轮廓匹配方法是通过控制航行器航行的速度和时间，间隔的进行地形高程采样，从而获得地形高程值的实测数据。通过将实测高程序列与机载数字地形高程图上的高程序列进行相关计算，获得当前地形匹配位置信息。

上述这种形轮廓匹配辅助导航方法，所说在速度累积误差较小的情况下可以消除惯性导航系统的位置误差，但对于惯性导航系统的速度累积误差无法进行修正，导致航行偏移。这是因为在惯性导航系统工作的初期，由于惯性导航系统工作时间较短，惯性导航系统的速度累积误差较小，对现有的地形轮廓匹配辅助导航方法影响较小；随着惯性导航系统工作时间的增加，惯性导航系统的速度产生较大的累积误差，严重的影响了地形轮廓匹配辅助导航的性能，最终导致辅助导航系统产生较大的定位导航误差，产生航行偏移。

发明内容

本发明的目的在于针对现有的形轮廓匹配辅助导航方法的不足，提出一种基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，以减小惯性导航系统速度累积误差，提高了地形轮廓匹配的性能和稳定性，避免航行偏移。

本发明的技术关键是：利用地形轮廓匹配技术和卡尔曼滤波器技术，对惯性导航系统的速度累积误差进行修正，其实现步骤如下：

(1)读取当前机载惯性导航系统指示的航行器位置坐标，根据此位置坐标，在机载数字地形高程图中找到当前航行器的位置；

(2)判断航行器是否处在地形匹配区域中：

若不在地形匹配区域中，则返回(1)；

若处在地形匹配区域中，则调整航行器的航行姿态和速度v进行匀速直线航行，并把当前惯性导航系统指示位置的坐标作为实测高程序列的起始坐标；

(3)根据实测高程序列的起始坐标，获取实测地形高程序列；

(4)根据实测高程序列的起始坐标，构造地形匹配起始点搜索区；

(5)判断地形起始点匹配搜索区是否完全在地形匹配区域中：

若不完全在该地形匹配区域中，则不能进行本次地形匹配；

若完全在地形匹配区域中，则执行(6)；

(6)分别计算搜索区各高程序列和实测高程序列的平方差值，并比较各平方差值，得到地形匹配位置坐标；

(7)根据实测高程序列起始坐标和地形匹配位置坐标对惯性导航系统的速度进行修正：

7a)计算k时刻实测高程序列起始点位置坐标和k时刻地形匹配位置坐标的差值矩阵Z_k：

$Z_k=\left[\begin{array}{c}{x}_{INS}^k-x_{TER}^k\\ y_{INS}^k-y_{TER}^k\end{array}\right]$

其中，k为大于0的自然数，为k时刻实测高程序列起始点位置的东向坐标值，为k时刻实测高程序列起始点位置的北向坐标值，为k时刻地形匹配位置的东向坐标值，为k时刻地形匹配位置的北向坐标值；

7b)将差值矩阵Z_k作为卡尔曼滤波器的测量值输入给卡尔曼滤波器，并输入卡尔曼滤波器均方误差矩阵初始值p₀和卡尔曼滤波器状态估计值初始值进行卡尔曼滤波递推计算，输出卡尔曼滤波状态估计值

7c)根据卡尔曼滤波器输出滤波速度状态估计值对惯性导航系统的速度进行修正，得到修正后的速度

$\hat{v}=v+{\hat{x}}_k,$

其中，v为前时刻惯性导航系统的速度,(·)^T表示矩阵的转置。

本发明与现有技术相比具有如下优点：

本发明方法利用卡尔曼滤波器对惯性导航系统速度累积误差进行了修正，较好的抑制了由速度累积误差引起的地形匹配位置误差，从而提高了现有的地形轮廓匹配辅助导航方法的性能和稳定性。

附图说明

下面结合附图和具体实施方式对本发明做进一步详细说明。

图1是本发明的实现流程图；

图2是分别使用现有地形轮廓匹配方法和本发明方法进行地形匹配后的航迹对比图；

图3是分别使用现有地形轮廓匹配方法和本发明方法的速度变化曲线对比图。

具体实施方式

参照图1，本发明的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，实现步骤如下：

步骤1，读取当前惯性导航系统指示航行器所处位置的坐标，并确定该坐标在机载数字高程图中的位置。

1a)在航行器航行的过程中实时的读取当前机载惯性导航系统指示航行器所处位置的坐标；

1b)根据机载惯性导航系统指示的航行器位置坐标，在机载数字地形高程图中找到航行器的位置。

步骤2，获取实测高程序列的起始坐标。

根据步骤1在机载数字地形高程图中找到航行器所处位置的坐标，判断航行器是否在地形匹配区域中：

若不在地形匹配区域中，则返回步骤1；

若处在地形匹配区域中，则调整航行器的航行姿态和速度v，使其进行匀速直线航行，并把当前惯性导航系统指示位置的坐标作为实测高程序列的起始坐标。

步骤3，根据实测高程序列的起始坐标，获取实测地形高程序列。

3a)根据当前航行速度v和机载数字地形高程图网格距离d，计算高度采样时间间隔Δt：

$\mathrm{\Δ}t=\frac{d}{v}$

3b)在航行器航行过程中，以实测高程序列的起始坐标为起始点，以Δt为时间间隔，依次读取i次雷达高度表实测航行器的离地高度组成离地高度序列H_a＝[r₁,…,r_m,…r_i]，同时依次读取i次气压高度表实测航行器的海拔高度组成海拔高度序列H_b＝[l₁,…,l_m,…l_i]；

其中r_m为第m个离地高度测量值，l_m为第m个海拔高度测量值，m为整数且1<m≤i，H_a和H_b分别为1×i维的矩阵；

3c)根据雷达高度表实测航行器的离地高度序列H_a和气压高度表实测航行器的海拔高度序列H_b计算航行器航线下方实测高程序列ΔH：

ΔH＝H_b-H_a＝[ΔH₁,…,ΔH_m,…ΔH_i]，

其中ΔH_m为第m个地形高度测量值，m为整数且1<m≤i，i为实测高程序列矩阵中元素的总数，ΔH为1×i维的矩阵。

步骤4，根据实测高程序列的起始坐标，构造地形匹配起始点搜索区。

4a)根据惯性导航系统的性能设定当前惯性导航系统位置误差估计值矩阵σ：

σ＝[σ_x,σ_y]，

其中，σ_x和σ_y分别为在东北天导航坐标系中东向位置误差估计值和北向位置误差估计值；

4b)根据实测高程序列的起始坐标和东向位置误差估计值σ_x和北向位置误差估计值σ_y，构造匹配高程序列起始点的搜索区域，该匹配高程序列起始点的搜索区域是以实测高程序列的起始点坐标值为中心，其长为6σ_x，宽为6σ_y的矩形区域。

步骤5，判断地形起始点的匹配搜索区。

判断地形起始点匹配搜索区是否完全在地形匹配区域中：

若地形起始点匹配搜索区不完全在地形匹配区域中，则不能进行本次地形匹配；

若地形起始点匹配搜索区完全在地形匹配区域中，则执行步骤6。

步骤6，分别计算搜索区各高程序列和实测高程序列的平方差值，并比较各平方差值，得到地形匹配位置坐标。

6a)以匹配高程序列起始点搜索区域中的各网格坐标为匹配高程序列起始点的坐标，然后沿航行器的速度方向依次在机载数字地形高程图中读取N×i个地形高程值h_nm；

其中，h_nm为第n个匹配高程序列中第m个匹配地形高程值，n为整数且1<n≤N，N为匹配高程序列的总个数，m＝1,2,…,i，m为整数且1<m≤i，i为实测高程序列矩阵中元素的总数。

6b)计算各匹配高程序列和实测高程序列的平方差值矩阵j：

j＝[j₁,…,j_n,…,j_N]

其中，为第n个平方差值，ΔH_m为实测高程序列矩阵H中第m个地形高度测量值，n为整数且1<n≤N，N为匹配高程序列的总个数；

6c)比较平方差值矩阵j中各元素的大小，得出平方差值矩阵j中元素的最小值j_min，该j_min所对应匹配高程序列的起始点坐标即为地形匹配位置坐标。

步骤7，根据卡尔曼滤波器输出的速度误差估计值修正惯性导航系统的速度。

7a)构造卡尔曼滤波器的状态变量x：

x＝[δ_x,δ_y,δ_vx,δ_vy]^T

其中δ_x表示在导航坐标系中东向位置误差，δ_y表示在导航坐标系中北向位置误差，表示在导航坐标系中东向速度误差，表示在导航坐标系中北向速度误差，(·)^T表示矩阵的转置；

7b)构造卡尔曼滤波器的状态方程，其具体表达式如下：

x_k＝A_kx_k-1+ω_k-1

其中k表示时刻，取值为大于0的自然数，x_k为k时刻的状态变量，为k时刻的状态转换矩阵，dt为卡尔曼滤波器的滤波周期，x_k-1为k-1时刻的状态变量，ω_k-1为k-1时刻的系统动态噪声，且ω_k-1是一个4×1维的列向量；

7c)构造卡尔曼滤波器的测量方程：

Z_k＝c_kx_k+v_k

其中，Z_k为k时刻卡尔曼滤波器的输入差值矩阵，且Z_k是一个2×1维的矩阵，为k时刻的测量矩阵，v_k为k时刻的测量噪声，且v_k是一个2×1维的列向量；

7d)计算k时刻实测高程序列起始点位置坐标和k时刻地形匹配位置坐标的差值矩阵Z_k：

$Z_k=\left[\begin{array}{c}{x}_{INS}^k-x_{TER}^k\\ y_{INS}^k-y_{TER}^k\end{array}\right]$

其中，为k时刻实测高程序列起始点东向位置坐标，为k时刻实测高程序列起始点北向位置坐标，为k时刻地形匹配位置的东向坐标，为k时刻地形匹配位置的北向坐标；

7e)将差值矩阵Z_k作为卡尔曼滤波器的测量值输入给卡尔曼滤波器，并输入卡尔曼滤波器均方误差矩阵初始值p₀和卡尔曼滤波器状态估计值初始值通过如下递推滤波方程组进行卡尔曼滤波递推计算，得到输出卡尔曼滤波速度状态估计值

$\left\{\begin{array}{c}{p}_k^{\&prime;}=A_k{p}_{k-1}{A}_k^T+Q_{k-1}\\ H_k=p_k^{\&prime;}{c}_k^T(c_k{p}_k^{\&prime;}{c}_k^T+R_k)\\ p_k=(I-H_k{c}_k)p_k^{\&prime;}\\ {\hat{x}}_k=A_k{\hat{x}}_{k-1}+H_k(Z_k-c_k{A}_k{\hat{x}}_{k-1})\end{array}\right.$

其中，p′_k为k时刻未考虑噪声误差的卡尔曼滤波均方误差矩阵，A_k为k时刻的状态转换矩阵，p_k-1为k-1时刻卡尔曼滤波输出的均方误差矩阵，(·)^T表示矩阵的转置，Q_k-1为k-1时刻的系统动态噪声协方差矩阵，H_k为k时刻卡尔曼滤波最小均方误差条件下的增益矩阵，c_k为k时刻的测量矩阵，R_k为k时刻的测量噪声协方差矩阵，p_k为k时刻卡尔曼滤波输出的均方误差矩阵，I为单位矩阵，为k时刻卡尔曼滤波器输出的滤波状态估计值，为k-1时刻卡尔曼滤波器输出的滤波状态估计值；

7f)根据卡尔曼滤波器输出滤波速度状态估计值对惯性导航系统的速度进行修正，得到修正后的速度

$\hat{v}=v+{\hat{x}}_k,$

其中，v为前时刻惯性导航系统的速度,(·)^T表示矩阵的转置。

本发明的效果可通过以下仿真进一步验证。

1.实验场景

航行器的航行速度为50m/s。选取的地形匹配区域是一块40km×40km地形起伏明显的地域，数字高程图的网格距离为50m。惯性导航系统指示的东向和北向初始定位误差均为50m，指示的位置误差增长率东向和北向分量均为0.14m/s，指示的速度初始误差东向和北向分量均为1m/s，指示的速度误差增长率东向分量为0.06m/s、北向分量为0.04m/s。地形高度测量误差为零均值标准差为5m的高斯白噪声，卡尔曼滤波周期为16s，仿真航行时间为480s。

(2)实验内容与结果分析

实验1

根据实验场景中参数设置，使航行器在选定的地形匹配区域上空模拟航行480s。分别使用现有的地形轮廓匹配辅助导航方法和本发明的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法进行地形匹配，并且画出匹配定位后的航迹图，结果如图2所示。

从图2可以看出随着航行时间的增大现有的地形轮廓匹配辅助导航方法指示的航迹会较大幅度的偏离真实航迹，而本发明方法的指示航迹就较好的和真实航迹重叠。说明本发明方法的性能优于现有的地形轮廓匹配辅助导航方法，能更好抑制地形轮廓匹配辅助导航的匹配位置误差。

实验2

根据实验场景中参数设置，使航行器在选定的地形匹配区域上空模拟航行480s。分别使用现有的地形轮廓匹配辅助导航方法和本发明的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法进行地形匹配，并且画出航行过程中的速度变化曲线图，结果如图3所示。

从图3可以看出，在使用现有的地形轮廓匹配辅助导航方法的时候不能对速度累积误差进行修正，随着时间的增大惯性导航系统的速度误差越来越大，而本发明方法增加了速度误差修正能力，使得惯性导航系统的速度误差在航行过程中保持在较小的范围内，能较好的抑制惯性导航系统的速度误差。说明本发明方法在修正惯性导航系统速度误差方面性能优于现有的方法。

Claims (5)

1.基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，包括：

(1)读取当前机载惯性导航系统指示的航行器位置坐标，根据此位置坐标，在机载数字地形高程图中找到当前航行器的位置；

(2)判断航行器是否处在地形匹配区域中：

若不在地形匹配区域中，则返回(1)；

若处在地形匹配区域中，则调整航行器的航行姿态和速度v进行匀速直线航行，并把当前惯性导航系统指示位置的坐标作为实测高程序列的起始坐标；

(3)根据实测高程序列的起始坐标，获取实测地形高程序列；

(4)根据实测高程序列的起始坐标，构造地形匹配起始点搜索区；

(5)判断地形起始点匹配搜索区是否完全在地形匹配区域中：

若不完全在该地形匹配区域中，则不能进行本次地形匹配；

若完全在地形匹配区域中，则执行(6)；

(6)分别计算搜索区各高程序列和实测高程序列的平方差值，并比较各平方差值，得到地形匹配位置坐标；

(7)根据实测高程序列起始坐标和地形匹配位置坐标对惯性导航系统的速度进行修正：

7a)计算k时刻实测高程序列起始点位置坐标和k时刻地形匹配位置坐标的差值矩阵Z_k：

$Z_k=\left[\begin{array}{c}{x}_{INS}^k-x_{TER}^k\\ y_{INS}^k-y_{TER}^k\end{array}\right]$

其中，k为大于0的自然数，为k时刻实测高程序列起始点位置的东向坐标值，为k时刻实测高程序列起始点位置的北向坐标值，为k时刻地形匹配位置的东向坐标值，为k时刻地形匹配位置的北向坐标值；

7b)将差值矩阵Z_k作为卡尔曼滤波器的测量值输入给卡尔曼滤波器，并输入卡尔曼滤波器均方误差矩阵初始值p₀和卡尔曼滤波器状态估计值初始值进行卡尔曼滤波递推计算，输出卡尔曼滤波速度状态估计值

7c)根据卡尔曼滤波器输出滤波速度状态估计值对惯性导航系统的速度进行修正，得到修正后的速度

$\hat{v}=v+{\hat{x}}_k,$

其中，v为前时刻惯性导航系统的速度,(·)^T表示矩阵的转置。

2.根据权利要求1所述的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，其中步骤(3)中根据实测高程序列的起始坐标，获取实测地形高程序列，按如下步骤进行：

3a)根据当前航行速度v和机载数字地形高程图网格距离d，计算高度采样时间间隔Δt：

$\mathrm{\&Delta;}t=\frac{d}{v}$

3b)在航行器航行过程中，以实测高程序列的起始坐标为起始点，以Δt为时间间隔，依次读取i次雷达高度表实测航行器的离地高度组成离地高度序列H_a＝[r₁,…,r_m,…r_i]，同时依次读取i次气压高度表实测航行器的海拔高度组成海拔高度序列H_b＝[l₁,…,l_m,…l_i]；

其中r_m为第m个离地高度测量值，l_m为第m个海拔高度测量值，m为整数且1<m≤i，H_a和H_b分别为1×i维的矩阵；

3c)根据雷达高度表实测航行器的离地高度序列H_a和气压高度表实测航行器的海拔高度序列H_b计算航行器航线下方实测高程序列ΔH：

ΔH＝H_b-H_a＝[ΔH₁,…,ΔH_m,…ΔH_i]，

其中ΔH_m为第m个地形高度测量值，m为整数且1<m≤i，i为实测高程序列矩阵中元素的总数，ΔH为1×i维的矩阵。

3.根据权利要求1所述的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，其中步骤(4)中根据实测高程序列的起始坐标，构造地形匹配起始点搜索区，是根据实测高程序列的起始坐标和当前设定惯性导航系统的东向位置误差估计值σ_x和北向位置误差估计值σ_y，构造匹配高程序列起始点的搜索区域，该匹配高程序列起始点的搜索区域是以实测高程序列的起始点坐标值为中心，长为6σ_x，宽为6σ_y的矩形区域。

4.根据权利要求1所述的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，其中步骤(6)中分别计算搜索区各高程序列和实测高程序列的平方差值，并比较各平方差值，得到地形匹配位置坐标，按如下步骤进行：

6a)以匹配高程序列起始点搜索区域中的各网格坐标为匹配高程序列起始点的坐标，然后沿航行器的速度方向依次在机载数字地形高程图中读取N×i个地形高程值h_nm，其中，h_nm为第n个匹配高程序列中第m个匹配地形高程值，n为整数且1<n≤N，N为匹配高程序列的总个数，m＝1,2,…,i，m为整数且1<m≤i，i为实测高程序列矩阵中元素的总数；

6b)计算各匹配高程序列和实测高程序列的平方差值矩阵j：

j＝[j₁,…,j_n,…,j_N]

$j_n=\frac{1}{i}\underset{m=1}{\overset{i}{\&Sigma;}}|h_{nm}-{\mathrm{\&Delta;H}}_m{|}^2$

其中，j为1×N维矩阵，ΔH_m为实测高程序列ΔH中第m个地形高度测量值，j_n第n个平方差值，n为整数且1<n≤N，N为匹配高程序列的总个数；

6c)比较平方差值矩阵j中各元素的大小，得出平方差值矩阵j中元素的最小值j_min，则j_min所对应匹配高程序列的起始点坐标即为地形匹配位置坐标。

5.根据权利要求1所述的基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，其中步骤(7b)中进行卡尔曼滤波递推计算，通过如下递推滤波方程组进行：

$\left\{\begin{array}{c}{p}_k^{\&prime;}=A_k{p}_{k-1}{A}_k^T+Q_{k-1}\\ H_k=p_k^{\&prime;}{c}_k^T(c_k{p}_k^{\&prime;}{c}_k^T+R_k)\\ p_k=(I-H_k{c}_k)p_k^{\&prime;}\\ {\hat{x}}_k=A_k{\hat{x}}_{k-1}+H_k(Z_k-c_k{A}_k{\hat{x}}_{k-1})\end{array}\right.$

其中，p′_k为k时刻未考虑噪声误差的卡尔曼滤波均方误差矩阵，A_k为k时刻的状态转换矩阵，p_k-1为k-1时刻卡尔曼滤波输出的均方误差矩阵，(·)^T表示矩阵的转置，Q_k-1为k-1时刻的系统动态噪声协方差矩阵，H_k为k时刻卡尔曼滤波最小均方误差条件下的增益矩阵，c_k为k时刻的测量矩阵，R_k为k时刻的测量噪声协方差矩阵，p_k为k时刻卡尔曼滤波输出的均方误差矩阵，I为单位矩阵，为k时刻卡尔曼滤波器输出的滤波状态估计值，为k-1时刻卡尔曼滤波器输出的滤波状态估计值。