[发明专利]基于状态相关李群滤波的捷联惯性导航系统初始对准方法

权利要求书

1.基于状态相关李群滤波的捷联惯性导航初始对准方法，地球坐标系e系，原点选取地球中心，X轴位于赤道平面内，从地心指向载体所在点经线，Z轴沿地球自转轴方向，随地球自转而转动，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，是在粗对准起始时刻将地球坐标系e系惯性凝固后形成的坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在该坐标系下进行，原点位于舰载机重心，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；

其特征在于：该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得载体所在位置的经度λ、纬度L的基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息和加速度计的输出信息f^b；

(2)对采集到的陀螺和加速度计的数据进行处理，应用状态相关李群滤波方法估计姿态矩阵；

描述坐标系变换的旋转矩阵符合李群三维特殊正交群SO(3)的性质，构成了SO(3)群：

其中，R对应了旋转矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(*)表示矩阵的行列式；根据姿态矩阵的链式法则，从载体坐标系到导航坐标系的姿态矩阵可以分解为三个连续特殊正交阵的乘积形式：导航坐标系从初始时刻到当前时刻的旋转矩阵；初始载体运动速度的惯性矩阵；载体坐标系从当前时刻与初始时刻之间的旋转矩阵；

其中，t表示时间变量，n(t)表示t时刻的导航坐标系，n(0)表示t₀时刻的导航坐标系，b(t)表示t时刻的载体坐标系，b(0)表示t₀时刻的载体坐标系，和分别为导航坐标系和机体坐标系下从初始t₀时刻到t时刻的姿态转换矩阵；和由陀螺和加速度计的信息计算得到；那么，初始对准的任务由求解姿态矩阵的问题转化为求解初始姿态阵的问题；为初始t₀时刻的机体系与导航坐标系之间的姿态转换矩阵，是一个常值矩阵；根据李群的微分方程，得到和的表达式；

式中表示导航坐标系和惯性系之间的相对角速率；表示地球自转速率；表示导航坐标系相与地球坐标系之间的相对角速率；为陀螺测得的载体相对于惯性系的角速率；[·]×表示将括号内的矢量转换成反对称矩阵的过程；即

由旋转角度矢量到姿态矩阵的指数映射关系得更新矩阵如下

式中，t_M表示经过了M个时间间隔之后的时刻；表示对在采样间隔的积分，采用双子样算法Δθ₁和Δθ₂是两个相邻采样周期陀螺测量增量；

由于导航坐标系相对于惯性系角度变化速率比较缓慢近似为

由定义可知

当前时刻的和由公式(8)和(9)迭代计算得到；

在根据李群姿态描述和惯性导航信息确定和后，只需要确定惯性系姿态转换矩阵便可根据公式(3)实时地求解出导航所需要的姿态矩阵；

惯性导航的速度微分方程为

式中，fⁿ表示加速度计测得的比力在导航坐标系下的投影，f^b表示比力在载体坐标系下的投影，gⁿ表示地球重力加速度在导航坐标系下的投影，vⁿ表示载体速度在导航坐标系下的投影；

将公式(2)代入公式(10)得

公式两边同乘以

移项得

对上式在[0,t]上对其进行积分整理得

所以基于李群描述的量测模型为

式子中

GPS、陀螺仪、加速度计的采样频率可知，所以β(t)根据离散化数值算法计算得到；采样频率足够高时，角速率和加速度近似为线性的形式

f^b＝a_ft+b_f (20)

对角速度积分可得角度的增量

从上式可得

同理可得

使用离散数值的方法，使用采样时间的陀螺和加速度的采样数据进行积分时的近似运算可得β(t)在采样时刻的离散化数值；

计算β(t)所用到的物理量和gⁿ可直接得到较为精确，更新频率不高变化几乎可忽略，vⁿ为GPS采集的相对可靠信息，所以β(t)较为精确；而α(t)完全靠IMU信息高频更新获得其中包含了大量的陀螺和加速度的噪声项；下面对α(t)误差项的组成进行分析

其中ε_g为陀螺仪随机误差，ε_a为加速度计随机误差，k为比例系数；在上式中

将公式(28)代入公式(29)得

式子中

由公式(29)可知α(t)所包含的噪声项是由陀螺仪噪声、加速度噪声、陀螺仪和加速度测量值信息耦合而成；

包含耦合噪声项的李群描述下的系统离散化量测方程为

β_k＝R_k(α_k+κε_a+λε_g) (31)

其中β_k和α_k分别为β(t)和α(t)在t_k时刻的观测值；

基于李群描述，本方法直接将旋转矩阵作为状态值进行估计；由于需要估计的为两个惯性坐标系之间旋转矩阵，是一个常值矩阵，所以系统李群形式下的状态方程和量测方程表示为

R_k+1＝R_k (32)

β_k＝R_k(α_k+δα_k) (33)

为了表示简明，将量测矩阵重写为

β_k＝R_kα_k+V_k (34)

其中，

V_k＝R_kδα_k

认为δα_k为两个高斯白噪声之和，其协方差矩阵为P^v＝(σ₁+σ₂)I₃，高斯白噪声的均值为零，所以有E{δα_k}＝0；噪声和状态量在统计学上相互独立，传统方法中将V_k的均值近似为零，E{R_kδα_k}＝0，其中R_k为状态量的真实值；状态相关量测噪声协方差矩阵的表示形式如下

在实际的滤波过程中，R_k是一个未知量，传统的与状态相关量测噪声协方差矩阵被近似为

式中表示姿态矩阵的状态量的估计值；状态量估计值和真实值之间的误差导致了传统的量测噪声协方差矩阵不够精确；假设则V_k的协方差矩阵的真实表示形式为

该矩阵被拆分为两部分

上式简写为

公式右侧的第二项为由与状态相关的测量噪声所导致的状态量和噪声项耦合的矩阵表示形式；本方法提出一种状态相关量测噪声协方差矩阵的精确表达形式；

R_k表示t_k时刻状态量的估计值，ξ_k为李代数形式下的状态误差；ξ_k和V_k的协方差矩阵分别为P_k和的具体表示形式为

式中表示克罗内克尔积，矩阵的具体形式为其中E_i＝-[e_i]×，i＝1,2,3向量e_i，i＝1,2,3是三维欧式空间中的标准基底向量；的精确表达形式的为下面李群形式下的相关噪声项的具体形式；

用A_l∈G^m×m表示一个李群，其李代数为a，李代数形式下的状态误差协方差矩阵为表示一个零均值白噪声序列，协方差矩阵为假设{a_k}和{l_k}在统计学上相互独立；有定义如下

v_k＝G(a_k)l_k (41)

其中G(·):a→A_l为李代数到李群的映射；v_k的协方差矩阵定义如下

定义为Γ＝[G_k(e₁) G_k(e₂) ··· G_k(e_i)]，i＝1,2,...,m；

列向量e_i为下的标准基底向量，G_k(·)为满足G_k(e_i)x＝G_k(x)e_i的线性转换函数。

2.基于状态相关李群滤波的捷联惯性导航初始对准方法，其特征在于：

公式(41)中的第一项为传统的状态相关噪声协方差矩阵，第二项为包含李群形式下状态误差协方差矩阵和噪声协方差矩阵的耦合项；

公式(42)融合了噪声协方差矩阵和状态协方差矩阵，将矩阵维数扩展至mm；

式中i＝1,2,...,m

根据李群的同构性原理，通过G_k(·)将状态误差量分解到各个基底上完成了状态误差协方差和噪声协方差的耦合并保证了的对称性；

由误差分析可知，假设测量噪声为两个均值为零的高斯白噪声，协方差矩阵分别为σ₁I₃和σ₂I₃，由李群性质可得

再由李群中矩阵运算的性质计算得

将公式(44)和(45)代入(39)可得状态相关噪声的计算公式为

使用上述优化的噪声协方差矩阵设计状态相关李群滤波器对公式(31)提出的系统模型进行估计，滤波过程如下：

根据以上滤波方法进行递推迭代，求出再根据公式(2)求出从而完成捷联惯导系统初始对准过程。