CN109931955B - 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 - Google Patents

基于状态相关李群滤波的捷联惯性导航系统初始对准方法 Download PDF

Info

Publication number
CN109931955B
CN109931955B CN201910200910.7A CN201910200910A CN109931955B CN 109931955 B CN109931955 B CN 109931955B CN 201910200910 A CN201910200910 A CN 201910200910A CN 109931955 B CN109931955 B CN 109931955B
Authority
CN
China
Prior art keywords
matrix
coordinate system
state
formula
noise
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
CN201910200910.7A
Other languages
English (en)
Other versions
CN109931955A (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.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201910200910.7A priority Critical patent/CN109931955B/zh
Publication of CN109931955A publication Critical patent/CN109931955A/zh
Application granted granted Critical
Publication of CN109931955B publication Critical patent/CN109931955B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

本发明公开了基于状态相关李群滤波的捷联惯性导航初始对准方法，使用李群和李代数描述载体的姿态，建立模型，将初始对准问题转化成姿态估计问题进行对准。SVD分解法是基于最小二乘原理的数学解算方法，无法考虑到观测矢量中的噪声项，解算过程包含了大量噪声信息，在实际环境当中干扰较大，惯性单元噪声很难统计造成数值解算精度大幅下降。由于模型线性化时带来的误差导致估计精度下降。四元数滤波算法的精度优于数值解算的方法，然而构建四元数量测方程时需要也需要对模型进行线性变换对滤波结果的精度和收敛速度都有影响。本发明提出的方法基于李群直接对量测模型进行建模不需要线性化，并且考虑到状态相关不确定噪声对系统的影响。

Description

基于状态相关李群滤波的捷联惯性导航系统初始对准方法
技术领域
本发明公开了一种基于状态相关李群滤波的捷联惯性导航系统初始对准方法，该方法属于导航方法及应用技术领域。
背景技术
所谓导航，就是正确地引导载体沿着预定的航线、以要求的精度、在指定的时间内将载体引导至目的地的过程。惯性导航系统根据自身传感器的输出，以牛顿第二定律为理论基础，对载体的各项导航参数进行解算。它是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。
早期的惯导系统以平台惯导为主，随着惯性器件的成熟和计算机技术的发展，上世纪60年代开始出现了惯性器件与载体直接固联的捷联惯导系统。与平台惯导相比，捷联惯导系统省去了复杂的实体稳定平台，具有成本低、体积小、重量轻、可靠性高等优点。近年来，捷联惯导系统日趋成熟，精度逐步提高，应用范围也逐渐扩大。捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。
捷联式惯性导航系统进入导航任务前要先进行初始对准，目的是建立精确的初始姿态矩阵，从而得到载体相对空间的姿态。对准时间和对准精度是初始对准的两个重要指标。对准时间反映系统的快速反应能力，对准精度反映系统的导航性能。作为惯导系统的一个关键技术，初始对准是国内外学者多年来的研究热点。惯导系统的初始对准分为粗对准和精对准两个阶段。
比较传统的初始对准方法，仅适用于静止或微幅晃动的对准环境。对于工作在复杂环境中的载体，诸如发动机处于高频振动的汽车、格斗状态下的战斗机、浪涌下的舰船等，载体的角振动和线振动会导致初始对准的精度和稳定性下降。捷联惯导系统在自对准过程中的姿态时刻都在发生变化，初始对准的时间和精度会受到影响。因此，在运载体晃动干扰环境下完成初始对准过程，就必须屏蔽这些无法消除的扰动影响。对准领域的研究重点是动态情况下捷联式惯性导航系统的初始对准，致力于研究出抗扰能力强、对准精度高、适应于各种复杂恶劣环境的对准方法。本发明将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，并考虑可有效地解决晃动条件下初始对准的问题。
四元数的表示方法弥补了欧拉角的的不足，计算过程中不存在奇异点的问题，但是四元数存在约束条件，在采用四元数描述姿态运动时，在滤波过程中由于难以满足其约束条件而出现问题。四元数卡尔曼初始对准方法为了避免非线性模型，构造了伪量测方程进行滤波估计。构造过程中的求逆运算会导致滤波系统的不稳定。李群表示方法弥补了四元数存在的问题。基于SO(3)群的概率分布函数推导，由于SO(3)群是紧的，李群表示方法有效的避免了姿态解算中的奇异之问题，可以对姿态进行全局表示。基于李群描述的滤波运算，也大大减少了矩阵和向量转化中的计算误差，提升了计算速度和计算精度。但是现有的基于李群滤波的初始对准方法没有考虑到状态相关噪声对滤波过程的影响，导致计算结果中存在误差，影响对准精度。
针对上述问题，本发明对李群滤波中的状态相关噪声项进行计算与归纳，设计新的状态相关李群滤波代替原有的李群滤波完成初始对准任务。根据状态相关噪声的形式，设计状态噪声协方差矩阵，使其可以随着滤波过程进行迭代更新，逐渐向真实值逼近。状态相关李群滤波器相比于现有的李群滤波器，拥有更高的计算精度，能更好的完成晃动条件下的初始对准任务。
发明内容
基于状态相关李群滤波的初试对准方法是应用状态相关李群滤波进行惯性导航的初始对准过程。使用李群和李代数对载体姿态进行描述，将初始对准转化成为姿态估计的问题，构建滤波器进行估计。将初始姿态矩阵分解为三个SO(3)群的连续相乘，通过惯性元件的积分计算和地理位置信息，建立新的观测模型。采用李群和李代数描述捷联解算，应用状态相关李群滤波器进行递归迭代，求出姿态矩阵。本发明能快速、精确的完成初始对准任务，去除了状态相关噪声对滤波估计过程中的影响，适应性强，在实际工程中具有良好的应用前景。
在本发明方法的详细描述中坐标系定义如下：地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上。
在本发明中，根据现有的捷联惯性导航系统初始对准中的问题，提出了基于状态相关李群滤波的捷联惯性导航初试对准方法。
为实现方法流程，本发明采用的技术方案为基于状态相关李群滤波的捷联惯性导航初始对准方法，该方法通过下述流程实现，
(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息
Figure GDA0002727001730000031
和加速度计的输出信息fb等；
(2)对采集到的陀螺和加速度计的数据进行处理，应用状态相关李群滤波方法估计姿态矩阵；
方法的整体计算流程如图1所示。
描述坐标系变换的旋转矩阵符合李群三维特殊正交群SO(3)的性质，构成了SO(3)群：
Figure GDA0002727001730000032
其中，R对应了旋转矩阵，
Figure GDA0002727001730000033
表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(*)表示矩阵的行列式。根据姿态矩阵的链式法则，从载体系到导航系的姿态矩阵可以分解为三个连续特殊正交阵的乘积形式：导航系从初始时刻到当前时刻的旋转矩阵；初始载体运动速度的惯性矩阵；载体系从当前时刻与初始时刻之间的旋转矩阵。
Figure GDA0002727001730000034
其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t0时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t0时刻的载体坐标系，
Figure GDA0002727001730000035
和
Figure GDA0002727001730000036
分别为导航坐标系和机体坐标系下从初始t0时刻到t时刻的姿态转换矩阵。
Figure GDA0002727001730000037
和
Figure GDA0002727001730000038
由陀螺和加速度计的信息计算得到。那么，初始对准的任务由求解姿态矩阵
Figure GDA0002727001730000039
的问题转化为求解初始姿态阵
Figure GDA00027270017300000310
的问题。
Figure GDA00027270017300000311
为初始t0时刻的机体系与导航系之间的姿态转换矩阵，
Figure GDA0002727001730000041
是一个常值矩阵。根据李群的微分方程，得到
Figure GDA0002727001730000042
和
Figure GDA0002727001730000043
的表达式。
Figure GDA0002727001730000044
Figure GDA0002727001730000045
式中
Figure GDA0002727001730000046
Figure GDA0002727001730000047
表示导航系和惯性系之间的相对角速率；
Figure GDA0002727001730000048
表示地球自转速率；
Figure GDA0002727001730000049
表示导航系相与地球坐标系之间的相对角速率。
Figure GDA00027270017300000410
为陀螺测得的载体相对于惯性系的角速率。[·]×表示将括号内的矢量转换成反对称矩阵的过程。即
Figure GDA00027270017300000411
由旋转角度矢量到姿态矩阵的指数映射关系得更新矩阵如下
Figure GDA00027270017300000412
Figure GDA00027270017300000413
式中，tM表示经过了M个时间间隔之后的时刻；
Figure GDA00027270017300000414
表示对
Figure GDA00027270017300000415
在采样间隔的积分，
Figure GDA00027270017300000416
采用双子样算法
Figure GDA00027270017300000417
Δθ1和Δθ2是两个相邻采样周期陀螺测量增量。
由于导航系相对于惯性系角度变化速率比较缓慢近似为
Figure GDA00027270017300000418
由定义可知
Figure GDA00027270017300000419
当前时刻的
Figure GDA00027270017300000420
和
Figure GDA00027270017300000421
由公式(8)和(9)迭代计算得到。
Figure GDA00027270017300000422
Figure GDA00027270017300000423
在根据李群姿态描述和惯性导航信息确定
Figure GDA00027270017300000424
和
Figure GDA00027270017300000425
后，只需要确定惯性系姿态转换矩阵
Figure GDA0002727001730000051
便可根据公式(3)实时地求解出导航所需要的姿态矩阵。
惯性导航的速度
Figure GDA0002727001730000052
微分方程为
Figure GDA0002727001730000053
式中，fn表示加速度计测得的比力在导航坐标系下的投影，fb表示比力在载体坐标系下的投影，gn表示地球重力加速度在导航坐标系下的投影，vn表示载体速度在导航坐标系下的投影。
将公式(2)代入公式(10)得
Figure GDA0002727001730000054
公式两边同乘以
Figure GDA0002727001730000055
Figure GDA0002727001730000056
移项得
Figure GDA0002727001730000057
对上式在[0,t]上对其进行积分整理得
Figure GDA0002727001730000058
所以基于李群描述的量测模型为
Figure GDA0002727001730000059
式子中
Figure GDA00027270017300000510
Figure GDA00027270017300000511
GPS、陀螺仪、加速度计的采样频率可知，所以β(t)根据离散化数值算法计算得到。采样频率足够高时，角速率和加速度近似为线性的形式
Figure GDA00027270017300000512
fb＝aft+bf (20)
对角速度积分可得角度的增量
Figure GDA00027270017300000513
Figure GDA0002727001730000061
从上式可得
Figure GDA0002727001730000062
Figure GDA0002727001730000063
同理可得
Figure GDA0002727001730000064
Figure GDA0002727001730000065
使用离散数值的方法，使用采样时间的陀螺和加速度的采样数据进行积分时的近似运算可得β(t)在采样时刻的离散化数值。
Figure GDA0002727001730000066
计算β(t)所用到的物理量
Figure GDA0002727001730000067
和gn可直接得到较为精确，
Figure GDA0002727001730000068
更新频率不高变化几乎可忽略，vn为GPS采集的相对可靠信息，所以β(t)较为精确。而α(t)完全靠IMU信息高频更新获得其中包含了大量的陀螺和加速度的噪声项。下面对α(t)误差项的组成进行分析
Figure GDA0002727001730000069
其中εg为陀螺仪随机误差，εa为加速度计随机误差，k为比例系数。在上式中
Figure GDA0002727001730000071
将公式(28)代入公式(29)得
Figure GDA0002727001730000072
式子中
Figure GDA0002727001730000073
Figure GDA0002727001730000074
Figure GDA0002727001730000075
Figure GDA0002727001730000076
由公式(29)可知α(t)所包含的噪声项是由陀螺仪噪声、加速度噪声、陀螺仪和加速度测量值等多种信息耦合而成。
包含耦合噪声项的李群描述下的系统离散化量测方程为
βk＝Rk(αk+κεa+λεg) (31)
其中βk和αk分别为β(t)和α(t)在tk时刻的观测值。
基于李群描述，本方法直接将旋转矩阵作为状态值进行估计。由于需要估计的
Figure GDA0002727001730000081
为两个惯性坐标系之间旋转矩阵，是一个常值矩阵，所以系统李群形式下的状态方程和量测方程表示为
Rk+1＝Rk (32)
βk＝Rk(αk+δαk) (33)
为了表示简明，将量测矩阵重写为
βk＝Rkαk+Vk (34)
其中，
Vk＝Rkδαk
认为δαk为两个高斯白噪声之和，其协方差矩阵为Pv＝(σ1+σ2)I3，高斯白噪声的均值为零，所以有E{δαk}＝0。噪声和状态量在统计学上相互独立，传统方法中将Vk的均值近似为零，E{Rkδαk}＝0，其中Rk为状态量的真实值。状态相关量测噪声协方差矩阵的表示形式如下
Figure GDA0002727001730000082
在实际的滤波过程中，Rk是一个未知量，传统的与状态相关量测噪声协方差矩阵通常被近似为
Figure GDA0002727001730000083
式中
Figure GDA0002727001730000084
表示姿态矩阵的状态量的估计值。状态量估计值和真实值之间的误差导致了传统的量测噪声协方差矩阵不够精确。假设
Figure GDA0002727001730000085
则Vk的协方差矩阵的真实表示形式为
Figure GDA0002727001730000086
该矩阵可以被拆分为两部分
Figure GDA0002727001730000091
上式简写为
Figure GDA0002727001730000092
公式右侧的第二项为由与状态相关的测量噪声所导致的状态量和噪声项耦合的矩阵表示形式。本发明提出一种状态相关量测噪声协方差矩阵的精确表达形式。
根据定义给出一般形式下李群状态相关噪声协方差矩阵的表示形式，将一般性的结论应用到本发明提出的系统模型上。Rk表示tk时刻状态量的估计值，ξk为李代数形式下的状态误差。ξk和Vk的协方差矩阵分别为Pk和
Figure GDA0002727001730000093
Figure GDA0002727001730000094
的具体表示形式为
Figure GDA0002727001730000095
式中
Figure GDA00027270017300000917
表示克罗内克尔积，矩阵
Figure GDA0002727001730000096
的具体形式为
Figure GDA0002727001730000097
其中Ei＝-[ei]×，i＝1,2,3向量ei，i＝1,2,3是三维欧式空间中的标准基底向量。
Figure GDA0002727001730000098
的精确表达形式的为下面李群形式下的相关噪声项的具体形式。
用Al∈Gm×m表示一个李群，其李代数为a，李代数形式下的状态误差协方差矩阵为
Figure GDA0002727001730000099
Figure GDA00027270017300000910
表示一个零均值白噪声序列，协方差矩阵为
Figure GDA00027270017300000911
假设{ak}和{lk}在统计学上相互独立。有
Figure GDA00027270017300000912
定义如下
vk＝G(ak)lk (41)
其中G(·):a→Al为李代数到李群的映射。vk的协方差矩阵
Figure GDA00027270017300000913
定义如下
Figure GDA00027270017300000914
Figure GDA00027270017300000915
定义为Γ＝[Gk(e1)Gk(e2)···Gk(ei)]，i＝1,2,...,m。
列向量ei为
Figure GDA00027270017300000916
下的标准基底向量，Gk(·)为满足Gk(ei)x＝Gk(x)ei的线性转换函数。
证明：李群表示形式下，状态误差和状态误差协方差矩阵的定义已经给出。公式(41)中的第一项为传统的状态相关噪声协方差矩阵，第二项为包含李群形式下状态误差协方差矩阵和噪声协方差矩阵的耦合项。
Figure GDA0002727001730000101
公式(42)融合了噪声协方差矩阵和状态协方差矩阵，将矩阵维数扩展至mm。
Figure GDA0002727001730000102
式中i＝1,2,...,m
根据李群的同构性原理，通过Gk(·)将状态误差量分解到各个基底上完成了状态误差协方差和噪声协方差的耦合并保证了
Figure GDA0002727001730000103
的对称性。
由上节误差分析可知，假设测量噪声为两个均值为零的高斯白噪声，协方差矩阵分别为σ1I3和σ2I3，由李群性质可得
Figure GDA0002727001730000104
再由李群中矩阵运算的性质计算得
Figure GDA0002727001730000105
将公式(44)和(45)代入(39)可得状态相关噪声
Figure GDA0002727001730000106
的计算公式为
Figure GDA0002727001730000107
使用上述优化的噪声协方差矩阵设计状态相关李群滤波器对公式(31)提出的系统模型进行估计，滤波过程如下：
Figure GDA0002727001730000111
根据以上滤波方法进行递推迭代，求出
Figure GDA0002727001730000112
再根据公式(2)求出
Figure GDA0002727001730000113
从而完成捷联惯导系统初始对准过程。
附图说明
图1基于状态相关李群滤波的动态初始对准过程框图。
图2状态相关李群滤波算法流程图。
图3导航系到机体系变换示意图。
图4初始对准仿真结果图。
图5实验装置示意图。
图6上位机采集页面示意图。
图7初始对准实验结果图。
具体实施方式
本发明是基于状态相关李群滤波的捷联惯性导航系统初试对准设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：
步骤1：捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息
Figure GDA0002727001730000114
和加速度计的输出信息fb等.
步骤2：通过
Figure GDA0002727001730000115
利用公式(3)，(5)，(8)更新计算
Figure GDA0002727001730000116
步骤3：通过陀螺仪测量的数据利用公式(4)，(6)，(9)更新计算
Figure GDA0002727001730000117
步骤4：建立滤波的系统方程，即状态方程和观测方程。由于状态量
Figure GDA0002727001730000118
是常数矩阵，以公式(31)作为滤波的状态方程。利用加速度计和外部辅助信息得到的速度矢量构建与状态量相关的观测方程，方程如公式(32)所示。
步骤5：使用状态相关李群滤波器进行滤波估计
Figure GDA0002727001730000121
滤波过程如公式(47)所示。
步骤6：利用估计得到的
Figure GDA0002727001730000122
计算
Figure GDA0002727001730000123
计算过程如公式(2)所示。由
Figure GDA0002727001730000124
解算出载体的姿态角作为对准结果进行输出。
本发明的有益效果如下：
(1)在以下仿真环境下，对该方法进行仿真实验。
载体处于动态情况下，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：
Figure GDA0002727001730000125
存在各个方向上振荡所导致的线速度：
Figure GDA0002727001730000128
其中，i＝x,y,z，ADx＝0.02m，ADy＝0.03m，ADz＝0.3m；ωDi＝2π/TDi，其中TDx＝7s，TDx＝6s，TDx＝8s；
Figure GDA0002727001730000126
为[0,2π]上服从均匀分布的随机相位。加速度计的常值偏差1×10-4g，随机测量噪声为
Figure GDA0002727001730000127
陀螺仪常值漂移为0.01°/h，随机漂移为0.001°/h。初始经纬度为东经118°，北纬40°。
常数设置：
赤道半径：Re＝6378165.0m；
椭球扁率：e＝3.352e-3；
由万有引力得地球表面重力加速度：g0＝9.78049；
地球自转角速度(单位为rad/s)：7.2921158e-5；
常数：π＝3.1415926；
方法仿真结果如下：
进行了600s仿真，仿真结果如图4所示。以姿态误差角的估计误差作为衡量精对准的指标。航向角误差在200s能够达到10′，俯仰角和横滚角误差在200s降低到了0.3′左右。数值解算的SVD方法在收敛速度上较快，但是最终航向角的对准精度较差，约为30′，不能满足初始对准精度的要求。使用递推估计的滤波方法能够考虑到系统噪声对初始对准的影响精度相对较高。四元数滤波航向角的最终对准精度为24′，精度达到2°以下需要280s。李群滤波航向角最终对准精度为14′左右，精度达到2°以下需要200s，状态相关李群滤波在此基础上精度和收敛速度又有一定提高。通过仿真结果可知李群滤波的对准精度和收敛速度相对于传统的方法均有所提高。仿真结果表明本发明所提出的算法在三轴角度对准上都有比较好的收敛速度和对准精度。
(2)通过真实实验对本发明提出的基于李群滤波的捷联惯导初试系统进行验证。真实试验中，系统装置放置在车上，有人员上下车、开关车门、对车进行晃动等干扰。实验装置如图5所示。实验历时600s，试验地点在北京工业大学羽毛球馆南广场位置。上位导航计算机控制导航系统，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1°、姿态精度达0.05°的实际三轴姿态信息，其上位机采集页面示意图如图6。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。
系统实验结果如下：
截取600s的实际数据，结果如图7所示。以姿态误差角的估计误差作为衡量精对准的指标。本发明提出的方法解算出的姿态角的最终精度要优于传统的四元数滤波和SVD分解法。从图7中可以更直观地看出，四元数滤波和SVD分解法航向角的误差分别大约在110′和150′，本发明提出的状态相关李群算法航向角误差在24′左右，大幅地提高了对准精度。由图7的(b)、(c)可以看出本发明所提出的算法对俯仰和横滚角的精度和稳定性也有一定的提高。结果证明该算法适用于动态条件下的初始对准。

Claims (2)

1.基于状态相关李群滤波的捷联惯性导航初始对准方法，地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；
其特征在于：该方法通过下述流程实现，
(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息
Figure FDA0002727001720000011
和加速度计的输出信息fb；
(2)对采集到的陀螺和加速度计的数据进行处理，应用状态相关李群滤波方法估计姿态矩阵；
描述坐标系变换的旋转矩阵符合李群三维特殊正交群SO(3)的性质，构成了SO(3)群：
Figure FDA0002727001720000012
其中，R对应了旋转矩阵，
Figure FDA0002727001720000013
表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(*)表示矩阵的行列式；根据姿态矩阵的链式法则，从载体坐标系到导航坐标系的姿态矩阵可以分解为三个连续特殊正交阵的乘积形式：导航坐标系从初始时刻到当前时刻的旋转矩阵；初始载体运动速度的惯性矩阵；载体坐标系从当前时刻与初始时刻之间的旋转矩阵；
Figure FDA0002727001720000014
其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t0时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t0时刻的载体坐标系，
Figure FDA0002727001720000015
和
Figure FDA0002727001720000016
分别为导航坐标系和机体坐标系下从初始t0时刻到t时刻的姿态转换矩阵；
Figure FDA0002727001720000017
和
Figure FDA0002727001720000018
由陀螺和加速度计的信息计算得到；那么，初始对准的任务由求解姿态矩阵
Figure FDA0002727001720000019
的问题转化为求解初始姿态阵
Figure FDA00027270017200000110
的问题；
Figure FDA00027270017200000111
为初始t0时刻的机体系与导航坐标系之间的姿态转换矩阵，
Figure FDA0002727001720000021
是一个常值矩阵；根据李群的微分方程，得到
Figure FDA0002727001720000022
和
Figure FDA0002727001720000023
的表达式；
Figure FDA0002727001720000024
Figure FDA0002727001720000025
式中
Figure FDA0002727001720000026
Figure FDA0002727001720000027
表示导航坐标系和惯性系之间的相对角速率；
Figure FDA0002727001720000028
表示地球自转速率；
Figure FDA0002727001720000029
表示导航坐标系相与地球坐标系之间的相对角速率；
Figure FDA00027270017200000210
为陀螺测得的载体相对于惯性系的角速率；[·]×表示将括号内的矢量转换成反对称矩阵的过程；即
Figure FDA00027270017200000211
由旋转角度矢量到姿态矩阵的指数映射关系得更新矩阵如下
Figure FDA00027270017200000212
Figure FDA00027270017200000213
式中，tM表示经过了M个时间间隔之后的时刻；
Figure FDA00027270017200000214
表示对
Figure FDA00027270017200000215
在采样间隔的积分，
Figure FDA00027270017200000216
采用双子样算法
Figure FDA00027270017200000217
Δθ1和Δθ2是两个相邻采样周期陀螺测量增量；
由于导航坐标系相对于惯性系角度变化速率比较缓慢近似为
Figure FDA00027270017200000218
由定义可知
Figure FDA00027270017200000219
当前时刻的
Figure FDA00027270017200000220
和
Figure FDA00027270017200000221
由公式(8)和(9)迭代计算得到；
Figure FDA00027270017200000222
Figure FDA00027270017200000223
在根据李群姿态描述和惯性导航信息确定
Figure FDA00027270017200000224
和
Figure FDA00027270017200000225
后，只需要确定惯性系姿态转换矩阵
Figure FDA0002727001720000031
便可根据公式(3)实时地求解出导航所需要的姿态矩阵；
惯性导航的速度
Figure FDA0002727001720000032
微分方程为
Figure FDA0002727001720000033
式中，fn表示加速度计测得的比力在导航坐标系下的投影，fb表示比力在载体坐标系下的投影，gn表示地球重力加速度在导航坐标系下的投影，vn表示载体速度在导航坐标系下的投影；
将公式(2)代入公式(10)得
Figure FDA0002727001720000034
公式两边同乘以
Figure FDA0002727001720000035
Figure FDA0002727001720000036
移项得
Figure FDA0002727001720000037
对上式在[0,t]上对其进行积分整理得
Figure FDA0002727001720000038
所以基于李群描述的量测模型为
Figure FDA0002727001720000039
式子中
Figure FDA00027270017200000310
Figure FDA00027270017200000311
GPS、陀螺仪、加速度计的采样频率可知，所以β(t)根据离散化数值算法计算得到；采样频率足够高时，角速率和加速度近似为线性的形式
Figure FDA00027270017200000312
fb＝aft+bf (20)
对角速度积分可得角度的增量
Figure FDA00027270017200000313
Figure FDA0002727001720000041
从上式可得
Figure FDA0002727001720000042
Figure FDA0002727001720000043
同理可得
Figure FDA0002727001720000044
Figure FDA0002727001720000045
使用离散数值的方法，使用采样时间的陀螺和加速度的采样数据进行积分时的近似运算可得β(t)在采样时刻的离散化数值；
Figure FDA0002727001720000046
计算β(t)所用到的物理量
Figure FDA0002727001720000047
和gn可直接得到较为精确，
Figure FDA0002727001720000048
更新频率不高变化几乎可忽略，vn为GPS采集的相对可靠信息，所以β(t)较为精确；而α(t)完全靠IMU信息高频更新获得其中包含了大量的陀螺和加速度的噪声项；下面对α(t)误差项的组成进行分析
Figure FDA0002727001720000049
其中εg为陀螺仪随机误差，εa为加速度计随机误差，k为比例系数；在上式中
Figure FDA0002727001720000051
将公式(28)代入公式(29)得
Figure FDA0002727001720000052
式子中
Figure FDA0002727001720000053
Figure FDA0002727001720000054
Figure FDA0002727001720000055
Figure FDA0002727001720000056
由公式(29)可知α(t)所包含的噪声项是由陀螺仪噪声、加速度噪声、陀螺仪和加速度测量值信息耦合而成；
包含耦合噪声项的李群描述下的系统离散化量测方程为
βk＝Rk(αk+κεa+λεg) (31)
其中βk和αk分别为β(t)和α(t)在tk时刻的观测值；
基于李群描述，本方法直接将旋转矩阵作为状态值进行估计；由于需要估计的
Figure FDA0002727001720000061
为两个惯性坐标系之间旋转矩阵，是一个常值矩阵，所以系统李群形式下的状态方程和量测方程表示为
Rk+1＝Rk (32)
βk＝Rk(αk+δαk) (33)
为了表示简明，将量测矩阵重写为
βk＝Rkαk+Vk (34)
其中，
Vk＝Rkδαk
认为δαk为两个高斯白噪声之和，其协方差矩阵为Pv＝(σ1+σ2)I3，高斯白噪声的均值为零，所以有E{δαk}＝0；噪声和状态量在统计学上相互独立，传统方法中将Vk的均值近似为零，E{Rkδαk}＝0，其中Rk为状态量的真实值；状态相关量测噪声协方差矩阵的表示形式如下
Figure FDA0002727001720000062
在实际的滤波过程中，Rk是一个未知量，传统的与状态相关量测噪声协方差矩阵被近似为
Figure FDA0002727001720000063
式中
Figure FDA0002727001720000064
表示姿态矩阵的状态量的估计值；状态量估计值和真实值之间的误差导致了传统的量测噪声协方差矩阵不够精确；假设
Figure FDA0002727001720000065
则Vk的协方差矩阵的真实表示形式为
Figure FDA0002727001720000066
该矩阵被拆分为两部分
Figure FDA0002727001720000071
上式简写为
Figure FDA0002727001720000072
公式右侧的第二项为由与状态相关的测量噪声所导致的状态量和噪声项耦合的矩阵表示形式；本方法提出一种状态相关量测噪声协方差矩阵的精确表达形式；
Rk表示tk时刻状态量的估计值，ξk为李代数形式下的状态误差；ξk和Vk的协方差矩阵分别为Pk和
Figure FDA0002727001720000073
Figure FDA0002727001720000074
的具体表示形式为
Figure FDA0002727001720000075
式中
Figure FDA0002727001720000076
表示克罗内克尔积，矩阵
Figure FDA0002727001720000077
的具体形式为
Figure FDA0002727001720000078
其中Ei＝-[ei]×，i＝1,2,3向量ei，i＝1,2,3是三维欧式空间中的标准基底向量；
Figure FDA0002727001720000079
的精确表达形式的为下面李群形式下的相关噪声项的具体形式；
用Al∈Gm×m表示一个李群，其李代数为a，李代数形式下的状态误差协方差矩阵为
Figure FDA00027270017200000710
Figure FDA00027270017200000711
表示一个零均值白噪声序列，协方差矩阵为
Figure FDA00027270017200000712
假设{ak}和{lk}在统计学上相互独立；有
Figure FDA00027270017200000713
定义如下
vk＝G(ak)lk (41)
其中G(·):a→Al为李代数到李群的映射；vk的协方差矩阵
Figure FDA00027270017200000714
定义如下
Figure FDA00027270017200000715
Figure FDA00027270017200000716
定义为Γ＝[Gk(e1) Gk(e2) ··· Gk(ei)]，i＝1,2,...,m；
列向量ei为
Figure FDA00027270017200000717
下的标准基底向量，Gk(·)为满足Gk(ei)x＝Gk(x)ei的线性转换函数。
2.基于状态相关李群滤波的捷联惯性导航初始对准方法，其特征在于：
公式(41)中的第一项为传统的状态相关噪声协方差矩阵，第二项为包含李群形式下状态误差协方差矩阵和噪声协方差矩阵的耦合项；
Figure FDA0002727001720000081
公式(42)融合了噪声协方差矩阵和状态协方差矩阵，将矩阵维数扩展至mm；
Figure FDA0002727001720000082
式中i＝1,2,...,m
根据李群的同构性原理，通过Gk(·)将状态误差量分解到各个基底上完成了状态误差协方差和噪声协方差的耦合并保证了
Figure FDA0002727001720000083
的对称性；
由误差分析可知，假设测量噪声为两个均值为零的高斯白噪声，协方差矩阵分别为σ1I3和σ2I3，由李群性质可得
Figure FDA0002727001720000084
再由李群中矩阵运算的性质计算得
Figure FDA0002727001720000085
将公式(44)和(45)代入(39)可得状态相关噪声
Figure FDA0002727001720000086
的计算公式为
Figure FDA0002727001720000087
使用上述优化的噪声协方差矩阵设计状态相关李群滤波器对公式(31)提出的系统模型进行估计，滤波过程如下：
Figure FDA0002727001720000088
根据以上滤波方法进行递推迭代，求出
Figure FDA0002727001720000091
再根据公式(2)求出
Figure FDA0002727001720000092
从而完成捷联惯导系统初始对准过程。
CN201910200910.7A 2019-03-18 2019-03-18 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 Active CN109931955B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910200910.7A CN109931955B (zh) 2019-03-18 2019-03-18 基于状态相关李群滤波的捷联惯性导航系统初始对准方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910200910.7A CN109931955B (zh) 2019-03-18 2019-03-18 基于状态相关李群滤波的捷联惯性导航系统初始对准方法

Publications (2)

Publication Number Publication Date
CN109931955A CN109931955A (zh) 2019-06-25
CN109931955B true CN109931955B (zh) 2020-11-27

Family

ID=66987459

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910200910.7A Active CN109931955B (zh) 2019-03-18 2019-03-18 基于状态相关李群滤波的捷联惯性导航系统初始对准方法

Country Status (1)

Country Link
CN (1) CN109931955B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110595503B (zh) * 2019-08-05 2021-01-15 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110926499B (zh) * 2019-10-19 2023-09-01 北京工业大学 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110702143B (zh) * 2019-10-19 2021-07-30 北京工业大学 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN110929402A (zh) * 2019-11-22 2020-03-27 哈尔滨工业大学 一种基于不确定分析的概率地形估计方法
CN112212889A (zh) * 2020-09-16 2021-01-12 北京工业大学 基于特殊正交群最优估计的sins捷联惯性导航系统晃动基座粗对准方法
CN112902950B (zh) * 2021-01-21 2022-10-21 武汉大学 一种面向低速运动载体中mems级imu的初始对准方法
CN113503893A (zh) * 2021-06-02 2021-10-15 北京自动化控制设备研究所 一种动基座惯导系统初始对准算法
CN113532479B (zh) * 2021-08-01 2024-03-29 北京工业大学 基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法
CN118362134A (zh) * 2024-06-20 2024-07-19 清华大学 基于李群建模的综合导航系统设计方法、装置及存储介质

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106871928B (zh) * 2017-01-18 2020-09-25 北京工业大学 基于李群滤波的捷联惯性导航初始对准方法
CN107588771B (zh) * 2017-08-28 2020-08-07 北京工业大学 基于李群描述的捷联惯性导航解算方法

Also Published As

Publication number Publication date
CN109931955A (zh) 2019-06-25

Similar Documents

Publication Publication Date Title
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN106871928B (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN104655131B (zh) 基于istssrckf的惯性导航初始对准方法
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN105737823B (zh) 一种基于五阶ckf的gps/sins/cns组合导航方法
CN105043415B (zh) 基于四元数模型的惯性系自对准方法
CN110595503B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN105371844B (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN109931957B (zh) 基于lgmkf的sins捷联惯性导航系统自对准方法
CN108680186B (zh) 基于重力仪平台的捷联式惯导系统非线性初始对准方法
CN103344260B (zh) 基于rbckf的捷联惯导系统大方位失准角初始对准方法
CN105091907B (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN109269511B (zh) 未知环境下行星着陆的曲线匹配视觉导航方法
CN102508954A (zh) Gps/sins组合导航全数字仿真方法及装置
CN104713559B (zh) 一种高精度sins模拟器的设计方法
CN106840211A (zh) 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN112229421B (zh) 基于李群最优估计的捷联惯性导航晃动基座粗对准方法
CN112325886A (zh) 一种基于重力梯度仪和陀螺仪组合的航天器自主定姿系统
CN110926499B (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法
CN110926465A (zh) 一种mems/gps松组合导航方法
CN113340298A (zh) 一种惯导和双天线gnss外参标定方法
CN106643726B (zh) 一种统一惯性导航解算方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
CN111207773A (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