一种基于相对安装位置关系的多惯导协同导航方法及装置与流程

1.本发明涉及舰艇导航应用领域，具体涉及一种基于相对安装位置关系的多惯导协同导航方法及装置。


背景技术：

2.惯性导航系统具有独立自主、导航信息完备、隐蔽性强等优点，是舰艇导航领域必不可少的导航手段。然而，受制于惯导工作机理，其定位误差随时间累积，需要外部辅助信息进行修正。对于大型舰艇而言，为了保证导航系统的可靠性，通常冗余配备多套惯导系统，选择其中一套惯导作为主用惯导，其他作为备用惯导。
3.鉴于多套惯导的安装方式近似为刚性连接，主用惯导与备用惯导间的相对安装位置信息关系稳定、可靠，且可以事先精确测量。在无外部辅助信息条件下，利用多惯导间的相对安装位置关系，以此构建惯导间导航参数的联系，实现多惯导协同导航，实现主用惯导与备用惯导精度的整体提升。


技术实现要素：

4.有鉴于此，本发明提供了一种基于相对安装位置关系的多惯导协同导航方法及装置，针对惯导定位误差随时间累积的问题，结合舰艇冗余配置多套惯导系统的特点，在无外部辅助信息条件下实现导航精度提升。
5.为实现本发明的目的，本发明所采用的技术方案是：
6.一种基于相对安装位置关系的多惯导协同导航方法，包括以下步骤：
7.步骤s1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
8.步骤s2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
9.步骤s3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
10.优选地，所述步骤s1，包括：
11.步骤s11：通过测量各惯导的相对距离δr
12
，构建所述主用惯导与所述备用惯导位置间的数学关系，则有
[0012][0013]
式中，
[0014]
[0015][0016]
和分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，l1、λ1和l2、λ1分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δl1、δλ1和δl2、δλ2分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；rn、re分别表示地球子午圈、卯酉圈曲率半径；h＝(h1+h2)/2表示惯导输出载体的平均高度信息；表示惯导输出载体的平均纬度信息，h1、h2分别表示所述主用惯导、所述备用惯导输出载体的高度。本实施例中，使用长度测量工具获取距离信息；
[0017]
步骤s12：使用标校装置进行安装标校，使得构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有
[0018][0019]
式中，
[0020][0021][0022]
b1和b2分别表示所述主用惯导和所述备用惯导的载体坐标系；i表示单位阵，表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；为所述主用惯导载体坐标系b1相对导航坐标系n的计算姿态矩阵，为所述备用惯导载体坐标系b2相对导航坐标系n的计算姿态矩阵，φ
1n
为所述主用惯导的姿态失准角,为所述备用惯导的姿态失准角，为所述主用惯导载体坐标系b1相对导航坐标系n的理想姿态矩阵，为所述备用惯导载体坐标系b2相对导航坐标系n的理想姿态矩阵。
[0023]
优选地，所述步骤s2，包括：
[0024]
步骤s21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量x表示如下：
[0025][0026]
式中，φ
in
表示姿态失准角，δv
in
表示速度误差，表示位置误差，δhi为高度误差，表示陀螺常值误差，表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2；
[0027]
步骤s22：构建协同导航系统的量测方程：
[0028]
令
[0029]
则有
[0030][0031]
其中，δθ
12
为构建的所述主用惯导与所述备用惯导间姿态误差角向量，δθ
x
、δθy、δθz分别为姿态误差角向量δθ
12
三个元素；
[0032]
获得协同导航系统的量测方程：
[0033]
z＝[δθ
x δθ
y δθ
z δr
12
]
t
＝h(x)+v
ꢀꢀꢀ
(9)
[0034]
式中，z为量测变量，v为量测噪声向量；h(x)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取。
[0035]
优选地，所述步骤s3，包括：
[0036]
步骤s31：基于所述协同导航系统的误差传播方程，构建扩展卡尔曼滤波的状态方程
[0037][0038]
其中，f为状态转移矩阵，g为协同导航系统的干扰矩阵，w为协同导航系统的随机干扰向量；
[0039]
步骤s32：获取离散化的扩展卡尔曼滤波状态方程和量测方程：
[0040][0041]
式中，xk为k时刻的状态估计，x
k-1
为k-1时刻的状态估计，zk为k时刻的观测值，φ
k,k-1
为非奇异状态一步转移矩阵，γ
k,k-1
为所述协同导航系统过程噪声输入矩阵，h(xk)为xk的量测函数，w
k-1
是所述协同导航系统随机过程噪声序列，vk是协同导航系统随机量测噪声序列；
[0042]
步骤s33：给定初始值和p0，根据k时刻的观测值zk，实时递推计算得到k时刻的状态估计具体求解过程如下：
[0043][0044]
式中，和p0分别表示一步预测状态估计和估计误差方差阵的初始值，为k-1
时刻的状态估计，和p
k,k-1
分别表示一步预测的状态估计和估计误差方差阵；kk表示滤波增益矩阵；为量测矩阵；为的量测函数，rk为系统量测噪声的方差矩阵，p
k-1
为k-1的估计误差方差阵，q
k-1
为协同导航系统过程噪声的方差矩阵；
[0045]
步骤s34：基于k时刻的状态估计修正所述主用惯导与所述备用惯导输出的导航参数，生成导航信息。
[0046]
一种基于相对安装位置关系的多惯导协同导航装置，包括：
[0047]
关系获取模块：配置为基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
[0048]
量测方式构建模块：配置为获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
[0049]
导航信息生成模块：配置为对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
[0050]
一种计算机可读存储介质，所述存储介质中存储有多条指令；所述多条指令，用于由处理器加载并执行如前所述方法。
[0051]
一种电子设备，所述电子设备，包括：
[0052]
处理器，用于执行多条指令；
[0053]
存储器，用于存储多条指令；
[0054]
其中，所述多条指令，用于由所述存储器存储，并由所述处理器加载并执行如前所述方法。
[0055]
有益效果：与现有技术相比，本发明的技术方案具有以下有益的技术效果：
[0056]
1、本发明提出了一种基于相对安装位置关系的多惯导协同导航方法，该方法仅仅只需利用多套惯导间的相对安装位置关系，不需要额外的外部辅助导航传感器或信息源即可实现协同导航，方法简便可靠。
[0057]
2、本发明使用的量测信息为多惯导之间的相对安装位置信息，相比与传统的多平台协同导航，多惯导间的相对姿态和相对距离信息更精确可靠、更新频率更高、信息源更丰富、且不需要实时测量。
[0058]
3、本发明提出的基于相对安装位置关系的多惯导协同导航系统能够有效地实现导航系统定位精度的提升。
附图说明
[0059]
图1为根据本发明实施例的基于相对安装位置关系的多惯导协同导航方法流程示意图。
[0060]
图2为根据本发明实施例的两套惯导系统的协同导航模型图。
[0061]
图3为根据本发明实施例的基于相对安装位置关系的多惯导协同导航装置结构示意图。
具体实施方式
[0062]
下面结合附图和实施例，对本发明的技术方案作进一步说明。
[0063]
本发明提出一种基于相对安装位置关系的多惯导协同导航方法，该方法在不额外增加外部辅助导航传感器或信息源的条件下，仅仅只需基于两套惯导之间的相对安装位置关系，利用两套惯导之间的相对姿态和相对距离信息去抑制惯导系统误差发散，实现两套惯导的有机融合，有效提高导航系统精度和性能。如图1-图2所示，本发明所述的一种基于相对安装位置关系的多惯导协同导航方法，包括以下步骤：
[0064]
步骤s1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
[0065]
步骤s2：获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
[0066]
步骤s3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
[0067]
所述步骤s1：基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系，包括：
[0068]
步骤s11：通过测量各惯导的相对距离δr
12
，构建所述主用惯导与所述备用惯导位置间的数学关系，则有
[0069][0070]
式中，
[0071][0072][0073]
和分别表示所述主用惯导和所述备用惯导实际输出的纬度、经度信息，l1、λ1和l2、λ1分别表示所述主用惯导和所述备用惯导理想或预期输出的纬度、经度信息，δl1、δλ1和δl2、δλ2分别表示所述主用惯导和所述备用惯导的纬度误差、经度误差；rn、re分别表示地球子午圈、卯酉圈曲率半径；h＝(h1+h2)/2表示惯导输出载体的平均高度信息；表示惯导输出载体的平均纬度信息，h1、h2分别表示所述主用惯导、所述备用惯导输出载体的高度。本实施例中，使用长度测量工具获取距离信息。
[0074]
步骤s12：使用标校装置进行安装标校，使得构建所述主用惯导的姿态矩阵和所述备用惯导的姿态矩阵之间的数学关系，则有
[0075][0076]
式中，
[0077][0078][0079]
b1和b2分别表示所述主用惯导和所述备用惯导的载体坐标系；i表示单位阵，表示所述主用惯导载体坐标系相对所述备用惯导载体坐标系的姿态矩阵；为所述主用惯导载体坐标系b1相对导航坐标系n的计算姿态矩阵，为所述备用惯导载体坐标系b2相对导航坐标系n的计算姿态矩阵，φ
1n
为所述主用惯导的姿态失准角,为所述备用惯导的姿态失准角，为所述主用惯导载体坐标系b1相对导航坐标系n的理想姿态矩阵，为所述备用惯导载体坐标系b2相对导航坐标系n的理想姿态矩阵。
[0080]
为了保证空间基准的一致性，使用标校装置进行安装标校。
[0081]
所述步骤s2：基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程，包括：
[0082]
步骤s21：选取状态变量，所述状态变量包括所述主用惯导及所述备用惯导的姿态误差、速度误差、位置误差、陀螺常值误差和加速度计常值误差，构建的状态变量x表示如下：
[0083][0084]
式中，φ
in
表示姿态失准角，δv
in
表示速度误差，表示位置误差，δhi为高度误差，表示陀螺常值误差，表示加速度计常值误差，1≤i≤num，num为所述主用惯导数量与所述备用惯导数量的和，num＝2。
[0085]
本实施例中，由所述多个惯导组成协同导航系统，构建的状态变量x是30维的，num＝2。
[0086]
步骤s22：构建协同导航系统的量测方程：
[0087]
令
[0088]
则有
[0089][0090]
其中，δθ
12
为构建的所述主用惯导与所述备用惯导间姿态误差角向量，δθ
x
、δθy、δθz分别为姿态误差角向量δθ
12
三个元素，可以通过矩阵c计算获得。
[0091]
基于式(1)和式(8)可以获得协同导航系统的量测方程：
[0092]
z＝[δθ
x δθ
y δθ
z δr
12
]
t
＝h(x)+v
ꢀꢀꢀ
(9)
[0093]
式中，z为量测变量，v为量测噪声向量；h(x)为量测函数，基于所述主用惯导与所述备用惯导的导航参数之间的数学关系获取，具体表达式由式(1)和式(8)获取。本实施例的目的在于选取合适的量测变量z，获取协同导航系统的量测方程。
[0094]
所述步骤s3：对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息，包括：
[0095]
步骤s31：基于所述协同导航系统的误差传播方程，构建扩展卡尔曼滤波的状态方程
[0096][0097]
其中，f为状态转移矩阵，g为协同导航系统的干扰矩阵，w为协同导航系统的随机干扰向量。
[0098]
步骤s32：获取离散化的扩展卡尔曼滤波状态方程和量测方程：
[0099][0100]
式中，xk为k时刻的状态估计，x
k-1
为k-1时刻的状态估计，zk为k时刻的观测值，φ
k,k-1
为非奇异状态一步转移矩阵，γ
k,k-1
为所述协同导航系统过程噪声输入矩阵，h(xk)为xk的量测函数，w
k-1
是所述协同导航系统随机过程噪声序列，vk是协同导航系统随机量测噪声序列。本实施例中，通过离散化处理式(10)和式(9)，得到离散化的扩展卡尔曼滤波状态方程和量测方程。
[0101]
步骤s33：给定初始值和p0，根据k时刻的观测值zk，实时递推计算得到k时刻的状态估计具体求解过程如下：
[0102][0103]
式中，和p0分别表示一步预测状态估计和估计误差方差阵的初始值，为k-1时刻的状态估计，和p
k,k-1
分别表示一步预测的状态估计和估计误差方差阵；kk表示滤波增益矩阵；为量测矩阵；为的量测函数，rk为协同导航系统量测噪声的方差矩阵，p
k-1
为k-1的估计误差方差阵，q
k-1
为协同导航系统过程噪声的方差矩阵。
[0104]
步骤s34：基于k时刻的状态估计修正所述主用惯导与所述备用惯导输出的导
航参数，生成导航信息。从而实现抑制惯导误差发散的目的。
[0105]
进一步地，修正所述主用惯导与所述备用惯导中输出的导航参数的方式为：
[0106]
利用估计的欧拉失准角去修正所述主用惯导与所述备用惯导输出的姿态矩阵，具体方法如下：
[0107][0108][0109]
利用估计的水平位置误差修正所述主用惯导与所述备用惯导输出的水平位置，具体方法如下：
[0110][0111][0112]
本发明还提供了一种基于相对安装位置关系的多惯导协同导航装置，如图3所示，该装置包括：
[0113]
关系获取模块：配置为基于多惯导系统中各惯导的相对安装位置测量，建立主用惯导与备用惯导的导航参数之间的数学关系；
[0114]
量测方式构建模块：配置为获取所述主用惯导与备用惯导之间的相对距离信息和相对姿态信息，基于所述主用惯导与备用惯导的导航参数之间的数学关系构建协同导航系统的量测方程；
[0115]
导航信息生成模块：配置为对所述多惯导系统的误差进行扩展卡尔曼滤波，构建扩展卡尔曼滤波状态方程；基于所述扩展卡尔曼滤波状态方程及所述协同导航系统的量测方程，生成导航信息。
[0116]
以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。