CN106595701B - 一种基于加性四元数的大方位失准角线性对准方法 - Google Patents
一种基于加性四元数的大方位失准角线性对准方法 Download PDFInfo
- Publication number
- CN106595701B CN106595701B CN201610835249.3A CN201610835249A CN106595701B CN 106595701 B CN106595701 B CN 106595701B CN 201610835249 A CN201610835249 A CN 201610835249A CN 106595701 B CN106595701 B CN 106595701B
- Authority
- CN
- China
- Prior art keywords
- error
- alignment
- equation
- quaternion
- additive
- 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
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
本发明涉及一种基于加性四元数的大方位失准角线性对准方法,步骤:(1)根据粗对准的状态方程和观测方程,利用GPS观测信息,通过线性卡尔曼滤波,进行粗对准过程;(2)在滑动窗口内,利用本发明提出的阈值条件,直接由加性四元数计算当前航向角收敛水平,并判断其是否全部满足阈值条件;若满足,则保存系统协方差信息,转入精对准;若不满足,则继续步骤(1)的粗对准;(3)延续粗对准的系统状态变量及其位置、姿态误差方程和观测方程,使用新的速度误差方程,并将保存的协方差阵作为初始条件,进行精对准,使精对准能快速进入收敛状态,至对准达到预期水平。本发明既能实现平稳的模型切换,又满足全状态应用条件,且计算简便。
Description
技术领域
本发明涉及一种基于加性四元数的大方位失准角线性对准方法,属于惯性导航技术领域。
背景技术
初始对准是惯性导航的关键技术,也是INS/GNSS组合导航关键技术之一。在基于MEMS-INS/GNSS组合导航系统中,由于MEMS器件特别是陀螺仪的限制,导致无法通过自对准实现方位失准角的初始化,从而造成了大方位失准角问题,其中一种解决方法是直接进行动基座的对准。
建立准确的INS误差传播方程和采用适当的滤波技术是进行初始对准的主要问题。在大方位失准角的情况下的动基座对准模型本质上是非线性的,而非线性滤波方法计算量大,不适宜工程应用。
常用的姿态角描述方法有欧拉角法和四元数法。相应的,对准方案也可据此分为两类。
欧拉角法用于描述姿态角的优点在于直观性,各参数物理意义明确,但其存在奇异性问题。现有的技术方案中,除直接进行非线性滤波外,主要是将对准过程分为粗对准和精对准两个过程,并在粗对准中用航向角误差的正余弦项作为系统状态变量,实现系统方程的线性化;在精对准中则采用小角度近似,即sinα≈α,cosα≈1实现线性化,从而进行线性滤波。一般在粗对准精度达到某一阈值条件时实现对准模型的切换。
四元数由于计算简单,无奇异性等优点,常被用于导航解算中。现有的技术方案中推导了加性四元数描述的姿态误差方程,其优点在于姿态误差方程并没有经过任何小角度假设,并且是误差四元数的线性函数。不足之处在于其速度误差方程仍然保留非线性。现有的大失准角对准方案中,对加性四元数的利用主要停留在非线性滤波的程度上。
现有的技术方案中,大失准角条件下线性化的速度误差方程已经被推导出来,并且证明了小角度近似下,加性四元数方法和等倾角模型的等价性,这为线性滤波模型的建立提供了条件。参见Yu M J,Park H W,Jeon C B.Equivalent Nonlinear Error Modelsof Strapdown Inertial[C].Proceedings of 1997Guidance,Navigation,and ControlConference,August 11-13,1997,p.581-587.和Kong X.INS algorithm usingquaternion model for low cost IMU[J].Robotics and Autonomous Systems,2004,46(4):221-246.
但现有方案中的非线性滤波模型,计算量大,不适宜工程应用。欧拉角在计算复杂度上要高于四元数,且存在奇异性问题,无法应用于全状态。欧拉角法在粗对准和精对准需要采用不同的状态变量和误差模型,系统状态变量的协方差阵无法直接传递,使精对准需要重新对状态的协方差阵初始化,从而造成模型切换过渡不平稳,影响收敛速度。虽然加性四元数的粗对准和精对准模型都被推导出来,但由于四元数与姿态角之间的关系不直观,对准过程的收敛情况判断难度大,由粗到精的模型切换时刻难以有效确定,所以加性四元数应用于线性滤波模型的大失准角初始对准方案仍存在困难。
发明内容
本发明技术解决问题:克服了现有技术中欧拉角法粗对准和精对准模型切换不平稳,而四元数法滤波模型非线性的问题,提供了一种基于加性四元数的大方位失准角线性对准方法,既能实现平稳的模型切换,又满足全状态应用条件,且计算简便。
本发明的技术要点:
1.采用加性四元数表示姿态角,分别用线性化的状态方程(线性化主要指速度误差方程)实现粗对准和精对准过程;
2.提出一种粗对准到精对准的模型切换准则;
假设失准角在导航坐标系中的分量分别为φE、φN、φU,则总误差水平可表示为:
则加性四元数与导航坐标系n系和计算坐标系p系的旋转矢量之间存在如下关系:
在大方位失准角条件下可认为φE、φN为小量,φ≈φU。当粗对准收敛后,有:
假定δqi和φU都是无偏的,即其期望满足E(δqi)=0和E(φU)=0,则航向角误差的方差可以表示为:
若设定由粗到精的切换阈值为φth,则切换条件可表示为:
3.平稳过渡方案;
采用滑动窗口法,设定长度为Δt的时间窗口,只有当滑动窗口内的失准角均满足阈值条件时,才判定粗对准完成。
本发明技术解决方案:一种基于加性四元数的大方位失准角线性对准方法,步骤如下:
(1)选择经度误差,纬度误差,高程误差,东向、北向和天向速度误差,加性四元数误差作为系统状态变量,根据粗对准的状态和观测方程,利用GPS观测信息,通过卡尔曼滤波,进行粗对准,其中,状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成;
(2)在粗对准期间,采用滑动窗口法来进行切换准则判定,即设定固定长度的时间窗口,在窗口内,直接由加性四元数计算当前航向角收敛水平,并判断航向角误差是否全部满足阈值条件;若满足,则保存对准系统的协方差阵,转入精对准;若不满足,则继续步骤(1)的粗对准;
(3)延续粗对准的系统状态变量及其位置误差方程、姿态误差方程和观测方程,使用小角度下的线性化速度误差方程,并将保存的协方差阵作为卡尔曼滤波的初始条件,进行精对准,使精对准能快速进入收敛状态,至对准达到预期水平。
所述步骤(1)中,所述卡尔曼滤波为线性卡尔曼滤波。
所述步骤(2)中的阈值条件为: 表示计算得到的加性四元数,表示加性四元数误差的估计值,φth为设定的航向角阈值。
所述步骤(3)中的小角度为小于等于5°。
本发明与现有技术相比的优点在于:
(1)本发明分为粗对准和精对准过程,均采用线性化误差模型,避免非线性滤波;这样采用线性滤波方案,易于工程实现。
(2)采用加性四元数描述系统姿态,计算简便,无奇异性,适用于全状态。
(3)本发明的粗对准和精对准过程采用相同的系统状态变量,粗对准完成时的协方差阵可以直接作为精对准的初始条件,精对准过程收敛快。
(4)采用加性四元数构造模型切换准则,可以由四元数直接导出航向角误差水平,实现阈值判别,有效实现对粗对准收敛状态的度量,避免由四元数求解欧拉角的复杂过程。
(5)采用滑动窗口方案判断阈值条件,保证线性化假设成立,避免提前切换造成系统发散。
附图说明
图1为本发明方法实现流程图;
图2为本发明的切换准则检验流程图。
具体实施方式
如图1所示,本发明具体实现如下:
(1)粗对准过程
定义为载体坐标系b系到导航坐标系n系的转动四元数(各元素记为q0,q1,q2,q3),为计算四元数(各元素记为),表示任意变量x的计算值,‘δx’表示任意变量x的误差。定义加性四元数误差(AQE)为计算四元数与真实四元数的差,记为:
系统状态变量可以表示为:
x=[δq0 δq1 δq2 δq3 δve δvn δvu δL δλ δh]T (7)
姿态误差方程:
其中,为陀螺的理论测量值在载体系的投影,表示导航坐标系的角速度的真实值。
式中:
线性化后的速度误差方程:
其中,vn=[ve vn vu]T是速度在载体系上的投影,是载体系到导航系的姿态转换矩阵,fb是比力测量值,是地球自转角速率在导航系的投影,是导航系相对地球坐标系的角速度在导航系的投影,gn是重力加速度在导航系的投影。
位置误差方程:
其中L、λ和h分别为纬度、经度和高度,RM,RN分别是子午圈半径和卯酉圈半径。
动基座对准中采用GPS输出的速度和位置作为观测量,观测方程如下:
式中
pGPS和vGPS分别为GPS提供的位置和速度;δpGPS和δvGPS为GPS的位置和速度误差,构成观测噪声n;pIMU和vIMU分别为INS提供的位置和速度;0m×n和Ik分别表示大小为m×n的零矩阵和k×k的单位矩阵。
最后,通过系统状态方程和观测方程构成线性卡尔曼滤波器,进行粗对准过程。
(2)切换准则检验
在粗对准过程开始后,采用滑动窗口法判别阈值条件,具体执行过程如下:
首先,记录当前时刻t1,作为窗口时间起点;
其次,由四元数,计算每一时刻的航向角误差标准差,并与设定阈值对比。若在设定的Δt时间长度内,每一时刻的航向角误差标准差均满足阈值条件,则判定符合切换准则,记录当前时刻粗对准的协方差阵,转入精对准过程;若Δt时间长度内,出现任意时刻的航向角误差标准差不满足阈值条件,则更新t1,滑动窗口,重新判定。
(3)精对准过程
当粗对准角度收敛到小角度时,切换到精对准方程。精对准方程只需将粗对准的速度误差方程调整为如下即可:
将记录的粗对准的协方差阵作为精度准系统的滤波初值。
提供以上实施例仅仅是为了描述本发明的目的,而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改,均应涵盖在本发明的范围之内。
Claims (4)
1.一种基于加性四元数的大方位失准角线性对准方法,其特征在于步骤如下:
(1)选择经度误差,纬度误差,高程误差,东向、北向和天向速度误差,加性四元数误差作为状态变量,根据状态方程和观测方程,利用GPS观测信息,通过卡尔曼滤波,进行粗对准,其中状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成;
(2)在粗对准期间,采用滑动窗口法来进行切换准则判定,即设定固定长度的时间窗口,在窗口内,直接由加性四元数计算当前航向角收敛水平,并判断航向角误差是否全部满足阈值条件;若满足,则保存协方差阵,转入精对准;若不满足,则继续步骤(1)的粗对准;
(3)延续步骤(1)的状态变量及其位置误差方程、姿态误差方程和观测方程,使用小角度下的线性化速度误差方程,并将保存的协方差阵作为卡尔曼滤波的初始条件,进行精对准,使精对准能快速进入收敛状态,至对准达到预期水平。
2.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法,其特征在于:所述步骤(1)中,所述卡尔曼滤波为线性卡尔曼滤波。
3.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法,其特征在于:所述步骤(2)中的阈值条件为: 表示计算得到的加性四元数,表示加性四元数误差的估计值,φth为设定的航向角阈值。
4.根据权利要求1所述的基于加性四元数的大方位失准角线性对准方法,其特征在于:所述步骤(3)中的小角度为小于等于5°。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610835249.3A CN106595701B (zh) | 2016-09-20 | 2016-09-20 | 一种基于加性四元数的大方位失准角线性对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610835249.3A CN106595701B (zh) | 2016-09-20 | 2016-09-20 | 一种基于加性四元数的大方位失准角线性对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106595701A CN106595701A (zh) | 2017-04-26 |
CN106595701B true CN106595701B (zh) | 2019-07-09 |
Family
ID=58555751
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610835249.3A Active CN106595701B (zh) | 2016-09-20 | 2016-09-20 | 一种基于加性四元数的大方位失准角线性对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106595701B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110779550A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的大方位失准角两阶段线性对准方法 |
CN110779551A (zh) * | 2019-11-11 | 2020-02-11 | 南京喂啊游通信科技有限公司 | 一种基于加性四元数的两阶段线性对准在线切换方法 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7142981B2 (en) * | 2003-08-05 | 2006-11-28 | The Boeing Company | Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous |
CN101162147A (zh) * | 2007-11-27 | 2008-04-16 | 哈尔滨工程大学 | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 |
WO2012068359A2 (en) * | 2010-11-17 | 2012-05-24 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic |
CN103245360A (zh) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 |
CN103900608A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
-
2016
- 2016-09-20 CN CN201610835249.3A patent/CN106595701B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7142981B2 (en) * | 2003-08-05 | 2006-11-28 | The Boeing Company | Laser range finder closed-loop pointing technology of relative navigation, attitude determination, pointing and tracking for spacecraft rendezvous |
CN101162147A (zh) * | 2007-11-27 | 2008-04-16 | 哈尔滨工程大学 | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 |
WO2012068359A2 (en) * | 2010-11-17 | 2012-05-24 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic |
CN103245360A (zh) * | 2013-04-24 | 2013-08-14 | 北京工业大学 | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 |
CN103900608A (zh) * | 2014-03-21 | 2014-07-02 | 哈尔滨工程大学 | 一种基于四元数ckf的低精度惯导初始对准方法 |
CN104374401A (zh) * | 2014-10-15 | 2015-02-25 | 哈尔滨工程大学 | 一种捷联惯导初始对准中重力扰动的补偿方法 |
Non-Patent Citations (4)
Title |
---|
Applied Quaternion Optimization Method in Transfer Alignment for Airborne AHRS Under Large Misalignment Angle;Jiazhen Lu,et al.;《 IEEE Transactions on Instrumentation and Measurement 》;20160228;第65卷(第2期);全文 |
一种自适应滤波方法在捷联惯导系统大失准角初始对准中的应用;柏猛 等;《传感技术学报》;20080630;第21卷(第6期);全文 |
低精度INS/GPS组合导航大方位失准角初始对准方案;吴松羽 等;《全球定位系统》;20141231;第39卷(第6期);全文 |
基于UKF的捷联惯导大失准角初始对准方法;张宁;《计算机仿真》;20110630;第28卷(第06期);全文 |
Also Published As
Publication number | Publication date |
---|---|
CN106595701A (zh) | 2017-04-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103245360B (zh) | 晃动基座下的舰载机旋转式捷联惯导系统自对准方法 | |
CN104344837B (zh) | 一种基于速度观测的冗余惯导系统加速度计系统级标定方法 | |
CN103900608B (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN105737854B (zh) | 一种捷联惯导系统在线标定方法 | |
CN106767787A (zh) | 一种紧耦合gnss/ins组合导航装置 | |
CN100541132C (zh) | 大失准角下船用光纤陀螺捷联航姿系统系泊精对准方法 | |
CN106840194B (zh) | 一种大方位失准角线性对准方法 | |
CN101261130B (zh) | 一种船用光纤捷联惯导系统传递对准精度评估方法 | |
CN105180968A (zh) | 一种imu/磁强计安装失准角在线滤波标定方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN106153073B (zh) | 一种全姿态捷联惯导系统的非线性初始对准方法 | |
CN101963512A (zh) | 船用旋转式光纤陀螺捷联惯导系统初始对准方法 | |
CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
CN103398713A (zh) | 一种星敏感器/光纤惯性设备量测数据同步方法 | |
CN103900565A (zh) | 一种基于差分gps的惯导系统姿态获取方法 | |
CN105928515B (zh) | 一种无人机导航系统 | |
CN101963513A (zh) | 消除水下运载体捷联惯导系统杆臂效应误差的对准方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN102168978B (zh) | 一种船用惯性导航系统摇摆基座开环对准方法 | |
CN109489661B (zh) | 一种卫星初始入轨时陀螺组合常值漂移估计方法 | |
CN102519485A (zh) | 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 | |
CN109945895A (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
Bistrov | Performance analysis of alignment process of MEMS IMU | |
CN103148868A (zh) | 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法 | |
CN106595701B (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 |