CN107389099A - 捷联惯导系统空中快速对准装置及方法 - Google Patents

捷联惯导系统空中快速对准装置及方法 Download PDF

Info

Publication number
CN107389099A
CN107389099A CN201710820696.6A CN201710820696A CN107389099A CN 107389099 A CN107389099 A CN 107389099A CN 201710820696 A CN201710820696 A CN 201710820696A CN 107389099 A CN107389099 A CN 107389099A
Authority
CN
China
Prior art keywords
mrow
msub
mtd
msubsup
mfrac
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
CN201710820696.6A
Other languages
English (en)
Other versions
CN107389099B (zh
Inventor
夏红伟
王聪
王常虹
安昊
马广程
考永贵
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Original Assignee
Harbin 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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN201710820696.6A priority Critical patent/CN107389099B/zh
Publication of CN107389099A publication Critical patent/CN107389099A/zh
Application granted granted Critical
Publication of CN107389099B publication Critical patent/CN107389099B/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

本发明提供了一种捷联惯导系统空中快速对准装置及方法，属于惯导系统空中快速对准装置及方法技术领域。装置包括：基于DSP的导航计算机、GPS接收模块和惯性测量单元。方法为：在飞行器起飞前，从GPS中获得所在位置的经度λ、纬度L和高度h，得到当地的重力加速度大小；粗对准只需要数秒就完成，这时飞行器起飞，失准角为大失准角，建立大失准角四元数误差模型，坐标系选择东、北、天坐标系；由于大失准角误差模型是非线性模型，于是采用扩展卡尔曼滤波方法估计姿态误差角；失准角检测模块不断检测失准角的大小，当失准角达到小失准角范围内且短时间内没有增大为大失准角，则系统自动切换为捷联惯导小失准角误差模型。

Description

捷联惯导系统空中快速对准装置及方法
技术领域
本发明涉及一种捷联惯导系统空中快速对准装置及方法，属于惯导系统空中快速对准装置及方法技术领域。
背景技术
对于捷联惯导系统来说，在正常开始导航工作前，首先要建立其数学平台，也就是确定初始捷联矩阵，即为捷联式惯性导航系统的初始对准。初始对准根据对准时运载体状态分为静基座对准和动基座对准，根据对准是否需要外界信息分为自对准和传递对准，根据对准时姿态误差角的大小又分为小失准角对准和大失准角对准。目前初始对准在工程中较多使用静基座对准方式，即在运载体静止时，先根据当地重力加速度和自转角速度进行解析式粗对准，待失准角达到小失准角量级时，再采用小失准角模型滤波进行精对准；而对于动基座对准，研究较多的是传递对准，运载体在飞行过程中，需要对准的子惯导系统与高精度的主惯导系统的导航信息进行比较，估算出子惯导系统对主惯导系统的相对失准角，从而对子惯导实现对准；而对于运载体在空间飞行过程中的自对准，即空中对准则研究较少。
现有技术主要是静基座初始对准和动基座的传递对准。静基座初始对准精度比较高，但是对准时间较长，一般需要数分钟才能完成，这对需要提高飞行器的反应速度的情况不利，如果飞行器在飞行过程中出现了瞬时故障，导致捷联式惯导系统的数据丢失或被破坏，故障排除首要任务就是重新对其捷联式惯导系统进行对准，这种情况静基座初始对准也无能为力。动基座对准能在运载体运动的情况下工作，研究较多的是传递对准，传递对准需要有主惯导系统提供导航信息，离开主惯导就无法工作，不够灵活；另外由于主惯导系统和子惯导系统之间有一定的距离，因此产生杆臂效应影响，而且传递对准会受到主、子惯导系统的安装误差角以及载体的弹性形变等因素影响，从而使算法性能大大降低。
发明内容
本发明的目的是为了解决上述现有技术存在的问题，进而提供一种捷联惯导系统空中快速对准装置及方法。
本发明的目的是通过以下技术方案实现的：
一种捷联惯导系统空中快速对准装置，包括：基于DSP的导航计算机、GPS接收模块和惯性测量单元，所述GPS接收模块的信号输出端与基于DSP的导航计算机的信号输入端相连接，所述惯性测量单元的信号输出端与基于DSP的导航计算机的信号输入端相连接。
一种捷联惯导系统空中快速对准方法，
步骤一、在飞行器起飞前，从GPS中获得所在位置的经度λ、纬度L和高度h，得到当地的重力加速度大小为：
地球自转角速度ωie也是确定的，于是根据双矢量定姿理论，得到初始姿态矩阵T如下：
步骤二、粗对准只需要数秒就完成，这时飞行器起飞，失准角为大失准角，建立大失准角四元数误差模型，坐标系选择东、北、天坐标系，模型如下：
四元数误差方程：
速度误差方程：
位置误差方程：
步骤三、由于大失准角误差模型是非线性模型，于是采用扩展卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为扩展卡尔曼滤波器的观测量，从而估计出大失准角的数值，并且不断反馈校正姿态矩阵；
EKF滤波器状态方程即是前面的四元数误差方程，速度误差方程和位置误差方程组成的微分方程组；
卡尔曼滤波状态方程：
系统状态变量为：
X＝[δL δλ δh δvE δvN δvU δq0 δq1 δq2 δq3]T
EKF滤波器量测方程：
Z＝HX+V
量测量：
Z＝[δL δλ δh δvE δvN δvU]T
量测矩阵：
H＝[I6×6 O6×10]
量测噪声：
V＝[vδL vδλ vδh vvE vvN vvU]T
上式V各元素分别为位置和速度量测噪声；
步骤四、失准角检测模块不断检测失准角的大小，当失准角达到小失准角范围内且短时间内没有增大为大失准角，则系统自动切换为捷联惯导小失准角误差模型，同时将EKF切换到KF；这里给出小失准角Φ方程以及卡尔曼滤波器模型：
姿态误差方程：
速度误差方程：
位置误差方程与小失准角相同；
由于小失准角误差模型是线性模型，于是采用卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为卡尔曼滤波器的观测量，从而估计出小失准角的数值，并且不断反馈校正姿态矩阵；
卡尔曼滤波状态方程：
系统状态变量为：
其中，φE、φN、φU为数学平台失准角；δvE、δvN、δvU为载体的东向、北向和天向速度误差；δL、δλ、δh分别为纬度误差、经度误差和高度误差；εx、εy、εz、 分别为陀螺随机常值漂移和加速度计随机常值零偏；
系统噪声矢量由陀螺仪和加速度计的随机误差组成，为：
该状态方程即是前面的Φ角方程。
卡尔曼滤波量测方程：
Z(t)＝H(t)X(t)+V(t)
量测量：
其中I代表INS测得数据，G代表GPS测得数据；
量测矩阵：
H＝[zeros(6,3),eye(6),zeros(6)]；
量测噪声：
V＝[vδE vδN vδU vδL vδλ vδh]T
上式V各元素分别为位置和速度量测噪声；
通过上面的步骤最后可以估计失准角，校正初始姿态矩阵。
本发明的有益效果：
1、本发明不需要漫长的静基座初始对准等待，只需几秒钟就可以完成地面粗对准，然后起飞进行空中对准，在飞行过程中遇到故障使导航数据丢失时，故障结束后可进行二次空中对准。
2、本发明不需要主惯导提供导航信息，因此也没有主从惯导杆臂效应和安装误差。
3、本发明可以对任何初始姿态的飞行器进行空中对准，由于大失准角时采用大失准角误差模型和扩展卡尔曼滤波器，小失准角时切换到小失准角误差模型和卡尔曼滤波器，因此能够结合两者的优势，在失准角较大时快速收敛，在失准角较小时提高精度，因此该空中对准速度较快，精度较高。
4、由于本发明只需要一个GPS模块和一个惯性测量单元模块，甚至可以选择低成本的微惯性测量单元，因此成本较低。
附图说明
图1为本发明捷联惯导系统空中快速对准装置结构示意图。
图中的附图标记，1为基于DSP的导航计算机，2为GPS接收模块，3为惯性测量单元(加速度计和陀螺仪)。
具体实施方式
下面将结合附图对本发明做进一步的详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式，但本发明的保护范围不限于下述实施例。
如图1所示，本实施例所涉及的一种捷联惯导系统空中快速对准装置，包括：基于DSP的导航计算机1、GPS接收模块2和惯性测量单元3，所述GPS接收模块2的信号输出端与基于DSP的导航计算机1的信号输入端相连接，所述惯性测量单元3的信号输出端与基于DSP的导航计算机1的信号输入端相连接。
所述惯性测量单元3由加速度计和陀螺仪组成。
一种捷联惯导系统空中快速对准方法，按以下步骤实现：
步骤一、在飞行器起飞前，首先从GPS中获得所在位置的经度λ、纬度L和高度h，得到当地的重力加速度大小为：
地球自转角速度ωie也是确定的，于是根据双矢量定姿理论，可以得到初始姿态矩阵T如下：
步骤二、粗对准只需要数秒就完成，这时飞行器起飞，失准角为大失准角，建立大失准角四元数误差模型，坐标系选择东、北、天坐标系，模型如下：
四元数误差方程：
速度误差方程：
位置误差方程：
步骤三、由于大失准角误差模型是非线性模型，于是采用扩展卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为扩展卡尔曼滤波器的观测量，从而估计出大失准角的数值，并且不断反馈校正姿态矩阵。
EKF滤波器状态方程即是前面的四元数误差方程，速度误差方程和位置误差方程组成的微分方程组；
卡尔曼滤波状态方程：
系统状态变量为：
X＝[δL δλ δh δvE δvN δvU δq0 δq1 δq2 δq3]T
EKF滤波器量测方程：
Z＝HX+V
量测量：
Z＝[δL δλ δh δvE δvN δvU]T
量测矩阵：
H＝[I6×6 O6×10]
量测噪声：
V＝[vδL vδλ vδh vvE vvN vvU]T
上式V各元素分别为位置和速度量测噪声。
步骤四、失准角检测模块不断检测失准角的大小，当失准角达到小失准角范围内且短时间内没有增大为大失准角，则系统自动切换为捷联惯导小失准角误差模型，同时将EKF切换到KF。这里给出小失准角Φ方程以及卡尔曼滤波器模型：
姿态误差方程：
速度误差方程：
位置误差方程与小失准角相同，不在赘述。
由于小失准角误差模型是线性模型，于是采用卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为卡尔曼滤波器的观测量，从而估计出小失准角的数值，并且不断反馈校正姿态矩阵。
卡尔曼滤波状态方程：
系统状态变量为：
其中，φE、φN、φU为数学平台失准角；δvE、δvN、δvU为载体的东向、北向和天向速度误差；δL、δλ、δh分别为纬度误差、经度误差和高度误差；εx、εy、εz、 分别为陀螺随机常值漂移和加速度计随机常值零偏。
系统噪声矢量由陀螺仪和加速度计的随机误差组成，为：
该状态方程即是前面的Φ角方程。
卡尔曼滤波量测方程：
Z(t)＝H(t)X(t)+V(t)
量测量：
其中I代表INS测得数据，G代表GPS测得数据。
量测矩阵：
H＝[zeros(6,3),eye(6),zeros(6)]；
量测噪声：
V＝[vδE vδN vδU vδL vδλ vδh]T
上式V各元素分别为位置和速度量测噪声。
通过上面的步骤最后可以估计失准角，校正初始姿态矩阵。
本发明采用GPS辅助的空中对准方式来估计失准角，对要求反映快速的小型飞行器，不需要进行静基座初始对准，也不需要主惯导系统提供导航信息，可以根据需要直接起飞，然后在飞行中对准；如果飞行过程中出现瞬时故障而使导航信息丢失，恢复正常后立即进行重新对准，确定初始姿态失准角；对于一般先粗对准再精对准的过程中，动基座粗对准后，很难保证失准角为小失准角，如果再应用小失准角误差模型会引起较大对准误差，因此本发明引入适用于大失准角的四元数误差模型，并且不断反馈校正姿态矩阵，当失准角减小到小失准角量级时，再切换到小失准角误差模型，这样在失准角较大时估计速度较快，在失准角较小时估计精度较高；对于具有任意初始姿态的飞行器，本发明都能够进行对准工作，而且对准速度较快、精度较高。
以上所述，仅为本发明较佳的具体实施方式，这些具体实施方式都是基于本发明整体构思下的不同实现方式，而且本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (3)

1.一种捷联惯导系统空中快速对准装置，其特征在于，包括：基于DSP的导航计算机(1)、GPS接收模块(2)和惯性测量单元(3)，所述GPS接收模块(2)的信号输出端与基于DSP的导航计算机(1)的信号输入端相连接，所述惯性测量单元(3)的信号输出端与基于DSP的导航计算机(1)的信号输入端相连接。
2.根据权利要求1所述的捷联惯导系统空中快速对准装置，其特征在于，所述惯性测量单元(3)由加速度计和陀螺仪组成。
3.根据权利要求1所述的捷联惯导系统空中快速对准方法，其特征在于，
步骤一、在飞行器起飞前，从GPS中获得所在位置的经度λ、纬度L和高度h，得到当地的重力加速度大小为：
<mrow> <mi>g</mi> <mo>=</mo> <msub> <mi>g</mi> <mn>0</mn> </msub> <mrow> <mo>(</mo> <mn>1</mn> <mo>-</mo> <mfrac> <mi>h</mi> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>)</mo> </mrow> </mrow>
地球自转角速度ωie也是确定的，于是根据双矢量定姿理论，得到初始姿态矩阵T如下：
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>11</mn> </msub> <mo>=</mo> <mfrac> <mrow> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>g&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mrow> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>z</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>y</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mi>y</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>z</mi> <mi>b</mi> </msubsup> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>12</mn> </msub> <mo>=</mo> <mfrac> <mrow> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>g&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mrow> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>x</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>z</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mi>z</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>x</mi> <mi>b</mi> </msubsup> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>13</mn> </msub> <mo>=</mo> <mfrac> <mrow> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>g&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mrow> </mfrac> <mrow> <mo>(</mo> <mrow> <msubsup> <mi>&amp;omega;</mi> <mi>y</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>x</mi> <mi>b</mi> </msubsup> <mo>-</mo> <msubsup> <mi>&amp;omega;</mi> <mi>x</mi> <mi>b</mi> </msubsup> <msubsup> <mi>f</mi> <mi>y</mi> <mi>b</mi> </msubsup> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>21</mn> </msub> <mo>=</mo> <mfrac> <msubsup> <mi>f</mi> <mi>x</mi> <mi>b</mi> </msubsup> <mi>g</mi> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msubsup> <mi>&amp;omega;</mi> <mi>x</mi> <mi>b</mi> </msubsup> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>22</mn> </msub> <mo>=</mo> <mfrac> <msubsup> <mi>f</mi> <mi>y</mi> <mi>b</mi> </msubsup> <mi>g</mi> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msubsup> <mi>&amp;omega;</mi> <mi>y</mi> <mi>b</mi> </msubsup> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>23</mn> </msub> <mo>=</mo> <mfrac> <msubsup> <mi>f</mi> <mi>z</mi> <mi>b</mi> </msubsup> <mi>g</mi> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msubsup> <mi>&amp;omega;</mi> <mi>z</mi> <mi>b</mi> </msubsup> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>31</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>32</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>T</mi> <mn>33</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced>
步骤二、粗对准只需要数秒就完成，这时飞行器起飞，失准角为大失准角，建立大失准角四元数误差模型，坐标系选择东、北、天坐标系，模型如下：
四元数误差方程：
<mrow> <mi>&amp;delta;</mi> <mover> <mi>Q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>&lt;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&gt;</mo> <msubsup> <mi>&amp;delta;Q</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> <mo>&amp;rsqb;</mo> <msubsup> <mi>&amp;delta;Q</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mo>&lt;</mo> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&gt;</mo> <msubsup> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>b</mi> <mi>n</mi> </msubsup> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mrow> <mo>&amp;lsqb;</mo> <mrow> <msubsup> <mi>&amp;delta;&amp;omega;</mi> <mrow> <mi>i</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mrow> <mo>&amp;rsqb;</mo> </mrow> <msubsup> <mover> <mi>Q</mi> <mo>^</mo> </mover> <mi>b</mi> <mi>n</mi> </msubsup> </mrow>
速度误差方程：
位置误差方程：
<mrow> <mi>&amp;delta;</mi> <mover> <mi>L</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>N</mi> </msub> </mrow> <mrow> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mrow>
<mrow> <mi>&amp;delta;</mi> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>E</mi> </msub> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msub> <mi>v</mi> <mi>E</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mi> </mi> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mi>&amp;delta;</mi> <mi>L</mi> </mrow>
<mrow> <mi>&amp;delta;</mi> <mover> <mi>h</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <msub> <mi>&amp;delta;v</mi> <mi>U</mi> </msub> </mrow>
步骤三、由于大失准角误差模型是非线性模型，于是采用扩展卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为扩展卡尔曼滤波器的观测量，从而估计出大失准角的数值，并且不断反馈校正姿态矩阵；
EKF滤波器状态方程即是前面的四元数误差方程，速度误差方程和位置误差方程组成的微分方程组；
卡尔曼滤波状态方程：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mi>X</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mo>+</mo> <mi>G</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> <mi>W</mi> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow>
系统状态变量为：
X＝[δL δλ δh δvE δvN δvU δq0 δq1 δq2 δq3]T
EKF滤波器量测方程：
Z＝HX+V
量测量：
Z＝[δL δλ δh δvE δvN δvU]T
量测矩阵：
H＝[I6×6 O6×10]
量测噪声：
V＝[vδL vδλ vδh vvE vvN vvU]T
上式V各元素分别为位置和速度量测噪声；
步骤四、失准角检测模块不断检测失准角的大小，当失准角达到小失准角范围内且短时间内没有增大为大失准角，则系统自动切换为捷联惯导小失准角误差模型，同时将EKF切换到KF；这里给出小失准角Φ方程以及卡尔曼滤波器模型：
姿态误差方程：
<mrow> <msub> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>N</mi> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>E</mi> </msub> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mi>&amp;delta;</mi> <mi>L</mi> <mo>-</mo> <mrow> <mo>(</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msub> <mi>v</mi> <mi>E</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>E</mi> </msub> <mo>-</mo> <mfrac> <msub> <mi>v</mi> <mi>N</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <msub> <mi>&amp;phi;</mi> <mi>U</mi> </msub> <mo>+</mo> <msub> <mi>&amp;epsiv;</mi> <mi>N</mi> </msub> </mrow>
<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>U</mi> </msub> <mo>=</mo> <mfrac> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>E</mi> </msub> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msub> <mi>v</mi> <mi>E</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> </mrow> <mo>)</mo> </mrow> <mi>&amp;delta;</mi> <mi>L</mi> <mo>+</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <msub> <mi>v</mi> <mi>E</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mrow> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>E</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>+</mo> <mfrac> <msub> <mi>v</mi> <mi>N</mi> </msub> <mrow> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <msub> <mi>&amp;phi;</mi> <mi>N</mi> </msub> <mo>+</mo> <msub> <mi>&amp;epsiv;</mi> <mi>U</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced>
速度误差方程：
N位置误差方程与小失准角相同；
由于小失准角误差模型是线性模型，于是采用卡尔曼滤波方法估计姿态误差角，GPS能够测得运载体的速度和位置，将惯性导航系统测得位置速度与GPS测得数据作差，作为卡尔曼滤波器的观测量，从而估计出小失准角的数值，并且不断反馈校正姿态矩阵；
卡尔曼滤波状态方程：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>F</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> <mi>W</mi> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow>
系统状态变量为：
<mrow> <mi>X</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>E</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>N</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>U</mi> </msub> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>E</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>N</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;delta;v</mi> <mi>U</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>&amp;lambda;</mi> </mrow> </mtd> <mtd> <mrow> <mi>&amp;delta;</mi> <mi>h</mi> </mrow> </mtd> <mtd> <msub> <mi>&amp;epsiv;</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;epsiv;</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;epsiv;</mi> <mi>z</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mo>&amp;dtri;</mo> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
其中，φE、φN、φU为数学平台失准角；δvE、δvN、δvU为载体的东向、北向和天向速度误差；δL、δλ、δh分别为纬度误差、经度误差和高度误差；εx、εy、εz、 分别为陀螺随机常值漂移和加速度计随机常值零偏；
系统噪声矢量由陀螺仪和加速度计的随机误差组成，为：
<mrow> <mi>W</mi> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>w</mi> <mrow> <mi>&amp;epsiv;</mi> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>w</mi> <mrow> <mi>&amp;epsiv;</mi> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>w</mi> <mrow> <mi>&amp;epsiv;</mi> <mi>z</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>w</mi> <mrow> <mo>&amp;dtri;</mo> <mi>x</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>w</mi> <mrow> <mo>&amp;dtri;</mo> <mi>y</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>w</mi> <mrow> <mo>&amp;dtri;</mo> <mi>z</mi> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow>
该状态方程即是前面的Φ角方程；
卡尔曼滤波量测方程：
Z(t)＝H(t)X(t)+V(t)
量测量：
<mrow> <mi>Z</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>I</mi> <mi>E</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>v</mi> <mrow> <mi>G</mi> <mi>E</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>I</mi> <mi>N</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>v</mi> <mrow> <mi>G</mi> <mi>M</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>v</mi> <mrow> <mi>I</mi> <mi>U</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>v</mi> <mrow> <mi>G</mi> <mi>U</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>L</mi> <mrow> <mi>I</mi> <mi>E</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>L</mi> <mrow> <mi>G</mi> <mi>E</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>I</mi> <mi>N</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>&amp;lambda;</mi> <mrow> <mi>G</mi> <mi>M</mi> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>h</mi> <mrow> <mi>I</mi> <mi>U</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>h</mi> <mrow> <mi>G</mi> <mi>U</mi> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
其中I代表INS测得数据，G代表GPS测得数据；
量测矩阵：
H＝[zeros(6,3),eye(6),zeros(6)]；
量测噪声：
V＝[vδE vδN vδU vδL vδλ vδh]T
上式V各元素分别为位置和速度量测噪声；
通过上面的步骤最后估计失准角，校正初始姿态矩阵。
CN201710820696.6A 2017-09-13 2017-09-13 捷联惯导系统空中快速对准装置及方法 Active CN107389099B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710820696.6A CN107389099B (zh) 2017-09-13 2017-09-13 捷联惯导系统空中快速对准装置及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710820696.6A CN107389099B (zh) 2017-09-13 2017-09-13 捷联惯导系统空中快速对准装置及方法

Publications (2)

Publication Number Publication Date
CN107389099A true CN107389099A (zh) 2017-11-24
CN107389099B CN107389099B (zh) 2019-11-12

Family

ID=60351172

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710820696.6A Active CN107389099B (zh) 2017-09-13 2017-09-13 捷联惯导系统空中快速对准装置及方法

Country Status (1)

Country Link
CN (1) CN107389099B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109141418A (zh) * 2018-09-27 2019-01-04 东南大学 过载环境下捷联惯导数据处理装置及其多源误差建模方法
CN109460075A (zh) * 2018-11-01 2019-03-12 湖北航天技术研究院总体设计所 一种快速方位角对准的方法及系统
CN109612499A (zh) * 2018-12-04 2019-04-12 东南大学 一种基于自适应补偿h无穷滤波的传递对准方法
CN109724624A (zh) * 2018-12-29 2019-05-07 湖北航天技术研究院总体设计所 一种适用于机翼挠曲变形的机载自适应传递对准算法
CN110108300A (zh) * 2019-05-10 2019-08-09 哈尔滨工业大学 一种基于卧式三轴转台的imu正六面体标定方法
CN111024128A (zh) * 2019-12-30 2020-04-17 哈尔滨工程大学 一种机载光电吊舱光轴稳定状态传递对准方法
CN111912427A (zh) * 2019-05-10 2020-11-10 中国人民解放军火箭军工程大学 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN114061575A (zh) * 2021-11-26 2022-02-18 上海机电工程研究所 大失准角条件下的导弹姿态角精对准方法及系统

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103557871A (zh) * 2013-10-22 2014-02-05 北京航空航天大学 一种浮空飞行器捷联惯导空中初始对准方法
CN104236586A (zh) * 2014-09-05 2014-12-24 南京理工大学 基于量测失准角的动基座传递对准方法
CN104330092A (zh) * 2014-07-24 2015-02-04 南京理工大学 一种基于双模型切换的二次传递对准方法
CN104374405A (zh) * 2014-11-06 2015-02-25 哈尔滨工程大学 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103245360A (zh) * 2013-04-24 2013-08-14 北京工业大学 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN103557871A (zh) * 2013-10-22 2014-02-05 北京航空航天大学 一种浮空飞行器捷联惯导空中初始对准方法
CN104330092A (zh) * 2014-07-24 2015-02-04 南京理工大学 一种基于双模型切换的二次传递对准方法
CN104236586A (zh) * 2014-09-05 2014-12-24 南京理工大学 基于量测失准角的动基座传递对准方法
CN104374405A (zh) * 2014-11-06 2015-02-25 哈尔滨工程大学 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109141418A (zh) * 2018-09-27 2019-01-04 东南大学 过载环境下捷联惯导数据处理装置及其多源误差建模方法
CN109460075A (zh) * 2018-11-01 2019-03-12 湖北航天技术研究院总体设计所 一种快速方位角对准的方法及系统
CN109460075B (zh) * 2018-11-01 2021-10-01 湖北航天技术研究院总体设计所 一种快速方位角对准的方法及系统
CN109612499A (zh) * 2018-12-04 2019-04-12 东南大学 一种基于自适应补偿h无穷滤波的传递对准方法
CN109612499B (zh) * 2018-12-04 2022-04-26 东南大学 一种基于自适应补偿h无穷滤波的传递对准方法
CN109724624A (zh) * 2018-12-29 2019-05-07 湖北航天技术研究院总体设计所 一种适用于机翼挠曲变形的机载自适应传递对准算法
CN110108300A (zh) * 2019-05-10 2019-08-09 哈尔滨工业大学 一种基于卧式三轴转台的imu正六面体标定方法
CN111912427A (zh) * 2019-05-10 2020-11-10 中国人民解放军火箭军工程大学 一种多普勒雷达辅助捷联惯导运动基座对准方法及系统
CN111024128A (zh) * 2019-12-30 2020-04-17 哈尔滨工程大学 一种机载光电吊舱光轴稳定状态传递对准方法
CN114061575A (zh) * 2021-11-26 2022-02-18 上海机电工程研究所 大失准角条件下的导弹姿态角精对准方法及系统

Also Published As

Publication number Publication date
CN107389099B (zh) 2019-11-12

Similar Documents

Publication Publication Date Title
CN107389099A (zh) 捷联惯导系统空中快速对准装置及方法
CN103575299B (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN104165640B (zh) 基于星敏感器的近空间弹载捷联惯导系统传递对准方法
CN101825467B (zh) 捷联惯性导航系统与天文导航系统实现组合导航的方法
CN102829781B (zh) 一种旋转式捷联光纤罗经实现的方法
CN110221332A (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN102519485B (zh) 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法
CN103245360A (zh) 晃动基座下的舰载机旋转式捷联惯导系统自对准方法
CN109459008B (zh) 一种小型中高精度光纤陀螺寻北装置及方法
CN105928515B (zh) 一种无人机导航系统
CN101963512A (zh) 船用旋转式光纤陀螺捷联惯导系统初始对准方法
CN101319902A (zh) 一种低成本组合式定位定向装置及组合定位方法
CN103245359A (zh) 一种惯性导航系统中惯性传感器固定误差实时标定方法
CN110887507B (zh) 一种快速估计惯性测量单元全部零偏的方法
CN104698486A (zh) 一种分布式pos用数据处理计算机系统实时导航方法
CN101162147A (zh) 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法
CN104457748A (zh) 一种嵌入式瞄准吊舱测姿系统及其传递对准方法
CN102168978B (zh) 一种船用惯性导航系统摇摆基座开环对准方法
CN103245357A (zh) 一种船用捷联惯导系统二次快速对准方法
CN113503894B (zh) 基于陀螺基准坐标系的惯导系统误差标定方法
CN109708663A (zh) 基于空天飞机sins辅助的星敏感器在线标定方法
CN112556724A (zh) 动态环境下的微型飞行器低成本导航系统初始粗对准方法
CN105928519B (zh) 基于ins惯性导航与gps导航以及磁力计的导航算法
CN105300407B (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