CN112798021B - 基于激光多普勒测速仪的惯导系统行进间初始对准方法 - Google Patents

基于激光多普勒测速仪的惯导系统行进间初始对准方法 Download PDF

Info

Publication number
CN112798021B
CN112798021B CN202110403858.2A CN202110403858A CN112798021B CN 112798021 B CN112798021 B CN 112798021B CN 202110403858 A CN202110403858 A CN 202110403858A CN 112798021 B CN112798021 B CN 112798021B
Authority
CN
China
Prior art keywords
laser doppler
doppler velocimeter
error
inertial navigation
quaternion
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
CN202110403858.2A
Other languages
English (en)
Other versions
CN112798021A (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202110403858.2A priority Critical patent/CN112798021B/zh
Publication of CN112798021A publication Critical patent/CN112798021A/zh
Application granted granted Critical
Publication of CN112798021B publication Critical patent/CN112798021B/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

Abstract

本申请涉及一种基于激光多普勒测速仪的惯导系统行进间初始对准方法。所述方法包括：在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值、比力方程和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型和测量模型。以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器，得到状态向量的预测值对捷联惯导系统进行行进间初始对准。上述方法的过程模型和测量模型利用激光多普勒测速仪的高精度速度输出，在初始对准中考虑惯性测量单元误差，以及激光多普勒测速仪的安装倾角和刻度系数，能提高姿态估计精度。

Description

基于激光多普勒测速仪的惯导系统行进间初始对准方法
技术领域
本申请涉及组合导航技术领域，特别是涉及一种基于激光多普勒测速仪的惯导系统行进间初始对准方法。
背景技术
在整个导航过程中，初始对准精度的高低直接关系到惯导工作性能的好坏。现有的对准方式有传递对准，静基座对准以及行进间对准等。传递对准需要高精度的惯导作为参考，一般用于大型船舶或者某些特殊应用场合。静基座对准是高精度惯导常用的对准方式，需要载体在对准过程中保持静止。这使得静基座对准虽然有着很高的精度，但限制了载体的机动性，不能做到随用随走。行进间对准能够使惯导在运动情况下完成系统初始化，对于提高载体机动能力有着重要作用。与静基座对准不同，行进间对准需要外部设备来提供载体运动信息来对惯导系统的输出进行补偿和校正。
现阶段利用GPS提供精确的速度与位置信息来行进间初始对准最为普遍，但利用GPS有如下缺点：（1）GPS信号不稳定，GPS接收机抗干扰能力较差且GPS信号易被高大建筑物或者树木遮挡；（2）GPS是美国研制的第二代卫星导航系统，是一种非自主式系统，具有高度不可控性。虽然我国北斗卫星导航系统已经组网完成，但它同样是非自主的。此外，里程计也可以为车辆提供速度和里程信息，用里程计辅助的行进间初始对准具有完全自主的特点，因此被广泛采用，由于里程计的测量结果与车辆的车轮周长有关，并且车轮的周长受到车轮温度、压强以及磨损状况的影响，因此测量精度较低，另外车轮打滑、跳动也会使里程计的测量结果出现较大偏差。
激光多普勒测速仪是一种新型的速度传感器，能够实时提供精确的载体对地速度，因此利用激光多普勒测速仪辅助的行进间初始对准比里程计辅助的行进间初始对准有着更高的精度。由于激光多普勒测速仪辅助的行进间初始对准是完全自主的，因此比起GPS辅助的行进间初始对准有着更高抗干扰能力。
发明内容
基于此，有必要针对上述技术问题，提供一种基于激光多普勒测速仪的惯导系统行进间初始对准方法。
一种基于激光多普勒测速仪的惯导系统行进间初始对准方法，包括：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
其中一个实施例中，捷联惯导系统对准过程的过程模型为：
Figure 264508DEST_PATH_IMAGE001
其中，
Figure 501455DEST_PATH_IMAGE002
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 39883DEST_PATH_IMAGE003
为姿态方向余弦矩阵估计的导数，
Figure 34384DEST_PATH_IMAGE004
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 712533DEST_PATH_IMAGE005
表示求转动角速度的斜对称矩阵。
其中一个实施例中，基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度的方式包括：
Figure 413773DEST_PATH_IMAGE006
其中，
Figure 185420DEST_PATH_IMAGE007
，
Figure 729534DEST_PATH_IMAGE008
，
Figure 877618DEST_PATH_IMAGE009
表示n系中的东向速度，
Figure 698944DEST_PATH_IMAGE010
表示n系中的北向速度，
Figure 969388DEST_PATH_IMAGE011
和
Figure 672902DEST_PATH_IMAGE012
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 296781DEST_PATH_IMAGE013
表示载体所在地点的纬度。
其中一个实施例中，捷联惯导系统对准过程的测量模型为：
Figure 362826DEST_PATH_IMAGE014
其中，
Figure 476276DEST_PATH_IMAGE015
为激光多普勒测速仪在
Figure 604769DEST_PATH_IMAGE016
系中的速度，表示为，
Figure 94656DEST_PATH_IMAGE017
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 749628DEST_PATH_IMAGE018
为从
Figure 502821DEST_PATH_IMAGE019
系到
Figure 180927DEST_PATH_IMAGE020
系的转换矩阵，
Figure 271242DEST_PATH_IMAGE021
为激光多普勒测速仪的刻度系数，
Figure 718404DEST_PATH_IMAGE022
为激光多普勒测速仪的刻度系数误差，
Figure 376919DEST_PATH_IMAGE023
为激光多普勒测速仪与惯导设备的安装角误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器的k时刻的状态向量为：
Figure 870217DEST_PATH_IMAGE024
其中，
Figure 701907DEST_PATH_IMAGE025
Figure 941258DEST_PATH_IMAGE026
其中，
Figure 832991DEST_PATH_IMAGE027
为姿态误差的误差四元数
Figure 548006DEST_PATH_IMAGE028
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 121070DEST_PATH_IMAGE029
为误差四元数的标量部分，
Figure 542824DEST_PATH_IMAGE030
为误差四元数的矢量部分。
Figure 667775DEST_PATH_IMAGE031
的取值范围为[0,1]，
Figure 542190DEST_PATH_IMAGE032
为比例因子，
Figure 918944DEST_PATH_IMAGE033
、
Figure 523101DEST_PATH_IMAGE034
、
Figure 756636DEST_PATH_IMAGE035
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 56031DEST_PATH_IMAGE036
、
Figure 767635DEST_PATH_IMAGE037
、
Figure 226298DEST_PATH_IMAGE038
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 630734DEST_PATH_IMAGE039
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 683004DEST_PATH_IMAGE040
为激光多普勒测速仪在m系的航向安装角误差，
Figure 260616DEST_PATH_IMAGE041
为刻度系数误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器对协方差矩阵的平方根处理为：
Figure 511469DEST_PATH_IMAGE042
Figure 24490DEST_PATH_IMAGE043
Figure 688689DEST_PATH_IMAGE044
Figure 7675DEST_PATH_IMAGE045
Figure 316297DEST_PATH_IMAGE046
表示协方差矩阵的权重值，
Figure 124853DEST_PATH_IMAGE047
表示QR分解运算，
Figure 682873DEST_PATH_IMAGE048
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 743233DEST_PATH_IMAGE049
Figure 30995DEST_PATH_IMAGE050
其中
Figure 948135DEST_PATH_IMAGE051
为新息向量，
Figure 196714DEST_PATH_IMAGE052
为由传感器得到的测量值，
Figure 857502DEST_PATH_IMAGE053
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 999771DEST_PATH_IMAGE054
为自适应系数，初值为
Figure 291075DEST_PATH_IMAGE055
，
Figure 823687DEST_PATH_IMAGE056
称为渐消因子，常取
Figure 79045DEST_PATH_IMAGE057
。
Figure 216765DEST_PATH_IMAGE058
、
Figure 741287DEST_PATH_IMAGE059
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。其中一个实施例中，所述方法还包括：
根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
一种基于激光多普勒测速仪的惯导系统行进间初始对准装置，包括：
过程模型构建模块，用于在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
测量模型构建模块，用于基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
鲁棒平方根无迹四元数估计器构建模块，用于以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
行进间初始对准模块，用于使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
与现有技术相比，上述一种基于激光多普勒测速仪的惯导系统行进间初始对准方法、装置、计算机设备和存储介质，在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。基于比力方程式建立捷联惯导系统对准过程的测量模型。以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。本申请在过程模型和测量模型中均直接利用了激光多普勒测速仪的高精度速度输出，能够提高鲁棒平方根无迹四元数估计器对载体姿态的估计精度；此外，在行进间初始对准的过程中不仅考虑了惯性测量单元的误差，还考虑了激光多普勒测速仪的安装倾角和刻度系数的影响并进行了估计，提高了姿态估计的精度。
附图说明
图1为一个实施例中一种基于激光多普勒测速仪的惯导系统行进间初始对准方法的流程示意图；
图2为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
捷联惯导系统在使用时，将激光多普勒测速仪安装在车上合适的位置，以便测速仪的光斑能有效的打在地面上，并实时输出车辆的速度信息。惯性测量单元经过初始标定后得到陀螺和加速度计的零偏、安装误差角以及刻度系数误差，并将惯性测量单元安装在车辆后轮轴中央。向捷联惯导系统输入初始的位置信息后，车辆开始运动，利用运动的前十分钟进行行进间初始对准，也可以根据需要调整行进间初始对准的时间，在整个对准过程中无需用到GPS的信息，是一种完全自主的行进间对准过程。
本申请将整个行进间初始对准分为过程模型和测量模型的构建与鲁棒平方根无迹四元数估计过程两个过程，其中过程模型和测量模型的构建过程将激光多普勒测速仪的安装误差和刻度系数误差考虑进去构建更加完备的测量模型并采用了一种更加完备的过程模型。鲁棒平方根无迹四元数估计过程提出了一种鲁棒平方根无迹四元数估计方法，该方法避免了传统无迹四元数估计方法带来的协方差矩阵非正定问题、收敛速度慢的问题以及鲁棒性不高的问题，这将提高对准精度。与传统基于姿态确定的对准方法不同，本申请提出的对准方法有着更高的对准精度与鲁棒性，并能对激光多普勒测速仪的安装误差和刻度系数进行标定。与传统的姿态确定方法相比，本申请能够处理模型中的噪声并且还能估计除姿态以外的其他参数，这能够减小实际中的噪声干扰，并且在过程模型和测量模型中均利用了激光多普勒测速仪的输出而不是近似值，这大大提高了估计精度。
在一个实施例中，如图1所示，提供了一种基于激光多普勒测速仪的惯导系统行进间初始对准方法，包括以下步骤：
步骤102，在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
首先，根据姿态矩阵的链式法则对姿态矩阵进行分解：
Figure 557934DEST_PATH_IMAGE060
（1）
其中，
Figure 826104DEST_PATH_IMAGE061
系为捷联惯性导航系统的坐标系，定义为右-前-上，
Figure 818331DEST_PATH_IMAGE062
系为当地导航坐标系，定义为东-北-天。
Figure 310492DEST_PATH_IMAGE063
和
Figure 817697DEST_PATH_IMAGE064
为惯性非旋转结构，不随捷联惯导系统在地球表面运动而运动，在初始时刻分别与
Figure 561662DEST_PATH_IMAGE065
系和
Figure 533029DEST_PATH_IMAGE066
系对齐。
Figure 399354DEST_PATH_IMAGE067
表示t时刻姿态的方向余弦矩阵，
Figure 65958DEST_PATH_IMAGE068
表示初始时刻姿态的方向余弦矩阵。
传统对准方案中的过程模型为
Figure 675931DEST_PATH_IMAGE069
，
Figure 767384DEST_PATH_IMAGE070
，
Figure 476714DEST_PATH_IMAGE071
表示在t时刻b系相对初始时刻姿态变化的方向余弦矩阵。
Figure 958511DEST_PATH_IMAGE072
表示对
Figure 168912DEST_PATH_IMAGE073
求导数，
Figure 990238DEST_PATH_IMAGE074
表示在t时刻n系相对初始时刻姿态变化的方向余弦矩阵。
Figure 932786DEST_PATH_IMAGE075
表示在c系中观察到的b系绕a系的转动角速度，对应地
Figure 698617DEST_PATH_IMAGE076
为惯性测量单元（IMU）中的陀螺测量得到的载体角速度，
Figure 650392DEST_PATH_IMAGE077
表示在n系中观察到的n系绕i系的转动角速度（i系为地心惯性坐标系），
Figure 60645DEST_PATH_IMAGE078
表示求
Figure 767570DEST_PATH_IMAGE079
的斜对称矩阵。
由于在未对准情况下，激光多普勒测速仪辅助的捷联惯导系统不能获得准确的实时地速和位置，因此在传统方案中需要对
Figure 958380DEST_PATH_IMAGE080
进行近似，这会引入额外的误差。为了避免这一缺点，本实施例将激光多普勒测速仪的速度投影到n系，并以下面的方式构建
Figure 385950DEST_PATH_IMAGE081
：
Figure 978605DEST_PATH_IMAGE082
其中，
Figure 856432DEST_PATH_IMAGE083
表示n系中的东向速度，
Figure 206641DEST_PATH_IMAGE084
表示n系中的北向速度。
Figure 500220DEST_PATH_IMAGE085
和
Figure 9698DEST_PATH_IMAGE086
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 730530DEST_PATH_IMAGE087
为载体所在地点的纬度。
由于激光多普勒测速仪提供的载体速度在整个对准过程中起着至关重要的作用，因此本实施例在过程模型中加入测速仪的速度。具体地，将初始时刻的b系表示为惯性系i系建立过程模型：
Figure 833615DEST_PATH_IMAGE088
（2）
步骤104，基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
对比力方程式
Figure 665305DEST_PATH_IMAGE089
进行变换，得到测量模型表达式，
Figure 294869DEST_PATH_IMAGE090
（3）
Figure 124285DEST_PATH_IMAGE091
（4）
其中，
Figure 511404DEST_PATH_IMAGE092
表示激光多普勒测速仪在
Figure 209101DEST_PATH_IMAGE093
系中的速度，由于激光多普勒测速仪与惯导系统存在安装倾角以及激光多普勒测速仪的输出与真实速度存在一定比例关系，建立跟激光多普勒测速仪相关的
Figure 630856DEST_PATH_IMAGE094
系，则激光多普勒测速仪在
Figure 631173DEST_PATH_IMAGE093
系中的输出表示为
Figure 833484DEST_PATH_IMAGE095
（5）
Figure 6976DEST_PATH_IMAGE096
为
Figure 220920DEST_PATH_IMAGE094
系到
Figure 454455DEST_PATH_IMAGE097
系的转换矩阵，
Figure 878483DEST_PATH_IMAGE098
为刻度系数。
Figure 855666DEST_PATH_IMAGE099
为激光多普勒测速仪与惯导系统的安装角误差，
Figure 455275DEST_PATH_IMAGE100
为激光多普勒测速仪的刻度系数误差。
步骤106，以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
本实施例设计的鲁棒平方根无迹四元数估计器采用姿态误差、陀螺零偏漂移、加速度计零偏漂移、激光多普勒测速仪俯仰角误差、激光多普勒测速仪航向角误差以及激光多普勒测速仪刻度系数误差作为状态量。步骤106可以进一步详细描述为：
1）构建状态向量为：
Figure 922028DEST_PATH_IMAGE101
（6）
其中，
Figure 505457DEST_PATH_IMAGE102
（7）
Figure 224014DEST_PATH_IMAGE103
（8）
Figure 474867DEST_PATH_IMAGE104
（9）
Figure 378101DEST_PATH_IMAGE105
为表征所求姿态误差的四元数的误差，
Figure 386508DEST_PATH_IMAGE106
为其广义罗德里格斯参数形式，
Figure 705494DEST_PATH_IMAGE107
为取值范围从0到1的参数，
Figure 879029DEST_PATH_IMAGE108
为比例因子，
Figure 828531DEST_PATH_IMAGE109
和
Figure 652130DEST_PATH_IMAGE108
的不同组合有着不同的物理意义，这里取较为常用的参数组合，即
Figure 571545DEST_PATH_IMAGE109
=1，
Figure 796990DEST_PATH_IMAGE110
=1。
Figure 651813DEST_PATH_IMAGE111
为陀螺漂移偏差估计，
Figure 25026DEST_PATH_IMAGE112
为加速度计漂移偏差估计，
Figure 685814DEST_PATH_IMAGE113
为激光多普勒测速仪的俯仰安装角误差，
Figure 703449DEST_PATH_IMAGE114
为激光多普勒测速仪的航向安装角误差，
Figure 791490DEST_PATH_IMAGE115
为刻度系数误差。
2）根据状态向量和相应的协方差矩阵，作SUT变换，其采样方程为：
Figure 651999DEST_PATH_IMAGE116
（10）
期望和协方差矩阵的加权权重
Figure 54162DEST_PATH_IMAGE117
和
Figure 988620DEST_PATH_IMAGE118
计算如下：
Figure 309879DEST_PATH_IMAGE119
（11）
Figure 533050DEST_PATH_IMAGE120
（12）
其中，
Figure 801221DEST_PATH_IMAGE121
为状态向量的维数，
Figure 652502DEST_PATH_IMAGE122
，
Figure 82346DEST_PATH_IMAGE123
为一大于零的比例因子，在本发明中取
Figure 527234DEST_PATH_IMAGE124
。
Figure 661412DEST_PATH_IMAGE125
为调节因子，用于捕捉高阶信息，通常取
Figure 304883DEST_PATH_IMAGE126
或
Figure 108891DEST_PATH_IMAGE127
，当状态维数
Figure 837813DEST_PATH_IMAGE128
时，
Figure 510103DEST_PATH_IMAGE129
取
Figure 211342DEST_PATH_IMAGE130
。对于高斯分布
Figure 248568DEST_PATH_IMAGE131
取2，对于其他分布则需要通过试验调整，在本发明中
Figure 527103DEST_PATH_IMAGE131
取2。
Figure 878450DEST_PATH_IMAGE132
为对协方差矩阵Cholesky分解得到的下三角矩阵。
3）经过SUT变换得到的采样点可分为姿态部分非姿态部分。
Figure 496513DEST_PATH_IMAGE133
（13）
利用（8）式的逆过程将姿态部分用误差四元数表示，即
Figure 766957DEST_PATH_IMAGE134
（14）
4）通过将误差四元数乘以当前估计值来定义新的基于四元数的sigma点，定义一组新的采样点
Figure 470471DEST_PATH_IMAGE135
，并通过过程模型向前传播得到预测的采样点
Figure 359930DEST_PATH_IMAGE136
（15）
5）利用预测四元数得到新的误差四元数
Figure 832499DEST_PATH_IMAGE137
（16）
利用（8）式得到误差四元数的修正罗德里格斯参数形式。新的状态向量预测值的
Figure 539424DEST_PATH_IMAGE138
点为
Figure 667917DEST_PATH_IMAGE139
（17）
6）状态预测和协方差为
Figure 157804DEST_PATH_IMAGE140
（18）
Figure 812777DEST_PATH_IMAGE141
（19）
Figure 300390DEST_PATH_IMAGE142
（20）
7）利用状态预测值和协方差的平方根形式重复（2.2）-（2.3）过程，并利用得到的误差四元数乘以状态预测值来定义新的基于四元数的sigma点
Figure 978496DEST_PATH_IMAGE143
定义新的采样点为
Figure 334391DEST_PATH_IMAGE144
（21）
8）（21）式中的sigma点通过测量模型（3）式传播，得到鲁棒平方根无迹四元数估计器的观测量
Figure 781553DEST_PATH_IMAGE145
为：
Figure 440067DEST_PATH_IMAGE146
得到预测的观测值后，通过加权求和得到系统预测的均值以及协方差：
Figure 339890DEST_PATH_IMAGE147
（22）
Figure 233897DEST_PATH_IMAGE148
（23）
Figure 4407DEST_PATH_IMAGE149
（24）
Figure 896139DEST_PATH_IMAGE150
（25）
9）定义新息矢量为：
Figure 345575DEST_PATH_IMAGE151
（26）
对测量噪声协方差矩阵进行自适应：
Figure 980956DEST_PATH_IMAGE152
（27）
Figure 340393DEST_PATH_IMAGE153
（28）
Figure 403027DEST_PATH_IMAGE154
（29）
其中
Figure 339759DEST_PATH_IMAGE155
为由传感器得到的测量值，
Figure 513251DEST_PATH_IMAGE156
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 992774DEST_PATH_IMAGE157
为自适应系数，初值为
Figure 306204DEST_PATH_IMAGE158
，
Figure 871178DEST_PATH_IMAGE159
称为渐消因子，常取
Figure 848361DEST_PATH_IMAGE160
。
Figure 41445DEST_PATH_IMAGE161
、
Figure 445882DEST_PATH_IMAGE162
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
10）然后进行量测更新：
Figure 232572DEST_PATH_IMAGE163
（30）
Figure 341342DEST_PATH_IMAGE164
（31）
Figure 326616DEST_PATH_IMAGE165
（32）
Figure 839637DEST_PATH_IMAGE166
（33）
11）最后进行姿态更新，（32）式可以写为
Figure 503836DEST_PATH_IMAGE167
，利用（8）式的逆方程得到
Figure 822822DEST_PATH_IMAGE168
对应的误差四元数
Figure 865865DEST_PATH_IMAGE169
，并通过
Figure 612104DEST_PATH_IMAGE170
更新姿态四元数。
最后将
Figure 498020DEST_PATH_IMAGE171
置零，然后进入下个循环。
步骤108，使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
步骤110，根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
本实施例提供的一种基于激光多普勒测速仪的惯导系统行进间初始对准方法具有以下优势：
1）所提出的鲁棒平方根无迹四元数估计器的过程模型和测量模型中均直接利用了激光多普勒测速仪的高精度速度输出，而不是只利用惯导本身的速度，提高了无迹四元数估计器对姿态的估计精度。
2）在行进间初始对准的过程中采用了基于姿态估计的方法而不是寻常的基于姿态确定的方法，在对姿态进行估计的同时还充分估计了其他误差参数，提高了姿态估计的准确性。
3）在行进间初始对准的过程中不仅仅考虑了惯性测量单元的误差，还考虑了激光多普勒测速仪的安装倾角和刻度系数的影响并进行了估计，对准过程中所估计的激光多普勒测速仪安装倾角和刻度系数可作为后续过程的的初始值，无需再另外获得初值或者另设初值，并基于对准的结果在后续过程中实时反馈校正，提高了姿态估计的精度。
4）传统方案在对准过程中的初值有着很大误差，且对准过程中存在许多近似，因此对准过程会产生较大的位置误差与速度误差。本实施例没有采用传统方案中估计恒定初始姿态
Figure 355118DEST_PATH_IMAGE172
的方式，不再对
Figure 518246DEST_PATH_IMAGE173
做近似，并且直接对
Figure 763282DEST_PATH_IMAGE174
进行估计，减小了传统方案近似对对准结果造成的影响。
5）使用了激光多普勒测速仪来辅助捷联惯导系统进行行进间初始对准，比使用GPS辅助的行进间对准有着更高的抗干扰能力和自主性，比使用里程计辅助的行进间对准有着更高的对准精度。
6）鲁棒平方根无迹四元数估计器与传统的无迹四元数估计器相比有着更好的鲁棒性和稳定性，避免了无迹卡尔曼滤波可能导致的协方差矩阵非正定问题，并使收敛速度大大提升。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，提供了一种基于激光多普勒测速仪的惯导系统行进间初始对准装置，包括：
过程模型构建模块，用于在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
测量模型构建模块，用于基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
鲁棒平方根无迹四元数估计器构建模块，用于以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
行进间初始对准模块，用于使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
其中一个实施例中，过程模型构建模块建立的过程模型为：
Figure 808599DEST_PATH_IMAGE175
其中，
Figure 672650DEST_PATH_IMAGE176
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 814918DEST_PATH_IMAGE177
为姿态方向余弦矩阵估计的导数，
Figure 902960DEST_PATH_IMAGE178
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 373255DEST_PATH_IMAGE179
表示求转动角速度的斜对称矩阵。
其中一个实施例中，过程模型构建模块用于基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度为：
Figure 837735DEST_PATH_IMAGE180
其中，
Figure 834510DEST_PATH_IMAGE181
，
Figure 296715DEST_PATH_IMAGE182
，
Figure 316624DEST_PATH_IMAGE183
表示n系中的东向速度，
Figure 381531DEST_PATH_IMAGE184
表示n系中的北向速度，
Figure 170496DEST_PATH_IMAGE185
和
Figure 803603DEST_PATH_IMAGE186
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 638703DEST_PATH_IMAGE187
表示载体所在地点的纬度。
其中一个实施例中，测量模型构建模块建立的测量模型为：
Figure 179406DEST_PATH_IMAGE188
其中，
Figure 26139DEST_PATH_IMAGE189
为激光多普勒测速仪在
Figure 626885DEST_PATH_IMAGE190
系中的速度，表示为，
Figure 683703DEST_PATH_IMAGE191
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 231359DEST_PATH_IMAGE192
为从
Figure 994915DEST_PATH_IMAGE193
系到
Figure 94458DEST_PATH_IMAGE194
系的转换矩阵，
Figure 310676DEST_PATH_IMAGE195
为激光多普勒测速仪的刻度系数，
Figure 662023DEST_PATH_IMAGE196
为激光多普勒测速仪的刻度系数误差，
Figure 342403DEST_PATH_IMAGE197
为激光多普勒测速仪与惯导设备的安装角误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器构建模块建立的状态向量为：
Figure 550531DEST_PATH_IMAGE198
其中，
Figure 191727DEST_PATH_IMAGE199
Figure 205820DEST_PATH_IMAGE200
其中，
Figure 943969DEST_PATH_IMAGE201
为姿态误差的误差四元数
Figure 260681DEST_PATH_IMAGE202
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 451490DEST_PATH_IMAGE203
为误差四元数的标量部分，
Figure 269274DEST_PATH_IMAGE204
为误差四元数的矢量部分。
Figure 534033DEST_PATH_IMAGE205
的取值范围为[0,1]，
Figure 83963DEST_PATH_IMAGE206
为比例因子，
Figure 824386DEST_PATH_IMAGE207
、
Figure 117964DEST_PATH_IMAGE208
、
Figure 502809DEST_PATH_IMAGE209
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 291817DEST_PATH_IMAGE210
、
Figure 457219DEST_PATH_IMAGE211
、
Figure 226592DEST_PATH_IMAGE212
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 793839DEST_PATH_IMAGE213
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 747889DEST_PATH_IMAGE214
为激光多普勒测速仪在m系的航向安装角误差，
Figure 338270DEST_PATH_IMAGE215
为刻度系数误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器构建模块对协方差矩阵的平方根处理为：
Figure 973651DEST_PATH_IMAGE216
Figure 192142DEST_PATH_IMAGE217
Figure 254776DEST_PATH_IMAGE218
Figure 66875DEST_PATH_IMAGE219
Figure 833842DEST_PATH_IMAGE220
表示协方差矩阵的权重值，
Figure 110103DEST_PATH_IMAGE221
表示QR分解运算，
Figure 281321DEST_PATH_IMAGE222
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 643032DEST_PATH_IMAGE223
Figure 682533DEST_PATH_IMAGE224
其中
Figure 16562DEST_PATH_IMAGE225
为新息向量，
Figure 420998DEST_PATH_IMAGE226
为由传感器得到的测量值，
Figure 332323DEST_PATH_IMAGE227
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 50880DEST_PATH_IMAGE228
为自适应系数，初值为
Figure 301733DEST_PATH_IMAGE229
，
Figure 939387DEST_PATH_IMAGE230
称为渐消因子，常取
Figure 275691DEST_PATH_IMAGE231
。
Figure 532360DEST_PATH_IMAGE232
、
Figure 637719DEST_PATH_IMAGE233
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
其中一个实施例中，还包括激光多普勒测速仪标定模块，用于根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
关于一种基于激光多普勒测速仪的惯导系统行进间初始对准装置的具体限定可以参见上文中对于一种基于激光多普勒测速仪的惯导系统行进间初始对准方法的限定，在此不再赘述。上述一种基于激光多普勒测速仪的惯导系统行进间初始对准装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图2所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于激光多普勒测速仪的惯导系统行进间初始对准方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图2中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立无迹四元数估计器。
使用无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立过程模型为：
Figure 711854DEST_PATH_IMAGE234
其中，
Figure 207558DEST_PATH_IMAGE235
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 330235DEST_PATH_IMAGE236
为姿态方向余弦矩阵估计的导数，
Figure 352417DEST_PATH_IMAGE237
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 269558DEST_PATH_IMAGE238
表示求转动角速度的斜对称矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于激光多普勒测速仪的速度计算在n系中观察到n系绕i系的转动角速度为：
Figure 783716DEST_PATH_IMAGE239
其中，
Figure 506821DEST_PATH_IMAGE240
，
Figure 321193DEST_PATH_IMAGE241
，
Figure 612497DEST_PATH_IMAGE242
表示n系中的东向速度，
Figure 145110DEST_PATH_IMAGE243
表示n系中的北向速度，
Figure 671906DEST_PATH_IMAGE244
和
Figure 544047DEST_PATH_IMAGE245
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 68569DEST_PATH_IMAGE246
表示载体所在地点的纬度。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立测量模型为：
Figure 150795DEST_PATH_IMAGE247
其中，
Figure 153386DEST_PATH_IMAGE248
为激光多普勒测速仪在
Figure 880033DEST_PATH_IMAGE249
系中的速度，表示为，
Figure 903353DEST_PATH_IMAGE250
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 144979DEST_PATH_IMAGE251
为从
Figure 888944DEST_PATH_IMAGE252
系到
Figure 797994DEST_PATH_IMAGE253
系的转换矩阵，
Figure 461056DEST_PATH_IMAGE254
为激光多普勒测速仪的刻度系数，
Figure 658819DEST_PATH_IMAGE255
为激光多普勒测速仪的刻度系数误差，
Figure 3213DEST_PATH_IMAGE256
为激光多普勒测速仪与惯导设备的安装角误差。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立状态向量为：
Figure 829087DEST_PATH_IMAGE257
其中，
Figure 600734DEST_PATH_IMAGE258
Figure 20214DEST_PATH_IMAGE259
其中，
Figure 168298DEST_PATH_IMAGE260
为姿态误差的误差四元数
Figure 848678DEST_PATH_IMAGE261
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 260068DEST_PATH_IMAGE262
为误差四元数的标量部分，
Figure 963582DEST_PATH_IMAGE263
为误差四元数的矢量部分。
Figure 706236DEST_PATH_IMAGE264
的取值范围为[0,1]，
Figure 444385DEST_PATH_IMAGE265
为比例因子，
Figure 495517DEST_PATH_IMAGE266
、
Figure 14223DEST_PATH_IMAGE267
、
Figure 504110DEST_PATH_IMAGE268
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 34449DEST_PATH_IMAGE269
、
Figure 584379DEST_PATH_IMAGE270
、
Figure 324802DEST_PATH_IMAGE271
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 352801DEST_PATH_IMAGE272
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 3225DEST_PATH_IMAGE273
为激光多普勒测速仪在m系的航向安装角误差，
Figure 786373DEST_PATH_IMAGE274
为刻度系数误差。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：对协方差矩阵的平方根处理为：
Figure 951775DEST_PATH_IMAGE275
Figure 721148DEST_PATH_IMAGE276
Figure 22816DEST_PATH_IMAGE277
Figure 242445DEST_PATH_IMAGE278
Figure 832826DEST_PATH_IMAGE279
表示协方差矩阵的权重值，
Figure 202628DEST_PATH_IMAGE280
表示QR分解运算，
Figure 686699DEST_PATH_IMAGE281
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 952595DEST_PATH_IMAGE282
其中
Figure 827010DEST_PATH_IMAGE283
为新息向量，
Figure 328399DEST_PATH_IMAGE284
为由传感器得到的测量值，
Figure 604659DEST_PATH_IMAGE285
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 775878DEST_PATH_IMAGE286
为自适应系数，初值为
Figure 137589DEST_PATH_IMAGE287
，
Figure 177089DEST_PATH_IMAGE288
称为渐消因子，常取
Figure 511118DEST_PATH_IMAGE289
。
Figure 915555DEST_PATH_IMAGE290
、
Figure 826879DEST_PATH_IMAGE291
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型。
基于比力方程式建立捷联惯导系统对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立过程模型为：
Figure 545436DEST_PATH_IMAGE292
其中，
Figure 796289DEST_PATH_IMAGE293
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 433944DEST_PATH_IMAGE294
为姿态方向余弦矩阵估计的导数，
Figure 770247DEST_PATH_IMAGE295
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 292495DEST_PATH_IMAGE296
表示求转动角速度的斜对称矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于激光多普勒测速仪的速度计算在n系中观察到n系绕i系的转动角速度为：
Figure 460172DEST_PATH_IMAGE297
其中，
Figure 206411DEST_PATH_IMAGE298
，
Figure 702114DEST_PATH_IMAGE299
，
Figure 824791DEST_PATH_IMAGE300
表示n系中的东向速度，
Figure 112553DEST_PATH_IMAGE301
表示n系中的北向速度，
Figure 967376DEST_PATH_IMAGE302
和
Figure 278272DEST_PATH_IMAGE303
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 1377DEST_PATH_IMAGE304
表示载体所在地点的纬度。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立测量模型为：
Figure 815750DEST_PATH_IMAGE305
其中，
Figure 107054DEST_PATH_IMAGE306
为激光多普勒测速仪在
Figure 967562DEST_PATH_IMAGE307
系中的速度，表示为，
Figure 166462DEST_PATH_IMAGE308
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 38603DEST_PATH_IMAGE309
为从
Figure 563126DEST_PATH_IMAGE310
系到
Figure 645351DEST_PATH_IMAGE311
系的转换矩阵，
Figure 851205DEST_PATH_IMAGE312
为激光多普勒测速仪的刻度系数，
Figure 640169DEST_PATH_IMAGE313
为激光多普勒测速仪的刻度系数误差，
Figure 132330DEST_PATH_IMAGE314
为激光多普勒测速仪与惯导设备的安装角误差。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立状态向量为：
Figure 639535DEST_PATH_IMAGE315
其中，
Figure 383500DEST_PATH_IMAGE316
其中，
Figure 26971DEST_PATH_IMAGE317
为姿态误差的误差四元数
Figure 227051DEST_PATH_IMAGE318
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 893656DEST_PATH_IMAGE319
为误差四元数的标量部分，
Figure 503629DEST_PATH_IMAGE320
为误差四元数的矢量部分。
Figure 329503DEST_PATH_IMAGE321
的取值范围为[0,1]，
Figure 101149DEST_PATH_IMAGE322
为比例因子，
Figure 520629DEST_PATH_IMAGE323
、
Figure 668714DEST_PATH_IMAGE324
、
Figure 349094DEST_PATH_IMAGE325
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 494905DEST_PATH_IMAGE326
、
Figure 198418DEST_PATH_IMAGE327
、
Figure 212511DEST_PATH_IMAGE328
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 888343DEST_PATH_IMAGE329
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 267372DEST_PATH_IMAGE330
为激光多普勒测速仪在m系的航向安装角误差，
Figure 520498DEST_PATH_IMAGE331
为刻度系数误差。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：对协方差矩阵的平方根处理为：
Figure 10386DEST_PATH_IMAGE332
Figure 540724DEST_PATH_IMAGE333
Figure 90654DEST_PATH_IMAGE334
Figure 831077DEST_PATH_IMAGE335
Figure 62338DEST_PATH_IMAGE336
表示协方差矩阵的权重值，
Figure 509500DEST_PATH_IMAGE337
表示QR分解运算，
Figure 292648DEST_PATH_IMAGE338
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 661313DEST_PATH_IMAGE339
Figure 493002DEST_PATH_IMAGE340
其中
Figure 856988DEST_PATH_IMAGE341
为新息向量，
Figure 748720DEST_PATH_IMAGE342
为由传感器得到的测量值，
Figure 135839DEST_PATH_IMAGE343
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 771220DEST_PATH_IMAGE344
为自适应系数，初值为
Figure 255291DEST_PATH_IMAGE345
，
Figure 317925DEST_PATH_IMAGE346
称为渐消因子，常取
Figure 395602DEST_PATH_IMAGE347
。
Figure 631412DEST_PATH_IMAGE348
、
Figure 907672DEST_PATH_IMAGE349
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。在一个实施例中，计算机程序被处理器执行时还实现以下步骤：根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (6)

1.一种基于激光多普勒测速仪的惯导系统行进间初始对准方法，其特征在于，所述方法包括：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导系统对准过程的过程模型；
基于比力方程式建立捷联惯导系统对准过程的测量模型；其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数；
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器；
使用所述鲁棒平方根无迹四元数估计器得到所述状态向量的预测值，根据所述预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导系统进行行进间初始对准；
所述捷联惯导系统对准过程的过程模型为：
Figure 86237DEST_PATH_IMAGE001
其中，
Figure 759795DEST_PATH_IMAGE002
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 489853DEST_PATH_IMAGE003
为所述姿态方向余弦矩阵估计的导数，
Figure 627574DEST_PATH_IMAGE004
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 542309DEST_PATH_IMAGE005
表示所述转动角速度的斜对称矩阵；
基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度的方式包括：
Figure 93376DEST_PATH_IMAGE006
其中，
Figure 236912DEST_PATH_IMAGE007
，
Figure 494718DEST_PATH_IMAGE008
，
Figure 455721DEST_PATH_IMAGE009
表示n系中的东向速度，
Figure 589025DEST_PATH_IMAGE010
表示n系中的北向速度，
Figure 926465DEST_PATH_IMAGE011
和
Figure 976461DEST_PATH_IMAGE012
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 842785DEST_PATH_IMAGE013
表示载体所在地点的纬度，
Figure 306128DEST_PATH_IMAGE014
表示在n系中观察到的e系绕i系的转动角速度，
Figure 40735DEST_PATH_IMAGE015
表示在n系中观察到的n系绕e系的转动角速度，
Figure 335450DEST_PATH_IMAGE016
表示地球自转角速度。
2.根据权利要求1所述的方法，其特征在于，捷联惯导系统对准过程的测量模型为：
Figure 248042DEST_PATH_IMAGE017
其中，
Figure 995418DEST_PATH_IMAGE018
为激光多普勒测速仪在
Figure 612344DEST_PATH_IMAGE019
系中的速度，表示为，
Figure 136734DEST_PATH_IMAGE020
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 610440DEST_PATH_IMAGE021
为从
Figure 720479DEST_PATH_IMAGE022
系到
Figure 672254DEST_PATH_IMAGE023
系的转换矩阵，
Figure 879245DEST_PATH_IMAGE024
为激光多普勒测速仪的刻度系数，
Figure 382907DEST_PATH_IMAGE025
为激光多普勒测速仪的刻度系数误差，
Figure 104876DEST_PATH_IMAGE026
为激光多普勒测速仪与惯导设备的安装角误差，
Figure 735708DEST_PATH_IMAGE027
表示t时刻载体坐标系b系到惯性坐标系i系之间的姿态转换矩阵，
Figure 797205DEST_PATH_IMAGE028
表示载体坐标系b系的加速度计测量的比力，
Figure 612714DEST_PATH_IMAGE029
表示初始时刻到t时刻载体在导航坐标系n系中姿态变换矩阵，
Figure 182498DEST_PATH_IMAGE030
表示导航坐标系中重力加速度矩阵，
Figure 741656DEST_PATH_IMAGE031
表示激光多普勒测速仪在基于激光多普勒测速仪建立的坐标系m系中的速度。
3.根据权利要求1所述的方法，其特征在于，鲁棒平方根无迹四元数估计器的k时刻的状态向量为：
Figure 657659DEST_PATH_IMAGE032
其中，
Figure 519436DEST_PATH_IMAGE033
其中，
Figure 950417DEST_PATH_IMAGE034
为姿态误差的误差四元数
Figure 437899DEST_PATH_IMAGE035
的广义罗德里格斯参数形式，
Figure 5147DEST_PATH_IMAGE036
为误差四元数的标量部分，
Figure 37825DEST_PATH_IMAGE037
为误差四元数的矢量部分，
Figure 159365DEST_PATH_IMAGE038
的取值范围为[0,1]，
Figure 794745DEST_PATH_IMAGE039
为比例因子，
Figure 370827DEST_PATH_IMAGE040
、
Figure 699040DEST_PATH_IMAGE041
、
Figure 42296DEST_PATH_IMAGE042
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 622313DEST_PATH_IMAGE043
、
Figure 164153DEST_PATH_IMAGE044
、
Figure 53481DEST_PATH_IMAGE045
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 680771DEST_PATH_IMAGE046
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 861217DEST_PATH_IMAGE047
为激光多普勒测速仪在m系的航向安装角误差，
Figure 398508DEST_PATH_IMAGE048
为刻度系数误差。
4.根据权利要求3所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对协方差矩阵的平方根处理为：
Figure 68524DEST_PATH_IMAGE049
Figure 74789DEST_PATH_IMAGE050
Figure 855663DEST_PATH_IMAGE051
Figure 575357DEST_PATH_IMAGE052
Figure 291640DEST_PATH_IMAGE053
表示协方差矩阵的权重值，
Figure 893523DEST_PATH_IMAGE054
表示QR分解运算，
Figure 868301DEST_PATH_IMAGE055
表示Cholesky更新运算，
Figure 239240DEST_PATH_IMAGE056
为预测状态协方差矩阵的平方根形式，
Figure 454320DEST_PATH_IMAGE057
表示k时刻的过程噪声协方差矩阵，
Figure 153286DEST_PATH_IMAGE058
表示求
Figure 807121DEST_PATH_IMAGE059
的正负符号，
Figure 661595DEST_PATH_IMAGE060
表示鲁棒平方根无迹四元数估计器得到的测量预测均值。
5.根据权利要求3所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 844314DEST_PATH_IMAGE061
其中
Figure 296155DEST_PATH_IMAGE062
为新息向量，
Figure 425785DEST_PATH_IMAGE063
为由传感器得到的测量值，
Figure 505737DEST_PATH_IMAGE064
为鲁棒平方根无迹四元数估计器得到的测量预测均值，
Figure 515150DEST_PATH_IMAGE065
为自适应系数，初值为
Figure 313342DEST_PATH_IMAGE066
，c称为渐消因子，其中
Figure 981083DEST_PATH_IMAGE067
，
Figure 56487DEST_PATH_IMAGE068
、
Figure 846588DEST_PATH_IMAGE069
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
6.根据权利要求1至5中任意一项所述方法，其特征在于，还包括：
根据所述预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
CN202110403858.2A 2021-04-15 2021-04-15 基于激光多普勒测速仪的惯导系统行进间初始对准方法 Active CN112798021B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110403858.2A CN112798021B (zh) 2021-04-15 2021-04-15 基于激光多普勒测速仪的惯导系统行进间初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110403858.2A CN112798021B (zh) 2021-04-15 2021-04-15 基于激光多普勒测速仪的惯导系统行进间初始对准方法

Publications (2)

Publication Number Publication Date
CN112798021A CN112798021A (zh) 2021-05-14
CN112798021B true CN112798021B (zh) 2021-07-13

Family

ID=75811408

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110403858.2A Active CN112798021B (zh) 2021-04-15 2021-04-15 基于激光多普勒测速仪的惯导系统行进间初始对准方法

Country Status (1)

Country Link
CN (1) CN112798021B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114396938B (zh) * 2021-12-07 2023-08-08 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种舰船捷联惯导系统的高精度初始对准方法
CN114609407B (zh) * 2022-03-16 2024-02-06 苏州邈航科技有限公司 基于mems陀螺仪的水平测速仪及其测速方法
CN115060274B (zh) * 2022-08-17 2022-11-18 南开大学 一种水下一体式自主导航装置及其初始对准方法
CN116147577A (zh) * 2023-03-06 2023-05-23 中国人民解放军国防科技大学 基于单轴rins/ldv组合的连续高程测量方法及系统
CN115979263B (zh) * 2023-03-21 2023-06-02 中国人民解放军国防科技大学 一种低空飞行载体导航方法及系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788565A (zh) * 2010-03-01 2010-07-28 龙兴武 多点分层型差动激光多普勒测速仪
US9791575B2 (en) * 2016-01-27 2017-10-17 Novatel Inc. GNSS and inertial navigation system utilizing relative yaw as an observable for an ins filter
CN109724627A (zh) * 2019-03-08 2019-05-07 哈尔滨工程大学 一种基于多普勒计程仪和星敏感器辅助的极区传递对准方法
CN109945857B (zh) * 2019-03-18 2022-08-26 东南大学 一种面向不动产实地测量的车载惯性定位方法及其装置
CN111323050B (zh) * 2020-03-19 2021-06-18 哈尔滨工程大学 一种捷联惯导和多普勒组合系统标定方法
CN111854747B (zh) * 2020-08-25 2022-08-12 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法

Also Published As

Publication number Publication date
CN112798021A (zh) 2021-05-14

Similar Documents

Publication Publication Date Title
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
JP6094026B2 (ja) 姿勢判定方法、位置算出方法及び姿勢判定装置
JP4199553B2 (ja) ハイブリッド航法装置
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
JP2012173190A (ja) 測位システム、測位方法
CN108592943B (zh) 一种基于opreq方法的惯性系粗对准计算方法
CN107389069B (zh) 基于双向卡尔曼滤波的地面姿态处理方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112857398B (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN112504298A (zh) 一种gnss辅助的dvl误差标定方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN116608853B (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