CN106840194A - 一种大方位失准角线性对准方法 - Google Patents
一种大方位失准角线性对准方法 Download PDFInfo
- Publication number
- CN106840194A CN106840194A CN201610835250.6A CN201610835250A CN106840194A CN 106840194 A CN106840194 A CN 106840194A CN 201610835250 A CN201610835250 A CN 201610835250A CN 106840194 A CN106840194 A CN 106840194A
- Authority
- CN
- China
- Prior art keywords
- error
- equation
- alignment
- coarse alignment
- angle
- 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
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
本发明涉及一种大方位失准角线性对准方法,步骤如下:(1)根据粗对准系统的状态方程和观测方程,利用GPS观测信息,通过线性卡尔曼滤波,实现粗对准过程,直至航向误差角满足小角度阈值条件;(2)保留粗对准收敛时的系统协方差阵,并将之作为精对准过程的初始条件;(3)延续粗对准的系统状态变量及其位置、速度误差方程和观测方程,使用本发明给出姿态误差方程,并将保存的协方差阵作为初始条件,进行精对准,至系统收敛至预期水平。本发明使系统误差的协方差阵能从粗对准模型直接传递给精对准模型,实现平稳的模型切换过程,改善精对准收敛过程。
Description
技术领域
本发明涉及一种大方位失准角线性对准方法,属于惯性导航技术领域。
背景技术
初始对准是惯性导航的关键技术,也是INS/GNSS组合导航关键技术之一。在基于MEMS-INS/GNSS组合导航系统中,由于MEMS器件特别是陀螺仪的限制,导致无法通过自对准实现方位失准角的初始化,从而造成了大方位失准角问题,其中一种解决方法是直接进行动基座的对准。
建立准确的INS误差传播方程和采用适当的滤波技术是进行初始对准的主要问题。在大方位失准角的情况下的动基座对准模型本质上式非线性的,而非线性滤波不适宜工程应用,故多采用线性化方法。
现有的技术方案将对准分为粗对准和精对准两个过程,在粗对准精度达到某一阈值条件时实现切换。在用欧拉角法描述失准角的条件下,粗对准将游动方位坐标系中的姿态误差状态替换为正弦和余弦项,从而实现方程线性化。同时,针对游动方位的三角函数可观度较弱的问题,还提出一种显著增加了可观度的改进状态方程。在精对准过程中,由于经过粗对准的失准角已足够小,可以通过sinα≈α,cosα≈1实现线性化。
现有对线性滤波方案中,在粗对准和精对准中使用不同的状态变量和状态方程,这造成了由粗到精过渡时的模型切换问题。由于系统状态变量不一致,粗对准得到的协方差阵不能直接用于精对准,相应的,精对准过程也需要重新对设定协方差阵。由于协方差阵的损失,模型切换往往使滤波过渡无法平稳实现,影响精对准过程的收敛速度。
发明内容
本发明技术解决问题:克服现有技术在粗对准和精对准模型切换中的过渡不平稳问题,提供了一种新型大方位失准角线性对准方法,使系统误差的协方差阵能从粗对准模型直接传递给精对准模型,实现平稳的模型切换过程,改善精对准收敛过程。
本发明的技术要点:
1.粗对准过程延续现有方案;
2.精对准过程中,延续粗对准系统的状态变量,即系统状态变量定义为:
其中,L、λ和h分别为纬度、经度和高度,δVE、δVN和δVU分别为东向、北向和天向的速度误差,θ、γ和分别为俯仰、滚动和航向角误差。
粗对准系统的姿态误差方程为:
其中为计算坐标系相对惯性坐标系的角速率在计算坐标系中的投影,εx、εy和εz为陀螺仪漂移项,Fs和Fc为非线性项,定义为:
粗对准中,Fs和Fc近似为0,精对准则不能这样简化。精对准时,式(2)中θ、γ、εz均是小量,也是随着方位角的减小而趋于零的小量,当方位角减小到8-10度时,的值减小到0.01以内。因此与θ、γ、εz的乘积为高阶小量,消去高阶小量近似得到:
此时,得到精对准的系统姿态误差方程:
精对准中,除姿态误差方程外,其他误差方程保持不变。对比粗对准的姿态误差方程,本发明提出的方案只需对传递函数部分进行调整,并增加陀螺漂移εz即可。
本发明技术解决方案:一种大方位失准角线性对准方法,步骤如下:
(1)粗对准采用现有的技术方案,选择经度误差,纬度误差,高程误差,东向、北向和天向速度误差,俯仰角误差,滚动角误差,以及航向角误差的正弦项和余弦项作为系统状态变量,根据粗对准系统的状态方程和观测方程,利用GPS观测信息,通过线性卡尔曼滤波,进行粗对准。其中,状态方程由位置误差方程、线性化的速度和姿态误差方程组成;
(2)当粗对准的航向角误差收敛到满足阈值条件时,保存滤波系统的协方差阵,其中为设定的阈值,使成立。
(3)保持粗对准的系统状态变量及其位置、速度误差方程和观测方程不变,使用本发明给出姿态误差方程,并将保存的协方差阵作为初始条件,进行精对准,至系统收敛至预期水平。本发明给出的系统的姿态误差方程为:
本发明与现有技术相比的优点在于:现有的粗对准至精对准的切换方案,粗对准和精对准使用不同的系统变量,相应的需要不同的系统模型。而本发明给出的切换方案的系统状态量保持一致,就状态方程而言,只需要将姿态误差方程的传递函数作微调,并添加陀螺漂移分量εz,其他方程保持变即可。
系统状态量保持不变的优势在于,衡量对准水平的系统协方差阵可以直接从粗对准过渡到精对准,使精对准过程有准确的初始条件。故而本发明给出的切换方案能够保持对准模型的一致性,实现平稳过渡。
附图说明
图1为本发明方法实现流程图。
具体实施方式
如图1所示,本发明具体实现如下:
(1)粗对准过程
定义系统状态变量为:
其中,L、λ和h分别为纬度、经度和高度,δVE、δVN和δVU分别为东向、北向和天向的速度误差,θ、γ和分别为俯仰、滚动和航向角误差。另外,‘x’表示任意变量x的计算值,‘δx’表示任意变量x的误差。
速度误差方程:
其中,和分别为地球自转角速率和计算坐标系相对地球坐标系的角速率在计算坐标系中的投影,fc为真实比力在计算坐标系上的投影,分别为三个方向上加速度计的零偏。式中F为非线性项,将模型线性化认为此项在车载应用中是小量,在粗对准时可以省略。
姿态误差方程:
其中,分别计算坐标系的角速率在计算坐标系中的投影,εx和εy为陀螺仪漂移项。
位置误差方程:
其中RM,RN分别是子午圈半径和卯酉圈半径。
姿态误差方程、速度误差方程和位置误差方程共同构成状态方程。
动基座对准中采用GPS输出的速度和位置作为观测量,观测方程如下:
式中
pGPS和vGPS分别为GPS提供的位置和速度;δpGPS和δvGPS为GPS的位置和速度误差,构成观测噪声n;pIMU和vIMU分别为INS提供的位置和速度;0m×n和Ik分别表示大小为m×n的零矩阵和k×k的单位矩阵。
最后,通过状态方程和观测方程构成线性卡尔曼滤波器,实现粗对准过程。
当航向误差角收敛至满足小角度条件时,结束粗对准过程。
(2)协方差阵传递
保留粗对准收敛时的系统协方差阵,将其作为精对准过程的协方差阵初始条件。
(3)精对准过程
当粗对准的航向误差收敛到满足阈值条件时,切换到精对准过程。精对准过程的系统状态变量、状态方程和观测方程延续粗对准过程,只将姿态误差方程替换为如下方程即可。
Claims (4)
1.一种大方位失准角线性对准方法,其特征在于步骤如下:
(1)选择经度误差,纬度误差,高程误差,东向、北向和天向速度误差,俯仰角误差,滚动角误差,以及航向角误差的正弦项和余弦项作为状态变量,根据状态方程和观测方程,利用GPS观测信息,通过线性卡尔曼滤波,进行粗对准;其中状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成;
(2)当粗对准的航向角误差收敛到满足阈值条件时,保存协方差阵;
(3)延续步骤(1)的状态变量及其位置误差方程、速度误差方程和观测方程,更新姿态误差方程,并将保存的协方差阵作为初始条件,进行精对准,至姿态角收敛至预期水平。
2.根据权利要求1所述的一种大方位失准角线性对准方法,其特征在于:所述步骤(1)采用航向角误差的正弦项和余弦项代替航向角误差作为状态变量的粗对准技术方案。
3.根据权利要求1所述的一种大方位失准角线性对准方法,其特征在于:所述步骤(2)中的阈值条件是指当航向角误差收敛至使成立的阈值
4.根据权利要求1所述的一种大方位失准角线性对准方法,其特征在于:所述步骤(3)中,姿态误差方程为:
其中θ、γ和分别为俯仰、滚动和航向角误差,为计算坐标系相对惯性坐标系的角速率在计算坐标系中的投影,εx、εy和εz为陀螺仪三轴漂移。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610835250.6A CN106840194B (zh) | 2016-09-20 | 2016-09-20 | 一种大方位失准角线性对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610835250.6A CN106840194B (zh) | 2016-09-20 | 2016-09-20 | 一种大方位失准角线性对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106840194A true CN106840194A (zh) | 2017-06-13 |
CN106840194B CN106840194B (zh) | 2019-09-27 |
Family
ID=59145279
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610835250.6A Active CN106840194B (zh) | 2016-09-20 | 2016-09-20 | 一种大方位失准角线性对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106840194B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107479076A (zh) * | 2017-08-08 | 2017-12-15 | 北京大学 | 一种动基座下联合滤波初始对准方法 |
CN110332933A (zh) * | 2019-07-09 | 2019-10-15 | 西安中兴物联软件有限公司 | 车辆定位方法、终端及计算机可读存储介质 |
CN110779551A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的两阶段线性对准在线切换方法 |
CN110779552A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种地球固联坐标系下的自适应对准方法 |
CN110779550A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的大方位失准角两阶段线性对准方法 |
CN116070066A (zh) * | 2023-02-20 | 2023-05-05 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102519460A (zh) * | 2011-12-09 | 2012-06-27 | 东南大学 | 一种捷联惯性导航系统非线性对准方法 |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103575298A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 基于自调节的ukf失准角初始对准方法 |
CN105806338A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法 |
-
2016
- 2016-09-20 CN CN201610835250.6A patent/CN106840194B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102519460A (zh) * | 2011-12-09 | 2012-06-27 | 东南大学 | 一种捷联惯性导航系统非线性对准方法 |
CN103344260A (zh) * | 2013-07-18 | 2013-10-09 | 哈尔滨工程大学 | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 |
CN103471616A (zh) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103471616B (zh) * | 2013-09-04 | 2016-01-27 | 哈尔滨工程大学 | 一种动基座sins大方位失准角条件下初始对准方法 |
CN103575298A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 基于自调节的ukf失准角初始对准方法 |
CN105806338A (zh) * | 2016-03-17 | 2016-07-27 | 孙红星 | 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法 |
Non-Patent Citations (5)
Title |
---|
HAN S等: "A Novel Initial Alignment Scheme for Low-Cost INS Aided by GPS for Land Vehicle Applications", 《JOURNAL OF NAVIGATION》 * |
KEDONG WANG等: "Practical Approaches to Kalman Filtering with Time-Correlated Measurement Errors", 《IEEE TRANSACTIONS ON AEROSPACE AND ELECTRONIC SYSTEMS》 * |
NIE QI等: "SINS In-Motion Alignment for Initial Attitude Uncertainty", 《2015 FIFTH INTERNATIONAL CONFERENCE ON INSTRUMENTATION AND MEASUREMENT,COMPUTER,COMMUNICATION AND CONTROL》 * |
吴松羽等: "低精度INS/GPS组合导航大方位失准角初始对准方案", 《全球定位系统》 * |
吴远方等: "捷联惯导系统的快速初始对准研究", 《计算机仿真》 * |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107479076A (zh) * | 2017-08-08 | 2017-12-15 | 北京大学 | 一种动基座下联合滤波初始对准方法 |
CN110332933A (zh) * | 2019-07-09 | 2019-10-15 | 西安中兴物联软件有限公司 | 车辆定位方法、终端及计算机可读存储介质 |
CN110779551A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的两阶段线性对准在线切换方法 |
CN110779552A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种地球固联坐标系下的自适应对准方法 |
CN110779550A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的大方位失准角两阶段线性对准方法 |
CN110779552B (zh) * | 2019-11-11 | 2022-05-03 | 南京喂啊游通信科技有限公司 | 一种地球固联坐标系下的自适应对准方法 |
CN116070066A (zh) * | 2023-02-20 | 2023-05-05 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
CN116070066B (zh) * | 2023-02-20 | 2024-03-15 | 北京自动化控制设备研究所 | 一种制导炮弹滚动角计算方法 |
Also Published As
Publication number | Publication date |
---|---|
CN106840194B (zh) | 2019-09-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106840194B (zh) | 一种大方位失准角线性对准方法 | |
Wang et al. | Consistent ST-EKF for long distance land vehicle navigation based on SINS/OD integration | |
CN101793523B (zh) | 一种组合导航和光电探测一体化系统 | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN106767787A (zh) | 一种紧耦合gnss/ins组合导航装置 | |
CN102538821B (zh) | 一种快速、参数分段式捷联惯性导航系统自对准方法 | |
CN103557876A (zh) | 一种用于天线跟踪稳定平台的捷联惯导初始对准方法 | |
CN105910606A (zh) | 一种基于角速度差值的方向修正方法 | |
CN105258698A (zh) | 一种高动态自旋制导炮弹空中组合导航方法 | |
CN108680186A (zh) | 基于重力仪平台的捷联式惯导系统非线性初始对准方法 | |
CN111895988A (zh) | 无人机导航信息更新方法及装置 | |
Sokolovic et al. | Integration of INS, GPS, magnetometer and barometer for improving accuracy navigation of the vehicle | |
CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
US20200033131A1 (en) | Low cost ins | |
CN107270896A (zh) | 一种行人定位与轨迹跟踪方法和系统 | |
Bistrov | Performance analysis of alignment process of MEMS IMU | |
CN103217174A (zh) | 一种基于低精度微机电系统的捷联惯导系统初始对准方法 | |
CN103644917A (zh) | 移动测量平台激光雷达旋转与平移参数计算方法 | |
CN102645223A (zh) | 一种基于比力观测的捷联惯导真空滤波修正方法 | |
CN116644264A (zh) | 一种非线性偏置-不变卡尔曼滤波方法 | |
Bodó et al. | State estimation for UAVs using sensor fusion | |
CN103226022B (zh) | 用于组合导航系统的动基座对准方法及系统 | |
CN109506662B (zh) | 一种小天体着陆初始对准方法、其相对导航基准确定方法及装置 | |
CN106595701B (zh) | 一种基于加性四元数的大方位失准角线性对准方法 | |
CN107797156B (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 |