[发明专利]基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法

权利要求书

1.基于李群滤波的捷联惯导系统运动状态初始对准方法，该方法通过下述步骤实现：

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

步骤(2)：初始对准获得准确的姿态信息，以用一个3×3的正交矩阵来表示，称为姿态矩阵；此姿态矩阵表示的是t时刻从载体坐标系b系到导航坐标系n系的正交旋转矩阵；该正交旋转矩阵符合李群的特殊正交群SO(n)的性质，构成三维旋转群SO(3)：

故姿态矩阵用李群中的三维特殊正交群来表示，记作在时间域下对分解，得到三个三维特殊正交群的乘积：

其中R表示三维特殊正交群的元素，表示t时刻n0系到n系的地球运动的旋转矩阵,是初始时刻b0系到n0系的初始旋转矩阵，表示t时刻b系到b0系的载体运动的旋转矩阵；

而和可以通过下列计算求得：

其中和是Δt期间的角速度积分项，所以和通过迭代(5)和(6)更新；为了得到还需要估计初始常值姿态矩阵利用IMU和GPS的输出构造出两个向量，并与有如下关系：

将时变特殊正交群中的元素的估计问题转化为时不变特殊正交群中的元素的估计问题；且由于的大小不随时间变化，根据卡尔曼滤波状态估计理论，改写上下角标，并考虑常值漂移，随机漂移和噪声综合影响构成的误差项Δα，将α改写为包含误差项的传感器输出α_m＝α+Δα，可以得到李群描述下的初始对准模型：

运用李群上真值与估计值之间误差旋转矩阵的定义，定义误差状态如下：

将式(11)带入式(10)中，原状态转换为误差状态，状态方程为：

η_k＝η_k-1 (12)

由于选择了误差姿态矩阵η作为状态进行估计，所以不再表示状态的估计值，而是新状态η的一个关联值，表示为参考值R_ref；根据式(11)，真值R、参考值R_ref和状态η之间的关系可以重写为:

R＝ηR_ref (13)

式中，R＝ηR_ref表示真实姿态R等于估计误差状态η乘以参考值R_ref；R_ref是在离散滤波每个时刻的最后一步进行校正的；R，η和R_ref都是SO(3)中的元素；

利用式(10)中的观测方程，考虑式(13)，基于李群误差状态η，得到如下观测方程：

β＝R(α_m-Δα)＝ηe_ref(α_m-Δα) (14)

重构的基于误差状态的李群对准模型为：

步骤(3)：基于步骤2，建立代数空间上等效滤波模型，并对η_k进行最优估计，再利用η_k和R_ref的关系更新R_ref：

为了建立代数空间上的等效滤波模型，将误差特殊正交群，同时也是正交矩阵的η用下列凯莱变换进行群空间到代数空间的转换：

是η在误差代数空间上的投影；其中符号(·×)表示将一个三维向量转换成一个反对称矩阵的运算，运算规则如下：

将式(16)代入式(15)，得到误差向量空间中的等效状态方程和观测方程：

对式(19)进行化简，两边左乘得到：

化简式(20)，得到误差代数空间的观测方程：

由于表示真值与估计值之间的误差代数估计值，且其高阶次项在短时间过滤间隔△t内变化不大，式(21)近似为：

综上，代数空间上的等效滤波模型为：

对式(23)使用乘性卡尔曼滤波器进行滤波，根据式(23)参数设置如下：

将这些参数带入乘性卡尔曼滤波器中，并综合考虑等效误差代数状态和原误差特殊正交群群状态，得到完整的滤波方程：

在修正误差状态后，姿态参考值R_ref仍然保留其上一步的值，因此不再表示最优估计；必须将更新信息从误差状态移动到参考值R_rEf；故最后考虑R_ref的更新并将误差归零：

在滤波的每一步中将作为的估计值并结合式(2)、式(3)和式(4)，即得到每一离散时刻对姿态矩阵的最优估计。