CN112857398A - 一种系泊状态下舰船的快速初始对准方法和装置 - Google Patents
一种系泊状态下舰船的快速初始对准方法和装置 Download PDFInfo
- Publication number
- CN112857398A CN112857398A CN202110035848.8A CN202110035848A CN112857398A CN 112857398 A CN112857398 A CN 112857398A CN 202110035848 A CN202110035848 A CN 202110035848A CN 112857398 A CN112857398 A CN 112857398A
- Authority
- CN
- China
- Prior art keywords
- quaternion
- error
- coordinate system
- representing
- attitude
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 238000012937 correction Methods 0.000 claims abstract description 13
- 230000009965 odorless effect Effects 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 238000004364 calculation method Methods 0.000 claims abstract description 11
- 239000013598 vector Substances 0.000 claims abstract description 7
- 238000013016 damping Methods 0.000 claims description 66
- 239000011159 matrix material Substances 0.000 claims description 58
- 238000005259 measurement Methods 0.000 claims description 37
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 238000012545 processing Methods 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000005070 sampling Methods 0.000 claims description 4
- 230000009967 tasteless effect Effects 0.000 claims description 3
- 241001421953 Eclipta <beetle> Species 0.000 claims description 2
- 241000764238 Isis Species 0.000 claims description 2
- 230000001133 acceleration Effects 0.000 claims description 2
- 238000006243 chemical reaction Methods 0.000 claims description 2
- 239000006185 dispersion Substances 0.000 claims description 2
- 230000003068 static effect Effects 0.000 claims description 2
- 238000004904 shortening Methods 0.000 abstract 1
- 238000009795 derivation Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000006467 substitution reaction Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000007711 solidification Methods 0.000 description 1
- 230000008023 solidification Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
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系中的投影; 表示三轴陀螺仪测量值,表示三轴陀螺仪测量误差;
式中:表示b系相对n′系方向余弦矩阵对应四元数的微分;表示b系相对n′系方向余弦矩阵对应的四元数;表示带有误差的 表示三参量对应的四元数; 表示三轴陀螺仪测量值,表示由速度阻尼信息和位置阻尼信息pg获得的 表示由位置阻尼信息pg获得的 表示由速度阻尼信息和位置阻尼信息pg获得的
由姿态乘性四元数误差可知:
对等式两边同时微分,得到:
考虑n系为理想坐标系,实际解算是在n′系中进行,将误差形式统一在n′系下,生成改进的四元数姿态误差阻尼微分方程如下:
所述步骤32具体为:
由比力微分方程知:
考虑矢量方向性,定义速度误差如下:
对等式两边同时微分,得到:
生成改进的速度阻尼误差微分方程如下:
所述步骤33具体为:
系泊状态下,载体相对静止,线速度近似为零,位置视为常值,根据改进的所述SINS四元数姿态阻尼误差微分方程、改进的所述SINS速度阻尼误差微分方程和速度误差量测,建立基于改进四元数阻尼误差方程的非线性初始对准误差模型如下:
式中:表示状态;f(g)表示非线性状态函数;G表示噪声驱动矩阵;w=[wg T,wa T]T表示陀螺仪和加速度计的测量白噪声;y=dvn′=vn′表示速度误差量测,H表示线性量测矩阵,v表示量测白噪声;
步骤34,将所述非线性初始对准误差模型离散化,形成非线性离散对准模型,具体为:
式中:wk和vk分别表示k时刻离散系统噪声和量测噪声,且
式中:E[g]表示均值,Qk为离散系统噪声wk的方差阵,Rk为离散量测噪声vk的方差阵,δkj为克罗内克函数。f(xk)和Hk分别为离散后的状态方程和量测矩阵。
所述步骤4具体为:
根据所述非线性离散模型,构建用于初始对准的四元数无味估计器估计模型的滤波算法,所述四元数无味估计器估计模型的滤波算法包括:初始化、导航解算、时间更新、量测更新、姿态更新和模型反馈校正;
其中,所述四元数无味估计器估计模型的滤波算法具体处理步骤如下:
步骤41,进行初始化,具体为:
定义四元数误差δq,将其转换成误差修正罗德里格斯参数δσ,重置状态为x=[δσT,xeT]T,其中xe表示非姿态误差状态;设置初始状态和其均方差状态噪声方差阵Q0、量测噪声方差阵R0以及SINS采样时间dt;设置姿态四元数初值和速度初值
步骤42,进行导航解算,具体为:
导航解算更新公式如下:
式中,表示k-1时刻b系相对n′系方向余弦矩阵对应的四元数;表示k-1时刻n′系速度;和分别表示k-1时刻陀螺仪测量值和加速度计测量值;和分别表示k-1时刻由位置阻尼信息pg,k-1获得的和gn,表示k时刻b系相对n′系方向余弦矩阵对应的四元数,表示k时刻n′系速度;
步骤43,进行时间更新,具体为:
产生状态sigma点,
式中:sigma(g,g)表示sigma点采样方程;表示k-1时刻由状态和均方差阵Pk-1生成的sigma点;表示k-1时刻误差罗德里格斯参数形式的sigma点;表示k-1时刻非姿态状态的sigma点;
计算非线性状态函数传播的积分点:
状态一步预测:
状态一步预测均方误差阵:
若未进入量测更新周期,直接进行如下状态和状态方差阵更新:
Pk=Pk/k-1
步骤4、进行量测更新,具体为:
若量测更新周期到,进行量测更新;
Kk=Pk/k-1Hk T(HkPk/k-1Hk T+Rk)-1
Pk=(In×n-KkHk)Pk/k-1
步骤45,进行姿态更新,具体为:
步骤46,闭环反馈校正,具体为:
本发明针对非线性误差对准模型进行改进,将误差统一定义在计算导航坐标系下,利用四元数无味估计器进行状态参数估计,应用于舰船系泊状态下的初始对准,可提高对准的快速性和精度。
本发明还提供一种系泊状态下舰船的快速初始对准装置,包括:
获取单元,获取舰船上的传感器的实时数据,所述实时数据包括:捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息;
第一建立单元,建立初始对准参考坐标系,所述初始对准参考坐标系包括:地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系;
第二建立单元,建立系泊状态下的改进的非线性初始对准误差模型,并对改进的所述非线性初始对准误差模型进行离散化,形成非线性离散对准模型;
处理单元,基于所述获取舰船上的传感器的实时数据,进行导航解算,生成所述舰船的姿态和速度;然后基于所述获取舰船上的传感器的实时数据,根据所述非线性离散对准模型,构建用于初始对准的四元数无味估计器估计模型;然后利用所述四元数无味估计器估计模型的状态参数,对所述姿态和速度进行闭环反馈校正,将校正后的姿态四元数转换成欧拉角实时输出,直至对准过程结束。
以下描述本发明的设计思想。
1基于四元数的SINS阻尼误差模型
1.1SINS状态微分方程
给出SINS姿态、速度和位置微分方程如下:
式中:
q=[q0,ρT]T为四元数具体表达,q0为标量,ρ=[q1,q2,q3]T为三维矢量;p=[L,λ,h]T,其中L表示纬度、λ表示经度、h表示高度,RP为当地曲率矩阵,RM和RN分别为子午圈与卯酉圈主曲率半径。是基于四元数的方向余弦阵,其与存在式(5)对应关系。
式中,I3×3表示3维单位矩阵。
实际阻尼状态微分方程如下:
1.2传统阻尼误差方程
传统的姿态四元数阻尼误差微分方程为:
将式(1)和式(6)代入上式,推导如下:
传统的速度误差定义为:
δvn′=vn′-vn (11)
将式(11)左右两边同时微分,并代入式(2)和式(7),可得传统速度阻尼误差方程如下:
位置误差定义为:
位置阻尼误差微分方程如下。
1.3改进阻尼误差方程
基于SINS导航解算实际在n′系下进行这一基本认识,观察1.2节传统非线性阻尼误差方程推导过程,不难发现有以下两处不够严格:
(2)式(11)速度误差定义只是标量意义上的减法,忽略了矢量的方向性,未对坐标系进行统一。
本节考虑上述问题,将误差统一定义在n′系下,重新推导了改进非线性阻尼误差方程,其中,改进姿态误差方程如式(15)所示。
考虑矢量方向性,重新定义速度误差为:
将式上左右两边同时微分可得:
将式(2)、(6)代入式(17),可得改进阻尼误差微分方程如下:
改进阻尼位置误差微分方程如式(19)所示。
2改进四元数阻尼模型正确性证明
本节从方向余弦矩阵姿态微分方程角度进一步证明改进四元数误差模型的正确性。具体推导如下:
由链式法则可知:
对上式两边求导得:
式(22)展开可得:
因此
四元数乘法法则有:
根据式(29),式(20)可展开如下:
对式(10)传统姿态阻尼误差方程进行如下定义:
故有:
代入式(51)和(52),可得
其中,
对式(15)改进姿态阻尼误差方程作如下定义:
可得下式:
其中,
将(38)和式(27)进行对比,不难得出与C2-A2-B2一致的结论,这也从另一个角度证明了改进四元数姿态阻尼误差模型的正确性,而传统四元数误差模型的推导过程没有问题,但未考虑投影坐标系一致性的问题,不如改进模型合理。
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。因此,本发明的保护范围应该以权利要求的保护范围为准。
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系中的投影; 表示三轴陀螺仪测量值,表示三轴陀螺仪测量误差;
式中:表示b系相对n′系方向余弦矩阵对应四元数的微分;表示b系相对n′系方向余弦矩阵对应的四元数;表示带有误差的 表示三参量对应的四元数; 表示三轴陀螺仪测量值,表示由速度阻尼信息和位置阻尼信息pg获得的 表示由位置阻尼信息pg获得的 表示由速度阻尼信息和位置阻尼信息pg获得的
由姿态乘性四元数误差可知:
对等式两边同时微分,得到:
考虑n系为理想坐标系,实际解算是在n′系中进行,将误差形式统一在n′系下,生成改进的四元数姿态误差阻尼微分方程如下:
5.根据权利要求3所述的方法,其特征在于,所述步骤32具体为:
由比力微分方程知:
考虑矢量方向性,定义速度误差如下:
对等式两边同时微分,得到:
生成改进的速度阻尼误差微分方程如下:
6.根据权利要求3所述的方法,其特征在于,所述步骤33具体为:
系泊状态下,载体相对静止,线速度近似为零,位置视为常值,根据改进的所述SINS四元数姿态阻尼误差微分方程、改进的所述SINS速度阻尼误差微分方程和速度误差量测,建立基于改进四元数阻尼误差方程的非线性初始对准误差模型如下:
式中:表示状态;f(g)表示非线性状态函数;G表示噪声驱动矩阵;w=[wg T,wa T]T表示陀螺仪和加速度计的测量白噪声;y=dvn′=vn′表示速度误差量测,H表示线性量测矩阵,v表示量测白噪声;
步骤34,将所述非线性初始对准误差模型离散化,形成非线性离散对准模型,具体为:
式中:wk和vk分别表示k时刻离散系统噪声和量测噪声,且
式中:E[g]表示均值,Qk为离散系统噪声wk的方差阵,Rk为离散量测噪声vk的方差阵,δkj为克罗内克函数。f(xk)和Hk分别为离散后的状态方程和量测矩阵。
7.根据权利要求1所述的方法,其特征在于:所述步骤4具体为:
根据所述非线性离散模型,构建用于初始对准的四元数无味估计器估计模型的滤波算法,所述四元数无味估计器估计模型的滤波算法包括:初始化、导航解算、时间更新、量测更新、姿态更新和模型反馈校正;
其中,所述四元数无味估计器估计模型的滤波算法具体处理步骤如下:
步骤41,进行初始化,具体为:
定义四元数误差δq,将其转换成误差修正罗德里格斯参数δσ,重置状态为x=[δσT,xeT]T,其中xe表示非姿态误差状态;设置初始状态和其均方差状态噪声方差阵Q0、量测噪声方差阵R0以及SINS采样时间dt;设置姿态四元数初值和速度初值
步骤42,进行导航解算,具体为:
导航解算更新公式如下:
式中,表示k-1时刻b系相对n′系方向余弦矩阵对应的四元数;表示k-1时刻n′系速度;和分别表示k-1时刻陀螺仪测量值和加速度计测量值;和分别表示k-1时刻由位置阻尼信息pg,k-1获得的和gn,表示k时刻b系相对n′系方向余弦矩阵对应的四元数,表示k时刻n′系速度;
步骤43,进行时间更新,具体为:
产生状态sigma点,
式中:sigma(g,g)表示sigma点采样方程;表示k-1时刻由状态和均方差阵Pk-1生成的sigma点;表示k-1时刻误差罗德里格斯参数形式的sigma点;表示k-1时刻非姿态状态的sigma点;
计算非线性状态函数传播的积分点:
状态一步预测:
状态一步预测均方误差阵:
若未进入量测更新周期,直接进行如下状态和状态方差阵更新:
Pk=Pk/k-1
步骤4、进行量测更新,具体为:
若量测更新周期到,进行量测更新;
Kk=Pk/k-1Hk T(HkPk/k-1Hk T+Rk)-1
Pk=(In×n-KkHk)Pk/k-1
步骤45,进行姿态更新,具体为:
步骤46,闭环反馈校正,具体为:
8.一种系泊状态下舰船的快速初始对准装置,其特征在于,包括:
获取单元,获取舰船上的传感器的实时数据,所述实时数据包括:捷联惯导系统SINS的陀螺仪测量值、加速度计测量值以及卫星导航接收机GNSS的速度阻尼信息和位置阻尼信息;
第一建立单元,建立初始对准参考坐标系,所述初始对准参考坐标系包括:地心惯性坐标系、地球坐标系、载体坐标系、导航坐标系、计算导航坐标系;
第二建立单元,建立系泊状态下的改进的非线性初始对准误差模型,并对改进的所述非线性初始对准误差模型进行离散化,形成非线性离散对准模型;
处理单元,基于所述获取舰船上的传感器的实时数据,进行导航解算,生成所述舰船的姿态和速度;然后基于所述获取舰船上的传感器的实时数据,根据所述非线性离散对准模型,构建用于初始对准的四元数无味估计器估计模型;然后利用所述四元数无味估计器估计模型的状态参数,对所述姿态和速度进行闭环反馈校正,将校正后的姿态四元数转换成欧拉角实时输出,直至对准过程结束。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110035848.8A CN112857398B (zh) | 2021-01-12 | 2021-01-12 | 一种系泊状态下舰船的快速初始对准方法和装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110035848.8A CN112857398B (zh) | 2021-01-12 | 2021-01-12 | 一种系泊状态下舰船的快速初始对准方法和装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112857398A true CN112857398A (zh) | 2021-05-28 |
CN112857398B CN112857398B (zh) | 2023-04-28 |
Family
ID=76002800
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110035848.8A Active CN112857398B (zh) | 2021-01-12 | 2021-01-12 | 一种系泊状态下舰船的快速初始对准方法和装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112857398B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113739806A (zh) * | 2021-09-07 | 2021-12-03 | 中国人民解放军海军工程大学 | 利用参数识别的惯导系统阻尼方法 |
CN115855104A (zh) * | 2022-11-25 | 2023-03-28 | 哈尔滨工程大学 | 一种组合导航滤波结果最优在线评价方法 |
CN114770598B (zh) * | 2022-04-08 | 2024-01-26 | 上海中车艾森迪海洋装备有限公司 | 水下机器人姿态估算方法、装置、电子设备及存储介质 |
CN113776529B (zh) * | 2021-09-13 | 2024-04-19 | 中国人民解放军海军工程大学 | 一种基于载体系四元数姿态误差的非线性误差模型 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20030149528A1 (en) * | 2002-02-06 | 2003-08-07 | Ching-Fang Lin | Positioning and navigation method and system thereof |
CN101162147A (zh) * | 2007-11-27 | 2008-04-16 | 哈尔滨工程大学 | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 |
AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
-
2021
- 2021-01-12 CN CN202110035848.8A patent/CN112857398B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20030149528A1 (en) * | 2002-02-06 | 2003-08-07 | Ching-Fang Lin | Positioning and navigation method and system thereof |
CN101162147A (zh) * | 2007-11-27 | 2008-04-16 | 哈尔滨工程大学 | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 |
AU2020101268A4 (en) * | 2020-07-06 | 2020-08-13 | Harbin Engineering University | The initial alignment method for sway base |
Non-Patent Citations (7)
Title |
---|
GUANG-JUN ZHANG 等: "Nonlinear_Initial_Alignment_based_on_Quaternion_Error_Model", 《2018 IEEE CSAA GUIDANCE, NAVIGATION AND CONTROL CONFERENCE (GNCC)》 * |
严恭敏 等: "基于欧拉平台误差角的SINS非线性误差模型研究", 《西北工业大学学报》 * |
常振军 等: "捷联惯导行进间对准位置更新与误差分析", 《系统工程与电子技术》 * |
徐景硕 等: "动基座条件下舰载机快速传递对准方法研究", 《宇航计测技术》 * |
梁锋等: "基于四元数的舰船捷联惯导粗对准方法研究", 《舰船科学技术》 * |
王伟 等: "GPS辅助的SINS系统快速动基座初始对准", 《哈尔滨工业大学学报》 * |
陈永冰 等: "基于双姿态的高阶罗德里格斯参数姿态估计", 《华中科技大学学报(自然科学版)》 * |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113739806A (zh) * | 2021-09-07 | 2021-12-03 | 中国人民解放军海军工程大学 | 利用参数识别的惯导系统阻尼方法 |
CN113739806B (zh) * | 2021-09-07 | 2023-09-19 | 中国人民解放军海军工程大学 | 利用参数识别的惯导系统阻尼方法 |
CN113776529B (zh) * | 2021-09-13 | 2024-04-19 | 中国人民解放军海军工程大学 | 一种基于载体系四元数姿态误差的非线性误差模型 |
CN114770598B (zh) * | 2022-04-08 | 2024-01-26 | 上海中车艾森迪海洋装备有限公司 | 水下机器人姿态估算方法、装置、电子设备及存储介质 |
CN115855104A (zh) * | 2022-11-25 | 2023-03-28 | 哈尔滨工程大学 | 一种组合导航滤波结果最优在线评价方法 |
Also Published As
Publication number | Publication date |
---|---|
CN112857398B (zh) | 2023-04-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111156987B (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
CN110030994B (zh) | 一种基于单目的鲁棒性视觉惯性紧耦合定位方法 | |
CN112857398A (zh) | 一种系泊状态下舰船的快速初始对准方法和装置 | |
CN106990426B (zh) | 一种导航方法和导航装置 | |
CN110221332B (zh) | 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法 | |
CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
CN100541135C (zh) | 基于多普勒的光纤陀螺捷联惯导系统初始姿态确定方法 | |
CN107110650B (zh) | 在能观测性方面受约束的导航状态的估计方法 | |
Bryne et al. | Nonlinear observers for integrated INS\/GNSS navigation: implementation aspects | |
CN109931957B (zh) | 基于lgmkf的sins捷联惯性导航系统自对准方法 | |
CN110702143B (zh) | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 | |
CN110398257A (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
Stančić et al. | The integration of strap-down INS and GPS based on adaptive error damping | |
CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
CN103941273B (zh) | 机载惯性/卫星组合导航系统的自适应滤波方法与滤波器 | |
US4032759A (en) | Shipboard reference for an aircraft navigation system | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
Xue et al. | In-motion alignment algorithm for vehicle carried SINS based on odometer aiding | |
CN108592943B (zh) | 一种基于opreq方法的惯性系粗对准计算方法 | |
CN112902956A (zh) | 一种手持式gnss/mems-ins接收机航向初值获取方法、电子设备、存储介质 | |
CN112325886A (zh) | 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿系统 | |
Ben et al. | A dual-state filter for a relative velocity aiding strapdown inertial navigation system | |
CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 | |
CN112197765A (zh) | 一种实现水下机器人精细导航的方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |