[发明专利]基于李群最优估计的捷联惯性导航晃动基座粗对准方法

权利要求书

1.基于李群最优估计的捷联惯性导航晃动基座粗对准方法，其特征在于，该方法通过下述步骤实现：

步骤(1)：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影gⁿ基本信息，采集惯性测量单元IMU中陀螺仪输出的载体系相对于惯性系的旋转角速率信息在载体系的投影和加速度计输出的载体系加速度信息f^b；

步骤(2)：对采集到的陀螺仪和加速度计的数据进行预处理，基于李群微分方程，建立基于李群描述的线性晃动基座自对准系统模型：

基于李群微分方程，建立基于李群描述的线性自对准系统模型：

根据SINS捷联惯性导航系统原理，SINS晃动基座自对准问题转化为姿态估计问题，姿态变换为两个坐标系之间的旋转变换，导航的姿态矩阵用一个3×3的正交变换矩阵表示；该正交变换矩阵符合李群的特殊正交群SO(n)的性质，构成三维旋转群SO(3)：

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

晃动基座自对准姿态估计问题转化为对基于李群描述的姿态矩阵R的求解问题；根据基于李群描述的姿态矩阵的链式法则，将导航姿态矩阵分解为三个矩阵的乘积形式：

其中，t表示时间变量，表示当前载体系相对于当前导航系的姿态矩阵，表示初始导航系相对于当前导航系的姿态矩阵，初始姿态矩阵表示初始载体系相对于初始导航系的姿态矩阵，表示当前载体系相对于初始载体系的姿态矩阵；

根据李群微分方程，姿态矩阵和随时间变化更新过程为：

其中，表示初始载体系相对于当前载体系的姿态矩阵，表示导航系相对于惯性系的旋转角速率在导航系的投影，在晃动基座条件下其等于地球自转角速率L表示当地纬度，表示陀螺仪输出的载体系相对于惯性系的旋转角速率在载体系的投影，符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

由公式(2)-(5)可知，和由IMU传感器数据实时计算得到，而表示初始时刻的姿态矩阵，其不随时间变化；因此，SINS自对准过程中姿态矩阵的求解问题，转化为对基于李群描述的初始姿态矩阵的求解问题；

根据捷联惯性导航晃动基座粗对准原理以及公式(2)，基于李群描述的晃动基座粗对准模型可以写成如下形式：

其中，Vⁿ(t)和V^b(t)分别表示载体在n系下的速度和b系下的速度，Vⁿ(t)和V^b(t)可通过下式求得：

并且由于初始旋转矩阵是常数矩阵，由此晃动基座粗对准模型写为如下形式：

其中R(t_k)表示t_k时刻的

步骤(3)：根据李代数的物理定义求解误差李代数，并利用李群与李代数之间的指数映射对R(t_k)进行估计；

根据旋转矩阵的定义和公式(9)，Vⁿ(t)和R(t_k)V^b(t)是n系下的同一个矢量，向量积为0；然而，由于误差，估计的旋转矩阵与实际的旋转矩阵R(t_k)之间存在偏差，这导致估计的旋转矩阵是b系到n′系的姿态矩阵，既：

根据链式法则，为补偿估计的旋转矩阵，我们只需要计算n′系与n系之间的旋转关系，也就是

根据罗德里格斯的旋转公式：

R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa^T+(sinθ)a_×       (11)

任意的旋转矩阵可以通过起对应的旋转李代数φ求得，并且任意旋转李代数(旋转向量)φ由转角θ以及转轴a通过下式求得：

φ＝θa                   (12)

根据坐标系变换的基本原理，两坐标系之间的旋转关系等价于两个系下同一矢量的旋转关系，由此n′系与n系之间的旋转关系，也就是等价与求解观测矢量Vⁿ(t)和预测矢量之间的旋转关系，因此根据式(11)和式(12)只要确定Vⁿ(t)和之间的转轴和转角确定Vⁿ(t)和之间的转轴和转角以及旋转关系；

转轴a通过对单位化来求得：

转角θ则通过下式求得：

于是误差李代数写成：

于是李群最优估计的更新方程可建立如下：

2.根据权利要求1所述的基于李群描述的捷联惯性导航动基座初始对准方法，其特征在于，步骤(3)中，根据李代数的物理定义求解误差李代数，并利用李群与李代数之间的指数映射对R(t_k)进行估计；

步骤(3.1)通过用李群的左乘不变性以及李群与李代数之间的指数映射分离状态相关噪声的方式为：

根据旋转矩阵的定义和公式(9)，Vⁿ(t)和R(t_k)V^b(t)是n系下的同一个矢量，其向量积为0；然而，由于误差，估计的旋转矩阵与实际的旋转矩阵R(t_k)之间存在偏差，这导致估计的旋转矩阵是b系到n′系的姿态矩阵：

根据链式法则，为补偿估计的旋转矩阵，只需要计算n′系与n系之间的旋转关系，也就是

根据罗德里格斯的旋转公式：

R＝exp(φ)＝exp(θa)＝(cosθ)I+(1-cosθ)aa^T+(sinθ)a_×      (18)

任意的旋转矩阵通过起对应的旋转李代数φ求得，并且任意旋转李代数φ由转角θ以及转轴a通过下式求得：

φ＝θa                  (19)

根据坐标系变换的基本原理，两坐标系之间的旋转关系等价于两个系下同一矢量的旋转关系，由此n′系与n系之间的旋转关系，也就是等价与求解观测矢量Vⁿ(t)和预测矢量之间的旋转关系，因此根据式(18)和式(19)只要确定Vⁿ(t)和之间的转轴和转角就确定Vⁿ(t)和之间的转轴和转角以及旋转关系；

由此转轴a通过对单位化来求得：

转角θ则通过下式求得：

于是误差李代数写成：

于是李群最优估计的更新方程可建立如下：