CN112857398A - 一种系泊状态下舰船的快速初始对准方法和装置 - Google Patents

Abstract

本发明实施例提供了一种系泊状态下舰船的快速初始对准方法和装置，主要步骤包括：获取舰船上的传感器的实时数据；建立初始对准参考坐标系；建立系泊状态下的改进非线性初始对准误差模型及其离散化；基于所述获取的舰船传感器实时数据进行导航解算，生成所述舰船的姿态和速度；然后采用四元数无味估计器进行滤波估计；利用所述滤波器估计的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成姿态欧拉角实时输出，直至对准过程结束。本发明优点在于考虑了矢量投影坐标系的一致性问题，将姿态四元数误差和速度误差统一投影至计算导航坐标系中，优化了误差模型，具有缩短对准时间，提升对准精度的特点。

Description

一种系泊状态下舰船的快速初始对准方法和装置

技术领域

本发明涉及舰船惯性导航技术领域，尤其涉及一种系泊状态下舰船的快速初始对准方法和装置。

背景技术

初始对准目的是为SINS提供准确的初始姿态等信息，对准的精度和快速性是影响SINS后续工作性能的两项重要指标。目前，对准方法主要可以归纳为以下两类：第一类是“粗对准+精对准”两阶段对准方法，经过粗对准获取粗略的姿态信息后，再进一步完成精对准。其中，粗对准方法主要为解析法和基于坐标系分解的凝固惯性系方法；精对准方法主要有卡尔曼滤波法、参数辨识法和罗经法。第二类是基于非线性误差模型的非线性对准方法，可以实现大失准角情形下的一步对准。目前，针对此类方法的主要研究有SINS非线性误差模型和非线性滤波方法两方面。非线性对准简化了对准过程，可以提升对准速度，但有研究发现其对准精度往往不如两阶段对准法。当前，此类对准的非线性滤波方法研究较多，非线性模型的区别多在姿态的表达形式上，而模型的准确性会对对准精度造成较大影响，传统的非线性模型只考虑了向量的大小差异，忽视了坐标系的一致性问题。

发明内容

本发明的实施例提供了一种系泊状态下舰船的快速初始对准方法和装置，能够提高初始对准的精度。

一种系泊状态下舰船的快速初始对准方法，包括：

步骤1，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

步骤2，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

步骤3，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

步骤4，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

一种系泊状态下舰船的快速初始对准装置，包括：

获取单元，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

第一建立单元，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

第二建立单元，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

处理单元，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

由上述本发明的实施例提供的技术方案可以看出，本发明实施例中，基于初始对准参考坐标系，建立非线性离散对准模型，由于考虑投影坐标系一致性的问题，因此提高了初始对准的精度。

本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的系泊状态下舰船的快速初始对准方法的示意图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

为便于对本发明实施例的理解，下面将结合附图以几个具体实施例为例做进一步的解释说明，且各个实施例并不构成对本发明实施例的限定。

如图1所述，为本发明所述的一种系泊状态下舰船的快速初始对准方法，包括：

步骤1，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

步骤2，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

步骤3，基于所述初始对准参考坐标系，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

步骤4，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

所述步骤2具体为：

i系-地心惯性坐标系，坐标系原点为地心，x轴指向赤道与黄道相交的春分点，z轴沿地球自转轴指向北极点，y轴与x轴和z轴构成右手坐标系，坐标系相对惯性空间保持不变；

e系-地球坐标系，坐标系原点为地心，x轴指向格林威治子午线，z轴沿地球自转轴指向北极点，y轴与x轴和z轴构成右手坐标系，坐标系跟随地球自转；

b系-载体坐标系，坐标系原点为载体重心，其x轴、y轴和z轴分别指向载体的右前上；

n系-导航坐标系，选用载体所在位置的当地地理坐标系，其x轴、y轴和z轴分别指向当地东向、北向和天向；

n′系-计算导航坐标系，表示存在误差的导航坐标系。

所述步骤3包括：

步骤31，基于所述初始对准参考坐标系，建立改进的SINS四元数姿态阻尼误差微分方程；

步骤32，基于所述初始对准参考坐标系，建立改进的SINS速度阻尼误差微分方程；

步骤33，基于所述改进的SINS四元数姿态阻尼误差微分方程和所述改进的SINS速度阻尼误差微分方程，以导航系解算速度和真实速度之间的误差为量测量，建立系泊状态下的改进的非线性初始对准误差模型。

所述步骤31具体为：

由姿态四元数微分方程知：

式中：

表示b系相对n系方向余弦矩阵

对应四元数的微分；

表示b系相对n系方向余弦矩阵

对应的四元数；

表示四元数乘法；

表示b系相对n系的角速度在b系中的投影；

表示三参量

对应的四元数；

表示b系中三轴陀螺仪无误差测量值，(g)^T表示转置，

表示n系相对i系角速度在n系中的投影；

表示地球自转角速度在n系中的投影，

表示n系相对e系角速度在n系中的投影；

表示三轴陀螺仪测量值，

表示三轴陀螺仪测量误差；

考虑误差影响，同时引入GNSS接收机提供的准确速度

和位置p_g阻尼信息，实际姿态四元数阻尼微分方程为：

式中：

表示b系相对n′系方向余弦矩阵

对应四元数的微分；

表示b系相对n′系方向余弦矩阵

对应的四元数；

表示带有误差的

表示三参量

对应的四元数；

表示三轴陀螺仪测量值，

表示由速度阻尼信息

和位置阻尼信息p_g获得的

表示由位置阻尼信息p_g获得的

表示由速度阻尼信息

和位置阻尼信息p_g获得的

由姿态乘性四元数误差可知：

式中：

表示n′系相对n系方向余弦矩阵

对应的姿态误差四元数；(g)^*表示共轭转置；

对等式两边同时微分，得到：

式中：

表示n′系相对n系方向余弦矩阵

对应的姿态误差四元数微分；

考虑n系为理想坐标系，实际解算是在n′系中进行，将误差形式统一在n′系下，生成改进的四元数姿态误差阻尼微分方程如下：

式中：

表示三参量

对应的四元数；

表示三参量

对应的四元数；

表示三参量

对应的四元数；

表示三参量

对应的四元数。

所述步骤32具体为：

由比力微分方程知：

式中：

表示n系速度的微分；

表示b系相对n系方向余弦矩阵；f^b表示比力；

表示加速度计测量值，δf^b表示加速度计测量误差；vⁿ表示n系速度；gⁿ表示重力加速度；(g×)表示反对称矩阵；

考虑误差影响，同时引入GNSS接收机提供的准确速度

和位置p_g阻尼信息，实际速度微分方程为：

式中：

表示n′系速度的微分；

表示b系相对n′系方向余弦矩阵；

表示由位置阻尼信息p_g获得的gⁿ；

考虑矢量方向性，定义速度误差如下：

式中：dv^n′表示速度误差；v^n′表示n′系速度；

表示n′系相对n系方向余弦矩阵；

对等式两边同时微分，得到：

式中：

表示速度误差微分；

表示n′系相对n系方向余弦矩阵的微分；

表示n′系相对n系的角速度在n′系中的投影；

生成改进的速度阻尼误差微分方程如下：

所述步骤33具体为：

系泊状态下，载体相对静止，线速度近似为零，位置视为常值，根据改进的所述SINS四元数姿态阻尼误差微分方程、改进的所述SINS速度阻尼误差微分方程和速度误差量测，建立基于改进四元数阻尼误差方程的非线性初始对准误差模型如下：

式中：

表示状态；f(g)表示非线性状态函数；G表示噪声驱动矩阵；w＝[w_g ^T,w_a ^T]^T表示陀螺仪和加速度计的测量白噪声；y＝dv^n′＝v^n′表示速度误差量测，H表示线性量测矩阵，v表示量测白噪声；

步骤34，将所述非线性初始对准误差模型离散化，形成非线性离散对准模型，具体为：

式中：w_k和v_k分别表示k时刻离散系统噪声和量测噪声，且

式中：E[g]表示均值，Q_k为离散系统噪声w_k的方差阵，R_k为离散量测噪声v_k的方差阵，δ_kj为克罗内克函数。f(x_k)和H_k分别为离散后的状态方程和量测矩阵。

所述步骤4具体为：

根据所述非线性离散模型，构建用于初始对准的四元数无味估计器估计模型的滤波算法，所述四元数无味估计器估计模型的滤波算法包括：初始化、导航解算、时间更新、量测更新、姿态更新和模型反馈校正；

其中，所述四元数无味估计器估计模型的滤波算法具体处理步骤如下：

步骤41，进行初始化，具体为：

定义四元数误差δq，将其转换成误差修正罗德里格斯参数δσ，重置状态为x＝[δσ^T,x^eT]^T，其中x^e表示非姿态误差状态；设置初始状态

和其均方差

状态噪声方差阵Q₀、量测噪声方差阵R₀以及SINS采样时间dt；设置姿态四元数初值

和速度初值

步骤42，进行导航解算，具体为：

导航解算更新公式如下：

式中，

表示k-1时刻b系相对n′系方向余弦矩阵

对应的四元数；

表示k-1时刻n′系速度；

和

分别表示k-1时刻陀螺仪测量值和加速度计测量值；

和

分别表示k-1时刻由位置阻尼信息p_g,k-1获得的

和gⁿ，

表示k时刻b系相对n′系方向余弦矩阵

对应的四元数，

表示k时刻n′系速度；

步骤43，进行时间更新，具体为：

产生状态sigma点，

式中：sigma(g,g)表示sigma点采样方程；

表示k-1时刻由状态

和均方差阵P_k-1生成的sigma点；

表示k-1时刻误差罗德里格斯参数形式的sigma点；

表示k-1时刻非姿态状态的sigma点；

式中：

表示k-1时刻

的四元数形式，||g||表示l₂范数；

式中：

表示k-1时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的估计值，

表示k-1时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的sigma点；

计算非线性状态函数传播的积分点：

式中：

表示选取的参考四元数；

表示由k-1时刻预测的四元数误差sigma点；

将

转换为修正罗德里格斯参数形式

状态一步预测：

式中：

表示sigma点权重；

表示k-1时刻预测的状态sigma点；

状态一步预测均方误差阵：

式中：

表示sigma点权重；Q_k-1表示k-1时刻状态噪声方差阵；

若未进入量测更新周期，直接进行如下状态和状态方差阵更新：

P_k＝P_k/k-1

步骤4、进行量测更新，具体为：

若量测更新周期到，进行量测更新；

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

P_k＝(I_n×n-K_kH_k)P_k/k-1

式中：K_k表示k-1时刻的滤波增益；y_k表示k时刻的量测输入；

和P_k分别对应k时刻的状态均值和方差阵；

步骤45，进行姿态更新，具体为：

经过时间更新和量测更新，得到k时刻状态

上式将

转换为

式中：

表示选取的参考四元数；

表示k时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的估计值；

步骤46，闭环反馈校正，具体为：

利用

和

分别对导航解算中更新的

和

进行反馈校正，将校正后的姿态四元数

转换成欧拉角输出，再将

置零，设置

进入下一滤波周期，直至对准结束。

本发明针对非线性误差对准模型进行改进，将误差统一定义在计算导航坐标系下，利用四元数无味估计器进行状态参数估计，应用于舰船系泊状态下的初始对准，可提高对准的快速性和精度。

本发明还提供一种系泊状态下舰船的快速初始对准装置，包括：

获取单元，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

第一建立单元，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

第二建立单元，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

处理单元，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

以下描述本发明的设计思想。

1基于四元数的SINS阻尼误差模型

1.1SINS状态微分方程

给出SINS姿态、速度和位置微分方程如下：

式中：

q＝[q₀,ρ^T]^T为四元数具体表达，q₀为标量，ρ＝[q₁,q₂,q₃]^T为三维矢量；p＝[L,λ,h]^T，其中L表示纬度、λ表示经度、h表示高度，R_P为当地曲率矩阵，R_M和R_N分别为子午圈与卯酉圈主曲率半径。

是基于四元数的方向余弦阵，其与

存在式(5)对应关系。

式中，I_3×3表示3维单位矩阵。

实际阻尼状态微分方程如下：

式中：

表示带有误差的位置，R_p,g表示由位置阻尼信息p_g表示的当地曲率矩阵。

1.2传统阻尼误差方程

传统的姿态四元数阻尼误差微分方程为：

将式(1)和式(6)代入上式，推导如下：

传统的速度误差定义为：

δv^n′＝v^n′-vⁿ (11)

将式(11)左右两边同时微分，并代入式(2)和式(7)，可得传统速度阻尼误差方程如下：

位置误差定义为：

位置阻尼误差微分方程如下。

1.3改进阻尼误差方程

基于SINS导航解算实际在n′系下进行这一基本认识，观察1.2节传统非线性阻尼误差方程推导过程，不难发现有以下两处不够严格：

(1)式(10)姿态误差方程右边

和

两项分别将

和

转换到n系进行解算，而非n′系；

(2)式(11)速度误差定义只是标量意义上的减法，忽略了矢量的方向性，未对坐标系进行统一。

本节考虑上述问题，将误差统一定义在n′系下，重新推导了改进非线性阻尼误差方程，其中，改进姿态误差方程如式(15)所示。

考虑矢量方向性，重新定义速度误差为：

将式上左右两边同时微分可得：

将式(2)、(6)代入式(17)，可得改进阻尼误差微分方程如下：

改进阻尼位置误差微分方程如式(19)所示。

2改进四元数阻尼模型正确性证明

本节从方向余弦矩阵姿态微分方程角度进一步证明改进四元数误差模型的正确性。具体推导如下：

由链式法则可知：

对上式两边求导得：

式(22)展开可得：

上式两边同时左乘

得：

再将式(24)两边同时右乘

得：

由于

和

代入式(25)可得：

因此

四元数乘法法则有：

根据式(29)，式(20)可展开如下：

对式(10)传统姿态阻尼误差方程进行如下定义：

故有：

代入式(51)和(52)，可得

其中，

对式(15)改进姿态阻尼误差方程作如下定义：

可得下式：

其中，

将(38)和式(27)进行对比，不难得出

与C₂-A₂-B₂一致的结论，这也从另一个角度证明了改进四元数姿态阻尼误差模型的正确性，而传统四元数误差模型的推导过程没有问题，但未考虑投影坐标系一致性的问题，不如改进模型合理。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (8)

1.一种系泊状态下舰船的快速初始对准方法，其特征在于，包括：

步骤1，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

步骤2，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

步骤3，基于所述初始对准参考坐标系，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

步骤4，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

2.根据权利要求1所述的方法，其特征在于，所述步骤2具体为：

i系-地心惯性坐标系，坐标系原点为地心，x轴指向赤道与黄道相交的春分点，z轴沿地球自转轴指向北极点，y轴与x轴和z轴构成右手坐标系，坐标系相对惯性空间保持不变；

e系-地球坐标系，坐标系原点为地心，x轴指向格林威治子午线，z轴沿地球自转轴指向北极点，y轴与x轴和z轴构成右手坐标系，坐标系跟随地球自转；

b系-载体坐标系，坐标系原点为载体重心，其x轴、y轴和z轴分别指向载体的右前上；

n系-导航坐标系，选用载体所在位置的当地地理坐标系，其x轴、y轴和z轴分别指向当地东向、北向和天向；

n′系-计算导航坐标系，表示存在误差的导航坐标系。

3.根据权利要求1所述的方法，其特征在于，所述步骤3包括：

步骤31，基于所述初始对准参考坐标系，建立改进的SINS四元数姿态阻尼误差微分方程；

步骤32，基于所述初始对准参考坐标系，建立改进的SINS速度阻尼误差微分方程；

步骤33，基于所述改进的SINS四元数姿态阻尼误差微分方程和所述改进的SINS速度阻尼误差微分方程，以导航系解算速度和真实速度之间的误差为量测量，建立系泊状态下的改进的非线性初始对准误差模型。

4.根据权利要求3所述的方法，其特征在于，所述步骤31具体为：

由姿态四元数微分方程知：

式中：

表示b系相对n系方向余弦矩阵

对应四元数的微分；

表示b系相对n系方向余弦矩阵

对应的四元数；

表示四元数乘法；

表示b系相对n系的角速度在b系中的投影；

表示三参量

对应的四元数；

表示b系中三轴陀螺仪无误差测量值，(g)^T表示转置，

表示n系相对i系角速度在n系中的投影；

表示地球自转角速度在n系中的投影，

表示n系相对e系角速度在n系中的投影；

表示三轴陀螺仪测量值，

表示三轴陀螺仪测量误差；

考虑误差影响，同时引入GNSS接收机提供的准确速度

和位置p_g阻尼信息，实际姿态四元数阻尼微分方程为：

式中：

表示b系相对n′系方向余弦矩阵

对应四元数的微分；

表示b系相对n′系方向余弦矩阵

对应的四元数；

表示带有误差的

表示三参量

对应的四元数；

表示三轴陀螺仪测量值，

表示由速度阻尼信息

和位置阻尼信息p_g获得的

表示由位置阻尼信息p_g获得的

表示由速度阻尼信息

和位置阻尼信息p_g获得的

由姿态乘性四元数误差可知：

式中：

表示n′系相对n系方向余弦矩阵

对应的姿态误差四元数；(g)^*表示共轭转置；

对等式两边同时微分，得到：

式中：

表示n′系相对n系方向余弦矩阵

对应的姿态误差四元数微分；

考虑n系为理想坐标系，实际解算是在n′系中进行，将误差形式统一在n′系下，生成改进的四元数姿态误差阻尼微分方程如下：

式中：

表示三参量

对应的四元数；

表示三参量

对应的四元数；

表示三参量

对应的四元数；

表示三参量

对应的四元数。

5.根据权利要求3所述的方法，其特征在于，所述步骤32具体为：

由比力微分方程知：

式中：

表示n系速度的微分；

表示b系相对n系方向余弦矩阵；f^b表示比力；

表示加速度计测量值，δf^b表示加速度计测量误差；vⁿ表示n系速度；gⁿ表示重力加速度；(g×)表示反对称矩阵；

考虑误差影响，同时引入GNSS接收机提供的准确速度

和位置p_g阻尼信息，实际速度微分方程为：

式中：

表示n′系速度的微分；

表示b系相对n′系方向余弦矩阵；

表示由位置阻尼信息p_g获得的gⁿ；

考虑矢量方向性，定义速度误差如下：

式中：dv^n′表示速度误差；v^n′表示n′系速度；

表示n′系相对n系方向余弦矩阵；

对等式两边同时微分，得到：

式中：

表示速度误差微分；

表示n′系相对n系方向余弦矩阵的微分；

表示n′系相对n系的角速度在n′系中的投影；

生成改进的速度阻尼误差微分方程如下：

6.根据权利要求3所述的方法，其特征在于，所述步骤33具体为：

系泊状态下，载体相对静止，线速度近似为零，位置视为常值，根据改进的所述SINS四元数姿态阻尼误差微分方程、改进的所述SINS速度阻尼误差微分方程和速度误差量测，建立基于改进四元数阻尼误差方程的非线性初始对准误差模型如下：

式中：

表示状态；f(g)表示非线性状态函数；G表示噪声驱动矩阵；w＝[w_g ^T,w_a ^T]^T表示陀螺仪和加速度计的测量白噪声；y＝dv^n′＝v^n′表示速度误差量测，H表示线性量测矩阵，v表示量测白噪声；

步骤34，将所述非线性初始对准误差模型离散化，形成非线性离散对准模型，具体为：

式中：w_k和v_k分别表示k时刻离散系统噪声和量测噪声，且

式中：E[g]表示均值，Q_k为离散系统噪声w_k的方差阵，R_k为离散量测噪声v_k的方差阵，δ_kj为克罗内克函数。f(x_k)和H_k分别为离散后的状态方程和量测矩阵。

7.根据权利要求1所述的方法，其特征在于：所述步骤4具体为：

根据所述非线性离散模型，构建用于初始对准的四元数无味估计器估计模型的滤波算法，所述四元数无味估计器估计模型的滤波算法包括：初始化、导航解算、时间更新、量测更新、姿态更新和模型反馈校正；

其中，所述四元数无味估计器估计模型的滤波算法具体处理步骤如下：

步骤41，进行初始化，具体为：

定义四元数误差δq，将其转换成误差修正罗德里格斯参数δσ，重置状态为x＝[δσ^T,x^eT]^T，其中x^e表示非姿态误差状态；设置初始状态

和其均方差

状态噪声方差阵Q₀、量测噪声方差阵R₀以及SINS采样时间dt；设置姿态四元数初值

和速度初值

步骤42，进行导航解算，具体为：

导航解算更新公式如下：

式中，

表示k-1时刻b系相对n′系方向余弦矩阵

对应的四元数；

表示k-1时刻n′系速度；

和

分别表示k-1时刻陀螺仪测量值和加速度计测量值；

和

分别表示k-1时刻由位置阻尼信息p_g,k-1获得的

和gⁿ，

表示k时刻b系相对n′系方向余弦矩阵

对应的四元数，

表示k时刻n′系速度；

步骤43，进行时间更新，具体为：

产生状态sigma点，

式中：sigma(g,g)表示sigma点采样方程；

表示k-1时刻由状态

和均方差阵P_k-1生成的sigma点；

表示k-1时刻误差罗德里格斯参数形式的sigma点；

表示k-1时刻非姿态状态的sigma点；

式中：

表示k-1时刻

的四元数形式，||g||表示l₂范数；

式中：

表示k-1时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的估计值，

表示k-1时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的sigma点；

计算非线性状态函数传播的积分点：

式中：

表示选取的参考四元数；

表示由k-1时刻预测的四元数误差sigma点；

将

转换为修正罗德里格斯参数形式

状态一步预测：

式中：

表示sigma点权重；

表示k-1时刻预测的状态sigma点；

状态一步预测均方误差阵：

式中：

表示sigma点权重；Q_k-1表示k-1时刻状态噪声方差阵；

若未进入量测更新周期，直接进行如下状态和状态方差阵更新：

P_k＝P_k/k-1

步骤4、进行量测更新，具体为：

若量测更新周期到，进行量测更新；

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

P_k＝(I_n×n-K_kH_k)P_k/k-1

式中：K_k表示k-1时刻的滤波增益；y_k表示k时刻的量测输入；

和P_k分别对应k时刻的状态均值和方差阵；

步骤45，进行姿态更新，具体为：

经过时间更新和量测更新，得到k时刻状态

上式将

转换为

式中：

表示选取的参考四元数；

表示k时刻n′系相对n系方向余弦矩阵对应姿态误差四元数的估计值；

步骤46，闭环反馈校正，具体为：

利用

和

分别对导航解算中更新的

和

进行反馈校正，将校正后的姿态四元数

转换成欧拉角输出，再将

置零，设置

进入下一滤波周期，直至对准结束。

8.一种系泊状态下舰船的快速初始对准装置，其特征在于，包括：

获取单元，获取舰船上的传感器的实时数据，所述实时数据包括：捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息；

第一建立单元，建立初始对准参考坐标系，所述初始对准参考坐标系包括：地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系；

第二建立单元，建立系泊状态下的改进的非线性初始对准误差模型，并对改进的所述非线性初始对准误差模型进行离散化，形成非线性离散对准模型；

处理单元，基于所述获取舰船上的传感器的实时数据，进行导航解算，生成所述舰船的姿态和速度；然后基于所述获取舰船上的传感器的实时数据，根据所述非线性离散对准模型，构建用于初始对准的四元数无味估计器估计模型；然后利用所述四元数无味估计器估计模型的状态参数，对所述姿态和速度进行闭环反馈校正，将校正后的姿态四元数转换成欧拉角实时输出，直至对准过程结束。

Images