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

摘要

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

主权项

基于地形轮廓匹配的惯性导航系统速度累积误差修正方法，包括：(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表示矩阵的转置。