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

摘要

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

主权项

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的线性转换函数。