CN104748763B - 适用于车载晃动的捷联惯组快速对准方法 - Google Patents

适用于车载晃动的捷联惯组快速对准方法 Download PDF

Info

Publication number
CN104748763B
CN104748763B CN201510122689.XA CN201510122689A CN104748763B CN 104748763 B CN104748763 B CN 104748763B CN 201510122689 A CN201510122689 A CN 201510122689A CN 104748763 B CN104748763 B CN 104748763B
Authority
CN
China
Prior art keywords
mrow
msub
mtd
msup
mtr
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
CN201510122689.XA
Other languages
English (en)
Other versions
CN104748763A (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.)
China Academy of Launch Vehicle Technology CALT
Beijing Aerospace Automatic Control Research Institute
Original Assignee
China Academy of Launch Vehicle Technology CALT
Beijing Aerospace Automatic Control Research Institute
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 China Academy of Launch Vehicle Technology CALT, Beijing Aerospace Automatic Control Research Institute filed Critical China Academy of Launch Vehicle Technology CALT
Priority to CN201510122689.XA priority Critical patent/CN104748763B/zh
Publication of CN104748763A publication Critical patent/CN104748763A/zh
Application granted granted Critical
Publication of CN104748763B publication Critical patent/CN104748763B/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、初始化导航计算参数、速度提取滤波器的滤波参数以及卡尔曼滤波器的滤波参数；导航计算参数包括地理坐标系上的速度Vn、导航纬度L、导航经度λ，捷联惯组姿态矩阵姿态四元数Q；Vn＝[Vn[0] Vn[1] Vn[2]]T，Vn[0]为东向速度、Vn[1]为北向速度，Vn[2]为天向速度，卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X；
步骤2、采集捷联惯组陀螺输出的角速率以及捷联惯组加速度计输出的比力fb；
步骤3、根据步骤2的角速率更新计算并归一化姿态四元数Q，根据归一化后的姿态四元数Q更新计算捷联惯组姿态矩阵
步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵更新计算速度Vn；
步骤5、利用步骤4更新计算后的速度Vn更新计算捷联惯组的导航经度λ和导航纬度L；
步骤6、利用步骤4更新计算后的速度Vn采用速度提取滤波器提取环境晃动瞬时速度；所述环境晃动瞬时速度包括东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]；计算东向速度Vn[0]和东向晃动瞬时速度Vn′[0]的差值得到东向速度误差观测量ΔVn[0]，计算北向速度Vn[1]和北向晃动瞬时速度Vn′[1]的差值得到北向速度误差观测量ΔVn[1]；
步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程；
步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程，以步骤6中得到的东向速度误差观测量ΔVn[0]、北向速度误差观测量ΔVn[1]作为观测信息进行卡尔曼滤波，更新计算卡尔曼滤波状态估计变量X；
步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获得修正后的姿态四元数Q′，根据修正后的姿态四元数Q′计算修正后的姿态矩阵根据修正后的姿态矩阵计算对准输出姿态角；
步骤10、判断是否达到预定的对准稳定时间，如果没有，重复步骤2至步骤9的计算；如果达到预定的对准稳定时间，将步骤9所计算出的对准输出姿态角作为捷联惯组的对准结果。
所述步骤1中地理坐标系上的速度Vn的初始值为0，即Vn＝[Vn[0] Vn[1] Vn[2]]T＝[0 0 0]T，导航纬度L和导航经度λ初值设定为捷联惯组所在点地理位置，捷联惯组姿态矩阵的初值为粗对准所得到的姿态矩阵。
所述步骤1中卡尔曼滤波器的滤波参数初始化方法如下：卡尔曼滤波状态估计变量X的初值设为零向量，状态协方差阵Re表示地球半径，Qs＝diag[0 0 (Δa)2 (Δa)2 (ε)2 (ε)2 (ε)2]，Δa表示捷联惯组加速度计的最大误差量，ε表示捷联惯组陀螺的最大误差量；Rs＝diag[(0.01)2 (0.01)2]。
所述步骤6的速度提取滤波器的计算公式如下：
式中，x(k)为第k次的输入信号，y(k)为第k次的滤波输出值，n为速度提取滤波器的阶数，以东向速度Vn[0]和北向速度Vn[1]作为输入信号，滤波输出值为东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]。
速度提取滤波器阶次为2阶。
卡尔曼滤波状态估计变量X＝[δL δλ δVn[0] δVn[1] φx φy φz]T，δL为纬度误差、δλ为经度误差、δVn[0]为东向速度误差、δVn[1]为北向速度误差，φx、φy、φz为三轴失准角；
卡尔曼滤波器状态方程如下：
Re表示地球半径，ωie表示地球自转角速率，g表示当地重力加速度值。
速度匹配量测方程为Z＝HX+ν，其中，
所述步骤9中修正后的姿态四元数Q′的计算公式为：
所述步骤4具体包括如下步骤：
步骤4a、根据公式计算地理系上的比力投影值fn；
步骤4b、根据公式计算速度量dVn，根据公式VVn＝Vn+dVn·ΔT计算速度量VVn，dVn＝[dVn[0] dVn[1] dVn[2]]T、VVn＝[VVn[0] VVn[1] VVn[2]]T，gn＝[0 0 g]T表示重力加速度在导航系系的投影，其中g表示当地重力加速度值；
步骤4c、令得到更新计算后的速度Vn。
本发明与现有技术相比具有如下优点：
利用捷联惯组计算速度误差周期长，车体晃动速度误差周期短的特点，设计环境速度提取滤波器从导航速度计算值中提取出车载环境实际晃动速度值，进行速度匹配的卡尔曼滤波对准。由于环境速度的有效提取能够大幅降低初始对准滤波器的量测误差，使车载晃动对初始对准精度和速度的影响得到有效抑制；从而实现了车载武器捷联惯组在车载晃动环境下快速、精确的初始对准。
附图说明
图1为本发明基于速度提取的车载武器捷联惯组快速初始对准技术的流程图。
图2为本发明中，真实环境速度提取效果仿真图。其中图a显示了真实晃动速度；图b为导航速度计算值；图c为速度提取滤波器所提取出的晃动速度；图d显示的是所提取出的晃动速度与真实晃动速度间的误差。
图3为使用本发明进行晃动基座初始对准的效果图。其中图a显示了在晃动基座条件下常见零速匹配卡尔曼滤波的对准结果曲线；图b显示的是在与图a相同晃动环境下采用本发明的方法进行对准的结果曲线。
具体实施方式
下面就结合附图对本发明做进一步介绍。
结合图1，本发明基于速度提取的车载武器捷联惯组快速初始对准技术具体实施办法如下：
步骤1、经过粗对准，初始化载体导航的计算参数、速度提取滤波器的滤波参数以及对准卡尔曼滤波器的滤波参数。
步骤1a、导航计算参数初始化。
初始化导航坐标系(东北天地理坐标系，简化表示为n系)上的速度为零。
Vn＝[Vn[0] Vn[1] Vn[2]]T＝[0 0 0]T (1)
导航纬度L初值和导航经度λ初值设定为车体所在点位值；直接使用粗对准所得到的姿态矩阵作为导航系统姿态矩阵初值
初始化计算姿态四元数Q
其中，表示矩阵中第x行，第y列的数据。
步骤1b、速度提取滤波器的滤波参数初始化
设定速度提取滤波器阶次为2阶；结合车载振动特点，将滤波器设定为截止频率为0.1HZ的高通滤波器。结合采样周期，使用滤波器生成工具(例MATLAB)生成巴特沃兹滤波器，给出速度提取滤波器系数a0、a1、a2和b0、b1、b2。
步骤1c、卡尔曼滤波器参数初始化
选取卡尔曼滤波状态估计变量X＝[δL δλ δVn[0] δVn[1] φx φy φz]T，其中7项分别为：纬度误差δL、经度误差δλ、东向速度误差δVn[0]、北向速度误差δVn[1]以及三轴失准角φx、φy、φz。将X的初值设为零向量。
初始化卡尔曼滤波器状态协方差阵P，由于初始位置已知、车体为停车状态，且经过了粗对准，可令
式中，Re表示地球半径。
设定卡尔曼滤波器过程噪声协方差阵Qs，令
Qs＝diag[0 0 (Δa)2 (Δa)2 (ε)2 (ε)2 (ε)2] (4)
其中，Δa表示捷联惯组加速度计的最大误差量，ε表示捷联惯组陀螺的最大误差量。
设定卡尔曼滤波器量测噪声协方差阵Rs，令
Rs＝diag[(0.01)2 (0.01)2]
步骤2、采集捷联惯组陀螺输出的角速率以及捷联惯组加速度计输出的比力fb；其中，即载体系(简化为b系)相对于惯性坐标系(简化为i系)的角速度在b系上的投影；fb表示惯组所受比力在b系上的投影。
步骤3、根据步骤2的角速率更新计算捷联惯组姿态矩阵
步骤3a、计算姿态矩阵更新角速率(即b系相对n系的角速率在b系上的投影)
其中，
其中，表示地球系(简化为e系)相对惯性坐标系(i系)的角速率在东北天地理坐标系(n系)上的投影，ωie表示地球自转角速率，表示n系相对e系的角速率在n系上的投影。
步骤3b、利用更新计算姿态四元数，并归一化四元数
计算方法如下：
其中，dq[0]、dq[1]、dq[2]、dq[3]和qq[0]、qq[1]、qq[2]、qq[3]为中间变量；ΔT表示系统导航计算周期。
然后将计算所得的四元数归一化
令
令
步骤3c、利用四元数，更新计算b系到n系的姿态矩阵
步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵更新计算速度Vn。
步骤4a、根据公式计算地理系上的比力投影值fn；
步骤4b、根据公式计算速度量dVn，dVn＝[dVn[0]dVn[1] dVn[2]]T；根据公式VVn＝Vn+dVn·ΔT计算速度量VVn，VVn＝[VVn[0] VVn[1] VVn[2]]T，gn＝[0 0 g]T表示重力加速度在导航系系的投影，其中g表示当地重力加速度值；
步骤4c、令得到更新计算后的速度Vn。
步骤5、利用步骤4更新计算后的速度Vn更新计算捷联惯组的导航经度λ和导航纬度L。
计算方法如下：
λ′＝λ+dλ·ΔT
L′＝L+dL·ΔT (18)
其中，dL、dλ分别表示一个计算周期内的导航纬度和经度的变化量，λ′为导航纬度中间变量，L′为导航经度的中间变量；
然后令L″得到更新计算后的导航经度λ和导航纬度L。
步骤6、利用步骤4更新计算后的速度Vn采用速度提取滤波器提取环境晃动瞬时速度；
计算公式如下：
式中，x(k)为第k次(当前时刻)的输入信号；y(k)为滤波输出值。n为速度提取滤波器的阶数。
以导航速度Vn[0]和Vn[1]作为输入信号，通过以上公式进行滤波，得到滤波后的晃动瞬时速度为Vn′[0]和Vn′[1]。
计算东向速度Vn[0]和东向晃动瞬时速度Vn′[0]的差值得到东向速度误差观测量ΔVn[0]，计算北向速度Vn[1]和北向晃动瞬时速度Vn′[1]的差值得到北向速度误差观测量ΔVn[1]。
步骤7、利用步骤4更新计算后的速度Vn、步骤5更新计算后的导航经度λ和导航纬度L，建立当前时刻卡尔曼滤波器状态方程和速度匹配量测方程。
步骤7a、确定卡尔曼滤波器状态方程
设纬度误差δL、经度误差δλ、东向速度误差δVn[0]、北向速度误差δVn[1]以及三轴失准角φx、φy、φz共7项状态估计变量，确定卡尔曼滤波器状态方程如下：
将以上方程写成形式。
其中，X＝[δL δλ δVn[0] δVn[1] φx φy φz]T
步骤7b、确定速度匹配量测方程
选择惯性导航系统的东向速度误差δVn[0]、北向速度误差δVn[1]作为观测量建立量测方程如下：
将以上方程写为
Z＝HX+ν (22)
其中，
其中，ν表示量测过程中的随机误差。
步骤8、利用步骤7中得到的状态方程和量测方程，以步骤6中得到的环境晃动瞬时速度为观测信息进行标准卡尔曼滤波，更新计算各状态估计变量X＝[δL δλ δVn[0] δVn[1]φx φy φz]T。标准卡尔曼滤波为通用算法，这里不进行详细说明。
步骤9、利用步骤8中得到的状态估计值修正步骤3更新计算后的姿态四元数Q获得修正后的姿态四元数Q′，根据修正后的姿态四元数Q′计算修正后的姿态矩阵根据修正后的姿态矩阵计算对准输出姿态角。
步骤9a、利用步骤8中估算的φx、φy、φz开环修正姿态矩阵四元数，得到修正后的姿态四元数Q′：
步骤9b、将修正后的姿态四元数Q′转化为修正后的姿态矩阵
步骤9c、根据修正后的姿态矩阵计算对准输出姿态角
表示姿态矩阵中第x行，第y列的数据，x＝1，2，3；y＝1，2，3；
步骤10、在初始对准稳定时间段内(如5min)，重复步骤2至步骤9的计算，在对准结束时刻，步骤9所计算出的输出姿态角和姿态矩阵即为捷联惯性系统的初始对准结果。
车载武器捷联惯组在车载晃动环境下是存在实际速度的，若仍然使用零速匹配的卡尔曼滤波对准方式则惯组的实际运动速度会以速度量测误差的形式引入到对准系统中。根据捷联惯性系统的输出分析可知，其速度输出主要由系统固有长周期速度误差和短周期车载晃动速度组成，由于周期差异大，可采用滤波器从中提取出晃动速度作为观测信息，进一步采用速度匹配的卡尔曼滤波对准方式减小量测误差，从而得到更好的对准速度和对准精度。
在图2中可以看出，导航输出速度中包含了长周期误差和晃动的速度分量，通过分离计算后图(c)的结果已经将长周期误差有效的分离，得到了晃动的速度分量，图(d)结果表明速度提取的误差小。
图3结果可以看出，以所提取出的晃动环境速度作为辅助信息进行速度匹配对准的稳定性和精度明显优于直接采用零速进行匹配滤波对准。
本发明未详细说明部分属本领域技术人员公知常识。

Claims (6)

1.一种适用于车载晃动的捷联惯组快速对准方法，其特征在于，包括如下步骤：
步骤1、初始化导航计算参数、速度提取滤波器的滤波参数以及卡尔曼滤波器的滤波参数；导航计算参数包括地理坐标系上的速度Vn、导航纬度L、导航经度λ，捷联惯组姿态矩阵姿态四元数Q；Vn＝[Vn[0] Vn[1] Vn[2]]T，Vn[0]为东向速度、Vn[1]为北向速度，Vn[2]为天向速度，卡尔曼滤波器的滤波参数包括卡尔曼滤波状态估计变量X；所述地理坐标系上的速度Vn的初始值为0，即Vn＝[Vn[0] Vn[1] Vn[2]]T＝[0 0 0]T，导航纬度L和导航经度λ初值设定为捷联惯组所在点地理位置，捷联惯组姿态矩阵的初值为粗对准所得到的姿态矩阵；
卡尔曼滤波器的滤波参数初始化方法如下：卡尔曼滤波状态估计变量X的初值设为零向量，状态协方差阵Re表示地球半径，设定卡尔曼滤波器过程噪声协方差阵Qs，令Qs＝diag[0 0 (Δa)2 (Δa)2(ε)2 (ε)2 (ε)2]，Δa表示捷联惯组加速度计的最大误差量，ε表示捷联惯组陀螺的最大误差量；设定卡尔曼滤波器量测噪声协方差阵Rs，令Rs＝diag[(0.01)2 (0.01)2]；
步骤2、采集捷联惯组陀螺输出的角速率以及捷联惯组加速度计输出的比力fb；
步骤3、根据步骤2的角速率更新计算并归一化姿态四元数Q，根据归一化后的姿态四元数Q更新计算捷联惯组姿态矩阵
步骤4、根据步骤2的比力fb和步骤3更新计算后的捷联惯组姿态矩阵更新计算速度Vn；
步骤5、利用步骤4更新计算后的速度Vn更新计算捷联惯组的导航经度λ和导航纬度L；
步骤6、利用步骤4更新计算后的速度Vn采用速度提取滤波器提取环境晃动瞬时速度；所述环境晃动瞬时速度包括东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]；计算东向速度Vn[0]和东向晃动瞬时速度Vn′[0]的差值得到东向速度误差观测量ΔVn[0]，计算北向速度Vn[1]和北向晃动瞬时速度Vn′[1]的差值得到北向速度误差观测量ΔVn[1]；速度提取滤波器的计算公式如下：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mi>k</mi> <mo>&amp;le;</mo> <mi>n</mi> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>y</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>b</mi> <mi>i</mi> </msub> <mi>x</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>-</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>j</mi> <mo>=</mo> <mn>0</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>a</mi> <mi>j</mi> </msub> <mi>y</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>-</mo> <mi>j</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <mi>k</mi> <mo>&gt;</mo> <mi>n</mi> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
式中，x(k)为第k次的输入信号，y(k)为第k次的滤波输出值，n为速度提取滤波器的阶数，以东向速度Vn[0]和北向速度Vn[1]作为输入信号，滤波输出值为东向晃动瞬时速度Vn′[0]和北向晃动瞬时速度Vn′[1]；
步骤7、建立卡尔曼滤波器状态方程和速度匹配量测方程；
步骤8、利用步骤7中得到的卡尔曼滤波器状态方程和速度匹配量测方程，以步骤6中得到的东向速度误差观测量ΔVn[0]、北向速度误差观测量ΔVn[1]作为观测信息进行卡尔曼滤波，更新计算卡尔曼滤波状态估计变量X；
步骤9、利用步骤8中得到的状态估计变量修正步骤3归一化后的姿态四元数Q获得修正后的姿态四元数Q′，根据修正后的姿态四元数Q′计算修正后的姿态矩阵根据修正后的姿态矩阵计算对准输出姿态角；
步骤10、判断是否达到预定的对准稳定时间，如果没有，重复步骤2至步骤9的计算；如果达到预定的对准稳定时间，将步骤9所计算出的对准输出姿态角作为捷联惯组的对准结果。
2.如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，速度提取滤波器阶次为2阶。
3.如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，卡尔曼滤波状态估计变量X＝[δL δλ δVn[0] δVn[1] φx φy φz]T，δL为纬度误差、δλ为经度误差、δVn[0]为东向速度误差、δVn[1]为北向速度误差，φx、φy、φz为三轴失准角；
卡尔曼滤波器状态方程如下：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mover> <mi>L</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <mover> <mi>&amp;lambda;</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mfrac> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>&amp;CenterDot;</mo> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>&amp;CenterDot;</mo> <mi>&amp;delta;</mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msup> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> <mo>=</mo> <mfrac> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>&amp;CenterDot;</mo> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> <mo>+</mo> <mrow> <mo>(</mo> <mn>2</mn> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> <mo>+</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>&amp;CenterDot;</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> <mo>+</mo> <mfrac> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>)</mo> <mi>&amp;delta;</mi> <mi>L</mi> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mi>g</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&amp;delta;</mi> <msup> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> <mo>=</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>&amp;CenterDot;</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> <mo>+</mo> <mfrac> <msup> <mrow> <mo>(</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> <mo>)</mo> </mrow> <mn>2</mn> </msup> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>&amp;CenterDot;</mo> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>)</mo> </mrow> <mi>&amp;delta;</mi> <mi>L</mi> <mo>+</mo> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mi>g</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>x</mi> </msub> <mo>=</mo> <mo>-</mo> <mfrac> <mrow> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> <mo>-</mo> <mrow> <mo>(</mo> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>y</mi> </msub> <mo>=</mo> <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>&amp;CenterDot;</mo> <mi>&amp;delta;</mi> <mi>L</mi> <mo>+</mo> <mfrac> <mrow> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mo>-</mo> <mfrac> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>&amp;phi;</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>z</mi> </msub> <mo>=</mo> <mrow> <mo>(</mo> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>)</mo> </mrow> <mi>&amp;delta;</mi> <mi>L</mi> <mo>+</mo> <mfrac> <mrow> <msup> <mi>&amp;delta;V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <mrow> <mo>(</mo> <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> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>)</mo> </mrow> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> <mo>+</mo> <mfrac> <mrow> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mrow> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>;</mo> </mrow>
Re表示地球半径，ωie表示地球自转角速率，g表示当地重力加速度值。
4.如权利要求3所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，速度匹配量测方程为Z＝HX+ν，其中，
5.如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述步骤9中修正后的姿态四元数Q′的计算公式为：
<mrow> <msup> <mi>Q</mi> <mo>&amp;prime;</mo> </msup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msup> <mi>q</mi> <mo>&amp;prime;</mo> </msup> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <msup> <mi>q</mi> <mo>&amp;prime;</mo> </msup> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <msup> <mi>q</mi> <mo>&amp;prime;</mo> </msup> <mo>&amp;lsqb;</mo> <mn>2</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <msup> <mi>q</mi> <mo>&amp;prime;</mo> </msup> <mo>&amp;lsqb;</mo> <mn>3</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>2</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>3</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&amp;phi;</mi> <mi>z</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;phi;</mi> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>&amp;phi;</mi> <mi>x</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>&amp;CenterDot;</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>0</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>2</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> <mtr> <mtd> <mi>q</mi> <mo>&amp;lsqb;</mo> <mn>3</mn> <mo>&amp;rsqb;</mo> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow>
6.如权利要求1所述的适用于车载晃动的捷联惯组快速对准方法，其特征在于，所述步骤4具体包括如下步骤：
步骤4a、根据公式计算地理系上的比力投影值fn；
步骤4b、根据公式计算速度量dVn，根据公式VVn＝Vn+dVn·ΔT计算速度量VVn，dVn＝[dVn[0] dVn[1] dVn[2]]T、VVn＝[VVn[0] VVn[1] VVn[2]]T，gn＝[0 0 g]T表示重力加速度在导航系的投影，其中g表示当地重力加速度值；表示地球系(e系)相对惯性坐标系(i系)的角速率在东北天地理坐标系(n系)上的投影，表示n系相对e系的角速率在n系上的投影；ΔT表示系统导航计算周期；
步骤4c、令得到更新计算后的速度Vn。
CN201510122689.XA 2015-03-19 2015-03-19 适用于车载晃动的捷联惯组快速对准方法 Active CN104748763B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510122689.XA CN104748763B (zh) 2015-03-19 2015-03-19 适用于车载晃动的捷联惯组快速对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510122689.XA CN104748763B (zh) 2015-03-19 2015-03-19 适用于车载晃动的捷联惯组快速对准方法

Publications (2)

Publication Number Publication Date
CN104748763A CN104748763A (zh) 2015-07-01
CN104748763B true CN104748763B (zh) 2017-11-28

Family

ID=53588800

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510122689.XA Active CN104748763B (zh) 2015-03-19 2015-03-19 适用于车载晃动的捷联惯组快速对准方法

Country Status (1)

Country Link
CN (1) CN104748763B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595652B (zh) * 2016-11-30 2019-06-21 西北工业大学 车辆运动学约束辅助的回溯式行进间对准方法
CN106895855B (zh) * 2017-04-13 2019-05-28 北京航天自动控制研究所 一种惯性导航初始基准的估计与补偿方法
CN109109866B (zh) * 2018-08-24 2020-12-18 深圳市国脉畅行科技股份有限公司 车辆行驶状态监测方法、装置、计算机设备及存储介质
CN110109164B (zh) * 2019-04-24 2020-11-03 湖北三江航天万峰科技发展有限公司 车载方位角传递对准装置与方法
CN110006456B (zh) * 2019-04-24 2021-05-14 北京星网宇达科技股份有限公司 一种检测车对准方法、装置和设备

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7512493B2 (en) * 2006-05-31 2009-03-31 Honeywell International Inc. High speed gyrocompass alignment via multiple Kalman filter based hypothesis testing
CN103090884B (zh) * 2013-02-19 2015-05-20 哈尔滨工程大学 基于捷联惯导系统的多普勒计程仪测速误差抑制方法
CN103148868B (zh) * 2013-02-22 2015-12-09 哈尔滨工程大学 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法
CN103776450B (zh) * 2014-02-28 2016-08-17 中北大学 适用于高速旋转飞行体的半捷联式惯性测量与导航算法

Also Published As

Publication number Publication date
CN104748763A (zh) 2015-07-01

Similar Documents

Publication Publication Date Title
CN104748763B (zh) 适用于车载晃动的捷联惯组快速对准方法
CN106342284B (zh) 一种飞行载体姿态确定方法
CN105180937B (zh) 一种mems‑imu初始对准方法
CN103917850B (zh) 一种惯性导航系统的运动对准方法
CN102538821B (zh) 一种快速、参数分段式捷联惯性导航系统自对准方法
CN101216321A (zh) 捷联惯性导航系统的快速精对准方法
CN104697526A (zh) 用于农业机械的捷联惯导系统以及控制方法
CN112504275B (zh) 一种基于级联卡尔曼滤波算法的水面舰船水平姿态测量方法
CN105783943A (zh) 一种基于无迹卡尔曼滤波的极区环境下舰船大方位失准角传递对准方法
CN105737823A (zh) 基于五阶ckf的gps/sins/cns组合导航方法
CN105180728B (zh) 基于前数据的旋转制导炮弹快速空中对准方法
CN104374405A (zh) 一种基于自适应中心差分卡尔曼滤波的mems捷联惯导初始对准方法
CN106767797A (zh) 一种基于对偶四元数的惯性/gps组合导航方法
CN105091907A (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN103727940A (zh) 基于重力加速度矢量匹配的非线性初始对准方法
CN109211231B (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN103424127A (zh) 一种速度加比力匹配传递对准方法
CN109059914A (zh) 一种基于gps和最小二乘滤波的炮弹滚转角估计方法
Wang et al. State transformation extended Kalman filter for SINS based integrated navigation system
CN109211232B (zh) 一种基于最小二乘滤波的炮弹姿态估计方法
CN104296747B (zh) 基于火箭橇轨道坐标系的惯性测量系统一维定位方法
CN109029499A (zh) 一种基于重力视运动模型的加速度计零偏迭代寻优估计方法
CN107957271A (zh) 一种用于水下无人航行器在极区导航的初始精对准方法
CN104154914A (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