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

权利要求书

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)对采集到的陀螺和加速度计的数据进行处理，应用李群滤波方法解算姿态矩阵；

将初始对准转化为姿态估计的问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态表示用一个3×3的正交变换矩阵来表示；导航的正交变换矩阵符合李群3维特殊正交群SO(3)的性质，构成了SO(3)群：

其中，任意转动群R∈SO(3)对应了特定的导航姿态矩阵，表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式；

姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，将姿态矩阵分解为三个部分，姿态矩阵的分解形式如下：

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

取地理坐标系为导航坐标系，惯性导航的基本方程，载体速度微分方程表示为：

其中，vⁿ表示相对于地球的载体速度；fⁿ表示为比力在导航坐标系下的投影；f^b表示为比力在载体坐标系下的投影，由加速度计测量得到；表示为地球坐标系相对于惯性坐标系的角速率；表示为导航坐标系相对于地球坐标系的角速率；gⁿ表示重力；

将式(2)代入式(3)得：

上式两边同时左乘姿态转换矩阵则有：

上式经整理，得到：

对式(5)的两边进行积分，得到：

式(5)的左边展开为：

其中，vⁿ(0)为初始t₀时刻的速度；

将式(8)和式(7)带入式(6)得：

简化表示为：

式(10)就表示为速率方程在惯性坐标系的积分形式，将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题；

式(10)-(12)是关于初始姿态阵的数学方程；α(t)、β(t)表示如上，由加速度计和陀螺的输出计算得到；

这样给出的是α(t)、β(t)实现的连续形式，通过α(t)、β(t)对应的积分迭代算法解算α和β具体的值，选择双子样旋转矢量算法对α(t)、β(t)进行积分迭代计算；

矢量α(t)近似为：

对式(13)右边的积分部分采用双子样旋转矢量法进行计算：

式(14)带入到式(13)简化得：

式(12)右边的最后一项为：

假设速度在[t_k,t_k+1]内呈线性变化，式(12)右边的倒数第二项近似为：

把式(16)和式(17)代入式(12)，化简得：

根据式(10)-式(18)，建立起系统的观测方程：

β_n＝R_nα_n+Q_v (19)

Q_v为系统观测噪声协方差阵；观测方程是由实测数据运算所得，存在误差项；

由于将求解姿态矩阵的问题转化为求解初始姿态阵的问题，在整个初始对准过程中为常值，建立系统状态方程如下：

R_n＝R_n-1 (20)

将姿态矩阵的求解问题转化为初始时刻惯性坐标系下的求解问题，建立起了具有李群结构的系统方程，表示为：

采用李群滤波方式，切合系统整体结构，大量减少计算误差，以最小均方误差为估计准则，快速有效地估计出系统姿态矩阵；

设计李群滤波器精确估计两个惯性系之间的关系，进而得到捷联姿态矩阵，完成初始对准；

建立系统的李群滤波方程：

其中，Q_w为系统状态噪声协方差阵，由于在整个初始对准过程中为常值，Q_w＝0_3×3；Q_v为系统观测噪声协方差阵；H_ξ为系统量测矩阵，H_ξ＝[α×]；为李群滤波一步预测估值；P_n为误差协方差矩阵；Δx为系统偏差量，以李群结构更新姿态阵；

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