CN102175095A - 一种捷联惯性导航传递对准算法并行实现方法 - Google Patents

Abstract

本发明公开了一种捷联惯性导航传递对准算法并行实现方法。由一次装订模块、地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块、卡尔曼滤波模块、对准输出模块组成，一次装订模块接收首帧主惯导系统的数据信息，经补偿计算后作为子惯导系统初始对准导航解算的初值；地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块和卡尔曼滤波模块组成传递对准的精对准过程，循环执行直到达到设定循环次数为止；对准输出模块在精对准过程结束后，对子惯导系统的姿态信息进行一次性修正，并输出子惯导系统导航解算所需的姿态、速度和位置初值。本发明方法加快了捷联惯导传递对准算法的计算速率，提高了传递对准的对准精度。

Description

一种捷联惯性导航传递对准算法并行实现方法

技术领域

本发明属于捷联惯性导航领域，特别是涉及到一种捷联惯性导航系统传递对准算法的并行实现方法。

背景技术

为适应现代战争的需要，战术导弹已发展成为日益重要的中等规模打击武器。同时，随着战争的发展对战术导弹的反应速度和命中精度的要求也越来越高。战术导弹一般由运载体进行发射，一般采用惯性中制导和光、电末制导，在导弹发射前，弹载捷联惯导系统的初始化通常采用传递对准完成。机翼和飞机结构的挠曲变形及子惯导的安装误差使装订值与子惯导的真实姿态阵不一致，所引起的子惯导的失准角可达一度左右。因此，快速而准确地在运载体上对战术导弹惯导系统进行初始对准就成为战术导弹的一项关键技术。

提高传递对准过程中对准算法中导航解算和数字滤波的运算频率，可在设定时间内提高传递对准的精度，进而提升武器的打击精确度。传统的提高运算频率的方法是采用更高频的计算芯片。目前，多采用DSP作为主处理芯片，在DSP芯片中所有运算指令都是串行执行的，这样的特点使得传递对准算法计算频率难以得到大幅度的提高。

近年来以FPGA为代表的可编程逻辑器件技术取得了快速发展，高端FPGA器件不仅集成了丰富的可配置逻辑块资源，还包含大量的面向计算密集应用的DSP48(E)单元。就硬件而言，FPGA在并行计算领域具有不可比拟的优势。

将传统的串行捷联惯导传递对准算法进行并行化处理，并由FPGA器件实现，是一种提高捷联惯导传递对准算法运算频率的可行方案。应用FPGA的并行计算特性，将传递对准算法执行过程进行并行化处理，并使传递算法各模块按多个流程同时进行，可大大加快传递对准算法的计算速率，对捷联惯导传递对准精度的提高具有重大价值。

发明内容

为解决传统串行捷联惯导传递对准算法运算速率难以有效提高的问题，本发明提供了一种基于FPGA的捷联惯导传递对准并行实现方法，该方法将传递对准算法模块化，对各模块进行并行设计，并在单个FPGA上设计实现，大大加快了捷联惯导传递对准算法运算速率，提高了传递对准的精度。

本发明解决其技术问题所采用的技术方案是：一种捷联惯性导航传递对准算法并行实现方法，采用速度加姿态匹配算法，由一次装订模块、地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块、卡尔曼滤波模块和对准输出模块组成。

所述的一次装订模块根据本地存储的补偿四元数、臂杆矢量对接收的主惯导数据，对运载体姿态四元数和运载体对地速度进行补偿计算后，作为传递对准算法导航解算的初始值。一次装订模块包括了姿态装订模块和速度装订模块两个并行运行的子模块。

所述的地球相关参数解算模块根据主惯导传递的运载体位置、速度信息计算得到地球自转角速度、主惯导所在导航坐标系相对地球的旋转角速度和主惯导所在位置的重力加速度等信息。地球相关参数解算模块包括了地球自转角速度解算模块、导航坐标系对地角速度解算模块和当地重力加速度解算模块三个并行运行的子模块。

所述的子惯导系统导航解算模块根据子惯导提供的角速度和比力信号，以一次装订后的输出为初值进行导航解算，其中姿态解算算法采用四元数算法，速度解算算法采用单子样速度算法。子惯导系统导航解算模块包括了姿态四元数解算模块和速度解算模块两个并行运行的子模块。

所述的滤波参数计算模块计算卡尔曼滤波所需噪声分配矩阵、状态转移矩阵和匹配量后，将计算结果传递给卡尔曼滤波模块，进行一次卡尔曼滤波计算。滤波参数计算模块包括了速度差值计算模块、计算姿态误差角计算模块、噪声分配矩阵计算模块和状态转移矩阵计算模块四个并行运行的子模块。

所述的卡尔曼滤波模块包括了状态预测模块、估计协方差预测模块、卡尔曼增益计算模块、状态估计模块和协方差估计模块五个模块，其中状态预测模块、估计协方差预测模块根据滤波参数并行运行，运行结束后估计协方差预测模块将估计协方差预测值送至卡尔曼增益计算模块计算出卡尔曼增益和量测均方差，最后启动并行模块状态估计模块和协方差估计模块，得出状态估计值和协方差估计值。

所述的对准输出模块根据卡尔曼滤波估计得到的误差角对子惯导姿态四元数做一次修正，并结合补偿后的子惯导速度值和主惯导位置信息作为子惯导导航解算的初始值输出。

与现有技术相比，本发明的优点是：将传递对准串行算法的各模块划分成了若干并行执行的子模块，并采用FPGA实现进一步提高算法并行度，大大提高了捷联惯导传递对准的运算速率。如表1所示，以北向不对准角的估计为例，运算速率越快则对准精度越高，因此，提高运算速率可以提高了传递对准的精度，具有重要的意义。

表1计算频率与估计误差关系表(北向加速15s)

计算频率(Hz)	50	100	200	500	1000
						估计误差(mrad)	-2.08	-1.20	-0.68	-0.33	-0.19

附图说明

图1是本发明的算法流程图。

图2是本发明的一次装订模块并行设计原理图。

图3是本发明的地球相关参数解算模块并行设计原理图。

图4是本发明的子惯导系统导航解算模块并行设计原理图。

图5是本发明的滤波参数计算模块并行设计原理图。

图6是本发明的卡尔曼滤波模块并行设计原理图。

图7是本发明的对准输出模块并行设计原理图。

具体实施方式

公式符号说明如下：

h    载体所在处的海拔高度

l    载体所在处的纬度

g₀   赤道海平面的重力加速度大小

T     计算周期

Q_k    系统噪声矩阵

R_k    量测噪声矩阵

f_a/q()将姿态角装换成相应姿态四元数的函数

以下以当前动基座传递对准中较常用的速度加姿态匹配算法为例，具体说明本发明的并行实现方法。

本发明的传递对准算法流程图如图1所示，包括：一次装订模块、地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块、卡尔曼滤波模块和对准输出模块。该算法的总流程为：1)执行一次装订模块(M1)，对输入数据中的主惯导姿态四元数、主惯导速度进行一次补偿后，作为子惯导姿态解算和速度解算的初值；2)执行地球相关数据解算模块(M2)，根据输入数据中的主惯导位置和主惯导速度，解算出当前的地球自转角速度、导航坐标系对地角速度以及当地重力加速度；3)调用子惯导导航解算模块(M3)，解算子惯导姿态四元数和子惯导对地速度；4)调用滤波参数计算模块(M4)，计算卡尔曼滤波中用到的时变参数；5)调用卡尔曼滤波模块(M5)，根据传递的滤波参数进行一次卡尔曼滤波计算；6)判断滤波次数是否达到设定值N，如果未达到则继续执行步骤2到5的过程；7)调用对准输出模块(M6)，对子惯导姿态四元数进行一次修正，并输出子惯导导航解算所需的姿态、速度和位置初值。

本发明的一次装订模块并行设计原理图如图2所示，一次装订模块(M1)包括并行运算的姿态装订模块(M1_1)和速度装订模块(M1_2)。姿态装订模块(M1_1)根据输入数据中的主惯导姿态四元数q_nb和系统给定的补偿四元数q_comp，对子惯导姿态进行装订，具体如下式：

$q_{\mathrm{ns}0}=q_{\mathrm{nb}}\⊗q_{\mathrm{comp}}---\left(1\right)$

速度装订模块(M1_2)根据主惯导速度

主惯导角速度

以及臂杆矢量r^b，对子惯导速度进行装订，具体如下式：

$V_{s0}^n=V_m^n+C_b^n({\mathrm{\ω}}_{\mathrm{ib}}^b\×r^b)---\left(2\right)$

一次装订模块(M1)运算结束后，输出q_ns0、

作为子惯导导航解算顶得姿态和速度初值。

本发明的地球相关参数解算模块并行设计原理图如图3所示，地球相关参数解算模块(M2)包括并行运算的地球自转角速度解算模块(M2_1)、导航坐标系转动角速度解算模块(M2_2)和当地重力加速度解算模块(M2_3)。地球自转角速度解算模块(M2_1)根据输入数据中的主惯导位置P_m，解算当前时刻导航坐标系下地球自转角速度

导航坐标系转动角速度解算模块(M2_2)根据输入数据中的主惯导位置P_m和主惯导速度

解算导航坐标系下导航坐标系相对地球坐标系的转动角速度

当地重力加速度解算模块(M2_3)根据输入数据中的主惯导位置P_m，解算出gⁿ，具体如下式：

$g^n=[g_0(1+5.27094*{10}^{-3}{\sin }^{2}l+2.32718*{10}^{-5}{\sin }^{4}l)-3.086*{10}^{-6}{h}^{}]\left[\begin{array}{c}0\\ 0\\ -1\end{array}\right]---\left(3\right)$

本发明的子惯导系统导航解算模块并行设计原理图如图4所示，子惯导系统导航解算模块(M3)包括并行运算的姿态四元数解算模块(M3_1)和速度解算模块(M3_2)。姿态四元数解算模块(M3_1)根据地球自转角速度导航坐标系相对地球坐标系的转动角速度

和子惯导角速度

对子惯导的姿态四元数q_ns进行更新，具体如下式：

$q_{\mathrm{ns}}\left(t_{k+1}\right)=q_{\mathrm{ns}}\left(t_k\right)\⊗q\left(h\right)---\left(4a\right)$

$q\left(h\right)=\cos \frac{\mathrm{\Φ}}{2}+\frac{\mathrm{\Φ}}{\mathrm{\Φ}}\sin \frac{\mathrm{\Φ}}{2}---\left(4b\right)$

$\mathrm{\Φ}=[{\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)]*T---\left(4c\right)$

速度解算模块(M3_2)根据地球自转角速度导航坐标系相对地球坐标系的转动角速度

子惯导角速度子惯导比力加速度

子惯导的姿态四元数q_ns和当地重力加速度gⁿ对子惯导的速度

进行更新，具体如下式：

$V_s^n\left(t_{k+1}\right)=V_s^n\left(t_k\right)+C_b^n\mathrm{\Δ}{V}_{\mathrm{sfm}}+\mathrm{\Δ}{V}_{g/\mathrm{corm}}---\left(5a\right)$

$\mathrm{\Δ}{V}_{g/\mathrm{corm}}=[g^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V_s^n\left(t_k\right)]*T---\left(5b\right)$

ΔV_sfm＝ΔV_m+Δθ_m×ΔV_m                 (5c)

$\mathrm{\Δ}{\mathrm{\θ}}_m={\mathrm{\ω}}_{\mathrm{is}}^{s}T---\left(5d\right)$

$\mathrm{\Δ}{V}_m=f_{\mathrm{sf}}^{s}T---\left(5e\right)$

本发明的滤波参数计算模块并行设计原理图如图5所示，滤波参数计算模块(M4)包括并行运算的速度差值计算模块(M4_1)、计算姿态误差角计算模块(M4_2)、噪声分配矩阵计算模块(M4_3)和状态转移矩阵计算模块(M4_4)。速度差值计算模块(M4_1)首先计算补偿后的主惯导速度

再用子惯导速度减去

得到速度误差ΔV_c；计算姿态误差角计算模块(M4_2)，根据主惯导四元数q_nb和子惯导解算姿态q_ns计算得到子惯导计算载体坐标系到主惯导载体坐标系的欧拉角，计算姿态误差角

噪声分配矩阵计算模块(M4_3)根据子惯导解算姿态q_ns计算得到噪声分配矩阵Γ_k/k-1；状态转移矩阵计算模块(M4_4)根据子惯导解算姿态q_ns、子惯导角速度地球自转角速度

导航坐标系对地角速度

以及子惯导比力加速度

解算出状态转移矩阵Φ_k/k-1。

本发明的卡尔曼滤波模块并行设计原理图如图6所示，卡尔曼滤波模块(M5)包括状态预测模块(M5_1)、估计协方差预测模块(M5_2)、卡尔曼增益计算模块(M5_3)、状态估计模块(M5_4)和协方差估计模块(M5_5)。状态预测模块(M5_1)根据状态转移矩阵Φ_k/k-1和上一时刻的系统状态X_k得到系统状态预测值X_k/k-1，具体如下式：

X_k/k-1＝Φ_k/k-1X_k                             (6)估计协方差预测模块(M5_2)根据状态转移矩阵Φ_k/k-1、噪声分配矩阵Γ_k/k-1和上一时刻的估计协方差P_k得到估计协方差预测P_k/k-1，具体如下式：

$P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_k{\mathrm{\Φ}}_{k/k-1}^T+{\mathrm{\Γ}}_{k/k-1}{Q}_k{\mathrm{\Γ}}_{k/k-1}^T---\left(7\right)$

状态预测模块(M5_1)和估计协方差预测模块(M5_2)并行执行，完成后调用卡尔曼增益计算模块(M5_3)。卡尔曼增益计算模块(M5_3)根据估计协方差预测P_k/k-1计算出卡尔曼滤波增益K_k，具体如下式：

$P_{\mathrm{zz}}=H_k{P}_{k/k-1}{H}_k^T+R_k---\left(8a\right)$

$K_k=P_{k/k-1}{H}_k^T{P_{\mathrm{zz}}}^{-1}---\left(8b\right)$

状态估计模块(M5_4)根据计算姿态误差角

速度误差ΔV_c、系统状态预测值X_k/k-1和卡尔曼滤波增益K_k计算得到估计的系统状态X_k，具体如下式：

X_k＝X_k/k-1+K_k(z_k-H_kX_k/k-1)                 (9b)协方差估计模块(M5_5)根据量测预测均方差P_zz、卡尔曼滤波增益K_k和估计协方差预测P_k/k-1计算得到估计的系统协方差P_k，具体如下式：

P_k＝(I-K_kH_k)P_k/k-1                         (10)状态估计模块(M5_4)和协方差估计模块(M5_5)并行执行，执行完成后判断滤波次数是否已达到设定值N。

本发明的对准输出模块并行设计原理图如图7所示，对准输出模块(M6)包括并行执行的姿态四元数修正模块(M6_1)和速度、位置赋值模块(M6_2)。姿态四元数修正模块(M6_1)根据估计系统状态X_k中的估计误差角对子惯导计算四元数q_ns经行一次修正得到子惯导导航解算的姿态初值q_ns/c0，具体如下式：

速度、位置赋值模块(M6_2)将补偿后的主惯导速度

作为子惯导导航解算的速度初值

将主惯导位置Pos_m作为子惯导导航解算的位置初值Pos_s/c0。

Claims (5)

1.一种捷联惯性导航传递对准算法并行实现方法，由一次装订模块、地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块、卡尔曼滤波模块和对准输出模块组成，其特征在于：

所述的一次装订模块由并行运算的姿态装订模块和速度装订模块组成；

所述的地球相关参数解算模块由并行运算的地球自转角速度解算模块、导航坐标系对地角速度解算模块和当地重力加速度解算模块组成；

所述的子惯导系统导航解算模块由并行运算的姿态四元数解算模块和速度解算模块组成；

所述的滤波参数计算模块由并行运算的速度差值计算模块、计算姿态误差角计算模块、噪声分配矩阵计算模块和状态转移矩阵计算模块组成；

所述的卡尔曼滤波模块由状态预测模块、估计协方差预测模块、卡尔曼增益计算模块、状态估计模块和协方差估计模块组成；

所述的对准输出模块由并行运算的姿态四元数修正模块和速度、位置赋值模块组成；

所述的一次装订模块接收首帧主惯导系统的导航信息和陀螺仪信号、加速度计信号，经补偿计算后作为子惯导系统初始对准导航解算的初值；所述的地球相关参数解算模块、子惯导系统导航解算模块、滤波参数计算模块和卡尔曼滤波模块组成传递对准的精对准过程，精对准过程循环执行直到达到设定循环次数N为止；所述的对准输出模块在精对准过程结束后，对子惯导系统的姿态信息进行一次性修正，并输出子惯导系统导航解算所需的姿态、速度和位置初值。

2.根据权利要求1所述的捷联惯性导航传递对准算法并行实现方法，其特征在于：所述的地球相关参数解算模块根据主惯导传递的位置、速度信息计算出地球自转角速度、主惯导所在导航坐标系相对地球的旋转角速度和主惯导所在位置的重力加速度。

3.根据权利要求1所述的捷联惯性导航传递对准算法并行实现方法，其特征在于：所述的子惯导系统导航解算模块根据子惯导提供的角速度和比力信号，以一次装订后的输出为初值进行导航解算，包括姿态四元数解算和速度解算。

4.根据权利要求1所述的捷联惯性导航传递对准算法并行实现方法，其特征在于：所述的滤波参数计算模块计算卡尔曼滤波所需的噪声分配矩阵、状态转移矩阵和匹配参数后，将计算结果传递给卡尔曼滤波模块，进行一次卡尔曼滤波计算。

5.根据权利要求1所述的捷联惯性导航传递对准算法并行实现方法，其特征在于：所述的主惯导系统的导航信息包括运载体姿态四元数、运载体对地速度、运载体位置。

Images