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

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

Info

Publication number
CN112798021A
CN112798021A CN202110403858.2A CN202110403858A CN112798021A CN 112798021 A CN112798021 A CN 112798021A CN 202110403858 A CN202110403858 A CN 202110403858A CN 112798021 A CN112798021 A CN 112798021A
Authority
CN
China
Prior art keywords
laser doppler
error
doppler velocimeter
inertial navigation
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
Application number
CN202110403858.2A
Other languages
English (en)
Other versions
CN112798021B (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

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

基于激光多普勒测速仪的惯导***行进间初始对准方法
技术领域
本申请涉及组合导航技术领域，特别是涉及一种基于激光多普勒测速仪的惯导***行进间初始对准方法。
背景技术
在整个导航过程中，初始对准精度的高低直接关系到惯导工作性能的好坏。现有的对准方式有传递对准，静基座对准以及行进间对准等。传递对准需要高精度的惯导作为参考，一般用于大型船舶或者某些特殊应用场合。静基座对准是高精度惯导常用的对准方式，需要载体在对准过程中保持静止。这使得静基座对准虽然有着很高的精度，但限制了载体的机动性，不能做到随用随走。行进间对准能够使惯导在运动情况下完成***初始化，对于提高载体机动能力有着重要作用。与静基座对准不同，行进间对准需要外部设备来提供载体运动信息来对惯导***的输出进行补偿和校正。
现阶段利用GPS提供精确的速度与位置信息来行进间初始对准最为普遍，但利用GPS有如下缺点：（1）GPS信号不稳定，GPS接收机抗干扰能力较差且GPS信号易被高大建筑物或者树木遮挡；（2）GPS是美国研制的第二代卫星导航***，是一种非自主式***，具有高度不可控性。虽然我国北斗卫星导航***已经组网完成，但它同样是非自主的。此外，里程计也可以为车辆提供速度和里程信息，用里程计辅助的行进间初始对准具有完全自主的特点，因此被广泛采用，由于里程计的测量结果与车辆的车轮周长有关，并且车轮的周长受到车轮温度、压强以及磨损状况的影响，因此测量精度较低，另外车轮打滑、跳动也会使里程计的测量结果出现较大偏差。
激光多普勒测速仪是一种新型的速度传感器，能够实时提供精确的载体对地速度，因此利用激光多普勒测速仪辅助的行进间初始对准比里程计辅助的行进间初始对准有着更高的精度。由于激光多普勒测速仪辅助的行进间初始对准是完全自主的，因此比起GPS辅助的行进间初始对准有着更高抗干扰能力。
发明内容
基于此，有必要针对上述技术问题，提供一种基于激光多普勒测速仪的惯导***行进间初始对准方法。
一种基于激光多普勒测速仪的惯导***行进间初始对准方法，包括：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
其中一个实施例中，捷联惯导***对准过程的过程模型为：
Figure 847727DEST_PATH_IMAGE001
其中，
Figure 537335DEST_PATH_IMAGE002
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 983360DEST_PATH_IMAGE004
为姿态方向余弦矩阵估计的导数，
Figure 442023DEST_PATH_IMAGE005
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 49722DEST_PATH_IMAGE006
表示求转动角速度的斜对称矩阵。
其中一个实施例中，基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度的方式包括：
Figure 226625DEST_PATH_IMAGE007
其中，
Figure 210762DEST_PATH_IMAGE008
，
Figure 664877DEST_PATH_IMAGE009
，
Figure 833690DEST_PATH_IMAGE011
表示n系中的东向速度，
Figure 373256DEST_PATH_IMAGE013
表示n系中的北向速度，
Figure 161083DEST_PATH_IMAGE015
和
Figure 591409DEST_PATH_IMAGE016
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 806489DEST_PATH_IMAGE018
表示载体所在地点的纬度。
其中一个实施例中，捷联惯导***对准过程的测量模型为：
Figure 692406DEST_PATH_IMAGE019
Figure 283924DEST_PATH_IMAGE020
其中，
Figure 712631DEST_PATH_IMAGE022
为激光多普勒测速仪在
Figure 957668DEST_PATH_IMAGE024
系中的速度，表示为，
Figure 737405DEST_PATH_IMAGE025
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 601456DEST_PATH_IMAGE026
为从
Figure 743724DEST_PATH_IMAGE028
系到
Figure 300608DEST_PATH_IMAGE030
系的转换矩阵，
Figure 161116DEST_PATH_IMAGE032
为激光多普勒测速仪的刻度系数，
Figure 94437DEST_PATH_IMAGE033
为激光多普勒测速仪的刻度系数误差，
Figure 91212DEST_PATH_IMAGE035
为激光多普勒测速仪与惯导设备的安装角误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器的k时刻的状态向量为：
Figure 818997DEST_PATH_IMAGE036
其中，
Figure 307747DEST_PATH_IMAGE037
Figure 638234DEST_PATH_IMAGE038
其中，
Figure 896040DEST_PATH_IMAGE039
为姿态误差的误差四元数
Figure 919360DEST_PATH_IMAGE040
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 629827DEST_PATH_IMAGE042
为误差四元数的标量部分，
Figure 639371DEST_PATH_IMAGE044
为误差四元数的矢量部分。
Figure 879247DEST_PATH_IMAGE045
的取值范围为[0,1]，
Figure 948834DEST_PATH_IMAGE046
为比例因子，
Figure 412176DEST_PATH_IMAGE047
、
Figure 84466DEST_PATH_IMAGE048
、
Figure 316864DEST_PATH_IMAGE049
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 416407DEST_PATH_IMAGE050
、
Figure 101467DEST_PATH_IMAGE051
、
Figure 577447DEST_PATH_IMAGE052
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 398773DEST_PATH_IMAGE053
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 75742DEST_PATH_IMAGE054
为激光多普勒测速仪在m系的航向安装角误差，
Figure 107152DEST_PATH_IMAGE056
为刻度系数误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器对协方差矩阵的平方根处理为：
Figure 262190DEST_PATH_IMAGE057
Figure 328235DEST_PATH_IMAGE058
Figure 644946DEST_PATH_IMAGE059
Figure 304598DEST_PATH_IMAGE060
Figure 122381DEST_PATH_IMAGE061
表示协方差矩阵的权重值，
Figure 183878DEST_PATH_IMAGE063
表示QR分解运算，
Figure 61704DEST_PATH_IMAGE064
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 208652DEST_PATH_IMAGE065
Figure 439913DEST_PATH_IMAGE066
Figure 680883DEST_PATH_IMAGE067
Figure 729610DEST_PATH_IMAGE068
其中
Figure 363854DEST_PATH_IMAGE070
为新息向量，
Figure 523440DEST_PATH_IMAGE072
为由传感器得到的测量值，
Figure 293950DEST_PATH_IMAGE074
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 654524DEST_PATH_IMAGE075
为自适应系数，初值为
Figure 635118DEST_PATH_IMAGE077
，
Figure 208182DEST_PATH_IMAGE078
称为渐消因子，常取
Figure 98778DEST_PATH_IMAGE080
。
Figure 489308DEST_PATH_IMAGE081
、
Figure 832564DEST_PATH_IMAGE082
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。其中一个实施例中，所述方法还包括：
根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
一种基于激光多普勒测速仪的惯导***行进间初始对准装置，包括：
过程模型构建模块，用于在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
测量模型构建模块，用于基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
鲁棒平方根无迹四元数估计器构建模块，用于以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
行进间初始对准模块，用于使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
与现有技术相比，上述一种基于激光多普勒测速仪的惯导***行进间初始对准方法、装置、计算机设备和存储介质，在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。基于比力方程式建立捷联惯导***对准过程的测量模型。以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。本申请在过程模型和测量模型中均直接利用了激光多普勒测速仪的高精度速度输出，能够提高鲁棒平方根无迹四元数估计器对载体姿态的估计精度；此外，在行进间初始对准的过程中不仅考虑了惯性测量单元的误差，还考虑了激光多普勒测速仪的安装倾角和刻度系数的影响并进行了估计，提高了姿态估计的精度。
附图说明
图1为一个实施例中一种基于激光多普勒测速仪的惯导***行进间初始对准方法的流程示意图；
图2为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
捷联惯导***在使用时，将激光多普勒测速仪安装在车上合适的位置，以便测速仪的光斑能有效的打在地面上，并实时输出车辆的速度信息。惯性测量单元经过初始标定后得到陀螺和加速度计的零偏、安装误差角以及刻度系数误差，并将惯性测量单元安装在车辆后轮轴中央。向捷联惯导***输入初始的位置信息后，车辆开始运动，利用运动的前十分钟进行行进间初始对准，也可以根据需要调整行进间初始对准的时间，在整个对准过程中无需用到GPS的信息，是一种完全自主的行进间对准过程。
本申请将整个行进间初始对准分为过程模型和测量模型的构建与鲁棒平方根无迹四元数估计过程两个过程，其中过程模型和测量模型的构建过程将激光多普勒测速仪的安装误差和刻度系数误差考虑进去构建更加完备的测量模型并采用了一种更加完备的过程模型。鲁棒平方根无迹四元数估计过程提出了一种鲁棒平方根无迹四元数估计方法，该方法避免了传统无迹四元数估计方法带来的协方差矩阵非正定问题、收敛速度慢的问题以及鲁棒性不高的问题，这将提高对准精度。与传统基于姿态确定的对准方法不同，本申请提出的对准方法有着更高的对准精度与鲁棒性，并能对激光多普勒测速仪的安装误差和刻度系数进行标定。与传统的姿态确定方法相比，本申请能够处理模型中的噪声并且还能估计除姿态以外的其他参数，这能够减小实际中的噪声干扰，并且在过程模型和测量模型中均利用了激光多普勒测速仪的输出而不是近似值，这大大提高了估计精度。
在一个实施例中，如图1所示，提供了一种基于激光多普勒测速仪的惯导***行进间初始对准方法，包括以下步骤：
步骤102，在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
首先，根据姿态矩阵的链式法则对姿态矩阵进行分解：
Figure 599532DEST_PATH_IMAGE083
（1）
其中，
Figure 79055DEST_PATH_IMAGE084
系为捷联惯性导航***的坐标系，定义为右-前-上，
Figure 781432DEST_PATH_IMAGE085
系为当地导航坐标系，定义为东-北-天。
Figure 471039DEST_PATH_IMAGE086
和
Figure 651485DEST_PATH_IMAGE087
为惯性非旋转结构，不随捷联惯导***在地球表面运动而运动，在初始时刻分别与
Figure 375727DEST_PATH_IMAGE089
系和
Figure 249005DEST_PATH_IMAGE090
系对齐。
Figure 301275DEST_PATH_IMAGE091
表示t时刻姿态的方向余弦矩阵，
Figure 147396DEST_PATH_IMAGE092
表示初始时刻姿态的方向余弦矩阵。
传统对准方案中的过程模型为
Figure 867090DEST_PATH_IMAGE093
，
Figure 911269DEST_PATH_IMAGE094
，
Figure 575469DEST_PATH_IMAGE095
表示在t时刻b系相对初始时刻姿态变化的方向余弦矩阵。
Figure 363296DEST_PATH_IMAGE097
表示对
Figure 796552DEST_PATH_IMAGE098
求导数，
Figure 11632DEST_PATH_IMAGE099
表示在t时刻n系相对初始时刻姿态变化的方向余弦矩阵。
Figure 772915DEST_PATH_IMAGE100
表示在c系中观察到的b系绕a系的转动角速度，对应地
Figure 489067DEST_PATH_IMAGE101
为惯性测量单元（IMU）中的陀螺测量得到的载体角速度，
Figure 652195DEST_PATH_IMAGE102
表示在n系中观察到的n系绕i系的转动角速度（i系为地心惯性坐标系），
Figure 38177DEST_PATH_IMAGE103
表示求
Figure 676969DEST_PATH_IMAGE101
的斜对称矩阵。
由于在未对准情况下，激光多普勒测速仪辅助的捷联惯导***不能获得准确的实时地速和位置，因此在传统方案中需要对
Figure 806599DEST_PATH_IMAGE102
进行近似，这会引入额外的误差。为了避免这一缺点，本实施例将激光多普勒测速仪的速度投影到n系，并以下面的方式构建
Figure 948867DEST_PATH_IMAGE102
：
Figure 505751DEST_PATH_IMAGE105
Figure 507205DEST_PATH_IMAGE106
Figure 34001DEST_PATH_IMAGE107
其中，
Figure 437300DEST_PATH_IMAGE108
表示n系中的东向速度，
Figure 430664DEST_PATH_IMAGE109
表示n系中的北向速度。
Figure 512890DEST_PATH_IMAGE111
和
Figure 249902DEST_PATH_IMAGE112
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 363832DEST_PATH_IMAGE113
为载体所在地点的纬度。
由于激光多普勒测速仪提供的载体速度在整个对准过程中起着至关重要的作用，因此本实施例在过程模型中加入测速仪的速度。具体地，将初始时刻的b系表示为惯性系i系建立过程模型：
Figure 262518DEST_PATH_IMAGE114
（2）
步骤104，基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
对比力方程式
Figure 972985DEST_PATH_IMAGE115
进行变换，得到测量模型表达式，
Figure 372743DEST_PATH_IMAGE116
（3）
Figure 219476DEST_PATH_IMAGE117
（4）
其中，
Figure 554642DEST_PATH_IMAGE118
表示激光多普勒测速仪在
Figure 611460DEST_PATH_IMAGE120
系中的速度，由于激光多普勒测速仪与惯导***存在安装倾角以及激光多普勒测速仪的输出与真实速度存在一定比例关系，建立跟激光多普勒测速仪相关的
Figure 690274DEST_PATH_IMAGE122
系，则激光多普勒测速仪在
Figure 781727DEST_PATH_IMAGE120
系中的输出表示为
Figure 22216DEST_PATH_IMAGE123
（5）
Figure 707275DEST_PATH_IMAGE124
为
Figure 448835DEST_PATH_IMAGE125
系到
Figure 270160DEST_PATH_IMAGE127
系的转换矩阵，
Figure 681550DEST_PATH_IMAGE128
为刻度系数。
Figure 712960DEST_PATH_IMAGE129
为激光多普勒测速仪与惯导***的安装角误差，
Figure 133577DEST_PATH_IMAGE130
为激光多普勒测速仪的刻度系数误差。
步骤106，以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
本实施例设计的鲁棒平方根无迹四元数估计器采用姿态误差、陀螺零偏漂移、加速度计零偏漂移、激光多普勒测速仪俯仰角误差、激光多普勒测速仪航向角误差以及激光多普勒测速仪刻度系数误差作为状态量。步骤106可以进一步详细描述为：
1）构建状态向量为：
Figure 934043DEST_PATH_IMAGE131
（6）
其中，
Figure 781913DEST_PATH_IMAGE132
（7）
Figure 175985DEST_PATH_IMAGE133
（8）
Figure 259348DEST_PATH_IMAGE134
（9）
Figure 55266DEST_PATH_IMAGE135
为表征所求姿态误差的四元数的误差，
Figure 74037DEST_PATH_IMAGE137
为其广义罗德里格斯参数形式，
Figure 817390DEST_PATH_IMAGE138
为取值范围从0到1的参数，
Figure 314230DEST_PATH_IMAGE139
为比例因子，
Figure 354867DEST_PATH_IMAGE138
和
Figure 278961DEST_PATH_IMAGE139
的不同组合有着不同的物理意义，这里取较为常用的参数组合，即
Figure 913205DEST_PATH_IMAGE141
=1，
Figure 338370DEST_PATH_IMAGE142
=1。
Figure 843301DEST_PATH_IMAGE143
为陀螺漂移偏差估计，
Figure 203875DEST_PATH_IMAGE145
为加速度计漂移偏差估计，
Figure 918890DEST_PATH_IMAGE146
为激光多普勒测速仪的俯仰安装角误差，
Figure 23112DEST_PATH_IMAGE147
为激光多普勒测速仪的航向安装角误差，
Figure 913708DEST_PATH_IMAGE149
为刻度系数误差。
2）根据状态向量和相应的协方差矩阵，作SUT变换，其采样方程为：
Figure 304238DEST_PATH_IMAGE150
（10）
期望和协方差矩阵的加权权重
Figure 647494DEST_PATH_IMAGE151
和
Figure 148883DEST_PATH_IMAGE152
计算如下：
Figure 893985DEST_PATH_IMAGE153
（11）
Figure 330783DEST_PATH_IMAGE154
（12）
其中，
Figure 285969DEST_PATH_IMAGE156
为状态向量的维数，
Figure 466415DEST_PATH_IMAGE158
，
Figure 331603DEST_PATH_IMAGE160
为一大于零的比例因子，在本发明中取
Figure 329514DEST_PATH_IMAGE161
。
Figure 116205DEST_PATH_IMAGE163
为调节因子，用于捕捉高阶信息，通常取
Figure 365921DEST_PATH_IMAGE164
或
Figure 965177DEST_PATH_IMAGE165
，当状态维数
Figure 9357DEST_PATH_IMAGE166
时，
Figure 407977DEST_PATH_IMAGE168
取
Figure 195804DEST_PATH_IMAGE169
。对于高斯分布
Figure 504426DEST_PATH_IMAGE171
取2，对于其他分布则需要通过试验调整，在本发明中
Figure 844140DEST_PATH_IMAGE171
取2。
Figure 871002DEST_PATH_IMAGE172
为对协方差矩阵Cholesky分解得到的下三角矩阵。
3）经过SUT变换得到的采样点可分为姿态部分非姿态部分。
Figure 55996DEST_PATH_IMAGE173
（13）
利用（8）式的逆过程将姿态部分用误差四元数表示，即
Figure 750282DEST_PATH_IMAGE174
（14）
4）通过将误差四元数乘以当前估计值来定义新的基于四元数的sigma点，定义一组新的采样点
Figure 870685DEST_PATH_IMAGE175
，并通过过程模型向前传播得到预测的采样点
Figure 509477DEST_PATH_IMAGE176
（15）
5）利用预测四元数得到新的误差四元数
Figure 373528DEST_PATH_IMAGE177
（16）
利用（8）式得到误差四元数的修正罗德里格斯参数形式。新的状态向量预测值的
Figure 922321DEST_PATH_IMAGE178
点为
Figure 603838DEST_PATH_IMAGE179
（17）
6）状态预测和协方差为
Figure 339713DEST_PATH_IMAGE180
（18）
Figure 273034DEST_PATH_IMAGE181
（19）
Figure 535388DEST_PATH_IMAGE182
（20）
7）利用状态预测值和协方差的平方根形式重复（2.2）-（2.3）过程，并利用得到的误差四元数乘以状态预测值来定义新的基于四元数的sigma点
Figure 263172DEST_PATH_IMAGE183
定义新的采样点为
Figure 610977DEST_PATH_IMAGE184
（21）
8）（21）式中的sigma点通过测量模型（3）式传播，得到鲁棒平方根无迹四元数估计器的观测量
Figure 347989DEST_PATH_IMAGE185
为：
Figure 605795DEST_PATH_IMAGE186
得到预测的观测值后，通过加权求和得到***预测的均值以及协方差：
Figure 366465DEST_PATH_IMAGE187
（22）
Figure 76932DEST_PATH_IMAGE188
（23）
Figure 352056DEST_PATH_IMAGE189
（24）
Figure 589002DEST_PATH_IMAGE190
（25）
9）定义新息矢量为：
Figure 658589DEST_PATH_IMAGE191
（26）
对测量噪声协方差矩阵进行自适应：
Figure 856352DEST_PATH_IMAGE192
（27）
Figure 59800DEST_PATH_IMAGE193
（28）
Figure 761040DEST_PATH_IMAGE194
（29）
其中
Figure 267108DEST_PATH_IMAGE195
为由传感器得到的测量值，
Figure 811222DEST_PATH_IMAGE196
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 428148DEST_PATH_IMAGE198
为自适应系数，初值为
Figure 374107DEST_PATH_IMAGE199
，
Figure 51076DEST_PATH_IMAGE200
称为渐消因子，常取
Figure 957852DEST_PATH_IMAGE201
。
Figure 237524DEST_PATH_IMAGE202
、
Figure 913356DEST_PATH_IMAGE204
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
10）然后进行量测更新：
Figure 885860DEST_PATH_IMAGE205
（30）
Figure 279932DEST_PATH_IMAGE206
（31）
Figure 238661DEST_PATH_IMAGE207
（32）
Figure 159212DEST_PATH_IMAGE208
（33）
11）最后进行姿态更新，（32）式可以写为
Figure 912405DEST_PATH_IMAGE209
，利用（8）式的逆方程得到
Figure 59352DEST_PATH_IMAGE210
对应的误差四元数
Figure 677897DEST_PATH_IMAGE211
，并通过
Figure 593900DEST_PATH_IMAGE213
更新姿态四元数。
最后将
Figure 642628DEST_PATH_IMAGE214
置零，然后进入下个循环。
步骤108，使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
步骤110，根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
本实施例提供的一种基于激光多普勒测速仪的惯导***行进间初始对准方法具有以下优势：
1）所提出的鲁棒平方根无迹四元数估计器的过程模型和测量模型中均直接利用了激光多普勒测速仪的高精度速度输出，而不是只利用惯导本身的速度，提高了无迹四元数估计器对姿态的估计精度。
2）在行进间初始对准的过程中采用了基于姿态估计的方法而不是寻常的基于姿态确定的方法，在对姿态进行估计的同时还充分估计了其他误差参数，提高了姿态估计的准确性。
3）在行进间初始对准的过程中不仅仅考虑了惯性测量单元的误差，还考虑了激光多普勒测速仪的安装倾角和刻度系数的影响并进行了估计，对准过程中所估计的激光多普勒测速仪安装倾角和刻度系数可作为后续过程的的初始值，无需再另外获得初值或者另设初值，并基于对准的结果在后续过程中实时反馈校正，提高了姿态估计的精度。
4）传统方案在对准过程中的初值有着很大误差，且对准过程中存在许多近似，因此对准过程会产生较大的位置误差与速度误差。本实施例没有采用传统方案中估计恒定初始姿态
Figure 11292DEST_PATH_IMAGE215
的方式，不再对
Figure 46244DEST_PATH_IMAGE216
做近似，并且直接对
Figure 206967DEST_PATH_IMAGE217
进行估计，减小了传统方案近似对对准结果造成的影响。
5）使用了激光多普勒测速仪来辅助捷联惯导***进行行进间初始对准，比使用GPS辅助的行进间对准有着更高的抗干扰能力和自主性，比使用里程计辅助的行进间对准有着更高的对准精度。
6）鲁棒平方根无迹四元数估计器与传统的无迹四元数估计器相比有着更好的鲁棒性和稳定性，避免了无迹卡尔曼滤波可能导致的协方差矩阵非正定问题，并使收敛速度大大提升。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，提供了一种基于激光多普勒测速仪的惯导***行进间初始对准装置，包括：
过程模型构建模块，用于在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
测量模型构建模块，用于基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
鲁棒平方根无迹四元数估计器构建模块，用于以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
行进间初始对准模块，用于使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
其中一个实施例中，过程模型构建模块建立的过程模型为：
Figure 567541DEST_PATH_IMAGE218
其中，
Figure 157923DEST_PATH_IMAGE220
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 386779DEST_PATH_IMAGE221
为姿态方向余弦矩阵估计的导数，
Figure 11795DEST_PATH_IMAGE223
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 543270DEST_PATH_IMAGE224
表示求转动角速度的斜对称矩阵。
其中一个实施例中，过程模型构建模块用于基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度为：
Figure 11161DEST_PATH_IMAGE225
其中，
Figure 387916DEST_PATH_IMAGE226
，
Figure 133018DEST_PATH_IMAGE227
，
Figure 960028DEST_PATH_IMAGE228
表示n系中的东向速度，
Figure 525002DEST_PATH_IMAGE229
表示n系中的北向速度，
Figure 971027DEST_PATH_IMAGE230
和
Figure 429690DEST_PATH_IMAGE231
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 302968DEST_PATH_IMAGE233
表示载体所在地点的纬度。
其中一个实施例中，测量模型构建模块建立的测量模型为：
Figure 479871DEST_PATH_IMAGE234
Figure 729587DEST_PATH_IMAGE235
其中，
Figure 183702DEST_PATH_IMAGE237
为激光多普勒测速仪在
Figure 89866DEST_PATH_IMAGE239
系中的速度，表示为，
Figure 895011DEST_PATH_IMAGE240
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 682838DEST_PATH_IMAGE241
为从
Figure 116094DEST_PATH_IMAGE243
系到
Figure 331174DEST_PATH_IMAGE244
系的转换矩阵，
Figure 358036DEST_PATH_IMAGE245
为激光多普勒测速仪的刻度系数，
Figure 543030DEST_PATH_IMAGE246
为激光多普勒测速仪的刻度系数误差，
Figure 971737DEST_PATH_IMAGE248
为激光多普勒测速仪与惯导设备的安装角误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器构建模块建立的状态向量为：
Figure 482353DEST_PATH_IMAGE249
其中，
Figure 996511DEST_PATH_IMAGE250
Figure 860562DEST_PATH_IMAGE251
其中，
Figure 533988DEST_PATH_IMAGE253
为姿态误差的误差四元数
Figure 90872DEST_PATH_IMAGE254
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 826747DEST_PATH_IMAGE255
为误差四元数的标量部分，
Figure 884701DEST_PATH_IMAGE256
为误差四元数的矢量部分。
Figure 22422DEST_PATH_IMAGE258
的取值范围为[0,1]，
Figure 750206DEST_PATH_IMAGE259
为比例因子，
Figure 363590DEST_PATH_IMAGE260
、
Figure 569443DEST_PATH_IMAGE261
、
Figure 827249DEST_PATH_IMAGE262
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 116148DEST_PATH_IMAGE263
、
Figure 826615DEST_PATH_IMAGE264
、
Figure 836160DEST_PATH_IMAGE265
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 70176DEST_PATH_IMAGE266
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 139763DEST_PATH_IMAGE267
为激光多普勒测速仪在m系的航向安装角误差，
Figure 603106DEST_PATH_IMAGE268
为刻度系数误差。
其中一个实施例中，鲁棒平方根无迹四元数估计器构建模块对协方差矩阵的平方根处理为：
Figure 275396DEST_PATH_IMAGE269
Figure 507794DEST_PATH_IMAGE270
Figure 872916DEST_PATH_IMAGE271
Figure 557975DEST_PATH_IMAGE272
Figure 174901DEST_PATH_IMAGE273
表示协方差矩阵的权重值，
Figure 120861DEST_PATH_IMAGE274
表示QR分解运算，
Figure 797830DEST_PATH_IMAGE275
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 704606DEST_PATH_IMAGE276
Figure 984277DEST_PATH_IMAGE277
Figure 191268DEST_PATH_IMAGE278
Figure 773559DEST_PATH_IMAGE279
其中
Figure 557844DEST_PATH_IMAGE280
为新息向量，
Figure 250994DEST_PATH_IMAGE281
为由传感器得到的测量值，
Figure 46911DEST_PATH_IMAGE282
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 924738DEST_PATH_IMAGE283
为自适应系数，初值为
Figure 71685DEST_PATH_IMAGE284
，
Figure 834105DEST_PATH_IMAGE285
称为渐消因子，常取
Figure 874742DEST_PATH_IMAGE286
。
Figure 533256DEST_PATH_IMAGE287
、
Figure 29484DEST_PATH_IMAGE288
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
其中一个实施例中，还包括激光多普勒测速仪标定模块，用于根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
关于一种基于激光多普勒测速仪的惯导***行进间初始对准装置的具体限定可以参见上文中对于一种基于激光多普勒测速仪的惯导***行进间初始对准方法的限定，在此不再赘述。上述一种基于激光多普勒测速仪的惯导***行进间初始对准装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图2所示。该计算机设备包括通过***总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***和计算机程序。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于激光多普勒测速仪的惯导***行进间初始对准方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图2中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立无迹四元数估计器。
使用无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立过程模型为：
Figure 330016DEST_PATH_IMAGE290
其中，
Figure 366105DEST_PATH_IMAGE291
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 320154DEST_PATH_IMAGE292
为姿态方向余弦矩阵估计的导数，
Figure 441694DEST_PATH_IMAGE294
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 545916DEST_PATH_IMAGE295
表示求转动角速度的斜对称矩阵。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：基于激光多普勒测速仪的速度计算在n系中观察到n系绕i系的转动角速度为：
Figure 29987DEST_PATH_IMAGE296
其中，
Figure 561463DEST_PATH_IMAGE297
，
Figure 639140DEST_PATH_IMAGE298
，
Figure 671687DEST_PATH_IMAGE299
表示n系中的东向速度，
Figure 151210DEST_PATH_IMAGE300
表示n系中的北向速度，
Figure 853587DEST_PATH_IMAGE301
和
Figure 808773DEST_PATH_IMAGE302
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 989219DEST_PATH_IMAGE304
表示载体所在地点的纬度。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立测量模型为：
Figure 588828DEST_PATH_IMAGE305
Figure 586739DEST_PATH_IMAGE306
其中，
Figure 639009DEST_PATH_IMAGE307
为激光多普勒测速仪在
Figure 623146DEST_PATH_IMAGE308
系中的速度，表示为，
Figure 201894DEST_PATH_IMAGE309
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 246074DEST_PATH_IMAGE310
为从
Figure 51219DEST_PATH_IMAGE312
系到
Figure 698101DEST_PATH_IMAGE313
系的转换矩阵，
Figure 272302DEST_PATH_IMAGE315
为激光多普勒测速仪的刻度系数，
Figure 487382DEST_PATH_IMAGE316
为激光多普勒测速仪的刻度系数误差，
Figure 370369DEST_PATH_IMAGE317
为激光多普勒测速仪与惯导设备的安装角误差。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：建立状态向量为：
Figure 961887DEST_PATH_IMAGE318
其中，
Figure 249649DEST_PATH_IMAGE319
Figure 635631DEST_PATH_IMAGE320
其中，
Figure 415368DEST_PATH_IMAGE321
为姿态误差的误差四元数
Figure 404053DEST_PATH_IMAGE322
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 687267DEST_PATH_IMAGE323
为误差四元数的标量部分，
Figure 509729DEST_PATH_IMAGE324
为误差四元数的矢量部分。
Figure 370238DEST_PATH_IMAGE326
的取值范围为[0,1]，
Figure 303559DEST_PATH_IMAGE327
为比例因子，
Figure 175700DEST_PATH_IMAGE328
、
Figure 293697DEST_PATH_IMAGE329
、
Figure 516868DEST_PATH_IMAGE330
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 988301DEST_PATH_IMAGE331
、
Figure 370741DEST_PATH_IMAGE332
、
Figure 269427DEST_PATH_IMAGE333
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 838948DEST_PATH_IMAGE334
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 114072DEST_PATH_IMAGE335
为激光多普勒测速仪在m系的航向安装角误差，
Figure 491964DEST_PATH_IMAGE336
为刻度系数误差。
在一个实施例中，处理器执行计算机程序时还实现以下步骤：对协方差矩阵的平方根处理为：
Figure 420605DEST_PATH_IMAGE337
Figure 883948DEST_PATH_IMAGE339
Figure 697183DEST_PATH_IMAGE340
Figure 791565DEST_PATH_IMAGE341
Figure 32054DEST_PATH_IMAGE342
表示协方差矩阵的权重值，
Figure 717113DEST_PATH_IMAGE344
表示QR分解运算，
Figure 458673DEST_PATH_IMAGE345
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 545578DEST_PATH_IMAGE346
Figure 956967DEST_PATH_IMAGE347
Figure 253957DEST_PATH_IMAGE348
Figure 408994DEST_PATH_IMAGE349
其中
Figure 350406DEST_PATH_IMAGE350
为新息向量，
Figure 57330DEST_PATH_IMAGE351
为由传感器得到的测量值，
Figure 451403DEST_PATH_IMAGE352
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 410131DEST_PATH_IMAGE353
为自适应系数，初值为
Figure 596262DEST_PATH_IMAGE354
，
Figure 349455DEST_PATH_IMAGE355
称为渐消因子，常取
Figure 496402DEST_PATH_IMAGE356
。
Figure 117876DEST_PATH_IMAGE357
、
Figure 768300DEST_PATH_IMAGE359
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。在一个实施例中，处理器执行计算机程序时还实现以下步骤：根据预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现以下步骤：
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型。
基于比力方程式建立捷联惯导***对准过程的测量模型。其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数。
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器。
使用鲁棒平方根无迹四元数估计器得到状态向量的预测值，根据预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立过程模型为：
Figure 957973DEST_PATH_IMAGE360
其中，
Figure 451272DEST_PATH_IMAGE362
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 751803DEST_PATH_IMAGE363
为姿态方向余弦矩阵估计的导数，
Figure 522313DEST_PATH_IMAGE364
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure 741942DEST_PATH_IMAGE365
表示求转动角速度的斜对称矩阵。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：基于激光多普勒测速仪的速度计算在n系中观察到n系绕i系的转动角速度为：
Figure 863481DEST_PATH_IMAGE367
其中，
Figure 702124DEST_PATH_IMAGE368
，
Figure 472282DEST_PATH_IMAGE369
，
Figure 738179DEST_PATH_IMAGE370
表示n系中的东向速度，
Figure 206069DEST_PATH_IMAGE371
表示n系中的北向速度，
Figure 113982DEST_PATH_IMAGE372
和
Figure 593505DEST_PATH_IMAGE373
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 420516DEST_PATH_IMAGE375
表示载体所在地点的纬度。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立测量模型为：
Figure 985489DEST_PATH_IMAGE376
Figure 165935DEST_PATH_IMAGE377
其中，
Figure 155756DEST_PATH_IMAGE378
为激光多普勒测速仪在
Figure 763455DEST_PATH_IMAGE379
系中的速度，表示为，
Figure 81304DEST_PATH_IMAGE380
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 190074DEST_PATH_IMAGE381
为从
Figure 644190DEST_PATH_IMAGE382
系到
Figure 688369DEST_PATH_IMAGE379
系的转换矩阵，
Figure 352568DEST_PATH_IMAGE383
为激光多普勒测速仪的刻度系数，
Figure 140396DEST_PATH_IMAGE384
为激光多普勒测速仪的刻度系数误差，
Figure 714597DEST_PATH_IMAGE385
为激光多普勒测速仪与惯导设备的安装角误差。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：建立状态向量为：
Figure 788732DEST_PATH_IMAGE386
其中，
Figure 815594DEST_PATH_IMAGE387
Figure 407112DEST_PATH_IMAGE388
其中，
Figure 694874DEST_PATH_IMAGE390
为姿态误差的误差四元数
Figure 80856DEST_PATH_IMAGE392
的广义罗德里格斯参数（GRP）形式，上标T表示矩阵的转置。
Figure 722577DEST_PATH_IMAGE393
为误差四元数的标量部分，
Figure 852207DEST_PATH_IMAGE395
为误差四元数的矢量部分。
Figure 135421DEST_PATH_IMAGE397
的取值范围为[0,1]，
Figure 816938DEST_PATH_IMAGE398
为比例因子，
Figure 818392DEST_PATH_IMAGE399
、
Figure 220555DEST_PATH_IMAGE400
、
Figure 748488DEST_PATH_IMAGE401
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 741852DEST_PATH_IMAGE402
、
Figure 699444DEST_PATH_IMAGE403
、
Figure 561089DEST_PATH_IMAGE405
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 553316DEST_PATH_IMAGE406
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 452002DEST_PATH_IMAGE407
为激光多普勒测速仪在m系的航向安装角误差，
Figure 287103DEST_PATH_IMAGE408
为刻度系数误差。
在一个实施例中，计算机程序被处理器执行时还实现以下步骤：对协方差矩阵的平方根处理为：
Figure 562226DEST_PATH_IMAGE409
Figure 674539DEST_PATH_IMAGE410
Figure 868760DEST_PATH_IMAGE411
Figure 66523DEST_PATH_IMAGE412
Figure 4392DEST_PATH_IMAGE413
表示协方差矩阵的权重值，
Figure 236790DEST_PATH_IMAGE415
表示QR分解运算，
Figure 477279DEST_PATH_IMAGE416
表示Cholesky更新运算。
其中一个实施例中，鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 286972DEST_PATH_IMAGE417
Figure 903898DEST_PATH_IMAGE418
Figure 725223DEST_PATH_IMAGE419
Figure 258317DEST_PATH_IMAGE420
其中
Figure 430672DEST_PATH_IMAGE422
为新息向量，
Figure 585710DEST_PATH_IMAGE423
为由传感器得到的测量值，
Figure 651755DEST_PATH_IMAGE425
为鲁棒平方根无迹四元数估计器得到的测量预测均值。
Figure 234046DEST_PATH_IMAGE426
为自适应系数，初值为
Figure 893698DEST_PATH_IMAGE427
，
Figure 977060DEST_PATH_IMAGE428
称为渐消因子，常取
Figure 38557DEST_PATH_IMAGE429
。
Figure 791750DEST_PATH_IMAGE430
、
Figure DEST_PATH_IMAGE431
为测量噪声协方差矩阵设置的上下限，上标（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 (8)

1.一种基于激光多普勒测速仪的惯导***行进间初始对准方法，其特征在于，所述方法包括：
在地心惯性坐标系和当地导航坐标系下，基于激光多普勒测速仪的速度测量值和姿态方向余弦矩阵估计，建立捷联惯导***对准过程的过程模型；
基于比力方程式建立捷联惯导***对准过程的测量模型；其中，激光多普勒测速仪的速度表示为刻度系数误差和安装角误差的函数；
以惯导设备的姿态误差、陀螺零偏漂移、加速度计零偏漂移，以及激光多普勒测速仪的安装角误差和刻度系数误差为分量构建状态向量，建立鲁棒平方根无迹四元数估计器；
使用所述鲁棒平方根无迹四元数估计器得到所述状态向量的预测值，根据所述预测值的姿态误差分量、陀螺零偏漂移分量和加速度计零偏漂移分量对捷联惯导***进行行进间初始对准。
2.根据权利要求1所述的方法，其特征在于，捷联惯导***对准过程的过程模型为：
Figure 342877DEST_PATH_IMAGE001
其中，
Figure 853492DEST_PATH_IMAGE002
为t时刻基于地心惯性坐标系i系和当地导航坐标系n系的姿态方向余弦矩阵估计，
Figure 367650DEST_PATH_IMAGE003
为所述姿态方向余弦矩阵估计的导数，
Figure 497280DEST_PATH_IMAGE005
表示基于激光多普勒测速仪的速度得到的在n系中观察到的n系绕i系的转动角速度，
Figure DEST_PATH_IMAGE006
表示求所述转动角速度的斜对称矩阵。
3.根据权利要求2所述的方法，其特征在于，基于激光多普勒测速仪的速度计算在n系中观察到的n系绕i系的转动角速度的方式包括：
Figure 639549DEST_PATH_IMAGE007
其中，
Figure 196432DEST_PATH_IMAGE008
，
Figure 56941DEST_PATH_IMAGE009
，
Figure 990262DEST_PATH_IMAGE011
表示n系中的东向速度，
Figure 252616DEST_PATH_IMAGE013
表示n系中的北向速度，
Figure 980400DEST_PATH_IMAGE014
和
Figure 469150DEST_PATH_IMAGE016
分别表示载体所在地点地球子午圈和卯酉圈曲率半径，
Figure 799638DEST_PATH_IMAGE018
表示载体所在地点的纬度。
4.根据权利要求2所述的方法，其特征在于，捷联惯导***对准过程的测量模型为：
Figure 57444DEST_PATH_IMAGE019
Figure 956129DEST_PATH_IMAGE020
其中，
Figure 53880DEST_PATH_IMAGE022
为激光多普勒测速仪在
Figure 797845DEST_PATH_IMAGE024
系中的速度，表示为，
Figure 175737DEST_PATH_IMAGE025
其中，m系为基于激光多普勒测速仪建立的坐标系，
Figure 369958DEST_PATH_IMAGE027
为从
Figure 567721DEST_PATH_IMAGE028
系到
Figure 380956DEST_PATH_IMAGE030
系的转换矩阵，
Figure 737988DEST_PATH_IMAGE031
为激光多普勒测速仪的刻度系数，
Figure 244056DEST_PATH_IMAGE032
为激光多普勒测速仪的刻度系数误差，
Figure 663536DEST_PATH_IMAGE034
为激光多普勒测速仪与惯导设备的安装角误差。
5.根据权利要求2所述的方法，其特征在于，鲁棒平方根无迹四元数估计器的k时刻的状态向量为：
Figure 405096DEST_PATH_IMAGE035
其中，
Figure 226421DEST_PATH_IMAGE036
Figure 903390DEST_PATH_IMAGE037
其中，
Figure 934800DEST_PATH_IMAGE038
为姿态误差的误差四元数
Figure 824259DEST_PATH_IMAGE039
的广义罗德里格斯参数形式，
Figure 155883DEST_PATH_IMAGE040
为误差四元数的标量部分，
Figure 738174DEST_PATH_IMAGE041
为误差四元数的矢量部分，
Figure 256880DEST_PATH_IMAGE042
的取值范围为[0,1]，
Figure 215609DEST_PATH_IMAGE044
为比例因子，
Figure 11526DEST_PATH_IMAGE045
、
Figure 889352DEST_PATH_IMAGE047
、
Figure 36300DEST_PATH_IMAGE048
分别为陀螺在n系下x、y、z轴方向上的漂移偏差估计，
Figure 798720DEST_PATH_IMAGE049
、
Figure 576707DEST_PATH_IMAGE050
、
Figure 500801DEST_PATH_IMAGE051
分别为加速度计在n系下x、y、z轴方向上的漂移偏差估计，m系为基于激光多普勒测速仪建立的坐标系，
Figure 135045DEST_PATH_IMAGE052
为激光多普勒测速仪在m系的俯仰安装角误差，
Figure 294631DEST_PATH_IMAGE053
为激光多普勒测速仪在m系的航向安装角误差，
Figure 330720DEST_PATH_IMAGE054
为刻度系数误差。
6.根据权利要求5所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对协方差矩阵的平方根处理为：
Figure 425715DEST_PATH_IMAGE055
Figure DEST_PATH_IMAGE056
Figure 140730DEST_PATH_IMAGE057
Figure 979373DEST_PATH_IMAGE058
Figure DEST_PATH_IMAGE059
表示协方差矩阵的权重值，
Figure 197864DEST_PATH_IMAGE061
表示QR分解运算，
Figure 729340DEST_PATH_IMAGE062
表示Cholesky更新运算。
7.根据权利要求5所述的方法，其特征在于，所述鲁棒平方根无迹四元数估计器对测量噪声协方差矩阵的自适应处理为：
Figure 197230DEST_PATH_IMAGE063
Figure 839564DEST_PATH_IMAGE064
Figure 584666DEST_PATH_IMAGE065
Figure DEST_PATH_IMAGE066
其中
Figure DEST_PATH_IMAGE067
为新息向量，
Figure DEST_PATH_IMAGE069
为由传感器得到的测量值，
Figure DEST_PATH_IMAGE070
为鲁棒平方根无迹四元数估计器得到的测量预测均值，
Figure DEST_PATH_IMAGE071
为自适应系数，初值为
Figure DEST_PATH_IMAGE072
，
Figure DEST_PATH_IMAGE073
称为渐消因子，
Figure DEST_PATH_IMAGE074
、
Figure DEST_PATH_IMAGE076
为测量噪声协方差矩阵设置的上下限，上标（i）代表对角矩阵的第i个元素。
8.根据权利要求1至7中任意一项所述方法，其特征在于，还包括：
根据所述预测值的俯仰角误差分量、航向角误差分量和刻度系数误差分量，对激光多普勒测速仪的安装角误差和刻度系数误差进行校正。
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 true CN112798021A (zh) 2021-05-14
CN112798021B 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)

Cited By (5)

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

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788565A (zh) * 2010-03-01 2010-07-28 龙兴武 多点分层型差动激光多普勒测速仪
US20170212248A1 (en) * 2016-01-27 2017-07-27 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 哈尔滨工程大学 一种基于多普勒计程仪和星敏感器辅助的极区传递对准方法
CN109945857A (zh) * 2019-03-18 2019-06-28 东南大学 一种面向不动产实地测量的车载惯性定位方法及其装置
CN111323050A (zh) * 2020-03-19 2020-06-23 哈尔滨工程大学 一种捷联惯导和多普勒组合***标定方法
CN111854747A (zh) * 2020-08-25 2020-10-30 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101788565A (zh) * 2010-03-01 2010-07-28 龙兴武 多点分层型差动激光多普勒测速仪
US20170212248A1 (en) * 2016-01-27 2017-07-27 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 哈尔滨工程大学 一种基于多普勒计程仪和星敏感器辅助的极区传递对准方法
CN109945857A (zh) * 2019-03-18 2019-06-28 东南大学 一种面向不动产实地测量的车载惯性定位方法及其装置
CN111323050A (zh) * 2020-03-19 2020-06-23 哈尔滨工程大学 一种捷联惯导和多普勒组合***标定方法
CN111854747A (zh) * 2020-08-25 2020-10-30 东南大学 一种载体大机动情况下的dvl辅助sins粗对准方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
LI KANG 等: "A Fast in-Motion Alignment Algorithm for DVL Aided SINS", 《MATHEMATICAL PROBLEMS IN ENGINEERING》 *
应智慧 等: "高精度激光多普勒测速仪在陆用自主导航***中的应用", 《中国激光》 *
王琦: "三维激光测速技术研究及其在惯导中的应用", 《中国博士学位论文全文数据库基础科学辑》 *

Cited By (8)

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

Also Published As

Publication number Publication date
CN112798021B (zh) 2021-07-13

Similar Documents

Publication Publication Date Title
CN112798021B (zh) 基于激光多普勒测速仪的惯导***行进间初始对准方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
JP4199553B2 (ja) ハイブリッド航法装置
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN108731664B (zh) 机器人状态估计方法、装置、计算机设备和存储介质
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航***晃动基座自对准方法
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN105806363A (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN107389069B (zh) 基于双向卡尔曼滤波的地面姿态处理方法
CN108592943B (zh) 一种基于opreq方法的惯性系粗对准计算方法
JP5164645B2 (ja) カルマンフィルタ処理における繰り返し演算制御方法及び装置
CN112857398A (zh) 一种系泊状态下舰船的快速初始对准方法和装置
CN113092822A (zh) 一种基于惯组的激光多普勒测速仪的在线标定方法和装置
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN111190207A (zh) 基于pstcsdref算法的无人机ins bds组合导航方法

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