CN105043415B - 基于四元数模型的惯性系自对准方法 - Google Patents

基于四元数模型的惯性系自对准方法 Download PDF

Info

Publication number
CN105043415B
CN105043415B CN201510409501.XA CN201510409501A CN105043415B CN 105043415 B CN105043415 B CN 105043415B CN 201510409501 A CN201510409501 A CN 201510409501A CN 105043415 B CN105043415 B CN 105043415B
Authority
CN
China
Prior art keywords
mrow
msub
msubsup
mtd
msup
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
CN201510409501.XA
Other languages
English (en)
Other versions
CN105043415A (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.)
Beijing University of Technology
Original Assignee
Beijing University of 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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201510409501.XA priority Critical patent/CN105043415B/zh
Publication of CN105043415A publication Critical patent/CN105043415A/zh
Application granted granted Critical
Publication of CN105043415B publication Critical patent/CN105043415B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

基于四元数模型的惯性系自对准方法，该方法通过下述流程实现，通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；捷联惯导系统进行预热准备，启动系统，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息fb(t)；对采集到的陀螺和加速度计的数据进行处理，利用重力投影矢量与初始姿态四元数，计算出的初始姿态矩阵。基于惯性系的捷联惯导系统自对准原理，可有效地隔离角晃动的影响，从而解决动态干扰情况下，传统解析的对准方法不能用的问题。

Description

基于四元数模型的惯性系自对准方法
技术领域
本发明涉及的是一种动态干扰情况下的捷联惯导系统自对准技术，用于诸如发动机处于高频振动的汽车，格斗状态下的战斗机，浪涌下的舰船等。
背景技术
惯性导航系统是一种自主式的导航系统，它利用陀螺仪和加速度计等惯性测量器件以及初始的导航信息来确定载体运行期间的各项导航参数。惯性导航系统不仅可以同时实时快速地测量线运动和角运动，而且还有一个极大的优点，惯性技术是完全自主式的测量方法，它不依赖光线，电磁波，声音，磁场等外部信息。所以近年来，尽管卫星导航、无线电导航和天文导航等技术的发展很快，但是它们不可能完全取代惯性导航的应用。尤其在军用领域内，惯导系统和以惯导为主的组合导航系统始终居于主导地位。捷联式惯性导航系统(strapdown inertial navigation system,SINS)将惯性敏感元件与载体直接固联，不需要实体的稳定平台，因此较平台式惯导系统，它具有成本低、体积小、重量轻、可靠性高等优点。近三十年来，捷联式惯导系统日趋成熟，精度逐步提高，应用范围也逐渐扩大。
初始对准是捷联惯导系统进行导航的前提，初始对准精度直接影响捷联式惯导系统的工作精度，初始对准时间也是反映武器系统快速反应能力的重要战术指标。初始对准作为捷联惯导系统的一个关键技术，一直是国内外学者研究的热点。初始对准的目的是进入导航任务之前，建立起精确的平台初始指向，即姿态矩阵。初始对准通常分为两个阶段：粗对准阶段和精对准阶段。在粗对准阶段可以利用外界方位信息进行直接装订或由捷联惯组利用对地球自转角速率和重力加速度的测量自主确定获取粗略的初始姿态矩阵；精对准阶段在粗对准的基础上，对粗对准的结果进一步修正得到更加准确的捷联矩阵。一般以速度误差作为观测量，建立初始对准误差模型,通过一定的滤波算法估计失准角，完成精对准。
目前，国内普遍采用静态初始对准方式，静态初始对准技术已经十分地成熟。但是对于诸如发动机处于高频振动的汽车，格斗状态下的战斗机，浪涌下的舰船等，这些载体都工作在动态环境中，载体的角振动和线振动会导致系统的精度下降。阵风或波浪干扰以及发动机待速等不良干扰影响，可能会导致初始对准结果误差很大。
如何在运载体晃动干扰或者运动环境下，实现初始对准是一个非常值得研究的课题。譬如舰船在风浪作用下作大幅度角运动的环境下，难以获得粗略初始姿态矩阵条件下，该技术在军事应用中将极大增强武器发射平台的机动性能，赢得战机，提高自身生存能力。由于我国制造工艺技术水平的限制和发达国家的技术封锁，高精度、高可靠性的光纤陀螺制造水平和发达国家还有一定差距。因此，为了提高SINS的精度，设计高性能的捷联算法十分重要。本发明就是对动态干扰情况下，捷联惯导系统的自对准方法展开研究。
发明内容
本发明的目的在于提供一种能够有效解决载体在阵风、海风、浪涌、发动机振动等随机干扰环境下自对准问题的动态干扰情况下的捷联惯导系统自对准方法。
为实现上述目的，本发明采用的技术方案为基于四元数模型的惯性系自对准方法，该方法通过下述流程实现，
(1)通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；
(2)捷联惯导系统进行预热准备，启动系统，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息fb(t)；
(3)对采集到的陀螺和加速度计的数据进行处理，利用重力投影矢量与初始姿态四元数，计算出的初始姿态矩阵。其具体步骤如下，首先，惯性系下捷联惯导系统的自对准过程中用到几个新的坐标系，定义如下，
n0―导航惯性坐标系，初始对准起始时刻，n系相对惯性空间凝固所得的惯性坐标系。
b0―载体惯性坐标系，初始对准起始时刻，b系相对惯性空间凝固所得的惯性坐标系。
在惯性系下捷联惯导系统的自对准算法中，姿态矩阵的表达式为：
其中，为导航惯性坐标系到导航坐标系的转换矩阵，根据对准点的地理位置信息和时间信息进行计算；为载体坐标系到载体惯性坐标系的转换矩阵，由陀螺测量信息跟踪计算；不随时间变化，是对准起始时刻载体的姿态方向余弦矩阵，此处称为初始姿态方向余弦矩阵，其确定过程是初始对准的核心。
设载体初始对准地地理纬度为L，则可由下式求出：
而利用陀螺输出的角速度信息对进行更新解算。
为常值矩阵，根据重力加速度g(t)在惯性坐标系中的投影和构造观测矢量计算。因为有关系式，
而重力加速度g(t)在惯性坐标系中是旋转的，因此，要获得两个不同时刻对应的和的矢量观测，进而确定从而完成初始对准。
可直接解析计算：
式中，gn(t)＝[0 0 -g]T，g为当地重力加速度。
通过陀螺和加速度计测量值计算得到：
对于这种典型的Wahba问题，通过q-method求解。通过双矢量定姿确定初始姿态方向余弦矩阵而本方法中不直接求解初始姿态矩阵而是估计与初始姿态方向余弦阵相对应的初始姿态四元数易知是常值。为表述方便，用Q表示n0到b0的旋转四元数且Q＝[q0,qT]T，其中q0为标量部分，q为矢量部分。
(4)建立惯性坐标系下的捷联惯导系统的四元数误差模型。以姿态四元数作为估计状态，利用四元数卡尔曼滤波对准算法完成初始对准。该方法不依赖于惯导误差方程且无需在粗对准的基础上进行精对准即可完成动态干扰条件下的初始对准。
1)构造无误差的四元数量测方程
精确求解，而是通过陀螺和加速度计量测计算，而加速度计测量有噪声干扰且基座存在随机线运动干扰，所以对和进行积分运算减少随机噪声和随机干扰误差的影响。
对和扩维得：
利用与同姿态四元数的关系写为：
令γ(x)和χ(x)表示将R3空间中的3×1的矢量x线性映射到R4×4空间：
对两端左乘四元数Q，得
两式相减移向得
即H°Q＝04×1
此式即为四元数的线性方程，是无误差的四元数量测方程。由于精确解析求解，而是变换后加速度计测量积分，存在量测误差
将该式代入重新构造量有误差的四元数量测方程。
2)建立有误差的四元数量测方程：
tk+1时刻初始姿态四元数记为Qk+1，量测阵记为Hk+1，可得tk+1时刻的量测方程为：
其中令
这是关于初始姿态四元数在tk+1时刻的伪量测模型，称之为伪量测模型是因为量测量始终为0。信号项是四元数的线性函数，的噪声项是与四元数相关的加性向量。
3)建立系统的状态方程：
Q表示的是两个惯性坐标系之间的姿态关系，所以初始姿态四元数Q在整个初始对准过程中为常值，故以Q作为估计参数时其状态空间模型非常简单而且无误差：
Qk+1＝Qk
式中Qk+1和Qk分别表示tk+1和tk时刻对应的初始姿态四元数。
(5)由于为不确定性量测干扰，设计模糊自适应Kalman滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准，进入导航状态。
建立系统的模糊自适应卡尔曼滤波方程：
tr为矩阵的迹，为Qk+1的期望值，和分别为Qk和Vk+1的方差，I4为单位矩阵，为阵的调整系数，ρ为常值调整系数，取值范围0≤ρ≤1，为[1,0,0,0]，为5I4。
残差方差的实测值为：
残差方差的理论值定义为：
定义残差实测方差与理论方差之间的差值函数：
为DORk建立模糊集，N＝负，Z＝零，P＝正，同样为建立模糊集I＝增加，M＝保持，D＝减少；
模糊规则如下：
如果DORk∈N，那么
如果DORk∈Z，那么
如果DORk∈P，那么
通过n0到b0之间的姿态四元数可以得到
根据式求取精确的捷联姿态矩阵
其中，和按照上述步骤(3)中的方法进行计算。
完成初始对准，进入导航状态。
本发明的优点：基于惯性系的捷联惯导系统自对准原理，可有效地隔离角晃动的影响，从而解决动态干扰情况下，传统解析的对准方法不能用的问题。以姿态四元数作为估计状态，利用重力投影矢量与初始姿态四元数的线性量测关系建立初始姿态四元数系统模型，不依赖于惯导误差方程且无需在粗对准的基础上进行精对准即可完成晃动条件下的初始对准。通过模糊自适应Kalman滤波法可以精确地估计捷联惯导系统的初始姿态矩阵，有效地抑制了量测噪声中的动态随机干扰，减小了环境对系统的影响，使初始对准的精度大幅提高；克服了传统解析粗对准和传统Kalman滤波法在动态随机干扰环境下无法使用的缺点，在没有其他外辅助测量设备、不增加成本的情况下，达到捷联惯导系统高精度快速自对准的适用要求。
附图说明
图1为白噪声干扰下的滤波仿真结果；其中，(a)为东向姿态误差角；(b)为北向姿态误差角；(c)为天向姿态误差角；
图2为随机干扰下的滤波仿真结果；其中，(a)为东向姿态误差角；(b)为北向姿态误差角；(c)为天向姿态误差角；
图3为实测数据仿真结果；(a)为俯仰角方向姿态角误差；(b)为横滚角方向姿态角误差；(c)为航向角方向姿态角误差；(d)为俯仰角方向失准角误差；(e)为横滚角方向失准角误差；(f)为航向角方向失准角误差。
图4为动态干扰情况下基于四元数的捷联惯导系统自对准方法流程图。
具体实施方式
下面结合附图对本发明的具体实施方式进行详细的描述：
结合图1～4，晃动基座下舰载机旋转式捷联惯导系统自对准方法的步骤如图4所示，具体如下：
(1)通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；
(2)捷联惯导系统进行预热准备，启动系统，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息fb(t)；
(3)对采集到的陀螺和加速度计的数据进行处理，利用重力投影矢量与初始姿态四元数，计算出的初始姿态矩阵。其具体步骤如下：
首先，惯性系下捷联惯导系统的自对准过程中用到新的坐标系，定义如下：
n0―导航惯性坐标系，初始对准起始时刻，n系相对惯性空间凝固所得的惯性坐标系。
b0―载体惯性坐标系，初始对准起始时刻，b系相对惯性空间凝固所得的惯性坐标系。
在惯性系下捷联惯导系统的自对准算法中，姿态矩阵的表达式为：
其中，为惯性导航坐标系到导航坐标系的转换矩阵，可根据对准点的地理位置信息和时间信息进行计算；为载体坐标系到惯性载体坐标系的转换矩阵，可由陀螺测量信息跟踪计算；是惯性载体坐标系与惯性导航坐标系的转换矩阵，不随时间变化，是对准起始时刻载体的姿态方向余弦矩阵，此处称为初始姿态方向余弦矩阵，其确定过程是初始对准的核心。
设载体初始对准地地理纬度为L，则由下式求出：
而利用陀螺输出的角速度信息对进行更新解算。
为常值矩阵，根据重力加速度g(t)在惯性坐标系中的投影和构造观测矢量计算。因为有关系式
而重力加速度g(t)在惯性坐标系中是旋转的，因此，理论上只要获得两个不同时刻对应的和的矢量观测就可确定从而完成初始对准。
直接解析计算：
式中，gn(t)＝[0 0 -g]T，g为当地重力加速度。
可通过陀螺和加速度计测量值计算得到：
对于这种典型的Wahba问题，通过q-method求解。通过双矢量定姿确定初始姿态方向余弦矩阵而本方法中不直接求解初始姿态矩阵而是估计与初始姿态方向余弦阵相对应的初始姿态四元数易知是常值。为表述方便，用Q表示n0到b0的旋转四元数且Q＝[q0,qT]T，其中q0为标量部分，q为矢量部分。
(4)建立惯性坐标系下的捷联惯导系统的四元数误差模型。以姿态四元数作为估计状态，利用四元数卡尔曼滤波对准算法完成初始对准。该方法不依赖于惯导误差方程且无需在粗对准的基础上进行精对准即可完成动态干扰条件下的初始对准。
1)构造无误差的四元数量测方程
可精确求解，而是通过陀螺和加速度计量测计算，而加速度计测量有噪声干扰且基座存在随机线运动干扰，所以对和进行积分运算以减少随机噪声和随机干扰误差的影响。
对和扩维得：
利用与同姿态四元数的关系写为：
令γ(x)和χ(x)表示将R3空间中的3×1的矢量x线性映射到R4×4空间：
对两端左乘四元数Q，得
两式相减移向得
即H°Q＝04×1
此式即为四元数的线性方程，是无误差的四元数量测方程。由于精确解析求解，而是变换后加速度计测量积分，存在量测误差
将该式代入重新构造量有误差的四元数量测方程。
2)建立有误差的四元数量测方程：
tk+1时刻初始姿态四元数记为Qk+1，量测阵记为Hk+1，可得tk+1时刻的量测方程为：
其中令
这是关于初始姿态四元数在tk+1时刻的伪量测模型，称之为伪量测模型是因为量测量始终为0。信号项是四元数的线性函数，的噪声项是与四元数相关的加性向量。
3)建立系统的状态方程：
Q表示的是两个惯性坐标系之间的姿态关系，所以初始姿态四元数Q在整个初始对准过程中为常值，故以Q作为估计参数时其状态空间模型非常简单而且无误差：
Qk+1＝Qk
式中Qk+1和Qk分别表示tk+1和tk时刻对应的初始姿态四元数。
(5)由于为不确定性量测干扰，设计模糊自适应Kalman滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准，进入导航状态。
建立系统的模糊自适应卡尔曼滤波方程：
tr为矩阵的迹，的期望值，和分别为Qk和Vk+1的方差，I4为单位矩阵，为阵的调整系数，ρ为常值调整系数，取值范围0≤ρ≤1，为[1,0,0,0]，为5I4。
残差方差的实测值为：
残差方差的理论值定义为：
定义残差实测方差与理论方差之间的差值函数：
为DORk建立模糊集，N＝负，Z＝零，P＝正，同样为建立模糊集I＝增加，M＝保持，D＝减少；
模糊规则如下：
如果DORk∈N，那么
如果DORk∈Z，那么
如果DORk∈P，那么
通过n0到b0之间的姿态四元数可以得到
根据式求取精确的捷联姿态矩阵
其中，和按照上述步骤(3)中的方法进行计算。
本发明的有益效果如下：
在以下仿真环境下，对该方法进行仿真实验：
舰船受风浪波动影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化：
存在横荡、纵荡和垂荡引起的线速度：
i＝x,y,z
ADx＝0.02m，ADy＝0.03m，ADz＝0.3m；ωDi＝2π/TDi,且TDx＝7s，TDy＝6s，TDz＝8s；为[0,2π]上服从均匀分布的随机相位；
初始地理位置：东经118°，北纬40°；
陀螺漂移：三个方向轴上的陀螺常值漂移为0.01°/h，随机漂移为
加速度计零位偏置：三个方向轴上的加速度计常值偏置为1×10-4g，随机偏置为
赤道半径：Re＝6378165.0m；
椭球扁率：e＝3.352E-3；
由万有引力可得地球表面重力加速度：g0＝9.78049；
地球自转角速度(单位为rad/s)：7.2921158E-5；
常数：π＝3.1415926；
为了说明模糊自适应卡尔曼滤波在动态干扰环境中的有效性，分别考虑测量噪声为白噪声和随机干扰的情况,比较了模糊自适应卡尔曼滤波器和传统的精对准卡尔曼滤波的效果。
每组仿真历时600s。将视为白噪声的情况下，精对准仿真结果如图1所示。考虑到动态环境下，为随机干扰，基于Kalman和模糊自适应Kalman滤波的自对准仿真结果如图2所示。以姿态误差角的估计误差作为衡量精对准的指标。
采用了西北工业大学严恭敏捷联惯性导航参考程序的实测数据lgimu.dat。其激光惯组放置车上，有人员上下车干扰，采集约半小时，采样周期8ms；共6列数据:前3列为角增量(当量0.932角秒/脉冲)，后3列为速度增量(当量500ug*s/脉冲，1ug*s＝9.7803267714*1E-6m/s)；采集地点为西工大自动化学院惯性技术实验室位置：纬度34.246048度；经度108.909663度。代入该实测数据，对数据进行前述初始对准方法的处理，以验证本文动态干扰情况下初始对准方法的可行性。实验结果如图3所示。
实验结果和结论：
由图1～2可知，在动态干扰情况下，本发明所述惯性系的初始对准算法能有效地隔离角晃动的影响，在此基础上利用模糊自适应Kalman滤波法对姿态矩阵作精确估计能够完成初始对准。由仿真结果得出，在动态干扰情况下，模糊自适应Kalman滤波器能够较好地抑制不确定性干扰，滤波估计稳定。相比之下，传统Kalman滤波器虽然收敛速度快，但是滤波精度低，方位姿态角的滤波估计发生畸变、甚至发散。
由图3可以看出两种方法得到的姿态变化趋势类似。水平姿态角的晃动幅值约达0.2°和0.3°，方位角受到干扰的影响较小。它与人员上下车、开关车门的干扰相符，如实地反映了载车角晃动的影响。由于上下车干扰的存在，传统Kalman方法效果不理想，俯仰角和横滚角产生发散。而本文惯性系下精对准方法能有效抗干扰，进而验证了本文动态干扰情况下自对准方法的可行性。
实验结果表明：
本发明所述的基于四元数对准方法具有较强的抗干扰能力，特别是对晃动基座上载体角运动和线运动引起的不确定性干扰的抑制效果明显。基于模糊自适应Kalman的自主精对准方法，有效地抑制了量测噪声中的不确定性干扰，实现动态干扰情况下捷联惯导系统的快速精确自对准。

Claims (1)

1.基于四元数模型的惯性系自对准方法，其特征在于：该方法通过下述流程实现，
(1)通过全球定位系统GPS获得载体所在位置的经度λ、纬度L，将它们装订至导航计算机中；
(2)捷联惯导系统进行预热准备，启动系统，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息fb(t)；
(3)对采集到的陀螺和加速度计的数据进行处理，利用重力投影矢量与初始姿态四元数，计算出初始姿态矩阵；其具体步骤如下，
首先，惯性系下捷联惯导系统的自对准过程中用到几个新的坐标系，定义如下，
n0―导航惯性坐标系，初始对准起始时刻，n系相对惯性空间凝固所得的惯性坐标系；
b0―载体惯性坐标系，初始对准起始时刻，b系相对惯性空间凝固所得的惯性坐标系；
在惯性系下捷联惯导系统的自对准算法中，姿态矩阵的表达式为：
<mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msubsup> <mi>C</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>,</mo> </mrow>
其中，为导航惯性坐标系到导航坐标系的转换矩阵，根据对准点的地理位置信息和时间信息进行计算；为载体坐标系到载体惯性坐标系的转换矩阵，由陀螺测量信息跟踪计算；不随时间变化，是对准起始时刻载体的姿态方向余弦矩阵，此处称为初始姿态方向余弦矩阵，其确定过程是初始对准的核心；
设载体初始对准地地理纬度为L，则由下式求出：
<mrow> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mi>cos</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msup> <mi>sin</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>cos</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mi>cos</mi> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>sin</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mi>cos</mi> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <msup> <mi>cos</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mrow> <mo>(</mo> <mi>&amp;omega;</mi> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <msup> <mi>sin</mi> <mn>2</mn> </msup> <mrow> <mo>(</mo> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
而利用陀螺输出的角速度信息对进行更新解算；
<mrow> <msubsup> <mover> <mi>C</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> </mrow>
为常值矩阵，根据重力加速度g(t)在惯性坐标系中的投影和构造观测矢量计算；因为有关系式，
<mrow> <msup> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
而重力加速度g(t)在惯性坐标系中是旋转的，因此，要获得两个不同时刻对应的和的矢量观测，进而确定从而完成初始对准；
直接解析计算：
<mrow> <msup> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>C</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <msup> <mi>g</mi> <mi>n</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
式中，gn(t)＝[0 0 -g]T，g为当地重力加速度；
通过陀螺和加速度计测量值计算得到：
<mrow> <msup> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>g</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <msup> <mi>f</mi> <mi>b</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
对于这种典型的Wahba问题，通过q-method求解；通过双矢量定姿确定初始姿态方向余弦矩阵而本方法中不直接求解初始姿态矩阵而是估计与初始姿态方向余弦阵相对应的初始姿态四元数易知是常值；为表述方便，用Q表示n0到b0的旋转四元数且Q＝[q0,qT]T，其中q0为标量部分，q为矢量部分；
(4)建立惯性坐标系下的捷联惯导系统的四元数误差模型；以姿态四元数作为估计状态，利用四元数卡尔曼滤波对准算法完成初始对准；该方法不依赖于惯导误差方程且无需在粗对准的基础上进行精对准即完成动态干扰条件下的初始对准；
1)构造无误差的四元数量测方程
精确求解，而是通过陀螺和加速度计量测计算，而加速度计测量有噪声干扰且基座存在随机线运动干扰，所以对和进行积分运算减少随机噪声和随机干扰误差的影响；
对和扩维得：
<mrow> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>,</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow>
<mrow> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>,</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow>
利用与同姿态四元数的关系写为：
<mrow> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <msup> <mi>Q</mi> <mo>*</mo> </msup> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>&amp;CircleTimes;</mo> <mi>Q</mi> </mrow>
令γ(x)和χ(x)表示将R3空间中的3×1的矢量x线性映射到R4×4空间：
<mrow> <mi>&amp;gamma;</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msup> <mi>x</mi> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>x</mi> </mtd> <mtd> <mrow> <mo>-</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>x</mi> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>&amp;chi;</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msup> <mi>x</mi> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mi>x</mi> </mtd> <mtd> <msup> <mrow> <mo>&amp;lsqb;</mo> <mi>x</mi> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> </mtd> </mtr> </mtable> </mfenced> </mrow>
对两端左乘四元数Q，得
<mrow> <mi>Q</mi> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <mi>&amp;gamma;</mi> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>Q</mi> </mrow>
<mrow> <msubsup> <mi>v</mi> <mi>q</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>&amp;CircleTimes;</mo> <mi>Q</mi> <mo>=</mo> <mi>&amp;chi;</mi> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>Q</mi> </mrow>
两式相减移向得
<mrow> <mo>&amp;lsqb;</mo> <mi>&amp;gamma;</mi> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mo>-</mo> <mi>&amp;chi;</mi> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mi>Q</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>-</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>-</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> </mrow> </mtd> <mtd> <mrow> <mo>&amp;lsqb;</mo> <mo>-</mo> <mrow> <mo>(</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>+</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>n</mi> <mn>0</mn> </msub> </msubsup> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mi>Q</mi> <mo>=</mo> <msub> <mn>0</mn> <mrow> <mn>4</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> </mrow>
即H°Q＝04×1
此式即为四元数的线性方程，是无误差的四元数量测方程；由于精确解析求解，而是变换后加速度计测量积分，存在量测误差
<mrow> <msubsup> <mi>&amp;delta;v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>v</mi> <mo>~</mo> </mover> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> <mo>-</mo> <msubsup> <mi>v</mi> <mi>g</mi> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> </mrow>
将该式代入重新构造无误差的四元数量测方程；
2)建立有误差的四元数量测方程：
tk+1时刻初始姿态四元数记为Qk+1，量测阵记为Hk+1，得tk+1时刻的量测方程为：
<mrow> <msub> <mn>0</mn> <mrow> <mn>4</mn> <mo>&amp;times;</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>0.5</mn> <mi>&amp;Xi;</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>&amp;delta;v</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>b</mi> <mn>0</mn> </msub> </msubsup> </mrow>
其中令
<mrow> <mi>H</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>d</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>d</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <mo>&amp;lsqb;</mo> <msub> <mi>s</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>&amp;Xi;</mi> <mrow> <mo>(</mo> <mi>Q</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mo>-</mo> <msup> <mi>q</mi> <mi>T</mi> </msup> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>&amp;lsqb;</mo> <mi>q</mi> <mo>&amp;times;</mo> <mo>&amp;rsqb;</mo> <mo>+</mo> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>I</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
这是关于初始姿态四元数在tk+1时刻的伪量测模型，称之为伪量测模型是因为量测量始终为0；信号项是四元数的线性函数，的噪声项是与四元数相关的加性向量；
3)建立系统的状态方程：
Q表示的是两个惯性坐标系之间的姿态关系，所以初始姿态四元数Q在整个初始对准过程中为常值，故以Q作为估计参数时其状态空间模型非常简单而且无误差：
Qk+1＝Qk
式中Qk+1和Qk分别表示tk+1和tk时刻对应的初始姿态四元数；
(5)由于为不确定性量测干扰，设计模糊自适应Kalman滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准，进入导航状态；
建立系统的模糊自适应卡尔曼滤波方程：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <msub> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>K</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>q</mi> </msubsup> <msubsup> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>P</mi> <mi>k</mi> <mi>q</mi> </msubsup> <msubsup> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msubsup> <mi>b</mi> <mi>k</mi> <mi>&amp;alpha;</mi> </msubsup> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>v</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mn>4</mn> </mfrac> <msub> <mi>&amp;rho;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;lsqb;</mo> <mi>t</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>M</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msub> <mi>I</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>M</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>M</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <msubsup> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>T</mi> </msubsup> <mo>+</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>q</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>q</mi> </msubsup> <mo>=</mo> <mrow> <mo>(</mo> <msub> <mi>I</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>K</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>H</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mi>q</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced>
tr为矩阵的迹，为Qk+1的期望值，和分别为Qk和Vk+1的方差，I4为单位矩阵，为阵的调整系数，ρ为常值调整系数，取值范围0≤ρ≤1，为[1,0,0,0]，为5I4；
残差方差的实测值为：
<mrow> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>=</mo> <mfrac> <mn>1</mn> <mi>N</mi> </mfrac> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>i</mi> <mo>=</mo> <mi>k</mi> <mo>-</mo> <mi>N</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>k</mi> </munderover> <mrow> <mo>(</mo> <msub> <mi>r</mi> <mi>i</mi> </msub> <msubsup> <mi>r</mi> <mi>i</mi> <mi>T</mi> </msubsup> <mo>)</mo> </mrow> </mrow>
残差方差的理论值定义为：
<mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mn>4</mn> </mfrac> <msub> <mi>&amp;rho;</mi> <mi>k</mi> </msub> <mo>&amp;lsqb;</mo> <mi>t</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>M</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <msub> <mi>I</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>M</mi> <mi>k</mi> </msub> <mo>&amp;rsqb;</mo> </mrow>
定义残差实测方差与理论方差之间的差值函数：
<mrow> <msub> <mi>DOR</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mfrac> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>(</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>&amp;GreaterEqual;</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mfrac> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> </mrow> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>(</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> <mo>-</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msubsup> <mi>P</mi> <mi>k</mi> <mi>v</mi> </msubsup> <mo>)</mo> </mrow> <mo>&amp;le;</mo> <mi>T</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>C</mi> <mi>k</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
为DORk建立模糊集，N＝负，Z＝零，P＝正，同样为建立模糊集I＝增加，M＝保持，D＝减少；
模糊规则如下：
如果DORk∈N，那么
如果DORk∈Z，那么
如果DORk∈P，那么
通过n0到b0之间的姿态四元数得到
根据式求取精确的捷联姿态矩阵
其中，和按照上述步骤(3)中的方法进行计算；
完成初始对准，进入导航状态。
CN201510409501.XA 2015-07-13 2015-07-13 基于四元数模型的惯性系自对准方法 Active CN105043415B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510409501.XA CN105043415B (zh) 2015-07-13 2015-07-13 基于四元数模型的惯性系自对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510409501.XA CN105043415B (zh) 2015-07-13 2015-07-13 基于四元数模型的惯性系自对准方法

Publications (2)

Publication Number Publication Date
CN105043415A CN105043415A (zh) 2015-11-11
CN105043415B true CN105043415B (zh) 2018-01-05

Family

ID=54450148

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510409501.XA Active CN105043415B (zh) 2015-07-13 2015-07-13 基于四元数模型的惯性系自对准方法

Country Status (1)

Country Link
CN (1) CN105043415B (zh)

Families Citing this family (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105509769B (zh) * 2015-12-11 2019-07-05 上海新跃仪表厂 一种运载火箭捷联惯导全自主对准方法
CN106052715B (zh) * 2016-05-23 2018-11-23 西北工业大学 单轴旋转捷联惯导系统回溯式自对准方法
CN106123921B (zh) * 2016-07-10 2019-05-24 北京工业大学 动态干扰条件下捷联惯导系统的纬度未知自对准方法
CN106052686B (zh) * 2016-07-10 2019-07-26 北京工业大学 基于dsptms320f28335的全自主捷联惯性导航系统
CN106595711A (zh) * 2016-12-21 2017-04-26 东南大学 一种基于递推四元数的捷联惯性导航系统粗对准方法
CN109387221B (zh) * 2017-08-03 2022-03-11 北京自动化控制设备研究所 一种微惯性导航系统后处理自对准方法
CN108981753B (zh) * 2018-08-30 2020-11-13 衡阳市衡山科学城科技创新研究院有限公司 基于多惯组信息约束的地面对准方法、系统及存储介质
CN109141475B (zh) * 2018-09-14 2020-06-05 苏州大学 一种dvl辅助sins鲁棒行进间初始对准方法
CN110887475B (zh) * 2019-12-09 2021-12-10 北京航空航天大学 一种基于偏振北极点及偏振太阳矢量的静基座粗对准方法
CN113155119B (zh) * 2020-06-02 2024-01-30 西安天和防务技术股份有限公司 天文导航的振动补偿方法、装置及电子设备
CN112284412B (zh) * 2020-09-09 2022-11-11 上海航天控制技术研究所 一种避免欧拉转换奇异导致精度下降的地面静态对准方法
CN113503892B (zh) * 2021-04-25 2024-03-01 中船航海科技有限责任公司 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN113959462B (zh) * 2021-10-21 2023-09-12 北京机电工程研究所 一种基于四元数的惯性导航系统自对准方法
CN114184211B (zh) * 2021-12-27 2023-07-14 北京计算机技术及应用研究所 一种惯导可靠性试验中性能变化机理一致性判定方法
CN115855038B (zh) * 2022-11-22 2024-01-09 哈尔滨工程大学 一种短时高精度姿态保持方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2590635A1 (en) * 2006-05-31 2007-11-30 Honeywell International Inc. Rapid self-alignment of a strapdown inertial system through real-time reprocessing
CN102538821A (zh) * 2011-12-17 2012-07-04 东南大学 一种快速、参数分段式捷联惯性导航系统自对准方法
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN104535080A (zh) * 2014-11-27 2015-04-22 哈尔滨工程大学 大方位失准角下基于误差四元数的传递对准方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2590635A1 (en) * 2006-05-31 2007-11-30 Honeywell International Inc. Rapid self-alignment of a strapdown inertial system through real-time reprocessing
CN102538821A (zh) * 2011-12-17 2012-07-04 东南大学 一种快速、参数分段式捷联惯性导航系统自对准方法
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN104535080A (zh) * 2014-11-27 2015-04-22 哈尔滨工程大学 大方位失准角下基于误差四元数的传递对准方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
"Nonlinear State Estimating Using Adaptive Particle Filter";Jian Zhou;《Proceedings of the 7th World Congress on Intelligent Control and Automation》;20080627;正文第6377-6380页 *
"动态干扰条件下的旋转式捷联惯导系统自对准方法";裴福俊;《自动化学报》;20140930;第40卷(第9期);正文第2050-2056页 *

Also Published As

Publication number Publication date
CN105043415A (zh) 2015-11-11

Similar Documents

Publication Publication Date Title
CN105043415B (zh) 基于四元数模型的惯性系自对准方法
Nassar Improving the inertial navigation system (INS) error model for INS and INS/DGPS applications
CN103674030B (zh) 基于天文姿态基准保持的垂线偏差动态测量装置和方法
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN106767787A (zh) 一种紧耦合gnss/ins组合导航装置
CN106871928A (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN103245360A (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN102768043B (zh) 一种无外观测量的调制型捷联系统组合姿态确定方法
CN102494699B (zh) 捷联式航空重力仪测量参数置信度评估方法
Yang et al. Global autonomous positioning in GNSS-challenged environments: A bioinspired strategy by polarization pattern
CN105928515B (zh) 一种无人机导航系统
CN104697520B (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN106405670A (zh) 一种适用于捷联式海洋重力仪的重力异常数据处理方法
CN103076026B (zh) 一种捷联惯导系统中确定多普勒计程仪测速误差的方法
CN109916395A (zh) 一种姿态自主冗余组合导航算法
CN109556631A (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN101162147A (zh) 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法
CN102519485A (zh) 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法
CN110345941A (zh) 深潜载人潜水器sins自辅助导航方法
CN110133692A (zh) 惯导技术辅助的高精度gnss动态倾斜测量系统及方法
CN105988129A (zh) 一种基于标量估计算法的ins/gnss组合导航方法
CN109764870A (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN106767928A (zh) 一种自适应快速传递对准方法
CN109856691A (zh) 一种基于梯度法的航空重力矢量向下延拓方法及系统

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant