CN110779552B - 一种地球固联坐标系下的自适应对准方法 - Google Patents
一种地球固联坐标系下的自适应对准方法 Download PDFInfo
- Publication number
- CN110779552B CN110779552B CN201911095446.6A CN201911095446A CN110779552B CN 110779552 B CN110779552 B CN 110779552B CN 201911095446 A CN201911095446 A CN 201911095446A CN 110779552 B CN110779552 B CN 110779552B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- equation
- error
- alignment
- self
- 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
Images
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种地球固联坐标系下的自适应对准方法,步骤:(1)选择地球固联坐标系作为导航坐标系,以位置误差、速度误差和姿态失准角的加性四元数误差作为***状态变量。(2)状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成。(3)利用GNSS接收机的速度和位置观测信息,构建观测方程。(4)设计自适应线性卡尔曼滤波算法,进行初始对准。本发明不需要进行静态调平,实时计算量小,对准过程收敛快,且对准精度高。
Description
技术领域
本发明涉及一种地球固联坐标系下的自适应对准方法,属于惯性导航技术领域。
背景技术
惯性导航***(Inertial Navigation System,INS)由于能同时提供姿态、速度和位置等全导航信息,且完全自主,抗干扰性能好,得到了广泛应用。特别是随着微电子加工技术的快速发展,基于MEMS(Micro-Electro-Mechanical System)加工的陀螺仪和加速度计精度的提升和价格的降低,为基于MEMS器件的INS的更广泛应用奠定了坚实的基础。
由于INS基于的是积分式工作原理,因此,在进行正式导航解算之前,需要进行初始姿态、速度和位置的确定,即初始对准。由于初始速度和位置可以通过卫星导航(GlobalNavigation Satellite System,GNSS)接收机或其它方式确定,初始对准的主要任务是完成初始姿态的确定。传统高精度的INS可以通过对地球自转角速度和重力加速度的静态测量,实现对初始姿态的高精度确定。但是,对于精度较低的MEMS INS,通过静基座对准只能完成初始俯仰角和滚转角的较高精度确定,初始方位角则无法通过静基座对准进行精确确定,即只能实现静态调平,从而造成了初始大方位失准角的问题。而且,需要注意的是,静态调平是需要一段时间的,导致整个对准时间加长。针对这些问题,其中一种解决方法是利用GNSS接收机输出的速度和位置,进行动基座的初始对准。
建立准确的INS误差传播方程和采用适当的滤波技术是进行初始对准的主要问题。如果不进行静态调平,在大失准角的情况下的动基座对准模型本质上是非线性的,而非线性滤波方法计算量大,不适宜工程应用。
常用的姿态角描述方法有欧拉角法和四元数法,对准方案也可据此分为两类。欧拉角法用于描述姿态角的优点在于直观性,各参数物理意义明确,但其存在奇异性问题。四元数法由于计算简单、无奇异性等优点,常被用于导航解算中。现有的技术方案中推导了加性四元数描述的姿态误差方程,其优点在于姿态误差方程并没有经过任何小角度假设,并且是误差四元数的线性函数。不足之处在于其速度误差方程仍然保留非线性。现有的大失准角对准方案中,对加性四元数的利用主要停留在非线性滤波的程度上。
在现有的技术方案中,初始对准常用的坐标系为地理坐标系,例如,专利“一种基于加性四元数的大方位失准角线性对准方法.中国发明专利,专利号:ZL201610835249.3”在“东-北-天”地理坐标系下提出了一种基于加性四元数的线性化对准方法,其中状态方程的耦合项较多,计算量偏大。如果初始对准过程在地球固联坐标系中进行,则滤波方程将大大简化,有利于降低对实时计算能力的要求,这对基于MEMS INS的低成本应用非常重要。但是,在地球固联坐标系下,无法通过静态调平降低非线性程度,因为此时的对准只能在三个初始姿态失准角都是大角度下进行,导致滤波方程的非线性程度大大增加。
因此,在地球固联坐标系下进行初始对准,虽然实时计算量小,但初始大失准角将导致滤波方程的非线性程度增加。
发明内容
本发明技术解决问题:克服了现有技术中在地理坐标系下初始对准实时计算量较大的问题,在地球固联坐标系下,针对初始大失准角,提供一种基于加性四元数的自适应对准方法,既能实现对准的快速收敛,又有利于降低实时计算量。
本发明的技术要点:
1.采用加性四元数表示姿态角,在地球固联坐标系下,用线性化的状态方程(线性化是指速度误差方程)实现初始对准过程;
2.针对速度误差方程中的非线性项,提出一种自适应滤波算法,以快速收敛。
本发明技术解决方案:一种地球固联坐标系下的自适应对准方法,步骤如下:
(1)选择地球固联坐标系作为导航坐标系,以位置误差、速度误差和姿态失准角的加性四元数误差作为***状态变量。
(2)状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成。
(3)利用GNSS接收机的速度和位置观测信息,构建观测方程。
(4)设计自适应线性卡尔曼滤波算法,进行初始对准。自适应线性卡尔曼滤波算法包括量测更新和状态预测两步,第k时刻的量测更新步骤如下:
Pk(+)=(I-KkHk)Pk(-)
状态预测步骤如下:
其中:为xk的估计值,Hk为量测矩阵,Rk为量测噪声矩阵,I为单位矩阵,zk为k时刻的观测量,(-)和(+)分别表示量测更新前和量测更新后的估计值,Φk-1为状态转移矩阵,Qk-1为状态噪声矩阵。和按如下方式确定:
所述步骤(3)中的GNSS接收机包括GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机等。
本发明与现有技术相比的优点在于:
(1)本发明提出的对准方法不需要进行静态调平,有利于缩短对准时间。现有的专利技术中,由于在地理坐标系中进行初始对准,先要进行静态调平,通过对重力加速度的测量,以估计俯仰角和滚转角,在随后进行的对准时,只有方位角为大失准角,有利于降低对准状态方程的非线性程度,但静态调平通常需要10s~20s的时间。而在本发明中,不需要进行静态调平,从而节省了这段时间。
(2)本发明的对准在地球固联坐标系下进行的,实时计算量小。现有的专利技术中,在地理坐标系下构建的状态方程中,耦合项多,导致对准中计算量大。而本发明的对准是在地球固联坐标系下进行的,状态方程中耦合项少,有利于降低实时计算量。
(3)本发明提出的自适应线性卡尔曼滤波算法具有收敛快和精度高的优势。现有的专利技术中,由于方位角仍然是大失准角,开始对准时状态方程仍然是强非线性的,因此,采用了由粗对准和精对准两个阶段,通过粗对准将方位失准角降低到小角度,再切换到精对准,如果切换准则在粗对准收敛程度判断上存在偏差,将导致切换过程不稳定或延长了整个对准时间。而在本发明中,针对对准开始时的强非线性采用自适应卡尔曼滤波方法,实时调整增益矩阵,与线性卡尔曼滤波方法相比,具有收敛速度快和精度高的优势,而且不需要将对准过程分为两个阶段和进行切换。
附图说明
图1为本发明方法实现流程图;
图2为本发明的自适应线性卡尔曼滤波算法流程图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
如图1所示,本发明具体实现如下:
(1)状态方程建模
以地球固联坐标系e系为导航坐标系,定义为载体坐标系b系到e系的转动四元数(各元素记为q0,q1,q2,q3),为计算四元数(各元素记为q0,q1,q2,q3),“x”表示任意变量x的计算值或真实值(包含计算误差或测量误差),“δx”表示任意变量x的误差。定义加性四元数误差(Additive Quaternion Error,AQE)为计算四元数与真实四元数的差,记为:
姿态误差方程:
线性化后的速度误差方程:
位置误差方程:
***状态变量可以表示为:
X=[δq0 δq1 δq2 δq3 δvx δvy δvz δx δy δz]T (11)
综合式(2)、式(6)、式(10)和式(11),可得如下状态方程:
其中:
其中0m×n和Im分别表示m×n阶零矩阵和m×m阶单位矩阵。
(2)观测方程建模
对准中采用GNSS接收机输出的速度和位置作为观测量,观测方程如下:
其中:
其中:rGNSS和vGNSS分别为GNSS接收机提供的位置和速度;δrGNSS和δvGNSS为GNSS接收机提供的位置和速度的误差,构成观测噪声n;rIMU和vIMU分别为INS提供的位置和速度。
(3)自适应线性卡尔曼滤波器设计,如图2所示。
首先,对式(12)和式(15)所示的状态方程和观测方程进行离散化。设采样周期为T,离散化的状态方程和观测方程如下:
xk=Φk-1xk-1+wk-1 (18)
zk=Hkxk+nk (19)
其中:k表示第k时刻,wk-1和nk分别为离散的状态噪声和观测噪声,且有:
Φk-1≈I+FT (20)
E(wkwl T)=Qkδkl (21)
E(nknl T)=Rkδkl (22)
其中:E(·)表示求期望,δkl为Kronecker δ函数。
自适应线性卡尔曼滤波算法包括量测更新和状态预测两步,量测更新步骤如下:
Pk(+)=(I-KkHk)Pk(-) (25)
状态预测步骤如下:
实际车载实验表明,与现有的专利技术中在地理坐标系下的初始对准方法相比,本发明所提出的地球固联坐标系下的自适应对准方法,在取得相同的对准精度时,所消耗的计算量降低约40%,对准收敛时间相当。
提供以上实施例仅仅是为了描述本发明的目的,而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改,均应涵盖在本发明的范围之内。
Claims (2)
1.一种地球固联坐标系下的自适应对准方法,其特征在于,步骤如下:
(1)选择地球固联坐标系作为导航坐标系,以位置误差、速度误差和姿态失准角的加性四元数误差作为***状态变量;
(2)状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成;
(3)利用GNSS接收机的速度和位置观测信息,构建观测方程;
(4)设计自适应线性卡尔曼滤波算法,进行初始对准,自适应线性卡尔曼滤波算法包括量测更新和状态预测两步,第k时刻的量测更新步骤如下:
Pk(+)=(I-KkHk)Pk(-)
状态预测步骤如下:
其中:为xk的估计值,Hk为量测矩阵,Rk为量测噪声矩阵,I为单位矩阵,zk为k时刻的观测量,(-)和(+)分别表示量测更新前和量测更新后的估计值,Φk-1为状态转移矩阵,Qk-1为状态噪声矩阵,和按如下方式确定:
2.根据权利要求1所述的一种地球固联坐标系下的自适应对准方法,其特征在于:所述步骤(3)中的GNSS接收机包括GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911095446.6A CN110779552B (zh) | 2019-11-11 | 2019-11-11 | 一种地球固联坐标系下的自适应对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911095446.6A CN110779552B (zh) | 2019-11-11 | 2019-11-11 | 一种地球固联坐标系下的自适应对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110779552A CN110779552A (zh) | 2020-02-11 |
CN110779552B true CN110779552B (zh) | 2022-05-03 |
Family
ID=69391046
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911095446.6A Active CN110779552B (zh) | 2019-11-11 | 2019-11-11 | 一种地球固联坐标系下的自适应对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110779552B (zh) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105004351A (zh) * | 2015-05-14 | 2015-10-28 | 东南大学 | 基于自适应upf的sins大方位失准角初始对准方法 |
CN106840194A (zh) * | 2016-09-20 | 2017-06-13 | 南京喂啊游通信科技有限公司 | 一种大方位失准角线性对准方法 |
CN110388941A (zh) * | 2019-08-28 | 2019-10-29 | 北京机械设备研究所 | 一种基于自适应滤波的车辆姿态对准方法 |
-
2019
- 2019-11-11 CN CN201911095446.6A patent/CN110779552B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105004351A (zh) * | 2015-05-14 | 2015-10-28 | 东南大学 | 基于自适应upf的sins大方位失准角初始对准方法 |
CN106840194A (zh) * | 2016-09-20 | 2017-06-13 | 南京喂啊游通信科技有限公司 | 一种大方位失准角线性对准方法 |
CN110388941A (zh) * | 2019-08-28 | 2019-10-29 | 北京机械设备研究所 | 一种基于自适应滤波的车辆姿态对准方法 |
Non-Patent Citations (4)
Title |
---|
Advanced state estimation for navigation of automated vehicles;ThomasKonrad 等;《Annual Reviews in Control》;20181002;第181-195页 * |
Application of Sage-Husa adaptive filtering algorithm for high precision SINS initial alignment;Su Wan-Xin 等;《2014 11th International Computer Conference on Wavelet Actiev Media Technology and Information Processing(ICCWAMTIP)》;20150402;第359-364页 * |
双轴连续旋转调制捷联惯导***初始对准技术研究;许昊天;《中国优秀硕士学位论文全文数据库》;20190615;第1-77页 * |
基于 MINS/GPS 的车载自适应组合导航算法研究;何 书 凡;《中国优秀硕士学位论文全文数据库》;20190415;第1-83页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110779552A (zh) | 2020-02-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110398257B (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
WO2018014602A1 (zh) | 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN104344837B (zh) | 一种基于速度观测的冗余惯导***加速度计***级标定方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN103344260B (zh) | 基于rbckf的捷联惯导***大方位失准角初始对准方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN103575299A (zh) | 利用外观测信息的双轴旋转惯导***对准及误差修正方法 | |
CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
CN104344836A (zh) | 一种基于姿态观测的冗余惯导***光纤陀螺***级标定方法 | |
CN113203429B (zh) | 一种陀螺仪温度漂移误差的在线估计及补偿方法 | |
CN104359496B (zh) | 基于垂线偏差补偿的高精度姿态修正方法 | |
CN103674059A (zh) | 一种基于外测速度信息的sins水平姿态误差修正方法 | |
CN110044385A (zh) | 一种大失准角情况下的快速传递对准方法 | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN105300407B (zh) | 一种用于单轴调制激光陀螺惯导***的海上动态启动方法 | |
CN111207734B (zh) | 一种基于ekf的无人机组合导航方法 | |
CN104634348B (zh) | 组合导航中的姿态角计算方法 | |
CN103901459B (zh) | 一种mems/gps组合导航***中量测滞后的滤波方法 | |
CN110873577B (zh) | 一种水下快速动基座对准方法及装置 | |
CN106595701B (zh) | 一种基于加性四元数的大方位失准角线性对准方法 | |
CN110940357B (zh) | 一种用于旋转惯导单轴自对准的内杆臂标定方法 | |
CN110779552B (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 |