CN110285834B - 基于一点位置信息的双惯导系统快速自主重调方法 - Google Patents

基于一点位置信息的双惯导系统快速自主重调方法 Download PDF

Info

Publication number
CN110285834B
CN110285834B CN201910609900.9A CN201910609900A CN110285834B CN 110285834 B CN110285834 B CN 110285834B CN 201910609900 A CN201910609900 A CN 201910609900A CN 110285834 B CN110285834 B CN 110285834B
Authority
CN
China
Prior art keywords
inertial navigation
navigation system
error
speed
information
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
CN201910609900.9A
Other languages
English (en)
Other versions
CN110285834A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN201910609900.9A priority Critical patent/CN110285834B/zh
Publication of CN110285834A publication Critical patent/CN110285834A/zh
Application granted granted Critical
Publication of CN110285834B publication Critical patent/CN110285834B/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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明提供的是一种基于一点位置信息的双惯导系统快速自主重调方法。一：根据获得的一点外部位置信息，对系统A进行位置重调，二：系统A实时解算的位置作为系统B的外部辅助信息进行kalman滤波，利用估计出来的误差对系统B的导航参数进行校正，系统B得到准确的速度、位置和姿态信息，三：系统B被校正之后，再利用系统B的位置和速度作为系统A的外部辅助信息进行滤波，对系统A的导航参数进行校正，系统A得到准确的位置、速度和姿态信息，最终实现两套系统快速自主重调。本发明无需连续外部测量设备辅助即可实现系统的快速自主重调，不仅缩短了重调时间，而且能够有效利用量测信息，适用于水下环境。

Description

基于一点位置信息的双惯导系统快速自主重调方法
技术领域
本发明涉及的是一种捷联惯性导航系统水下导航方法，具体地说是一种双惯导系统的自主重调方法。
背景技术
捷联惯性导航系统最大特点就是自主性，它可以不接收外界任何信息，利用自身的加速度计和陀螺仪敏感到运载体的线速度和角速度信息，从而提供速度、位置及姿态等多种导航信息。此外，它还具有全天候、抗干扰、隐蔽性高、体积小、价格低等诸多优点，可以满足多方面的需求。捷联惯性导航系统对水下航行器更是具有重要意义，其作用是为长期在水下潜航的水下航行器连续提供安全航行所需的导航参数和运动参数。
大中型水下航行器通常装备两套或者多套高精度惯性导航系统。采用两套或多套配置方案主要有两个优点，一是可提高水下航行器的任务可靠性，当一套惯导故障时，另一套惯导仍可保障任务的继续执行；二是如果多套惯导采取分区布置，可就近为水下航行器载荷系统提供更精确的姿态信息，减小了船体变形对载荷系统的影响。
但是如果惯导系统长时间工作不进行重调校正，会由于误差积累使系统导航精度下降。为了保证惯性导航系统能长期有效的工作，需要引入外部信息定期或者不定期的对系统进行重调，使其导航参数得到修正，提高惯导系统的精度。系统重调即综合校正，以往的文献中，例如在专利申请号为201310156535.3，名称为“一种惯性系下使用位置和航向信息的捷联惯导系统陀螺漂移校正方法”的专利文件中公开的校正方法，利用设备连续两次获得外部位置和航向信息，以平台漂移角为中间变量在惯性系下建立陀螺漂移与位置误差和航向误差的关系，经过一次位置和方位重调后，在第二次获得位置误差和航向误差后可以计算出陀螺漂移。但是该方法需要精确的航向信息基准，而在实际环境中，准确的航向信息通常难以获得，因此该方法有很大的局限性。又如在专利申请号为201310156551.2，名称为“一种惯性系下仅使用位置信息的捷联惯导系统陀螺漂移和航向误差校正方法”的专利文件中公开的校正方法，利用设备连续三次获得外部位置信息，以平台漂移角为中间变量在惯性系下建立陀螺漂移和位置误差的关系，经过两次位置重调后，在第三次获得位置误差后可以计算出陀螺漂移和航向误差。在以上方式中都需要花费几个小时的时间，并且需要多次利用位置信息，而对于一些水下载体而言，多点的位置信息难以获得，因此这种方式并不适用。再如专利申请号为201210015077.7，名称为“一种船用惯性导航系统单点海上校准方法”的专利文件中公开的方法，在船舶远航一段时间积累一定导航误差后，通过获得外部提供一次单点位置导航数据，即可对系统进行校准。而该单点校正算法只能对系统位置进行重调，不能对惯导系统的速度和姿态进行校正，因此无法满足高精度惯性导航系统精度的要求。
发明内容
本发明的目的在于提供一种无需连续外部测量设备辅助，能够提高导航精度的基于一点位置信息的双惯导系统快速自主重调方法。
本发明的目的是这样实现的：
步骤一：根据获得的一点外部位置信息，对惯导系统A进行位置重调，
步骤二：惯导系统A实时解算的位置作为惯导系统B的外部辅助信息进行kalman滤波，利用估计出来的误差对惯导系统B的导航参数进行校正，惯导系统B得到准确的速度、位置和姿态信息，
步骤三：惯导系统B被校正之后，再利用惯导系统B的位置和速度作为惯导系统A的外部辅助信息进行滤波，对惯导系统A的导航参数进行校正，惯导系统A得到准确的位置、速度和姿态信息，最终实现两套系统快速自主重调，
在滤波过程中根据惯导系统位置误差随时间发散的特性将量测噪声方差阵的对角元素设置成斜坡函数以调节量测信息的利用率。
本发明还可以包括：
1.步骤一具体包括：
1.1对惯导系统A和惯导系统B的惯性测量元件进行充分预热；
1.2初始化两套惯导系统的导航参数，实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度
Figure BDA0002122029590000021
惯导系统A采集到加速度计输出的比力为
Figure BDA0002122029590000022
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000023
惯导系统B采集到加速度计输出的比力为
Figure BDA0002122029590000024
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000025
1.3惯导系统A和惯导系统B进行初始对准，得到载体系b系到导航坐标系n系的初始捷联姿态矩阵
Figure BDA0002122029590000026
和
Figure BDA0002122029590000027
之后开始进入导航工作状态；
1.4水下航行器航行一段时间后，在t0时刻获得一点外部位置信息，利用该位置信息对此刻惯导系统A输出的位置数据进行重调，得到
Figure BDA0002122029590000028
其中，
Figure BDA0002122029590000029
表示t0时刻获取的一点位置纬度，λ(t0)表示t0时刻获取的一点位置经度，
Figure BDA00021220295900000210
表示t0时刻系统A解算的纬度，λA(t0)表示t0时刻系统A解算的经度；
1.5惯导系统A的位置被重调之后再继续进行惯导解算，实时输出位置信息，纬度为
Figure BDA0002122029590000031
经度为λA(t)，其中t表示当前时刻。
2.步骤二具体包括：
2.1选取惯导系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量；
2.2利用惯导系统A的位置被重调之后惯导系统A实时更新的导航纬度
Figure BDA0002122029590000032
和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息，与惯导系统B导航更新计算的位置信息纬度
Figure BDA0002122029590000033
和经度λB(t)分别作差，将所得到的位置差值作为惯导系统B进行kalman滤波时的量测值；
2.3建立惯导系统B的kalman滤波器的状态方程和量测方程；
2.4根据惯导系统A位置误差随时间积累的特性，对惯导系统B滤波时的量测噪声方差阵RB(t)进行实时调节的设置；
2.5基于kalman滤波算法对惯导系统B的误差进行实时估计，得到惯导系统B的位置误差、速度误差和平台失准角的状态估计值；
2.6利用步骤2.5中估计出来的误差来校正惯导系统B的导航参数，完成惯导系统B的自主重调。
3.步骤三具体包括：
3.1选取惯导系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量；
3.2惯导系统B的位置和速度都经过校正，将重调后惯导系统B实时输出的位置
Figure BDA0002122029590000034
和λB(t)以及速度
Figure BDA0002122029590000035
作为惯导系统A的外部辅助信息，与惯导系统A导航更新计算的位置信息
Figure BDA0002122029590000036
和λA(t)以及速度信息
Figure BDA0002122029590000037
分别作差，将所得到的位置和速度差值作为惯导系统A进行kalman滤波时的量测值；
3.3建立惯导系统A的kalman滤波器的状态方程和量测方程；
3.4基于kalman滤波算法对惯导系统A的误差进行实时估计，得到惯导系统A的位置误差、速度误差和平台失准角的状态估计值；
3.5利用步骤3.4估计出来的误差来校正惯导系统A的导航参数，完成惯导系统A的自主重调。
4.将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数，具体形式表示为：
RB(t)＝diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2}
其中，t表示当前时刻，t0表示获得一点外部位置信息的时刻。
5.惯导系统A进行kalman滤波时的量测值ZA(t)为：
Figure BDA0002122029590000041
其中，
Figure BDA0002122029590000042
为惯导系统A解算的纬度、λA(t)为其解算的经度；
Figure BDA0002122029590000043
为惯导系统B解算的纬度、λB(t)为其解算的经度；
Figure BDA0002122029590000044
为惯导系统A解算的东向速度、
Figure BDA0002122029590000045
为其北向速度、
Figure BDA0002122029590000046
为其天向速度；
Figure BDA0002122029590000047
为惯导系统B解算的东向速度、
Figure BDA0002122029590000048
为其北向速度、
Figure BDA0002122029590000049
为其天向速度。
本发明针对装备有两套高精度惯性导航系统的大中型水下航行器，提出了一种基于一点位置信息的高精度双惯导系统快速自主重调方法。该方法无需连续外部测量设备辅助(只需要从外部设备中获取一点位置信息即可)，大大缩短了系统重调的时间。该方法不仅可以对两套系统的位置进行重调，而且可以对系统的速度和姿态进行校正，提高导航精度。
本发明有益效果在于：
第一，无需连续外部测量设备辅助(只需要从外部设备中获取一点位置信息即可)，缩短了系统重调所需的时间，易于实施，适用于各种水下航行器。第二，本发明不仅可以对水下航行器上两套高精度惯性导航系统的位置进行重调，而且可以对其速度和姿态进行校正，提高导航精度。第三，本发明中系统B进行kalman滤波时会根据外观测量的特性对量测噪声方差阵进行实时调节，以提高估计精度，进一步提高了导航精度。
附图说明
图1本发明的实现流程图。
图2高精度捷联惯性导航系统B自主重调原理图。
图3高精度捷联惯性导航系统A自主重调原理图。
具体实施方式
下面举例对本发明做更详细的描述。
具体实施方式一：
步骤1、首先对两套高精度光纤陀螺捷联惯性导航系统A和B的惯性测量元件进行充分预热；
步骤2、初始化两套高精度惯导系统A和B的导航参数，实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度
Figure BDA0002122029590000051
(惯导系统A采集到加速度计输出的比力为
Figure BDA0002122029590000052
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000053
惯导系统B采集到加速度计输出的比力为
Figure BDA0002122029590000054
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000055
)；
步骤3、捷联惯性导航系统A和B进行初始对准，得到载体系(b系)到导航坐标系(n系，本发明中导航坐标系选取地理坐标系)的初始捷联姿态矩阵
Figure BDA0002122029590000056
和
Figure BDA0002122029590000057
之后开始进入导航工作状态；
步骤4、水下航行器航行一段时间后，在t0时刻获得一点外部位置信息，利用该位置信息对此刻惯性导航系统A输出的位置数据进行重调，得到
Figure BDA0002122029590000058
其中，
Figure BDA0002122029590000059
表示t0时刻获取的一点位置纬度，λ(t0)表示t0时刻获取的一点位置经度，
Figure BDA00021220295900000510
表示t0时刻系统A解算的纬度，λA(t0)表示t0时刻系统A解算的经度；
步骤5、系统A的位置被重调之后再继续进行惯导解算，实时输出位置信息，纬度为
Figure BDA00021220295900000511
经度为λA(t)，其中t表示当前时刻；
步骤6、选取系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量；
步骤7、利用步骤5中位置重调后惯导系统A实时更新的导航纬度
Figure BDA00021220295900000512
和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息，与惯导系统B导航更新计算的位置信息(纬度
Figure BDA00021220295900000513
和经度λB(t))分别作差，将所得到的位置差值作为系统B进行kalman滤波时的量测值ZB(t)：
Figure BDA00021220295900000514
步骤8、建立惯导系统B的kalman滤波器的状态方程和量测方程；
步骤9、系统B进行kalman滤波时是利用系统A的位置作为外部辅助信息的，因此根据系统A的位置随时间发散的特性对量测噪声方差阵RB(t)进行设置。本发明将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数，以便对其进行实时调节，具体形式如下：
RB(t)＝diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2} (3)
其中，t表示当前时刻，t0表示获得一点外部位置信息的时刻；
步骤10、基于kalman滤波算法对系统B的误差进行实时估计，得到系统B的位置误差、速度误差和平台失准角的状态估计值；
步骤11、利用步骤10中估计出来的误差来校正系统B的导航参数，完成系统B的自主重调；
步骤12、选取系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量；
步骤13、在步骤11之后，系统B的位置和速度都经过了校正，此处利用重调后的系统B来辅助系统A再次进行重调，进一步来提高系统A的导航精度。当用系统B来辅助系统A时，除了系统B实时输出的位置信息
Figure BDA0002122029590000061
λB(t)之外，还增加了速度信息
Figure BDA0002122029590000062
来作为系统A的外部辅助信息，系统A进行kalman滤波时的量测值ZA(t)为：
Figure BDA0002122029590000063
其中，
Figure BDA0002122029590000064
为系统A解算的纬度、λA(t)为其解算的经度；
Figure BDA0002122029590000065
为系统B解算的纬度、λB(t)为其解算的经度；
Figure BDA0002122029590000066
为系统A解算的东向速度、
Figure BDA0002122029590000067
为其北向速度、
Figure BDA0002122029590000068
为其天向速度；
Figure BDA0002122029590000069
为系统B解算的东向速度、
Figure BDA00021220295900000610
为其北向速度、
Figure BDA00021220295900000611
为其天向速度；
步骤14、建立惯导系统A的kalman滤波器的状态方程和量测方程；
步骤15、基于kalman滤波算法对系统A的误差进行实时估计，得到系统A的位置误差、速度误差和平台失准角的状态估计值；
步骤16、利用步骤15中估计出来的误差来校正系统A的导航参数，完成系统A的自主重调。
具体实施方式二：
步骤1、对两套高精度光纤陀螺捷联惯性导航系统A和B的惯性测量元件进行充分预热；
步骤2、初始化两套高精度惯导系统A和B的导航参数，实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度
Figure BDA0002122029590000071
(惯导系统A采集到加速度计输出的比力为
Figure BDA0002122029590000072
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000073
惯导系统B采集到加速度计输出的比力为
Figure BDA0002122029590000074
光纤陀螺仪输出的角速度为
Figure BDA0002122029590000075
)；
步骤3、捷联惯性导航系统A和B进行初始对准，分别得到载体系(b系)到导航坐标系(n系，本发明中导航坐标系选取地理坐标系)的初始捷联姿态矩阵
Figure BDA0002122029590000076
和
Figure BDA0002122029590000077
之后开始进入导航工作状态；
步骤4、根据步骤3中得到的光纤陀螺捷联惯性导航系统A的初始捷联姿态矩阵
Figure BDA0002122029590000078
和步骤2中实时采集到角速度
Figure BDA0002122029590000079
递推更新得到姿态四元数并对其进行归一化计算，归一化后的姿态四元数qA具体表达式为：
qA＝[qA1 qA2 qA3 qA4]T (1)
其中，qA1、qA2、qA3和qA4是归一化后四元数qA的元素；
步骤5、根据步骤4中得到的归一化后的姿态四元数qA计算得到新的捷联姿态矩阵
Figure BDA00021220295900000710
Figure BDA00021220295900000711
步骤6、利用步骤5得到的捷联姿态矩阵
Figure BDA00021220295900000712
将步骤2采集到的光纤陀螺捷联惯性导航系统A的加速度计输出的比力
Figure BDA00021220295900000713
转换到导航坐标系下：
Figure BDA00021220295900000714
其中，
Figure BDA00021220295900000715
表示加速度计输出比力在导航坐标系的投影；
步骤7、根据步骤6中系统A的加速度计输出比力在导航坐标系的投影
Figure BDA00021220295900000716
去除有害加速度后得到系统A的加速度
Figure BDA00021220295900000717
进一步更新计算得到速度，记为
Figure BDA00021220295900000718
Figure BDA00021220295900000719
其中，
Figure BDA00021220295900000720
为系统A解算的东向速度、
Figure BDA00021220295900000721
为其北向速度、
Figure BDA00021220295900000722
为其天向速度，t表示当前时刻；
步骤8、利用步骤7更新计算后的速度
Figure BDA0002122029590000081
更新计算捷联惯性导航系统A的导航纬度
Figure BDA0002122029590000082
和经度λA(t)；
步骤9、水下航行器航行一段时间后，在t0时刻获得一点外部位置信息，利用该位置信息对此刻惯性导航系统A输出的位置数据进行重调，得到
Figure BDA0002122029590000083
其中，
Figure BDA0002122029590000084
表示t0时刻获取的一点位置纬度，λ(t0)表示t0时刻获取的一点位置经度，
Figure BDA0002122029590000085
表示t0时刻系统A解算的纬度，λA(t0)表示t0时刻系统A解算的经度；
步骤10、系统A位置被重调之后再继续进行惯导解算，实时输出位置信息
Figure BDA0002122029590000086
和λA(t)；
步骤11、根据步骤3中得到的光纤陀螺捷联惯性导航系统B的初始捷联姿态矩阵
Figure BDA0002122029590000087
和步骤2中实时采集到角速度
Figure BDA0002122029590000088
递推得到姿态四元数并对其进行归一化计算，归一化后的姿态四元数qB为：
qB＝[qB1 qB2 qB3 qB4]T (6)
其中，qB1、qB2、qB3和qB4是归一化后四元数qB的元素；
根据归一化后的姿态四元数qB计算得到新的捷联姿态矩阵
Figure BDA0002122029590000089
Figure BDA00021220295900000810
步骤12、根据步骤2的采集到的光纤陀螺捷联惯性导航系统B的加速度计输出的比力
Figure BDA00021220295900000811
和步骤11更新计算后的新的捷联姿态矩阵
Figure BDA00021220295900000812
进一步计算得到速度
Figure BDA00021220295900000813
Figure BDA00021220295900000814
其中，
Figure BDA00021220295900000815
为系统B解算的东向速度、
Figure BDA00021220295900000816
为其北向速度、
Figure BDA00021220295900000817
为其天向速度。之后利用更新后的速度
Figure BDA00021220295900000818
计算出捷联惯性导航系统B的导航纬度
Figure BDA00021220295900000819
和经度λB(t)；
步骤13、选取系统B的位置误差
Figure BDA00021220295900000820
(纬度误差
Figure BDA00021220295900000821
经度误差δλB和高度误差δhB)、东北天三个方向的速度误差δVB＝[δVB_E δVB_N δVB_U]T、平台失准角误差φB＝[φB_x φB_y φB_z]T、载体系三轴的加速度计的零位偏移ΔAB＝[ΔAB_bx ΔAB_by ΔAB_bz]T和陀螺仪的常值漂移εB＝[εB_bx εB_by εB_bz]T为kalman滤波的状态估计量XB，即
XB＝[δrB δVB φB ΔAB εB]T
步骤14、选取系统B进行kalman滤波的系统噪声向量WB为：
Figure BDA0002122029590000091
其中，
Figure BDA0002122029590000092
和
Figure BDA0002122029590000093
为系统B在载体系中三轴加速度计的随机噪声，
Figure BDA0002122029590000094
和
Figure BDA0002122029590000095
为系统B在载体系中三轴陀螺仪的随机噪声，均为高斯白噪声；
步骤15、利用步骤10中位置重调后惯导系统A实时更新的导航纬度
Figure BDA0002122029590000096
和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息，与步骤12中惯导系统B导航更新计算的位置信息(纬度
Figure BDA0002122029590000097
和经度λB(t))分别作差，将所得到的位置差值作为系统B进行kalman滤波时的量测值ZB(t)：
Figure BDA0002122029590000098
步骤16、建立惯导系统B的kalman滤波器的状态方程和量测方程：
Figure BDA0002122029590000099
其中，FB(t)是15×15维的状态一步转移矩阵，GB(t)是15×6维的系统噪声驱动矩阵，两者是根据惯性导航系统误差方程得到的系统结构参数；HB(t)是2×15维的量测矩阵；WB(t)是6×1维的系统噪声向量，VB(t)是2×1维的量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)，且它们之间互不相关，系统噪声方差阵为QB(t)，量测噪声方差阵为RB(t)；
步骤17、系统B进行kalman滤波时是利用系统A的位置作为外部辅助信息的，根据系统A的位置随时间发散的特性对量测噪声方差阵RB(t)进行设置。将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数，以便对其进行实时调节，具体形式如下：
RB(t)＝diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2} (11)
其中，t表示当前时刻，t0表示获得一点外部位置信息的时刻；
步骤18、将步骤16中的连续的状态方程和量测方程离散化：
Figure BDA0002122029590000101
其中，k代表kalman滤波时刻，
Figure BDA0002122029590000102
是离散化后的状态一步转移矩阵；
Figure BDA0002122029590000103
是离散化后系统噪声驱动矩阵；
Figure BDA0002122029590000104
是离散化后的量测矩阵；
Figure BDA0002122029590000105
是离散化后的系统噪声向量，离散化后的系统噪声方差阵为
Figure BDA0002122029590000106
Figure BDA0002122029590000107
是离散化后的量测噪声向量，离散化后的量测噪声方差阵为
Figure BDA0002122029590000108
步骤19、初始化系统B的kalman滤波器的滤波参数，即设置kalman滤波状态估计量的初值
Figure BDA0002122029590000109
和状态估计均方误差的初值P0 B；
步骤20、基于kalman滤波算法对系统B的误差进行实时估计，滤波步骤如下：
(1)状态一步预测
Figure BDA00021220295900001010
Figure BDA00021220295900001011
(2)状态一步预测均方误差
Figure BDA00021220295900001012
Figure BDA00021220295900001013
(3)滤波增益
Figure BDA00021220295900001014
Figure BDA00021220295900001015
(4)状态估计
Figure BDA00021220295900001016
Figure BDA00021220295900001017
(5)状态估计均方误差
Figure BDA00021220295900001018
Figure BDA00021220295900001019
其中，I为15×15维的单位矩阵；
步骤21、根据步骤20中kalman滤波算法得到系统B误差进行实时估计，得到系统B的纬度误差估计值
Figure BDA00021220295900001020
经度误差估计值
Figure BDA00021220295900001021
东向速度误差估计值
Figure BDA00021220295900001022
北向速度误差估计值
Figure BDA0002122029590000111
天向速度误差估计值
Figure BDA0002122029590000112
以及三轴平台失准角的估计值
Figure BDA0002122029590000113
步骤22、利用步骤21中实时估计出来的纬度误差
Figure BDA0002122029590000114
经度误差
Figure BDA0002122029590000115
和东北天三个方向的速度误差
Figure BDA0002122029590000116
对系统B实时更新的纬度
Figure BDA0002122029590000117
经度λB(t)及速度
Figure BDA0002122029590000118
重调，即：
Figure BDA0002122029590000119
步骤23、利用步骤20中实时估计出来的平台失准角
Figure BDA00021220295900001110
来构造补偿四元数
Figure BDA00021220295900001111
对惯导系统B导航更新的当前时刻捷联姿态矩阵
Figure BDA00021220295900001112
对应的姿态四元数qB＝[qB1 qB2 qB3 qB4]T进行补偿得到修正后的四元数q′B＝[q′B1 q′B2q′B3 q′B4]T：
Figure BDA00021220295900001113
其中，修正后的四元数q′B的元素为：
Figure BDA00021220295900001114
步骤24、将步骤23中得到的修正后的四元数q′B进行归一化处理：
Figure BDA00021220295900001115
得到归一化后的四元数为：
Figure BDA00021220295900001116
其中，
Figure BDA00021220295900001117
和
Figure BDA00021220295900001118
是归一化后四元数
Figure BDA00021220295900001119
的元素；
步骤25、根据归一化后的姿态四元数
Figure BDA00021220295900001120
计算得到校正后的捷联姿态矩阵
Figure BDA00021220295900001121
Figure BDA0002122029590000121
至此，完成系统B的自主重调过程；
步骤26、选取系统A的位置误差
Figure BDA0002122029590000122
(纬度误差
Figure BDA0002122029590000123
经度误差δλA和高度误差δhA)、东北天三个方向的速度误差δVA＝[δVA_E δVA_N δVA_U]T、平台失准角误差φA＝[φA_x φA_y φA_z]T、载体系三轴加速度计的零位偏移ΔAA＝[ΔAA_bx ΔAA_by ΔAA_bz]T和陀螺仪的常值漂移εA＝[εA_bx εA_by εA_bz]T为kalman滤波的状态估计量XA，即
XA＝[δrA δVA φA ΔAA εA]T
步骤27、选取系统A进行kalman滤波的系统噪声向量为WA：
Figure BDA0002122029590000124
其中，
Figure BDA0002122029590000125
和
Figure BDA0002122029590000126
为系统A在载体系中三轴加速度计的随机噪声，
Figure BDA0002122029590000127
和
Figure BDA0002122029590000128
为系统A在载体系中三轴陀螺的随机噪声，均为高斯白噪声；
步骤28、在步骤25之后，系统B已经完成自主重调，将系统B实时输出的位置信息
Figure BDA0002122029590000129
λB(t)和速度信息
Figure BDA00021220295900001210
作为系统A的外部辅助信息，与惯导系统A导航更新计算的位置信息
Figure BDA00021220295900001211
λA(t)和速度信息
Figure BDA00021220295900001212
分别作差，并将所得到的位置和速度差值作为系统A进行kalman滤波时的量测值ZA(t)：
Figure BDA00021220295900001213
步骤29、建立惯导系统A的kalman滤波器的状态方程和量测方程：
Figure BDA00021220295900001214
其中，FA(t)是15×15维的状态一步转移矩阵，GA(t)是15×6维的系统噪声驱动矩阵，两者是根据惯性导航系统误差方程得到的系统结构参数；HA(t)是5×15维的量测矩阵；WA(t)是6×1维的系统噪声向量，VA(t)是5×1维的量测噪声向量，两者都是零均值的高斯白噪声向量序列(服从正态分布)，且它们之间互不相关，系统噪声方差阵为QA(t)，量测噪声方差阵为RA(t)；
步骤30、将步骤29中的连续的状态方程和量测方程离散化：
Figure BDA0002122029590000131
其中，
Figure BDA0002122029590000132
是离散化后的状态一步转移矩阵；
Figure BDA0002122029590000133
是离散化后系统噪声驱动矩阵；
Figure BDA0002122029590000134
是离散化后的量测矩阵；
Figure BDA0002122029590000135
是离散化后的系统噪声向量，离散化后的系统噪声方差阵为
Figure BDA0002122029590000136
Figure BDA0002122029590000137
是离散化后的量测噪声向量，离散化后的量测噪声方差阵为
Figure BDA0002122029590000138
步骤31、初始化系统A的kalman滤波器的滤波参数，即设置kalman滤波状态估计量的初值
Figure BDA0002122029590000139
和状态估计均方误差的初值
Figure BDA00021220295900001322
步骤32、基于kalman滤波算法对系统A的误差进行实时估计，滤波步骤如下：
(1)状态一步预测
Figure BDA00021220295900001310
Figure BDA00021220295900001311
(2)状态一步预测均方误差
Figure BDA00021220295900001312
Figure BDA00021220295900001313
(3)滤波增益
Figure BDA00021220295900001314
Figure BDA00021220295900001315
(4)状态估计
Figure BDA00021220295900001316
Figure BDA00021220295900001317
(5)状态估计均方误差Pk A：
Figure BDA00021220295900001318
其中，I为15×15维的单位矩阵；
步骤33、根据步骤32中kalman滤波算法对系统A的误差进行实时估计，得到系统A的纬度误差估计值
Figure BDA00021220295900001319
经度误差估计值
Figure BDA00021220295900001320
东向速度误差估计值
Figure BDA00021220295900001321
北向速度误差估计值
Figure BDA0002122029590000141
天向速度误差估计值
Figure BDA0002122029590000142
以及三轴平台失准角的估计值
Figure BDA0002122029590000143
步骤34、利用步骤33中实时估计出来的纬度误差
Figure BDA0002122029590000144
经度误差
Figure BDA0002122029590000145
和东北天三个方向的速度误差
Figure BDA0002122029590000146
对系统A实时更新的纬度
Figure BDA0002122029590000147
经度λA(t)及速度
Figure BDA0002122029590000148
重调，即：
Figure BDA0002122029590000149
步骤35、利用步骤33中实时估计出来的平台失准角
Figure BDA00021220295900001410
来构造补偿四元数
Figure BDA00021220295900001411
对惯导系统A导航更新的当前时刻捷联姿态矩阵
Figure BDA00021220295900001412
对应的姿态四元数qA＝[qA1 qA2 qA3 qA4]T进行补偿得到修正后的四元数q′A＝[q′A1 q′A2q′A3 q′A4]T：
Figure BDA00021220295900001413
其中修正后的四元数q′A的元素为：
Figure BDA00021220295900001414
步骤36、将步骤35中得到的修正后的四元数q′A进行归一化处理：
Figure BDA00021220295900001415
得到归一化后的四元数为：
Figure BDA00021220295900001416
其中，
Figure BDA00021220295900001417
和
Figure BDA00021220295900001418
是归一化后四元数
Figure BDA00021220295900001419
的元素；
步骤37、根据归一化后的姿态四元数
Figure BDA00021220295900001420
计算得到校正后的捷联姿态矩阵
Figure BDA00021220295900001421
Figure BDA0002122029590000151
完成系统A的自主重调过程；
至此，完成了水下航行器高精度双捷联惯导系统A和B的自主重调，为水下航行器上的两套高精度惯导系统的提供准确的导航参数。

Claims (5)

1.一种基于一点位置信息的双惯导系统快速自主重调方法，其特征是：
步骤一：根据获得的一点外部位置信息，对惯导系统A进行位置重调，
1.1对惯导系统A和惯导系统B的惯性测量元件进行充分预热；
1.2初始化两套惯导系统的导航参数，实时采集加速度计输出的比力fb和光纤陀螺仪输出的角速度
Figure FDA0003876908540000011
惯导系统A采集到加速度计输出的比力为
Figure FDA0003876908540000012
光纤陀螺仪输出的角速度为
Figure FDA0003876908540000013
惯导系统B采集到加速度计输出的比力为
Figure FDA0003876908540000014
光纤陀螺仪输出的角速度为
Figure FDA0003876908540000015
1.3惯导系统A和惯导系统B进行初始对准，得到载体系b系到导航坐标系n系的初始捷联姿态矩阵
Figure FDA0003876908540000016
和
Figure FDA0003876908540000017
之后开始进入导航工作状态；
1.4水下航行器航行一段时间后，在t0时刻获得一点外部位置信息，利用该位置信息对此刻惯导系统A输出的位置数据进行重调，得到
Figure FDA0003876908540000018
其中，
Figure FDA0003876908540000019
表示t0时刻获取的一点位置纬度，λ(t0)表示t0时刻获取的一点位置经度，
Figure FDA00038769085400000110
表示t0时刻系统A解算的纬度，λA(t0)表示t0时刻系统A解算的经度；
1.5惯导系统A的位置被重调之后再继续进行惯导解算，实时输出位置信息，纬度为
Figure FDA00038769085400000111
经度为λA(t)，其中t表示当前时刻；
步骤二：惯导系统A实时解算的位置作为惯导系统B的外部辅助信息进行kalman滤波，利用估计出来的误差对惯导系统B的导航参数进行校正，惯导系统B得到准确的速度、位置和姿态信息，
步骤三：惯导系统B被校正之后，再利用惯导系统B的位置和速度作为惯导系统A的外部辅助信息进行滤波，对惯导系统A的导航参数进行校正，惯导系统A得到准确的位置、速度和姿态信息，最终实现两套系统快速自主重调，
在滤波过程中根据惯导系统位置误差随时间发散的特性将量测噪声方差阵的对角元素设置成斜坡函数以调节量测信息的利用率。
2.根据权利要求1所述的基于一点位置信息的双惯导系统快速自主重调方法，其特征是步骤二具体包括：
2.1选取惯导系统B的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移为kalman滤波的状态估计量；
2.2利用惯导系统A的位置被重调之后惯导系统A实时更新的导航纬度
Figure FDA0003876908540000021
和导航经度λA(t)作为惯导系统B进行kalman滤波的外部辅助信息，与惯导系统B导航更新计算的位置信息纬度
Figure FDA0003876908540000022
和经度λB(t)分别作差，将所得到的位置差值作为惯导系统B进行kalman滤波时的量测值；
2.3建立惯导系统B的kalman滤波器的状态方程和量测方程；
2.4根据惯导系统A位置误差随时间积累的特性，对惯导系统B滤波时的量测噪声方差阵RB(t)进行实时调节的设置；
2.5基于kalman滤波算法对惯导系统B的误差进行实时估计，得到惯导系统B的位置误差、速度误差和平台失准角的状态估计值；
2.6利用步骤2.5中估计出来的误差来校正惯导系统B的导航参数，完成惯导系统B的自主重调。
3.根据权利要求2所述的基于一点位置信息的双惯导系统快速自主重调方法，其特征是步骤三具体包括：
3.1选取惯导系统A的位置误差、速度误差、平台失准角误差、三轴加速度计的零位偏移和陀螺仪的常值漂移作为kalman滤波的状态估计量；
3.2惯导系统B的位置和速度都经过校正，将重调后惯导系统B实时输出的位置
Figure FDA0003876908540000023
和λB(t)以及速度
Figure FDA0003876908540000024
作为惯导系统A的外部辅助信息，与惯导系统A导航更新计算的位置信息
Figure FDA0003876908540000025
和λA(t)以及速度信息
Figure FDA0003876908540000026
分别作差，将所得到的位置和速度差值作为惯导系统A进行kalman滤波时的量测值；
3.3建立惯导系统A的kalman滤波器的状态方程和量测方程；
3.4基于kalman滤波算法对惯导系统A的误差进行实时估计，得到惯导系统A的位置误差、速度误差和平台失准角的状态估计值；
3.5利用步骤3.4估计出来的误差来校正惯导系统A的导航参数，完成惯导系统A的自主重调。
4.根据权利要求3所述的基于一点位置信息的双惯导系统快速自主重调方法，其特征是将量测噪声方差阵RB(t)中对角元素设置为斜率为0.01的斜坡函数，具体形式表示为：
RB(t)＝diag{[0.01×(t-t0)+10]2,[0.01×(t-t0)+10]2}
其中，t表示当前时刻，t0表示获得一点外部位置信息的时刻。
5.根据权利要求4所述的基于一点位置信息的双惯导系统快速自主重调方法，其特征是惯导系统A进行kalman滤波时的量测值ZA(t)为：
Figure FDA0003876908540000031
其中，
Figure FDA0003876908540000032
为惯导系统A解算的纬度、λA(t)为其解算的经度；
Figure FDA0003876908540000033
为惯导系统B解算的纬度、λB(t)为其解算的经度；
Figure FDA0003876908540000034
为惯导系统A解算的东向速度、
Figure FDA0003876908540000035
为其北向速度、
Figure FDA0003876908540000036
为其天向速度；
Figure FDA0003876908540000037
为惯导系统B解算的东向速度、
Figure FDA0003876908540000038
为其北向速度、
Figure FDA0003876908540000039
为其天向速度。
CN201910609900.9A 2019-07-08 2019-07-08 基于一点位置信息的双惯导系统快速自主重调方法 Active CN110285834B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910609900.9A CN110285834B (zh) 2019-07-08 2019-07-08 基于一点位置信息的双惯导系统快速自主重调方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910609900.9A CN110285834B (zh) 2019-07-08 2019-07-08 基于一点位置信息的双惯导系统快速自主重调方法

Publications (2)

Publication Number Publication Date
CN110285834A CN110285834A (zh) 2019-09-27
CN110285834B true CN110285834B (zh) 2022-12-13

Family

ID=68020970

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910609900.9A Active CN110285834B (zh) 2019-07-08 2019-07-08 基于一点位置信息的双惯导系统快速自主重调方法

Country Status (1)

Country Link
CN (1) CN110285834B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110824583A (zh) * 2019-11-21 2020-02-21 中国船舶重工集团公司第七0七研究所 一种航空重力仪用在线自主标定方法
CN112525216B (zh) * 2020-09-28 2023-04-28 中国船舶重工集团公司第七0七研究所 一种惯导系统东向陀螺漂移及航向误差校准方法
CN114323065B (zh) * 2021-11-24 2023-04-28 中国船舶重工集团公司第七0七研究所 基于多手段融合的水下自主导航系统误差监测与估计方法
CN116519011B (zh) * 2023-03-11 2024-03-01 中国人民解放军国防科技大学 基于Psi角误差修正模型的长航时双惯导协同标定方法

Family Cites Families (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2002099453A2 (en) * 2001-06-04 2002-12-12 Novatel Inc. An inertial/gps navigation system
WO2005071431A1 (en) * 2004-01-23 2005-08-04 Novatel Inc. Inertial gps navigation system with modified kalman filter
US7702460B2 (en) * 2006-06-17 2010-04-20 Northrop Grumman Guidance And Electronics Company, Inc. Estimate of relative position between navigation units
CN101788296B (zh) * 2010-01-26 2011-11-16 北京航空航天大学 一种sins/cns深组合导航系统及其实现方法
KR101213975B1 (ko) * 2010-11-05 2012-12-20 국방과학연구소 비행 중 정렬 장치 및 그 방법
CN102589570B (zh) * 2012-01-17 2015-03-11 北京理工大学 一种船用惯性导航系统单点海上校准方法
CN103453903A (zh) * 2013-08-26 2013-12-18 哈尔滨工程大学 一种基于惯性测量组件的管道探伤系统导航定位方法
CN103575299B (zh) * 2013-11-13 2016-09-21 北京理工大学 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN104019828A (zh) * 2014-05-12 2014-09-03 南京航空航天大学 高动态环境下惯性导航系统杆臂效应误差在线标定方法
CN103968839B (zh) * 2014-05-21 2017-02-22 哈尔滨工程大学 一种基于蜂群算法改进ckf的单点重力匹配方法
CN104776847B (zh) * 2015-04-09 2017-10-03 哈尔滨工程大学 一种适用于水下导航系统单点估计陀螺漂移的方法
CN105371844B (zh) * 2015-12-02 2018-02-16 南京航空航天大学 一种基于惯性/天文互助的惯性导航系统初始化方法
CN105806339B (zh) * 2016-05-14 2018-09-25 中卫物联成都科技有限公司 一种基于gnss、ins和守时系统的组合导航方法和设备
CN105865488B (zh) * 2016-05-19 2018-05-08 北京航空航天大学 一种基于自主量测信息的静基座动态快速精确对准方法
CN106123850B (zh) * 2016-06-28 2018-07-06 哈尔滨工程大学 Auv配载多波束声呐水下地形测绘修正方法
CN106441292B (zh) * 2016-09-28 2019-08-02 哈尔滨工业大学 一种基于众包imu惯导数据的楼宇室内平面图建立方法
CN108180925B (zh) * 2017-12-15 2020-09-01 中国船舶重工集团公司第七0七研究所 一种里程计辅助车载动态对准方法
CN109211266A (zh) * 2018-07-13 2019-01-15 哈尔滨工程大学 一种船用格网惯性导航系统综合校正方法
CN109186597B (zh) * 2018-08-31 2020-09-22 武汉大学 一种基于双mems-imu的室内轮式机器人的定位方法
CN109781100A (zh) * 2019-03-08 2019-05-21 哈尔滨工程大学 一种基于卡尔曼滤波估计的极区组合导航双通道误差校正方法

Also Published As

Publication number Publication date
CN110285834A (zh) 2019-09-27

Similar Documents

Publication Publication Date Title
CN110285834B (zh) 基于一点位置信息的双惯导系统快速自主重调方法
CN110501024B (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN106767900B (zh) 一种基于组合导航技术的船用光纤捷联惯导系统的在线标定方法
CN110146075B (zh) 一种增益补偿自适应滤波的sins/dvl组合定位方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN112629538A (zh) 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法
CN110146076B (zh) 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
EP1582840B1 (en) Inertial navigation system error correction
US7133776B2 (en) Attitude alignment of a slave inertial measurement system
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN108955676B (zh) 深海潜器导航系统和用于深海潜器状态切换的数据融合方法
CN111141273A (zh) 基于多传感器融合的组合导航方法及系统
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN111121766A (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN112504275A (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN110631574A (zh) 一种惯性/里程计/rtk多信息融合方法
CN108955671B (zh) 一种基于磁偏角、磁倾角的卡尔曼滤波导航方法
CN110388942B (zh) 一种基于角度和速度增量的车载姿态精对准系统
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN112798016A (zh) 一种基于sins与dvl组合的auv行进间快速初始对准方法
CN112747748A (zh) 一种基于逆向解算的领航auv导航数据后处理方法
CN107764268B (zh) 一种机载分布式pos传递对准的方法和装置
CN111307114A (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