CN110779552A - 一种地球固联坐标系下的自适应对准方法 - Google Patents

一种地球固联坐标系下的自适应对准方法 Download PDF

Info

Publication number
CN110779552A
CN110779552A CN201911095446.6A CN201911095446A CN110779552A CN 110779552 A CN110779552 A CN 110779552A CN 201911095446 A CN201911095446 A CN 201911095446A CN 110779552 A CN110779552 A CN 110779552A
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.)
Granted
Application number
CN201911095446.6A
Other languages
English (en)
Other versions
CN110779552B (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.)
Nanjing Hello Tour Communications Technology Co Ltd
Original Assignee
Nanjing Hello Tour Communications Technology Co Ltd
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 Nanjing Hello Tour Communications Technology Co Ltd filed Critical Nanjing Hello Tour Communications Technology Co Ltd
Priority to CN201911095446.6A priority Critical patent/CN110779552B/zh
Publication of CN110779552A publication Critical patent/CN110779552A/zh
Application granted granted Critical
Publication of CN110779552B publication Critical patent/CN110779552B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

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时刻的量测更新步骤如下：
Figure BDA0002268181890000022
Pk(+)＝(I-KkHk)Pk(-)
状态预测步骤如下：
其中：
Figure BDA0002268181890000033
为xk的估计值，Hk为量测矩阵，Rk为量测噪声矩阵，I为单位矩阵，zk为k时刻的观测量，(-)和(+)分别表示量测更新前和量测更新后的估计值，Φk-1为状态转移矩阵，Qk-1为状态噪声矩阵。
Figure BDA0002268181890000034
和
Figure BDA0002268181890000035
按如下方式确定：
Figure BDA0002268181890000036
Figure BDA0002268181890000037
Figure BDA0002268181890000039
Figure BDA00022681818900000310
Figure BDA00022681818900000311
Figure BDA00022681818900000312
Figure BDA00022681818900000313
其中：ρ为小于1的正数，
Figure BDA00022681818900000315
λ为大于1的正数，max(·)表示取极大值，trace(·)表示取矩阵的迹。
所述步骤(3)中的GNSS接收机包括GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机等。
本发明与现有技术相比的优点在于：
(1)本发明提出的对准方法不需要进行静态调平，有利于缩短对准时间。现有的专利技术中，由于在地理坐标系中进行初始对准，先要进行静态调平，通过对重力加速度的测量，以估计俯仰角和滚转角，在随后进行的对准时，只有方位角为大失准角，有利于降低对准状态方程的非线性程度，但静态调平通常需要10s～20s的时间。而在本发明中，不需要进行静态调平，从而节省了这段时间。
(2)本发明的对准在地球固联坐标系下进行的，实时计算量小。现有的专利技术中，在地理坐标系下构建的状态方程中，耦合项多，导致对准中计算量大。而本发明的对准是在地球固联坐标系下进行的，状态方程中耦合项少，有利于降低实时计算量。
(3)本发明提出的自适应线性卡尔曼滤波算法具有收敛快和精度高的优势。现有的专利技术中，由于方位角仍然是大失准角，开始对准时状态方程仍然是强非线性的，因此，采用了由粗对准和精对准两个阶段，通过粗对准将方位失准角降低到小角度，再切换到精对准，如果切换准则在粗对准收敛程度判断上存在偏差，将导致切换过程不稳定或延长了整个对准时间。而在本发明中，针对对准开始时的强非线性采用自适应卡尔曼滤波方法，实时调整增益矩阵，与线性卡尔曼滤波方法相比，具有收敛速度快和精度高的优势，而且不需要将对准过程分为两个阶段和进行切换。
附图说明
图1为本发明方法实现流程图；
图2为本发明的自适应线性卡尔曼滤波算法流程图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
如图1所示，本发明具体实现如下：
(1)状态方程建模
以地球固联坐标系e系为导航坐标系，定义
Figure BDA0002268181890000041
为载体坐标系b系到e系的转动四元数(各元素记为q0,q1,q2,q3)，
Figure BDA0002268181890000042
为计算四元数(各元素记为q0,q1,q2,q3)，“x”表示任意变量x的计算值或真实值(包含计算误差或测量误差)，“δx”表示任意变量x的误差。定义加性四元数误差(Additive Quaternion Error，AQE)为计算四元数与真实四元数的差，记为：
Figure BDA0002268181890000043
姿态误差方程：
Figure BDA0002268181890000044
其中：
Figure BDA0002268181890000045
为陀螺的理论测量值在b系的投影，为陀螺的实际测量值在b系的投影，地球自转角速度ωie＝15.041°/h，
Figure BDA0002268181890000052
表示导航坐标系的角速度的真实值，且有：
Figure BDA0002268181890000053
Figure BDA0002268181890000054
Figure BDA0002268181890000055
线性化后的速度误差方程：
Figure BDA0002268181890000056
其中：ve＝[vx vy vz]T是速度在e系上的投影，是b系到e系的姿态转换矩阵，
Figure BDA0002268181890000058
为计算的姿态转换矩阵，
Figure BDA0002268181890000059
Figure BDA00022681818900000510
是比力在b系上的投影，δfb为加速度计误差，ge是重力加速度在e系的投影，且有：
Figure BDA00022681818900000511
Figure BDA00022681818900000513
其中：
Figure BDA00022681818900000514
表示四元数乘法，
Figure BDA00022681818900000515
为
Figure BDA00022681818900000516
的共轭复数。
位置误差方程：
Figure BDA00022681818900000517
系统状态变量可以表示为：
X＝[δq0 δq1 δq2 δq3 δvx δvy δvz δx δy δz]T (11)
综合式(2)、式(6)、式(10)和式(11)，可得如下状态方程：
Figure BDA00022681818900000518
其中：
Figure BDA0002268181890000061
Figure BDA0002268181890000062
其中0m×n和Im分别表示m×n阶零矩阵和m×m阶单位矩阵。
(2)观测方程建模
对准中采用GNSS接收机输出的速度和位置作为观测量，观测方程如下：
其中：
Figure BDA0002268181890000065
其中：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 δ函数。
自适应线性卡尔曼滤波算法包括量测更新和状态预测两步，量测更新步骤如下：
Figure BDA0002268181890000066
Figure BDA0002268181890000071
Pk(+)＝(I-KkHk)Pk(-) (25)
状态预测步骤如下：
Figure BDA0002268181890000072
其中：
Figure BDA0002268181890000074
为xk的估计值，(-)和(+)分别表示量测更新前和量测更新后的估计值。
Figure BDA0002268181890000075
和
Figure BDA0002268181890000076
按如下方式确定：
Figure BDA0002268181890000077
Figure BDA0002268181890000078
Figure BDA00022681818900000710
Figure BDA00022681818900000711
Figure BDA00022681818900000712
Figure BDA00022681818900000713
Figure BDA00022681818900000714
Figure BDA00022681818900000715
其中：ρ为小于1的正数(比如0.9)，
Figure BDA00022681818900000716
λ为大于1的正数(比如3)，max(·)表示取极大值，trace(·)表示取矩阵的迹。
实际车载实验表明，与现有的专利技术中在地理坐标系下的初始对准方法相比，本发明所提出的地球固联坐标系下的自适应对准方法，在取得相同的对准精度时，所消耗的计算量降低约40％，对准收敛时间相当。
提供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (2)

1.一种地球固联坐标系下的自适应对准方法，其特征在于，步骤如下：
(1)选择地球固联坐标系作为导航坐标系，以位置误差、速度误差和姿态失准角的加性四元数误差作为系统状态变量；
(2)状态方程由位置误差方程、线性化的速度误差方程和姿态误差方程组成；
(3)利用GNSS接收机的速度和位置观测信息，构建观测方程；
(4)设计自适应线性卡尔曼滤波算法，进行初始对准，自适应线性卡尔曼滤波算法包括量测更新和状态预测两步，第k时刻的量测更新步骤如下：
Figure FDA0002268181880000012
Pk(+)＝(I-KkHk)Pk(-)
状态预测步骤如下：
Figure FDA0002268181880000013
Figure FDA0002268181880000014
其中：为xk的估计值，Hk为量测矩阵，Rk为量测噪声矩阵，I为单位矩阵，zk为k时刻的观测量，(-)和(+)分别表示量测更新前和量测更新后的估计值，Φk-1为状态转移矩阵，Qk-1为状态噪声矩阵，和
Figure FDA0002268181880000017
按如下方式确定：
Figure FDA0002268181880000018
Figure FDA0002268181880000019
Figure FDA00022681818800000111
Figure FDA00022681818800000112
Figure FDA00022681818800000114
Figure FDA0002268181880000021
其中：ρ为小于1的正数，λ为大于1的正数，max(·)表示取极大值，trace(·)表示取矩阵的迹。
2.根据权利要求1所述的一种地球固联坐标系下的自适应对准方法，其特征在于：所述步骤(3)中的GNSS接收机包括GPS接收机、北斗接收机、Galileo接收机、GLONASS接收机或兼容接收机。
CN201911095446.6A 2019-11-11 2019-11-11 一种地球固联坐标系下的自适应对准方法 Active CN110779552B (zh)

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 true CN110779552A (zh) 2020-02-11
CN110779552B 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)

* Cited by examiner, † Cited by third party
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 北京机械设备研究所 一种基于自适应滤波的车辆姿态对准方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
SU WAN-XIN 等: "Application of Sage-Husa adaptive filtering algorithm for high precision SINS initial alignment", 《2014 11TH INTERNATIONAL COMPUTER CONFERENCE ON WAVELET ACTIEV MEDIA TECHNOLOGY AND INFORMATION PROCESSING(ICCWAMTIP)》 *
THOMASKONRAD 等: "Advanced state estimation for navigation of automated vehicles", 《ANNUAL REVIEWS IN CONTROL》 *
何 书 凡: "基于 MINS/GPS 的车载自适应组合导航算法研究", 《中国优秀硕士学位论文全文数据库》 *
许昊天: "双轴连续旋转调制捷联惯导系统初始对准技术研究", 《中国优秀硕士学位论文全文数据库》 *

Also Published As

Publication number Publication date
CN110779552B (zh) 2022-05-03

Similar Documents

Publication Publication Date Title
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
WO2018014602A1 (zh) 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN104344837B (zh) 一种基于速度观测的冗余惯导系统加速度计系统级标定方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN103575299A (zh) 利用外观测信息的双轴旋转惯导系统对准及误差修正方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN103557864A (zh) Mems捷联惯导自适应sckf滤波的初始对准方法
CN104344836A (zh) 一种基于姿态观测的冗余惯导系统光纤陀螺系统级标定方法
CN103674059A (zh) 一种基于外测速度信息的sins水平姿态误差修正方法
CN109489661B (zh) 一种卫星初始入轨时陀螺组合常值漂移估计方法
CN112504298A (zh) 一种gnss辅助的dvl误差标定方法
CN101929862A (zh) 基于卡尔曼滤波的惯性导航系统初始姿态确定方法
CN113203429B (zh) 一种陀螺仪温度漂移误差的在线估计及补偿方法
CN105300407B (zh) 一种用于单轴调制激光陀螺惯导系统的海上动态启动方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN110873577B (zh) 一种水下快速动基座对准方法及装置
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN110940357B (zh) 一种用于旋转惯导单轴自对准的内杆臂标定方法
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN110779552B (zh) 一种地球固联坐标系下的自适应对准方法
CN108279025B (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