CN110873577B - 一种水下快速动基座对准方法及装置 - Google Patents

一种水下快速动基座对准方法及装置 Download PDF

Info

Publication number
CN110873577B
CN110873577B CN201911214881.6A CN201911214881A CN110873577B CN 110873577 B CN110873577 B CN 110873577B CN 201911214881 A CN201911214881 A CN 201911214881A CN 110873577 B CN110873577 B CN 110873577B
Authority
CN
China
Prior art keywords
coordinate system
carrier
matrix
time
updating
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.)
Active
Application number
CN201911214881.6A
Other languages
English (en)
Other versions
CN110873577A (zh
Inventor
李万里
陈明剑
陈锐
李俊毅
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Information Engineering University of PLA Strategic Support Force
Original Assignee
Information Engineering University of PLA Strategic Support Force
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Information Engineering University of PLA Strategic Support Force filed Critical Information Engineering University of PLA Strategic Support Force
Priority to CN201911214881.6A priority Critical patent/CN110873577B/zh
Publication of CN110873577A publication Critical patent/CN110873577A/zh
Application granted granted Critical
Publication of CN110873577B publication Critical patent/CN110873577B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems

Abstract

本发明涉及一种水下快速动基座对准方法及装置，属于水下导航定位领域。其中方法包括以下步骤：1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；2)在每个惯性导航更新周期内，更新
Figure DDA0002299241060000011
3)在每个多普勒测速仪的更新周期内，更新
Figure DDA0002299241060000012
并建立系统方程和观测方程，利用非线性滤波算法计算
Figure DDA0002299241060000013
4)计算当前的姿态矩阵
Figure DDA0002299241060000014
并输出当前的姿态角信息；5)重复步骤2)‑4)，直至满足初始对准结束条件。本发明建立了非线性模型，可利用非线性滤波算法求得姿态矩阵，实现初始对准；不同于传统的两阶段对准，可以在不进行粗对准的条件下完成初始对准，提高了初始对准的快速性。

Description

一种水下快速动基座对准方法及装置
技术领域
本发明涉及一种水下快速动基座对准方法及装置，属于水下导航定位领域。
背景技术
水下航行器是探索海洋的重要手段，其在开发利用海洋中的作用绝不亚于火箭和航天飞机在探索宇宙空间中的作用。而水下导航是水下航行器应用的最大制约因素。目前，可以用于水下导航的设备仍然十分有限。惯性导航系统(Inertial Navigation System,INS)是以牛顿第二定律(惯性定律)为基础，利用陀螺仪和加速度计测量载体角运动和线运动，计算载体位置、速度和姿态的技术。它是水下自主导航的一个重要手段。然而，由于惯性传感器固有的漂移误差，惯性导航系统虽然能够在短时间内有很高的定位精度，但是导航误差会随着时间积累。为此，需要辅助传感器来抑制导航误差。惯性导航系统与声学多普勒(Doppler Velocity Log,DVL)组合导航是是实现水下自主导航的主要方式。
INS/DVL组合导航系统在开始导航任务之前，需要完成初始对准。初始对准的首要任务是确定初始时刻的姿态矩阵。初始对准的精度对后续的导航精度有着重要影响。然而多普勒仅能提供相对于自身载体系下的速度观测，且水下导航信息源少、环境复杂，如何实现惯性导航系统的快速动基座对准、且完成无位置观测条件下的位置更新仍然面临严峻挑战。
目前水下常用的初始对准方法一般由两个阶段组成：粗对准和精对准。即：首先通过粗对准获得一个粗略的姿态，再通过精对准获得精确的姿态。这种两阶段对准的方式，粗对准和精对准各占用一段对准时间，且两个对准阶段都需要一定的时间收敛。为此，该方式耗时较长，不适合快速对准。
发明内容
本发明的目的在于提供一种水下快速动基座对准方法及装置，以解决现有初始对准方法耗时较长的问题。
为实现上述目的，本发明提出一种水下快速动基座对准方法，包括以下步骤：
1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；
2)在每个惯性导航更新周期内，根据惯性导航系统输出的角速度信息更新
Figure BDA0002299241040000021
并根据多普勒输出的速度观测值更新
Figure BDA0002299241040000022
其中，多普勒测速仪的更新周期长于惯性导航系统的更新周期，在每个多普勒测速仪更新周期内，载体的速度为恒定值；
Figure BDA0002299241040000023
为载体坐标系b到惯性凝固坐标系
Figure BDA0002299241040000024
的方向余弦矩阵，
Figure BDA0002299241040000025
为初始时刻导航坐标系n0到导航坐标系n的转换矩阵；
3)在每个多普勒测速仪的更新周期内，根据初始时刻的经度、纬度和时间信息更新
Figure BDA0002299241040000026
并建立系统方程和观测方程，利用非线性滤波算法计算
Figure BDA0002299241040000027
为惯性凝固坐标系
Figure BDA0002299241040000028
到初始时刻导航坐标系n0的转换矩阵，
Figure BDA0002299241040000029
为惯性凝固载体坐标系
Figure BDA00022992410400000210
到惯性凝固导航坐标系
Figure BDA00022992410400000211
的方向余弦矩阵；
所述系统方程为：
Figure BDA00022992410400000212
其中，x＝[α β γ]T，α为方位角误差，β为俯仰角误差，γ为横滚角误差；
所述观测方程为：z＝(I-δC)V2(t)；其中，z为观测量，
Figure BDA00022992410400000213
δC为姿态误差矩阵，
Figure BDA00022992410400000214
Figure BDA00022992410400000215
Figure BDA00022992410400000216
Figure BDA00022992410400000217
为t时刻载体坐标系b到惯性凝固坐标系
Figure BDA00022992410400000218
的方向余弦矩阵，
Figure BDA00022992410400000219
为多普勒载体坐标系d到惯导载体坐标系b的方向余弦矩阵，
Figure BDA00022992410400000220
为t时刻多普勒测速仪输出的速度、
Figure BDA00022992410400000221
为t+Δt时刻载体坐标系b到惯性凝固坐标系
Figure BDA00022992410400000222
的方向余弦矩阵、
Figure BDA00022992410400000223
为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，fb为惯性导航系统输出的比力；
Figure BDA0002299241040000031
为矩阵
Figure BDA0002299241040000032
当前计算值的转置，ωie为地球自转角速度，
Figure BDA0002299241040000033
为地球自转角速度在导航坐标系n中的投影，
Figure BDA0002299241040000034
为重力加速度在导航坐标系n中的投影，rE为地球椭球的卯酉圈曲率半径，L表示纬度，
Figure BDA0002299241040000035
为离心力；
4)根据更新的
Figure BDA0002299241040000036
和
Figure BDA0002299241040000037
计算当前的姿态矩阵
Figure BDA0002299241040000038
并输出当前的姿态角信息；
5)重复步骤2)-4)，直至满足初始对准结束条件。
有益效果是：本发明建立了非线性模型，利用非线性滤波算法求解就可得到
Figure BDA0002299241040000039
依据
Figure BDA00022992410400000310
可求得姿态矩阵
Figure BDA00022992410400000311
实现了初始对准；本发明的对准方法可以在不进行粗对准的条件下完成初始对准，不同于传统的两阶段对准方法，提高了初始对准的快速性。
进一步的，计算
Figure BDA00022992410400000312
的过程包括：
A：在每个多普勒周期内根据建立的系统方程和观测方程，利用非线性滤波算法求α、β和γ；
B：根据α、β和γ，计算δC；
C：根据δC，利用
Figure BDA00022992410400000313
更新
Figure BDA00022992410400000314
将更新后的
Figure BDA00022992410400000315
作为
Figure BDA00022992410400000316
为了解决初始对准在无位置观测条件下的位置更新问题，减小位置误差，进一步的，
Figure BDA00022992410400000317
为
Figure BDA00022992410400000318
的转置矩阵，
Figure BDA00022992410400000319
的求解过程为：
1)设定初始时刻的
Figure BDA00022992410400000320
2)在每个惯性导航更新周期内，更新矩阵
Figure BDA00022992410400000321
在每个惯性导航更新周期，载体北向位置增量
Figure BDA00022992410400000322
和东向位置增量
Figure BDA00022992410400000323
为：
Figure BDA0002299241040000041
则当前的地理经纬度更新如下：
Figure BDA0002299241040000042
Figure BDA0002299241040000043
rN为地球椭球子午圈曲率半径，矩阵
Figure BDA0002299241040000044
的更新公式如下：
Figure BDA0002299241040000045
其中：
Figure BDA0002299241040000046
其中，k为惯性导航的周期数，tk为第k个周期对应的时间，tk+1为第k+1个周期对应的时间，rE为地球椭球的卯酉圈曲率半径，L(tk)为tk时刻载体所在位置的纬度，λ(tk)为tk时刻载体所在位置的经度，
Figure BDA0002299241040000047
为凝固惯性坐标系
Figure BDA0002299241040000048
到tk时刻的导航坐标系nk的转换矩阵。
进一步的，所述初始对准结束的条件为对准时间超过设定值，所述设定值为5-10分钟。
进一步的，所述非线性滤波算法为无迹卡尔曼滤波算法。
另外，本发明还提出一种水下快速动基座对准装置，包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，所述处理器在执行所述计算机程序时实现以下步骤：
1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；
2)在每个惯性导航更新周期内，根据惯性导航系统输出的角速度信息更新
Figure BDA0002299241040000051
并根据多普勒输出的速度观测值更新
Figure BDA0002299241040000052
其中，多普勒测速仪的更新周期长于惯性导航系统的更新周期，在每个多普勒测速仪更新周期内，载体的速度为恒定值；
Figure BDA0002299241040000053
为载体坐标系b到惯性凝固坐标系
Figure BDA0002299241040000054
的方向余弦矩阵，
Figure BDA0002299241040000055
为初始时刻导航坐标系n0到导航坐标系n的转换矩阵；
3)在每个多普勒测速仪的更新周期内，根据初始时刻的经度、纬度和时间信息更新
Figure BDA0002299241040000056
并建立系统方程和观测方程，利用非线性滤波算法计算
Figure BDA0002299241040000057
为惯性凝固坐标系
Figure BDA0002299241040000058
到初始时刻导航坐标系n0的转换矩阵，
Figure BDA0002299241040000059
为惯性凝固载体坐标系
Figure BDA00022992410400000510
到惯性凝固导航坐标系
Figure BDA00022992410400000511
的方向余弦矩阵；
所述系统方程为：
Figure BDA00022992410400000512
其中，x＝[αβγ]T，α为方位角误差，β为俯仰角误差，γ为横滚角误差；
所述观测方程为：z＝(I-δC)V2(t)；其中，z为观测量，
Figure BDA00022992410400000513
δC为姿态误差矩阵，
Figure BDA00022992410400000514
Figure BDA00022992410400000515
Figure BDA00022992410400000516
Figure BDA00022992410400000517
为t时刻载体坐标系b到惯性凝固坐标系
Figure BDA00022992410400000518
的方向余弦矩阵，
Figure BDA00022992410400000519
为多普勒载体坐标系d到惯导载体坐标系b的方向余弦矩阵，
Figure BDA00022992410400000520
为t时刻多普勒测速仪输出的速度、
Figure BDA00022992410400000521
为t+Δt时刻载体坐标系b到惯性凝固坐标系
Figure BDA00022992410400000522
的方向余弦矩阵、
Figure BDA00022992410400000523
为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，fb为惯性导航系统输出的比力；
Figure BDA0002299241040000061
为矩阵
Figure BDA0002299241040000062
当前计算值的转置，ωie为地球自转角速度，
Figure BDA0002299241040000063
为地球自转角速度在导航坐标系n中的投影，
Figure BDA0002299241040000064
为重力加速度在导航坐标系n中的投影，rE为地球椭球的卯酉圈曲率半径，L表示纬度，
Figure BDA0002299241040000065
为离心力；
4)根据更新的
Figure BDA0002299241040000066
和
Figure BDA0002299241040000067
计算当前的姿态矩阵
Figure BDA0002299241040000068
并输出当前的姿态角信息；
5)重复步骤2)-4)，直至满足初始对准结束条件。
有益效果是：本发明建立了非线性模型，利用非线性滤波算法求解就可得到
Figure BDA0002299241040000069
依据
Figure BDA00022992410400000610
可求得姿态矩阵
Figure BDA00022992410400000611
实现了初始对准；本发明的对准装置可以在不进行粗对准的条件下完成初始对准，不同于传统的两阶段对准装置，提高了初始对准的快速性。
进一步的，计算
Figure BDA00022992410400000612
的过程包括：
A：在每个多普勒周期内根据建立的系统方程和观测方程，利用非线性滤波算法求α、β和γ；
B：根据α、β和γ，计算δC；
C：根据δC，利用
Figure BDA00022992410400000613
更新
Figure BDA00022992410400000614
将更新后的
Figure BDA00022992410400000615
作为
Figure BDA00022992410400000616
为了解决初始对准在无位置观测条件下的位置更新问题，减小位置误差，进一步的，
Figure BDA00022992410400000617
为
Figure BDA00022992410400000618
的转置矩阵，
Figure BDA00022992410400000619
的求解过程为：
1)设定初始时刻的
Figure BDA00022992410400000620
2)在每个惯性导航更新周期内，更新矩阵
Figure BDA00022992410400000621
在每个惯性导航更新周期，载体北向位置增量
Figure BDA00022992410400000622
和东向位置增量
Figure BDA00022992410400000623
为：
Figure BDA0002299241040000071
则当前的地理经纬度更新如下：
Figure BDA0002299241040000072
Figure BDA0002299241040000073
rN为地球椭球子午圈曲率半径，矩阵
Figure BDA0002299241040000074
的更新公式如下：
Figure BDA0002299241040000075
其中：
Figure BDA0002299241040000076
其中，k为惯性导航的周期数，tk为第k个周期对应的时间，tk+1为第k+1个周期对应的时间，rE为地球椭球的卯酉圈曲率半径，L(tk)为tk时刻载体所在位置的纬度，λ(tk)为tk时刻载体所在位置的经度，
Figure BDA0002299241040000077
为凝固惯性坐标系
Figure BDA0002299241040000078
到tk时刻的导航坐标系nk的转换矩阵。
进一步的，所述初始对准结束的条件为对准时间超过设定值，所述设定值为5-10分钟。
进一步的，所述非线性滤波算法为无迹卡尔曼滤波算法。
附图说明
图1为本发明水下快速动基座对准方法实施例中水下快速动基座对准方法流程图。
具体实施方式
对水下快速动基座对准过程中所涉及的坐标系定义如下：
(1)惯性坐标系(i系)
坐标原点Oi位于地心，xi指向平春分点，zi与平均地球自转轴重合，yi与xi和zi构成右手坐标系。
(2)地固坐标系(e系)
坐标原点Oe位于地心，xe在赤道平面内指向平均格林尼治子午圈，ze平行于地球自转轴，ye与xe和ze构成右手坐标系。
(3)当地地理坐标系(n系)
坐标原点On位于载体质心，xn沿参考椭球子午圈方向指向北，yn沿参考椭球法线方向指向天，zn沿参考椭球卯酉圈方向指向东。
(4)惯导载体坐标系(b系)
坐标原点Ob位于载体中心，xb沿载体纵轴方向指向前，yb垂直载体平面指向上，zb与xb和yb构成右手坐标系。
(5)多普勒载体坐标系(d系)
坐标原点Od位于载体中心，xd沿载体纵轴方向指向前，yd垂直载体平面指向上，zd与xd和yd构成右手坐标系。
(6)惯性凝固载体坐标系
Figure BDA0002299241040000081
它是在t0时刻将载体坐标系b经惯性凝固后得到，t0为起始时刻。
(7)惯性凝固导航坐标系
Figure BDA0002299241040000082
它是在t0时刻将导航坐标系n经惯性凝固后得到，t0为起始时刻。
初始对准问题的转换：
初始对准要解决的主要问题就是获得惯性导航系统的姿态矩阵
Figure BDA0002299241040000083
该矩阵可以分解如下：
Figure BDA0002299241040000084
式中，
Figure BDA0002299241040000091
为初始时刻导航坐标系n0到导航坐标系n的转换矩阵，
Figure BDA0002299241040000092
为惯性凝固坐标系
Figure BDA0002299241040000093
到初始时刻导航坐标系n0的转换矩阵，
Figure BDA0002299241040000094
可以由以下式子确定：
Figure BDA0002299241040000095
Figure BDA0002299241040000096
为地球坐标系e到导航坐标系n的方向余弦矩阵，即
Figure BDA0002299241040000097
可由载体初始时刻所在点的经度λ0、纬度L0确定如下：
Figure BDA0002299241040000098
地球系从t0时刻到t时刻的转换矩阵
Figure BDA0002299241040000099
可由时间间隔Δt＝t-t0确定：
Figure BDA00022992410400000910
Figure BDA00022992410400000911
可由初始时刻载体所在点的经度λ0、纬度L0确定如下：
Figure BDA00022992410400000912
根据式(3)-(5)可以求得矩阵
Figure BDA00022992410400000913
为载体坐标系b到惯性凝固坐标系
Figure BDA00022992410400000914
的方向余弦矩阵，它的初值是一个单位矩阵：
Figure BDA00022992410400000915
这个矩阵可以利用陀螺输出进行更新：
Figure BDA00022992410400000916
式中，
Figure BDA00022992410400000917
为矩阵
Figure BDA00022992410400000918
的导数，
Figure BDA00022992410400000919
为惯性导航系统输出的角速度信息，
Figure BDA00022992410400000920
表示角速度信息
Figure BDA00022992410400000921
的斜对称矩阵。
在没有位置观测的情况下，
Figure BDA00022992410400000922
是未知的。它随着载体的运动而缓慢变化，在静止或者系泊的情况下，由于载体的位置没有变化，
Figure BDA00022992410400000923
是一个单位矩阵。在动基座的情况下则需要知道位置变化信息，才能得到该矩阵的值，位置信息可以通过多普勒的输出值与惯性导航系统输出的姿态通过航位推算来计算。由于坐标系
Figure BDA0002299241040000101
和
Figure BDA0002299241040000102
惯性凝固坐标系,因此初始时刻的
Figure BDA0002299241040000103
是一个常值，在数值上等于初始时刻的姿态矩阵。则初始对准问题转化为求取
Figure BDA0002299241040000104
求得该矩阵之后，利用公式(1)就可以得到矩阵
Figure BDA0002299241040000105
也就完成了初始对准。
水下快速动基座对准方法实施例：
为求取
Figure BDA0002299241040000106
建立以下非线性滤波模型：
系统方程
初始时刻姿态矩阵的计算值
Figure BDA0002299241040000107
可以表示为：
Figure BDA0002299241040000108
其中δC为姿态误差矩阵，它可以表示为：
Figure BDA0002299241040000109
α、β、γ分别为方位角误差、俯仰角误差和横滚角误差，由于
Figure BDA00022992410400001010
是一个常矩阵，α、β、γ均为常数，它们的微分方程可以表示为：
Figure BDA00022992410400001011
则系统方程可以表示为：
Figure BDA00022992410400001012
其中：
x＝[α β γ]T (12)
观测方程
在
Figure BDA00022992410400001013
系下，惯导的比力方程为：
Figure BDA0002299241040000111
其中，fb为惯性导航系统输出的比力，gm为万有引力，它是当地重力和离心力的和：
Figure BDA0002299241040000112
其中，
Figure BDA0002299241040000113
为离心力，
Figure BDA0002299241040000114
为当地重力在导航坐标系下的表示：
Figure BDA0002299241040000115
Figure BDA0002299241040000116
Figure BDA0002299241040000117
rE为地球椭球的卯酉圈曲率半径，
Figure BDA0002299241040000118
为地球自转角速度在导航坐标系中的表示，L表示纬度。在
Figure BDA0002299241040000119
系下的速度变化量可以通过对惯性导航的比力方程积分得到：
Figure BDA00022992410400001110
惯性系下的速度
Figure BDA00022992410400001111
也可以通过多普勒的测量值得到：
Figure BDA00022992410400001112
其中vr是由地球自转引起的速度，它为：
Figure BDA00022992410400001113
则根据多普勒测量值可以计算载体在惯性系
Figure BDA00022992410400001114
下的速度变化量：
Figure BDA00022992410400001115
联立(19)和(21)两式，可以得到：
Figure BDA0002299241040000121
因此，可以定义以下两个序列：
Figure BDA0002299241040000122
Figure BDA0002299241040000123
式中，
Figure BDA0002299241040000124
为t时刻载体坐标系b到惯性凝固坐标系
Figure BDA0002299241040000125
的方向余弦矩阵，
Figure BDA0002299241040000126
为多普勒载体坐标系d到惯导载体坐标系b的方向余弦矩阵，
Figure BDA0002299241040000127
为t时刻多普勒测速仪输出的速度、
Figure BDA0002299241040000128
为t+Δt时刻载体坐标系b到惯性凝固坐标系
Figure BDA0002299241040000129
的方向余弦矩阵、
Figure BDA00022992410400001210
为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，fb为惯性导航输出的比力。
可得：
Figure BDA00022992410400001211
对V2(t)和
Figure BDA00022992410400001212
作差，可以得到：
Figure BDA00022992410400001213
把公式(8)带入到公式(26)，则可以的系统的观测方程为：
Figure BDA00022992410400001214
由于公式(27)为非线性方程，因此必须采用非线性滤波的方式来完成初始对准。滤波方法可以采用目前常用的非线性滤波方式，如：扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)等。
如图1所示，本实施例完成初始对准的具体步骤如下：
步骤1：惯性导航系统、多普勒测速仪上电启动，导航计算机开始接收二者输出的原始数据。
步骤2：给初始对准中需要用到的变量赋初值：
(1)初值经度λ0、纬度L0，并根据公式(3)、(5)计算矩阵
Figure BDA0002299241040000131
的值。
(2)
Figure BDA0002299241040000132
步骤3：在每个惯性导航更新周期(多普勒的更新周期长于惯性导航系统的更新周期)：根据公式(7)更新矩阵
Figure BDA0002299241040000133
的值，并根据以下公式更新矩阵
Figure BDA0002299241040000134
的值，
Figure BDA0002299241040000135
假设在每个多普勒更新周期内，载体的速度都为匀速，则在每个惯导更新周期，载体北向位置增量
Figure BDA0002299241040000136
和东向位置增量
Figure BDA0002299241040000137
为：
Figure BDA0002299241040000138
则当前的地理经纬度更新如下：
Figure BDA0002299241040000139
Figure BDA00022992410400001310
rN为地球椭球子午圈曲率半径，矩阵
Figure BDA00022992410400001311
的更新公式如下：
Figure BDA00022992410400001312
其中：
Figure BDA00022992410400001313
k为惯性导航的周期数，tk为第k个周期对应的时间，tk+1为第k+1个周期对应的时间，L(tk)为tk时刻载体所在位置的纬度，λ(tk)为tk时刻载体所在位置的经度，
Figure BDA0002299241040000141
为凝固惯性坐标系
Figure BDA00022992410400001414
到tk时刻的导航坐标系nk的转换矩阵。
公式(28)-(30)解决了初始对准在无位置观测条件下的位置更新问题，
采用本实施例的初始对准方法可以把姿态误差快速减小，采用以下公式进行位置更新，位置误差也会比较小。
步骤4：在每个多普勒更新周期内：根据公式(3)、(4)、(5)更新矩阵
Figure BDA0002299241040000142
的值，并计算变量V1(t)和V2(t)的数值。
步骤5：根据公式(11)、(27)建立的非线性模型，利用非线性滤波(如：扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)等)的方式解算α、β、γ，并根据式(8)更新矩阵
Figure BDA0002299241040000143
更新矩阵
Figure BDA0002299241040000144
的过程为：
Figure BDA0002299241040000145
的初值是单位矩阵，也就是
Figure BDA0002299241040000146
根据解算的α、β、γ求出δC之后，可以根据
Figure BDA0002299241040000147
更新
Figure BDA0002299241040000148
更新完
Figure BDA0002299241040000149
后令
Figure BDA00022992410400001410
下一个多普勒周期，再次利用
Figure BDA00022992410400001411
更新
Figure BDA00022992410400001412
如此迭代。
本实施中非线性滤波算法的具体实现以非扩展UKF滤波为例，算法如下：
考虑非线性系统
xk＝f(xk-1)+wk-1 (33)
zk＝h(xk)+υk (34)
其中：xk为状态向量；zk为观测向量；f(·)和h(·)为非线性函数；wk-1和υk为零均值白噪声，它们的方差为：
Figure BDA00022992410400001413
在简单加性噪声的情况下，为了减小计算量，往往采用非扩展UKF。UKF与KF算法类似，都由时间更新与预测更新组成。非扩展UKF的具体计算流程如下：
(1)初始化：
Figure BDA0002299241040000151
Figure BDA0002299241040000152
(2)时间更新：
Figure BDA0002299241040000153
Figure BDA0002299241040000154
Figure BDA0002299241040000155
Figure BDA0002299241040000156
Figure BDA0002299241040000157
ηi,k|k-1＝h(χi,k|k-1) (43)
Figure BDA0002299241040000158
(3)测量更新：
Figure BDA0002299241040000159
Figure BDA00022992410400001510
Figure BDA00022992410400001511
Figure BDA00022992410400001512
Figure BDA00022992410400001513
其中，Sigma点的参数为：
Figure BDA00022992410400001514
各个参数的含义解释如下：W(m)和W(c)为权值参数，S为状态向量的维数；
Figure BDA00022992410400001515
为Pk|k-1的平方根矩阵；α决定了Sigma点绕均值的散布范围，通常取一个很小的正值(1e-4≤α＜1)；κ是一个比例系数，一般在状态估计时设为0，在参数估计时设为3-L；β是另一个比例系数，用于合并状态分布的先验知识，对于高斯分布，其最优值为2。
步骤6：根据公式(1)计算当前的姿态矩阵
Figure BDA0002299241040000161
并输出当前的姿态角信息。
步骤7：重复步骤3-6直到初始对准过程结束。
本实施例中对准过程结束的判据是判断对准时间是否超过了设定时间，设定时间一般为5-10分钟。
水下快速动基座对准装置实施例：
本实施例中水下快速动基座对准装置包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，所述处理器在执行所述计算机程序时实现水下快速动基座对准方法实施例中的水下快速动基座对准方法。
水下快速动基座对准方法的具体实施过程在上述水下快速动基座对准方法实施例中已经介绍，这里不做过多赘述。

Claims (6)

1.一种水下快速动基座对准方法，其特征在于，包括以下步骤：
1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；
2)在每个惯性导航更新周期内，根据惯性导航系统输出的角速度信息更新
Figure FDA0003579587730000011
并根据多普勒输出的速度观测值更新
Figure FDA0003579587730000012
其中，多普勒测速仪的更新周期长于惯性导航系统的更新周期，在每个多普勒测速仪更新周期内，载体的速度为恒定值；
Figure FDA0003579587730000013
为载体坐标系b到惯性凝固载体坐标系
Figure FDA0003579587730000014
的方向余弦矩阵，
Figure FDA0003579587730000015
为初始时刻导航坐标系n0到导航坐标系n的转换矩阵；
3)在每个多普勒测速仪的更新周期内，根据初始时刻的经度、纬度和时间信息更新
Figure FDA0003579587730000016
并建立系统方程和观测方程，利用非线性滤波算法计算
Figure FDA0003579587730000017
为惯性凝固导航坐标系
Figure FDA0003579587730000018
到初始时刻导航坐标系n0的转换矩阵，
Figure FDA0003579587730000019
为惯性凝固载体坐标系
Figure FDA00035795877300000110
到惯性凝固导航坐标系
Figure FDA00035795877300000111
的方向余弦矩阵；
所述系统方程为：
Figure FDA00035795877300000112
其中，x＝[α β γ]T，α为方位角误差，β为俯仰角误差，γ为横滚角误差；
所述观测方程为：z＝(I-δC)V2(t)；其中，z为观测量，
Figure FDA00035795877300000113
δC为姿态误差矩阵，
Figure FDA00035795877300000114
Figure FDA00035795877300000115
Figure FDA00035795877300000116
Figure FDA00035795877300000117
为t时刻载体坐标系b到惯性凝固载体坐标系
Figure FDA00035795877300000118
的方向余弦矩阵，
Figure FDA00035795877300000119
为多普勒载体坐标系d到载体坐标系b的方向余弦矩阵，
Figure FDA00035795877300000120
为t时刻多普勒测速仪输出的速度、
Figure FDA00035795877300000121
为t+Δt时刻载体坐标系b到惯性凝固载体坐标系
Figure FDA0003579587730000021
的方向余弦矩阵、
Figure FDA0003579587730000022
为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，fb为惯性导航系统输出的比力；
Figure FDA0003579587730000023
为矩阵
Figure FDA0003579587730000024
当前计算值的转置，ωie为地球自转角速度，
Figure FDA0003579587730000025
为地球自转角速度在导航坐标系n中的投影，
Figure FDA0003579587730000026
为重力加速度在导航坐标系n中的投影，rE为地球椭球的卯酉圈曲率半径，L表示纬度，
Figure FDA0003579587730000027
为离心力；
4)根据更新的
Figure FDA0003579587730000028
和
Figure FDA0003579587730000029
计算当前的姿态矩阵
Figure FDA00035795877300000210
并输出当前的姿态角信息；
5)重复步骤2)-4)，直至满足初始对准结束条件；
计算
Figure FDA00035795877300000211
的过程包括：
A：在每个多普勒周期内根据建立的系统方程和观测方程，利用非线性滤波算法求α、β和γ；
B：根据α、β和γ，计算δC；
C：根据δC，利用
Figure FDA00035795877300000212
更新
Figure FDA00035795877300000213
将更新后的
Figure FDA00035795877300000214
作为
Figure FDA00035795877300000215
所述非线性滤波算法为无迹卡尔曼滤波算法。
2.根据权利要求1所述的水下快速动基座对准方法，其特征在于，
Figure FDA00035795877300000216
为
Figure FDA00035795877300000217
的转置矩阵，
Figure FDA00035795877300000218
的求解过程为：
1)设定初始时刻的
Figure FDA00035795877300000219
2)在每个惯性导航更新周期内，更新矩阵
Figure FDA00035795877300000220
在每个惯性导航更新周期，载体北向位置增量
Figure FDA00035795877300000221
和东向位置增量
Figure FDA00035795877300000222
为：
Figure FDA00035795877300000223
则当前的地理经纬度更新如下：
Figure FDA0003579587730000031
Figure FDA0003579587730000032
rN为地球椭球子午圈曲率半径，矩阵
Figure FDA0003579587730000033
的更新公式如下：
Figure FDA0003579587730000034
其中：
Figure FDA0003579587730000035
其中，k为惯性导航的周期数，tk为第k个周期对应的时间，tk+1为第k+1个周期对应的时间，rE为地球椭球的卯酉圈曲率半径，L(tk)为tk时刻载体所在位置的纬度，λ(tk)为tk时刻载体所在位置的经度，
Figure FDA0003579587730000036
为凝固惯性坐标系
Figure FDA0003579587730000037
到tk时刻的导航坐标系nk的转换矩阵。
3.根据权利要求1所述的水下快速动基座对准方法，其特征在于，所述初始对准结束的条件为对准时间超过设定值，所述设定值为5-10分钟。
4.一种水下快速动基座对准装置，包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，其特征在于，所述处理器在执行所述计算机程序时实现以下步骤：
1)获取载体初始时刻的经度和纬度，载体上惯性导航系统输出的比力和角速度信息，以及多普勒测速仪输出的速度观测值；
2)在每个惯性导航更新周期内，根据惯性导航系统输出的角速度信息更新
Figure FDA0003579587730000038
并根据多普勒输出的速度观测值更新
Figure FDA0003579587730000039
其中，多普勒测速仪的更新周期长于惯性导航系统的更新周期，在每个多普勒测速仪更新周期内，载体的速度为恒定值；
Figure FDA0003579587730000041
为载体坐标系b到惯性凝固载体坐标系
Figure FDA0003579587730000042
的方向余弦矩阵，
Figure FDA0003579587730000043
为初始时刻导航坐标系n0到导航坐标系n的转换矩阵；
3)在每个多普勒测速仪的更新周期内，根据初始时刻的经度、纬度和时间信息更新
Figure FDA0003579587730000044
并建立系统方程和观测方程，利用非线性滤波算法计算
Figure FDA0003579587730000045
Figure FDA0003579587730000046
为惯性凝固导航坐标系
Figure FDA0003579587730000047
到初始时刻导航坐标系n0的转换矩阵，
Figure FDA0003579587730000048
为惯性凝固载体坐标系
Figure FDA0003579587730000049
到惯性凝固导航坐标系
Figure FDA00035795877300000410
的方向余弦矩阵；
所述系统方程为：
Figure FDA00035795877300000411
其中，x＝[α β γ]T，α为方位角误差，β为俯仰角误差，γ为横滚角误差；
所述观测方程为：z＝(I-δC)V2(t)；其中，z为观测量，
Figure FDA00035795877300000412
δC为姿态误差矩阵，
Figure FDA00035795877300000413
Figure FDA00035795877300000414
Figure FDA00035795877300000415
Figure FDA00035795877300000416
为t时刻载体坐标系b到惯性凝固载体坐标系
Figure FDA00035795877300000417
的方向余弦矩阵，
Figure FDA00035795877300000418
为多普勒载体坐标系d到载体坐标系b的方向余弦矩阵，
Figure FDA00035795877300000419
为t时刻多普勒测速仪输出的速度、
Figure FDA00035795877300000420
为t+Δt时刻载体坐标系b到惯性凝固载体坐标系
Figure FDA00035795877300000421
的方向余弦矩阵、
Figure FDA00035795877300000422
为t+Δt时刻多普勒测速仪输出的速度，t为时间变量，Δt为时间增量，fb为惯性导航系统输出的比力；
Figure FDA00035795877300000423
为矩阵
Figure FDA00035795877300000424
当前计算值的转置，ωie为地球自转角速度，
Figure FDA00035795877300000425
为地球自转角速度在导航坐标系n中的投影，
Figure FDA00035795877300000426
为重力加速度在导航坐标系n中的投影，rE为地球椭球的卯酉圈曲率半径，L表示纬度，
Figure FDA0003579587730000051
为离心力；
4)根据更新的
Figure FDA0003579587730000052
和
Figure FDA0003579587730000053
计算当前的姿态矩阵
Figure FDA0003579587730000054
并输出当前的姿态角信息；
5)重复步骤2)-4)，直至满足初始对准结束条件；
计算
Figure FDA0003579587730000055
的过程包括：
A：在每个多普勒周期内根据建立的系统方程和观测方程，利用非线性滤波算法求α、β和γ；
B：根据α、β和γ，计算δC；
C：根据δC，利用
Figure FDA0003579587730000056
更新
Figure FDA0003579587730000057
将更新后的
Figure FDA0003579587730000058
作为
Figure FDA0003579587730000059
所述非线性滤波算法为无迹卡尔曼滤波算法。
5.根据权利要求4所述的水下快速动基座对准装置，其特征在于，
Figure FDA00035795877300000510
为
Figure FDA00035795877300000511
的转置矩阵，
Figure FDA00035795877300000512
的求解过程为：
1)设定初始时刻的
Figure FDA00035795877300000513
2)在每个惯性导航更新周期内，更新矩阵
Figure FDA00035795877300000514
在每个惯性导航更新周期，载体北向位置增量
Figure FDA00035795877300000515
和东向位置增量
Figure FDA00035795877300000516
为：
Figure FDA00035795877300000517
则当前的地理经纬度更新如下：
Figure FDA00035795877300000518
Figure FDA00035795877300000519
rN为地球椭球子午圈曲率半径，矩阵
Figure FDA0003579587730000061
的更新公式如下：
Figure FDA0003579587730000062
其中：
Figure FDA0003579587730000063
其中，k为惯性导航的周期数，tk为第k个周期对应的时间，tk+1为第k+1个周期对应的时间，rE为地球椭球的卯酉圈曲率半径，L(tk)为tk时刻载体所在位置的纬度，λ(tk)为tk时刻载体所在位置的经度，
Figure FDA0003579587730000064
为凝固惯性坐标系
Figure FDA0003579587730000065
到tk时刻的导航坐标系nk的转换矩阵。
6.根据权利要求4所述的水下快速动基座对准装置，其特征在于，所述初始对准结束的条件为对准时间超过设定值，所述设定值为5-10分钟。
CN201911214881.6A 2019-12-02 2019-12-02 一种水下快速动基座对准方法及装置 Active CN110873577B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911214881.6A CN110873577B (zh) 2019-12-02 2019-12-02 一种水下快速动基座对准方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911214881.6A CN110873577B (zh) 2019-12-02 2019-12-02 一种水下快速动基座对准方法及装置

Publications (2)

Publication Number Publication Date
CN110873577A CN110873577A (zh) 2020-03-10
CN110873577B true CN110873577B (zh) 2022-05-17

Family

ID=69717409

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911214881.6A Active CN110873577B (zh) 2019-12-02 2019-12-02 一种水下快速动基座对准方法及装置

Country Status (1)

Country Link
CN (1) CN110873577B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114234974A (zh) * 2021-11-04 2022-03-25 中国人民解放军海军潜艇学院 一种基于发射坐标系的水下航行体导航方法
CN116295511B (zh) * 2022-12-16 2024-04-02 南京安透可智能系统有限公司 一种用于管道潜航机器人的鲁棒初始对准方法及系统

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6094163A (en) * 1998-01-21 2000-07-25 Min-I James Chang Ins alignment method using a doppler sensor and a GPS/HVINS
US7739045B2 (en) * 2006-05-31 2010-06-15 Honeywell International Inc. Rapid self-alignment of a strapdown inertial system through real-time reprocessing
CN102519460B (zh) * 2011-12-09 2014-11-05 东南大学 一种捷联惯性导航系统非线性对准方法
CN103017787A (zh) * 2012-07-03 2013-04-03 哈尔滨工程大学 适用于摇摆晃动基座的初始对准方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN105203129B (zh) * 2015-10-13 2019-05-07 上海华测导航技术股份有限公司 一种惯导装置初始对准方法
CN107063245B (zh) * 2017-04-19 2020-12-25 东南大学 一种基于5阶ssrckf的sins/dvl组合导航滤波方法
CN107479076B (zh) * 2017-08-08 2020-11-03 北京大学 一种动基座下联合滤波初始对准方法

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Initial Alignment for a Doppler Velocity Log-Aided Strapdown Inertial Navigation System With Limited Information";Chang, LB 等;《ASME Transactions on Mechatronics》;20171231;第22卷(第1期);正文第330-335页 *
"Optimization-based INS in-motion alignment approach for underwater vehicles";Li, WL 等;《Optik》;20131230;第124卷(第20期);正文第4581-4584页 *

Also Published As

Publication number Publication date
CN110873577A (zh) 2020-03-10

Similar Documents

Publication Publication Date Title
WO2020062807A1 (zh) 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN111102993A (zh) 一种旋转调制型捷联惯导系统晃动基座初始对准方法
CN112284414A (zh) 一种基于多渐消因子自适应动机座旋转调制精对准方法
CN110006427B (zh) 一种低动态高振动环境下的bds/ins紧组合导航方法
CN105806363A (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN111156986B (zh) 一种基于抗差自适应ukf的光谱红移自主组合导航方法
CN110849360B (zh) 面向多机协同编队飞行的分布式相对导航方法
Xue et al. In-motion alignment algorithm for vehicle carried SINS based on odometer aiding
CN110873577B (zh) 一种水下快速动基座对准方法及装置
CN103245357A (zh) 一种船用捷联惯导系统二次快速对准方法
CN108548535B (zh) 低速gnss/mems组合导航系统的初始化方法
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN115855049A (zh) 基于粒子群优化鲁棒滤波的sins/dvl导航方法
CN111722295A (zh) 一种水下捷联式重力测量数据处理方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN113008229A (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN104154914A (zh) 一种空间稳定型捷联惯导系统初始姿态测量方法
CN108279025B (zh) 一种基于重力信息的光纤陀螺罗经快速精对准方法
CN106643726B (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