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

摘要

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

主权项

1.基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，包括：(1)读取当前机载惯性导航系统指示的航行器位置坐标，根据此位置坐标，在机载数字地形高程图中找到当前航行器的位置；(2)判断航行器是否处在地形匹配区域中：若不在地形匹配区域中，则返回(1)；若处在地形匹配区域中，则调整航行器的航行姿态和速度v进行匀速直线航行，并把当前惯性导航系统指示位置的坐标作为实测高程序列的起始坐标；(3)根据实测高程序列的起始坐标，获取实测地形高程序列：3a)根据当前航行速度v和机载数字地形高程图网格距离d，计算高度采样时间间隔Δt：3b)在航行器航行过程中，以实测高程序列的起始坐标为起始点，以Δt为时间间隔，依次读取i次雷达高度表实测航行器的离地高度组成离地高度序列同时依次读取i次气压高度表实测航行器的海拔高度组成海拔高度序列H_b＝[l₁,…,l_m,…l_i]；其中rm为第m个离地高度测量值，lm为第m个海拔高度测量值，m为整数且1＜m≤i，Ha和Hb分别为1×i维的矩阵；3c)根据雷达高度表实测航行器的离地高度序列Ha和气压高度表实测航行器的海拔高度序列Hb计算航行器航线下方实测高程序列ΔH：ΔH＝Hb‑Ha＝[ΔH1,…,ΔHm,…ΔHi]，其中ΔHm为第m个地形高度测量值，m为整数且1＜m≤i，i为实测高程序列矩阵中元素的总数，ΔH为1×i维的矩阵；(4)根据实测高程序列的起始坐标，构造地形匹配起始点搜索区；(5)判断地形起始点匹配搜索区是否完全在地形匹配区域中：若不完全在该地形匹配区域中，则不能进行本次地形匹配；若完全在地形匹配区域中，则执行(6)；(6)分别计算搜索区各高程序列和实测高程序列的平方差值，并比较各平方差值，得到地形匹配位置坐标：6a)以匹配高程序列起始点搜索区域中的各网格坐标为匹配高程序列起始点的坐标，然后沿航行器的速度方向依次在机载数字地形高程图中读取N×i个地形高程值hnm，其中，hnm为第n个匹配高程序列中第m个匹配地形高程值，n为整数且1＜n≤N，N为匹配高程序列的总个数，m＝1,2,…,i，m为整数且1＜m≤i，i为实测高程序列矩阵中元素的总数；6b)计算各匹配高程序列和实测高程序列的平方差值矩阵j：j＝[j1,…,jn,…,jN]其中，j为1×N维矩阵，ΔHm为实测高程序列ΔH中第m个地形高度测量值，jn为第n个平方差值，n为整数且1＜n≤N，N为匹配高程序列的总个数；6c)比较平方差值矩阵j中各元素的大小，得出平方差值矩阵j中元素的最小值jmin，则jmin所对应匹配高程序列的起始点坐标即为地形匹配位置坐标；(7)根据实测高程序列起始坐标和地形匹配位置坐标对惯性导航系统的速度进行修正：7a)计算k时刻实测高程序列起始点位置坐标和k时刻地形匹配位置坐标的差值矩阵Zk：其中，k为大于0的自然数，为k时刻实测高程序列起始点位置的东向坐标值，为k时刻实测高程序列起始点位置的北向坐标值，为k时刻地形匹配位置的东向坐标值，为k时刻地形匹配位置的北向坐标值；7b)将差值矩阵Z_k作为卡尔曼滤波器的测量值输入给卡尔曼滤波器，并输入卡尔曼滤波器均方误差矩阵初始值p₀和卡尔曼滤波器状态估计值初始值进行卡尔曼滤波递推计算，输出卡尔曼滤波速度状态估计值，其计算公式如下：其中，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时刻卡尔曼滤波器输出的滤波状态估计值；7c)根据卡尔曼滤波器输出滤波速度状态估计值对惯性导航系统的速度进行修正，得到修正后的速度其中，v为前时刻惯性导航系统的速度,(·)T表示矩阵的转置。