CN107270937A - 一种离线小波降噪快速初始对准方法 - Google Patents

一种离线小波降噪快速初始对准方法 Download PDF

Info

Publication number
CN107270937A
CN107270937A CN201710406484.3A CN201710406484A CN107270937A CN 107270937 A CN107270937 A CN 107270937A CN 201710406484 A CN201710406484 A CN 201710406484A CN 107270937 A CN107270937 A CN 107270937A
Authority
CN
China
Prior art keywords
mrow
msup
msubsup
msub
mtd
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
CN201710406484.3A
Other languages
English (en)
Other versions
CN107270937B (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.)
Changshu Institute of Technology
Original Assignee
Changshu Institute 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 Changshu Institute of Technology filed Critical Changshu Institute of Technology
Priority to CN201710406484.3A priority Critical patent/CN107270937B/zh
Publication of CN107270937A publication Critical patent/CN107270937A/zh
Application granted granted Critical
Publication of CN107270937B publication Critical patent/CN107270937B/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

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

一种离线小波降噪快速初始对准方法
技术领域：
本发明涉及一种惯性测量单元在晃动基座条件下的初始对准方法，属于捷联惯性导航技术领域。
背景技术：
初始对准技术是捷联惯性导航技术的关键技术之一，对准精度的提高有利于捷联惯性导航解算精度的提高。目前，常用的捷联惯性导航初始对准技术主要分为粗对准和精对准两个过程，而精对准的对准精度又很大程度上取决于粗对准，故而提高粗对准的对准精度具有很高的现实意义。常用的捷联惯性粗对准方法有解析粗对准，惯性系粗对准和凝固解析粗对准，在这三种方法中，凝固解析粗对准在对准精度和抗晃动方面具有较好的性能。但是凝固解析粗对准中，采用双矢量实时解析计算，在矢量计算过程中，易受外部干扰及传感器量测噪声的影响，降低对准速度，增加对准时间，不利于惯性系统快速初始对准的要求。
为了克服上述存在的问题，采用数据存储及小波降噪算法，对观测矢量进行一次降噪处理，将降噪之后的观测矢量用于初始对准过程，能够有效的减小环境扰动及量测噪声的影响。而且，利用分离矩阵方法，更新矩阵可以实时迭代计算，初始姿态矩阵则可以离线处理，此过程同步进行，加速了对准过程，具有现实的使用价值。
发明内容：
1、本发明的目的
本发明的目的是为了提高捷联惯导系统的对准精度，提出了一种离线小波降噪快速初始对准方法。发明基于凝固惯性系对准的方法，采用离线小波变换对观测矢量进行一次降噪，再利用四元数最优基方法提取初始姿态四元数，通过旋转矩阵的乘法链式法则最终实现对准矩阵的计算。
2、本发明所采用技术方案
一种离线小波降噪快速初始对准方法，包括以下几个步骤：
步骤一：传感器数据获取，包括光纤陀螺仪和石英挠性加速度计数据；
步骤二：建立初始对准坐标系，包括初始载体坐标系、初始导航坐标系、初始地球坐标系、载体坐标系、导航坐标系、地球坐标系与惯性坐标系；
步骤三：建立重力视运动矢量模型，构造离线小波降噪算法；
步骤四：利用降噪滤波之后的重力视运动矢量，进行最优初始姿态的解算；
步骤五：利用计算得到的最优初始姿态四元数，结合姿态矩阵链式法则，实现快速初始对准过程。
3、本发明的有益效果
(1)本发明采用离线处理方式，通过小波变换算法进行视在重力重构，即对重力视运动矢量模型进行小波降噪，降低量测随机噪声，提高视运动矢量的识别精度；
(2)本发明采用最优迭代K-矩阵姿态确定方法，利用离线降噪之后的重力视运动矢量构造迭代K-矩阵，通过提取有效特征矢量，实现初始姿态四元数的快速确定；
(3)本发明采用凝固惯性对准方法与最优迭代K-矩阵姿态确定相结合的方法，实现了离线降噪、姿态确定以及初始对准过程，提高了初始对准的速度。
(4)本发明所采用的新对准方法，整个对准时间能在50s左右完成，相较于现有技术中的粗对准过程，本方法速度更快。
附图说明：
图1是初始对准算法流程图；
图2是各示意图坐标系；
图3是观测矢量降噪前后对比图；
图4是摇摆情况下对准误差曲线图。
具体实施方式：
下面结合附图和实施举例对本发明作进一步的详细说明：
本发明提出的一种离线小波降噪快速初始对准方法是通过Matlab仿真软件进行仿真实验。仿真硬件环境均为Intel(R)Core(TM)T9600 CPU 2.80GHz，4G RAM，Windows7操作系统。图1为整个算法的流程图，包括传感器数据获取，姿态信息更新，对准矩阵计算等过程；图2表示本专利涉及的常用姿态坐标系定义，包括初始载体系b0系，载体系b系，导航系n系，初始导航系n0系，地球系e系，初始地球系e系以及惯性系i系；图3为摇摆条件下，重力视运动矢量在降噪前和降噪后的对比图。从图中可以看出，由于采用了离线小波降噪处理，滤波之后的矢量相较于滤波之前的矢量更加平滑，而且能够准确体现重力视运动过程；图4是摇摆情况下计算的三轴姿态误差角，从图中可以看出，采用新算法之后整个对准时间能在50s左右完成，相较于先有的粗对准过程，本方法速度更快。
本发明是一种离线小波降噪快速初始对准方法，流程如图1所示，包括以下几个步骤：
步骤1：传感器数据获取，包括光纤陀螺仪和石英挠性加速度计数据；
步骤2：建立初始对准坐标系，包括初始载体坐标系、初始导航坐标系、初始地球坐标系、载体坐标系、导航坐标系、地球坐标系与惯性坐标系；
所述的初始时刻载体坐标系，记为b0系；所述的初始导航坐标系，记为n0系；所述的初始地球坐标系，记为e0系；所述载体坐标系表示与惯性测量单元的体坐标系重合的坐标系，记为b系；所述的导航系表示载体所在位置的东-北-天坐标系，记为n系；所述的地球坐标系为与地球固连，相对惯性坐标系以地球自转角速度ωie旋转，记为e系；
所述的坐标系存在如下转换关系：
地球系与导航系方向余弦矩阵表示为
式中：L表示载体所在纬度；
惯性系与地球系的转换矩阵表示为
式中：ωie表示地球自转速率；Δt表示采样时间；
考虑载体在地球上处于无线运动状态，则初始导航系与导航系之间的转换矩阵可以表示为
式中：为的转置；为的转置；通过即可得到导航系运动四元数
初始载体系与载体系之间的转换四元数表示为
式中：表示四元数的微分；表示载体相对惯性系的旋转角速率在载体系下的映射；表示四元数乘法；表示初始载体系与载体系之间的变换四元数；
步骤3：建立重力视运动矢量模型，构造离线小波降噪算法；
(3)重力视运动矢量模型
所述的重力视运动矢量模型采用如下变换得到：
运动载体的比力方程可以由下式得到
式中：表示导航系速度微分；fn表示导航系比力；表示地球自转角速率在导航系上的投影；表示导航系相对于地球系运动角速度在导航系上的投影；gn表示载体所在位置导航系重力；
由于载体处于静止状态，因此所述导航系速度vn与导航系速度微分为0，则可以得到简化变换模型为
式中：表示初始载体系与载体系之间的变换四元数；表示初始导航系与导航系之间的变换四元数；表示初始载体系与初始导航系之间的变换四元数；gn表示载体所在位置导航系重力；*表示四元数取共轭运算；
由于实际加速度计与陀螺仪输出含有随机噪声，即
式中：表示加速度计量测加速度；fb表示真实载体系加速度；表示加速度计测量零偏；∈b表示加速度计测量随机噪声；表示陀螺仪测量角速度；ωb表示真实载体运动角速度；εb表示陀螺仪测量零偏；ηb表示陀螺仪测量随机噪声；
因此，所述的重力视运动矢量模型可以表示为
式中：表示重力视运动矢量量测；表示初始载体系与载体系之间的变换四元数；表示误差四元数；fb表示真实载体系加速度；表示加速度计测量零偏；∈b表示加速度计测量随机噪声；*表示四元数取共轭运算；
对所述重力视运动矢量模型进行简化并忽略二阶噪声
式中：表示加速度计零偏在初始载体系上的投影；∈b0表示加速度计随机噪声在初始载体系上的投影；
(4)离线小波降噪
所述的离散小波降噪主要是实现对重力视运动矢量模型中的随机噪声进行降噪，从而提高重力视运动矢量的识别精度，加快粗对准过程。
对所述的重力视运动矢量模型采用小波分解，其分解过程为
式中，表示视在重力矢量；c0，m表示系数权重；φ0，m(t)表示尺度函数；
由Mallat小波变换可知
式中，cj，l表示第j层离散平滑逼近；dj，l表示第j层离散细节信号；cj-1，m表示第j-1层离散平滑逼近；h0，m-2l和h1，m-2l分别表示线性组合权重，可由下式计算
式中，φ0，m(t)和φ1，l(t)为尺度函数；ψ0，m(t)表示小波函数；
通过分解之后小波系数，结合相应的阈值压缩方法，可以得到小波重构系数为：
式中，表示重构之后的第j-1层离散平滑逼近；表示重构之后的第j层离散平滑逼近；表示重构之后第j层离散细节信号；h0，m-2l和h1，m-2l分别表示线性组合权重；
结合重构系数降噪之后的视在重力矢量模型为
式中，表示重构之后的视在重力矢量模型；表示重构之后初始离散逼近平滑；φ0，m(t)表示尺度函数；
步骤4：利用降噪滤波之后的重力视运动矢量，进行最优初始姿态的解算；
所述的快速初始对准过程中，参考矢量和观测矢量可由下式计算
式中，α表示参考矢量；β表示观测矢量；||||表示范数运算；表示重构之后的视在重力矢量模型；gn0表示初始导航系下的重力矢量，可由下式计算
式中，是由当前导航系到初始导航系之间的旋转矩阵；gn为当地重力矢量；gn0表示初始导航系下的重力矢量；
对观测矢量和量测矢量离散化之后，构造filter-QUEST K-矩阵
式中，
式中，βk表示离散化之后的观测矢量；αk表示离散化之后的参考矢量；
所述初始姿态四元数满足归一化条件
式中，T表示矢量转置；表示初始姿态四元数；
由拉格朗日乘数法可得
式中，Kk表示迭代计算的K-矩阵；λmax表示最大特征值；表示初始姿态四元数；
步骤5：利用计算得到的最优初始姿态四元数，结合姿态矩阵链式法则，实现快速初始对准过程；
由旋转矩阵链式法则可得
式中，可由迭代四元数计算；表示导航系与初始导航系之间得旋转矩阵；可由初始姿态四元数计算；表示对准之后的旋转矩阵；
MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：
纬度L＝31.64°，经度选为λ＝120.74°，地球自转角速率ωie＝15.0411°/hr，采样时间Δt＝0.005s。初始四元数初始旋转矩阵所用的光纤陀螺仪随机游走系数为零偏为εb＝0.02°/hr[1 1 1]T，石英挠性加速度计随机游走系数为零偏为所采取的对准方式分为摇摆机座对准，其中动机座对准中，三轴摇摆幅度分别为横滚ρ＝10°、俯仰航向ψ＝6°，摇摆频率为横滚0.2Hz、俯仰0.125Hz、、航向0.15Hz、。所用的小波函数为“sym4”，小波分解层数为6。

Claims (8)

1.一种离线小波降噪快速初始对准方法，其特征在于按照如下步骤进行：
步骤一、传感器数据获取，包括光纤陀螺仪和石英挠性加速度计数据；
步骤二、建立初始对准坐标系，包括初始载体坐标系、初始导航坐标系、初始地球坐标系、载体坐标系、导航坐标系、地球坐标系与惯性坐标系；
步骤三、建立重力视运动矢量模型，构造离线小波降噪算法；
步骤四、利用降噪滤波之后的重力视运动矢量，进行最优初始姿态的解算；
步骤五、利用计算得到的最优初始姿态四元数，结合姿态矩阵链式法则，实现快速初始对准过程。
2.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于所述的步骤二中：
所述的初始时刻载体坐标系，记为b0系；所述的初始导航坐标系，记为n0系；所述的初始地球坐标系，记为e0系；所述载体坐标系表示与惯性测量单元的体坐标系重合的坐标系，记为b系；所述的导航系表示载体所在位置的东-北-天坐标系，记为n系；所述的地球坐标系为与地球固连，相对惯性坐标系以地球自转角速度ωie旋转，记为e系；
所述的坐标系存在如下转换关系：
地球系与导航系方向余弦矩阵表示为
<mrow> <msubsup> <mi>C</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
式中：L表示载体所在纬度；
惯性系与地球系的转换矩阵表示为
<mrow> <msubsup> <mi>C</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> <mi>e</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mi>s</mi> <mi>i</mi> <mi>n</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>c</mi> <mi>o</mi> <mi>s</mi> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>&amp;Delta;</mi> <mi>t</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
式中：ωie表示地球自转速率；Δt表示采样时间；
考虑载体在地球上处于无线运动状态，则初始导航系与导航系之间的转换矩阵可以表示为
<mrow> <msubsup> <mi>C</mi> <mi>n</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> <msubsup> <mi>C</mi> <mi>e</mi> <mrow> <mi>e</mi> <mn>0</mn> </mrow> </msubsup> <msubsup> <mi>C</mi> <mi>n</mi> <mi>e</mi> </msubsup> </mrow>
式中：为的转置；为的转置；通过即可得到导航系运动四元数
初始载体系与载体系之间的转换四元数表示为
<mrow> <msubsup> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> </mrow>
式中：表示四元数的微分；表示载体相对惯性系的旋转角速率在载体系下的映射；表示四元数乘法；表示初始载体系与载体系之间的变换四元数。
3.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于所述的步骤三中建立重力视运动矢量模型，构造离线小波降噪算法：
(1)重力视运动矢量模型
所述的重力视运动矢量模型采用如下变换得到：
运动载体的比力方程可以由下式得到
<mrow> <msup> <mover> <mi>v</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <msup> <mi>f</mi> <mi>n</mi> </msup> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>e</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&amp;times;</mo> <msup> <mi>v</mi> <mi>n</mi> </msup> <mo>+</mo> <msup> <mi>g</mi> <mi>n</mi> </msup> </mrow>
式中：表示导航系速度微分；fn表示导航系比力；表示地球自转角速率在导航系上的投影；表示导航系相对于地球系运动角速度在导航系上的投影；gn表示载体所在位置导航系重力；
由于载体处于静止状态，因此所述导航系速度vn与导航系速度微分为0，则可以得到简化变换模型为
<mrow> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>&amp;CircleTimes;</mo> <msup> <mi>f</mi> <mi>b</mi> </msup> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>*</mo> </msup> <mo>=</mo> <mo>-</mo> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>&amp;CircleTimes;</mo> <msubsup> <mi>q</mi> <mi>n</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> <mo>&amp;CircleTimes;</mo> <msup> <mi>g</mi> <mi>n</mi> </msup> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>n</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>*</mo> </msup> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>*</mo> </msup> </mrow>
式中：表示初始载体系与载体系之间的变换四元数；表示初始导航系与导航系之间的变换四元数；表示初始载体系与初始导航系之间的变换四元数；gn表示载体所在位置导航系重力；*表示四元数取共轭运算；
由于实际加速度计与陀螺仪输出含有随机噪声，即
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <mover> <mi>f</mi> <mo>~</mo> </mover> <mi>b</mi> </msup> <mo>=</mo> <msup> <mi>f</mi> <mi>b</mi> </msup> <mo>+</mo> <msup> <mo>&amp;dtri;</mo> <mi>b</mi> </msup> <mo>+</mo> <msup> <mo>&amp;Element;</mo> <mi>b</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mover> <mi>&amp;omega;</mi> <mo>~</mo> </mover> <mi>b</mi> </msup> <mo>=</mo> <msup> <mi>&amp;omega;</mi> <mi>b</mi> </msup> <mo>+</mo> <msup> <mi>&amp;epsiv;</mi> <mi>b</mi> </msup> <mo>+</mo> <msup> <mi>&amp;eta;</mi> <mi>b</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中：表示加速度计量测加速度；fb表示真实载体系加速度；表示加速度计测量零偏；∈b表示加速度计测量随机噪声；表示陀螺仪测量角速度；ωb表示真实载体运动角速度；εb表示陀螺仪测量零偏；ηb表示陀螺仪测量随机噪声；
因此，所述的重力视运动矢量模型可以表示为
<mrow> <msup> <mover> <mi>f</mi> <mo>~</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>=</mo> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>&amp;CircleTimes;</mo> <mrow> <mo>(</mo> <msup> <mi>f</mi> <mi>b</mi> </msup> <mo>+</mo> <msup> <mo>&amp;dtri;</mo> <mi>b</mi> </msup> <mo>+</mo> <msup> <mo>&amp;Element;</mo> <mi>b</mi> </msup> <mo>)</mo> </mrow> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>*</mo> </msup> </mrow>
式中：表示重力视运动矢量量测；表示初始载体系与载体系之间的变换四元数；表示误差四元数；fb表示真实载体系加速度；表示加速度计测量零偏；εb表示加速度计测量随机噪声；*表示四元数取共轭运算；
对所述重力视运动矢量模型进行简化并忽略二阶噪声
<mrow> <msup> <mover> <mi>f</mi> <mo>~</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>=</mo> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>&amp;CircleTimes;</mo> <msup> <mi>f</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>&amp;CircleTimes;</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>&amp;delta;q</mi> <mi>b</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mo>*</mo> </msup> <mo>+</mo> <msup> <mo>&amp;dtri;</mo> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>+</mo> <msup> <mo>&amp;Element;</mo> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> </mrow>
式中：表示加速度计零偏在初始载体系上的投影；∈b0表示加速度计随机噪声在初始载体系上的投影；
(2)离线小波降噪
所述的离散小波降噪主要是实现对重力视运动矢量模型中的随机噪声进行降噪，从而提高重力视运动矢量的识别精度，加快粗对准过程；
对所述的重力视运动矢量模型采用小波分解，其分解过程为
<mrow> <msup> <mover> <mi>f</mi> <mo>~</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mi>m</mi> </munder> <msub> <mi>c</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <msub> <mi>&amp;phi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
式中，表示视在重力矢量；c0，m表示系数权重；φ0，m(t)表示尺度函数；
由Mallat小波变换可知
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>c</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <munder> <mi>&amp;Sigma;</mi> <mi>m</mi> </munder> <msub> <mi>h</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <msub> <mi>c</mi> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>d</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>=</mo> <munder> <mi>&amp;Sigma;</mi> <mi>m</mi> </munder> <msub> <mi>h</mi> <mrow> <mn>1</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <msub> <mi>c</mi> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中，cj，l表示第j层离散平滑逼近；dj，l表示第j层离散细节信号；cj-1，m表示第j-1层离散平滑逼近；h0，m-2l和h1，m-2l分别表示线性组合权重，可由下式计算
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>h</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mo>&lt;</mo> <msub> <mi>&amp;phi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>&amp;phi;</mi> <mrow> <mn>1</mn> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>&gt;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>h</mi> <mrow> <mn>1</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <mo>=</mo> <mo>&lt;</mo> <msub> <mi>&amp;psi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>&amp;phi;</mi> <mrow> <mn>1</mn> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>&gt;</mo> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中，φ0，m(t)和φ1，l(t)为尺度函数；ψ0，m(t)表示小波函数；
通过分解之后小波系数，结合相应的阈值压缩方法，可以得到小波重构系数为：
<mrow> <msub> <mover> <mi>c</mi> <mo>^</mo> </mover> <mrow> <mi>j</mi> <mo>-</mo> <mn>1</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mi>l</mi> </munder> <msub> <mi>h</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <msub> <mover> <mi>c</mi> <mo>^</mo> </mover> <mrow> <mi>j</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> <mo>+</mo> <munder> <mo>&amp;Sigma;</mo> <mi>l</mi> </munder> <msub> <mi>h</mi> <mrow> <mn>1</mn> <mo>,</mo> <mi>m</mi> <mo>-</mo> <mn>2</mn> <mi>l</mi> </mrow> </msub> <msub> <mover> <mi>d</mi> <mo>^</mo> </mover> <mrow> <mi>j</mi> <mo>,</mo> <mi>l</mi> </mrow> </msub> </mrow>
式中，表示重构之后的第j-1层离散平滑逼近；表示重构之后的第j层离散平滑逼近；表示重构之后第j层离散细节信号；h0，m-2l和h1，m-2l分别表示线性组合权重；
结合重构系数降噪之后的视在重力矢量模型为
<mrow> <msup> <mover> <mi>f</mi> <mo>^</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>=</mo> <munder> <mo>&amp;Sigma;</mo> <mi>m</mi> </munder> <msub> <mover> <mi>c</mi> <mo>^</mo> </mover> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <msub> <mi>&amp;phi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>m</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
式中，表示重构之后的视在重力矢量模型；表示重构之后初始离散逼近平滑；φ0，m(t)表示尺度函数。
4.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于所述的步骤四中利用降噪滤波之后的重力视运动矢量，进行最优初始姿态的解算：
所述的快速初始对准过程中，参考矢量和观测矢量可由下式计算
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&amp;alpha;</mi> <mo>=</mo> <mfrac> <msup> <mi>g</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msup> <mrow> <mo>|</mo> <mo>|</mo> <msup> <mi>g</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msup> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;beta;</mi> <mo>=</mo> <mfrac> <msup> <mover> <mi>f</mi> <mo>^</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mrow> <mo>|</mo> <mo>|</mo> <msup> <mover> <mi>f</mi> <mo>^</mo> </mover> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msup> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced>
式中，α表示参考矢量；β表示观测矢量；|| ||表示范数运算；表示重构之后的视在重力矢量模型；gn0表示初始导航系下的重力矢量，可由下式计算
<mrow> <msup> <mi>g</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msup> <mo>=</mo> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> <msup> <mi>g</mi> <mi>n</mi> </msup> </mrow>
式中，是由当前导航系到初始导航系之间的旋转矩阵；gn为当地重力矢量；gn0表示初始导航系下的重力矢量；
对观测矢量和量测矢量离散化之后，构造filter-QUESTK-矩阵
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;sigma;</mi> <mi>k</mi> </msub> </mtd> <mtd> <msubsup> <mi>z</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> <mtr> <mtd> <msub> <mi>z</mi> <mi>k</mi> </msub> </mtd> <mtd> <mrow> <msub> <mi>S</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>&amp;sigma;</mi> <mi>k</mi> </msub> <msub> <mi>I</mi> <mn>3</mn> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
式中，
式中，βk表示离散化之后的观测矢量；αk表示离散化之后的参考矢量；
所述初始姿态四元数满足归一化条件
<mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>=</mo> <mn>1</mn> </mrow>
式中，T表示矢量转置；表示初始姿态四元数；
由拉格朗日乘数法可得
<mrow> <msub> <mi>K</mi> <mi>k</mi> </msub> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <mo>=</mo> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>m</mi> <mi>a</mi> <mi>x</mi> </mrow> </msub> <msubsup> <mi>q</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> </mrow>
式中，Kk表示迭代计算的K-矩阵；λmax表示最大特征值；表示初始姿态四元数。
5.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于所述的步骤五中利用计算得到的最优初始姿态四元数，结合姿态矩阵链式法则，实现快速初始对准过程：
由旋转矩阵链式法则可得
<mrow> <msubsup> <mi>C</mi> <mi>n</mi> <mi>b</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mrow> <mi>b</mi> <mn>0</mn> </mrow> <mi>b</mi> </msubsup> <msubsup> <mi>C</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> <mrow> <mi>b</mi> <mn>0</mn> </mrow> </msubsup> <msubsup> <mi>C</mi> <mi>n</mi> <mrow> <mi>n</mi> <mn>0</mn> </mrow> </msubsup> </mrow>
式中，可由迭代四元数计算；表示导航系与初始导航系之间得旋转矩阵；可由初始姿态四元数计算；表示对准之后的旋转矩阵。
6.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于：步骤2中，纬度L＝31.64°，经度选为λ＝120.74°，地球自转角速率ωie＝15.0411°/hr，采样时间Δt＝0.005s。
7.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于：步骤3和步骤4中，初始四元数初始旋转矩阵
8.根据权利要求1所述的离线小波降噪快速初始对准方法，其特征在于：所用的光纤陀螺仪随机游走系数为零偏为εb＝0.02°/hr[1 1 1]T，石英挠性加速度计随机游走系数为零偏为所采取的对准方式分为摇摆机座对准，其中动机座对准中，三轴摇摆幅度分别为横滚ρ＝10°、俯仰航向ψ＝6°，摇摆频率为横滚0.2Hz、俯仰0.125Hz、、航向0.15Hz、。所用的小波函数为“sym4”，小波分解层数为6。
CN201710406484.3A 2017-06-02 2017-06-02 一种离线小波降噪快速初始对准方法 Active CN107270937B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710406484.3A CN107270937B (zh) 2017-06-02 2017-06-02 一种离线小波降噪快速初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710406484.3A CN107270937B (zh) 2017-06-02 2017-06-02 一种离线小波降噪快速初始对准方法

Publications (2)

Publication Number Publication Date
CN107270937A true CN107270937A (zh) 2017-10-20
CN107270937B CN107270937B (zh) 2020-07-31

Family

ID=60065745

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710406484.3A Active CN107270937B (zh) 2017-06-02 2017-06-02 一种离线小波降噪快速初始对准方法

Country Status (1)

Country Link
CN (1) CN107270937B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109029499A (zh) * 2018-06-26 2018-12-18 东南大学 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN109084756A (zh) * 2018-06-20 2018-12-25 东南大学 一种重力视运动参数辨识与加速度计零偏分离方法
CN109084755A (zh) * 2018-06-14 2018-12-25 东南大学 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN109186630A (zh) * 2018-07-16 2019-01-11 兰州交通大学 基于改进阈值小波降噪的mems粗对准方法及系统
CN110646012A (zh) * 2018-06-27 2020-01-03 北京自动化控制设备研究所 一种惯导系统单位置初始对准最优方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102829781A (zh) * 2012-08-29 2012-12-19 东南大学 一种旋转式捷联光纤罗经实现的方法
CN104296780A (zh) * 2014-10-16 2015-01-21 东南大学 一种基于重力视运动的sins自对准与纬度计算方法
CN105180937A (zh) * 2015-10-15 2015-12-23 常熟理工学院 一种mems-imu初始对准方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102829781A (zh) * 2012-08-29 2012-12-19 东南大学 一种旋转式捷联光纤罗经实现的方法
CN104296780A (zh) * 2014-10-16 2015-01-21 东南大学 一种基于重力视运动的sins自对准与纬度计算方法
CN105180937A (zh) * 2015-10-15 2015-12-23 常熟理工学院 一种mems-imu初始对准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
刘锡祥 等: "未知纬度条件下基于重力视运动与小波去噪的SINS自对准方法", 《中国惯性技术学报》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109084755A (zh) * 2018-06-14 2018-12-25 东南大学 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN109084755B (zh) * 2018-06-14 2021-06-25 东南大学 一种基于重力视速度与参数辨识的加速度计零偏估计方法
CN109084756A (zh) * 2018-06-20 2018-12-25 东南大学 一种重力视运动参数辨识与加速度计零偏分离方法
CN109084756B (zh) * 2018-06-20 2021-08-24 东南大学 一种重力视运动参数辨识与加速度计零偏分离方法
CN109029499A (zh) * 2018-06-26 2018-12-18 东南大学 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN109029499B (zh) * 2018-06-26 2021-06-11 东南大学 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN110646012A (zh) * 2018-06-27 2020-01-03 北京自动化控制设备研究所 一种惯导系统单位置初始对准最优方法
CN109186630A (zh) * 2018-07-16 2019-01-11 兰州交通大学 基于改进阈值小波降噪的mems粗对准方法及系统
CN109186630B (zh) * 2018-07-16 2020-10-23 兰州交通大学 基于改进阈值函数小波降噪的mems粗对准方法及系统

Also Published As

Publication number Publication date
CN107270937B (zh) 2020-07-31

Similar Documents

Publication Publication Date Title
CN107270937A (zh) 一种离线小波降噪快速初始对准方法
CN106342284B (zh) 一种飞行载体姿态确定方法
Chatfield Fundamentals of high accuracy inertial navigation
CN105180937B (zh) 一种mems‑imu初始对准方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN104165640B (zh) 基于星敏感器的近空间弹载捷联惯导系统传递对准方法
CN105486312B (zh) 一种星敏感器与高频角位移传感器组合定姿方法及系统
CN101706287B (zh) 一种基于数字高通滤波的旋转捷联系统现场标定方法
CN107065025A (zh) 一种基于重力梯度不变量的轨道要素估计方法
CN103900576B (zh) 一种深空探测自主导航的信息融合方法
CN101706284B (zh) 提高船用光纤陀螺捷联惯导系统定位精度的方法
CN103076026B (zh) 一种捷联惯导系统中确定多普勒计程仪测速误差的方法
CN107588771A (zh) 基于李群描述的捷联惯性导航解算方法
CN103743413B (zh) 倾斜状态下调制寻北仪安装误差在线估计与寻北误差补偿方法
CN103278163A (zh) 一种基于非线性模型的sins/dvl组合导航方法
CN106595711A (zh) 一种基于递推四元数的捷联惯性导航系统粗对准方法
CN103727940B (zh) 基于重力加速度矢量匹配的非线性初始对准方法
CN103900608A (zh) 一种基于四元数ckf的低精度惯导初始对准方法
CN107255474A (zh) 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN109141475A (zh) 一种dvl辅助sins鲁棒行进间初始对准方法
CN109596144A (zh) Gnss位置辅助sins行进间初始对准方法
CN103091722B (zh) 基于载荷误差分析原理的卫星重力反演方法
CN110196066B (zh) 基于格网姿态速度信息不变的虚拟极区方法
CN106153073A (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN103017787A (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