基于李群状态相关滤波的捷联惯导系统运动初始对准方法

技术领域

本发明公开了一种基于李群描述的捷联惯性导航运动初始对准方法，该方法属于导航方法及应用技术领域。

背景技术

导航是指通过各种手段和方法，确定和控制物体在空间中的姿态、位置、速度等运动参数的过程。它可以应用于飞行器、舰船、自动驾驶、智能机器人等多种领域。惯性导航不依赖任何外部信息，仅利用惯性传感器的输出就能完成导航任务，具有高精度、独立性、可靠性以及保密性的优点，因此具有广泛的应用场景。起初，惯性传感器安装在一个时刻与载体所在水平面保持平行的机械平台上，通常称为平台式惯导系统，这样虽然可以直接将传感器的测量信息投影到导航系，但机械平台体积庞大、维护复杂，且如果平台受到冲击或者震动，会导致传感器测量误差增大，从而影响导航精度，因此很快就被捷联惯导系统替代。捷联惯导系统直接将惯性器件固连在运载体上，具有精度高、干扰能力强、体积和重量更小等优点。

由于省略了物理平台，捷联惯导系统必须配准载体坐标系和导航坐标系统之间的旋转矩阵，这就是捷联惯导系统的初始对准。初始对准为后续的捷联积分解算提供初始条件，并直接影响捷联惯导系统的快速性和准确性。因此研究捷联惯导初始对准具有很重要的现实意义。

按照对准时载体所处的运动状态划分，初始对准可分为静基座对准、晃动对准以及运动对准，后两者常统称为动态对准

目前，主流的初始对准算法使用单位四元数描述模型。然而四元数对准模型除了存在非唯一性问题之外，还存在更难解决的模型非线性问题，若直接使用非线性滤波方法会消耗大量的导航计算机资源，不仅没有提高对准精度，还使得对准时间大幅增加。因此通常需要进行两步对准。在粗对准阶段快速地减小姿态误差角，使对准模型满足线性近似条件；在精对准阶段则主要利用卡尔曼滤波器提高对准精度。两步对准在实际应用中存在很多问题，一方面，在大失准角场景下，四元数两步对准算法不能满足线性化近似条件，使得对准效果大打折扣，而在实际应用中，大失准角场景是广泛存在的，这是一个无法回避的问题。另一方面，动态对准中的噪声类型和统计特性更加复杂，使得四元数两步对准算法的对准精度降低，收敛时间延长。

为了解决上述方法中存在的问题，进一步提高初始对准的精度与时间，本发明借鉴李群的性质和轴角模型及卡尔曼滤波的思想，提出了一种新的李群状态相关卡尔曼滤波，利用李群新推导符合实际情况的李群状态相关初始对准模型；然后，充分利用向量内积、叉乘的性质，以及拉格朗日公式，化简滤波更新方程，在此基础上设计完整的状态相关特殊正交群滤波算法。基于这种误差状态构建的估计模型避免了传统方法中的奇异值问题和非线性问题，同时还解决了传统李群方法中对观测值信息利用不够的问题。仿真实验证明了该算法的可行性，可以作为运动状态下的自对准方法使用。

发明内容

运动是载体最常见的状态，运动状态下的自对准算法具有很高的研究意义与应用价值。由于在运动状态下的初始对准过程中，载体更容易受到外界的各种干扰因素的影响，所以对算法有效性的要求也更高。本发明的目的是为了应对现有运动自对准方法存在的问题：(1)本发明通过李群描述初始姿态矩阵，且可用于大失准角下的运动对准，避免了奇异值分解法的大失准角下的奇异点问题；(2)本发明通过李群误差旋转矩阵的定义和轴角旋转模型，将李群误差旋转矩阵作为状态量并利用轴角变换恒等地映射到误差向量空间，构建了线性的向量空间上的对准模型；并利用乘性卡尔曼滤波进行解算，得到李群误差旋转矩阵的最优估计；最后误差旋转矩阵补偿，便实现了运动状态下的初始对准。通过这种方法，避免了传统四元数方法的非线性近似带来的精度问题。

为了达到上述目的，本发明提供如下技术方案：

基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态自对准方法，其特征在于，该方法通过下述步骤实现：

步骤(1)：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影g

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

本方法的详细描述中坐标系定义如下：

地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；

地心惯性坐标系i系，选取地球中心为原点，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；

导航坐标系n系，本方法中导航坐标系选取为地理坐标系，以载体重心为原点，与东-北-天坐标轴对齐，X轴与东向(E)重合，Y轴与北向(N)重合，Z轴与天向(U)重合；

载体坐标系b系，表示捷联惯性导航系统中惯性传感器的输出所在的坐标系，以载体重心为原点，X轴、Y轴、Z轴分别沿载体横轴指向右、沿纵轴指向前、沿立轴指向上；

初始导航坐标系n(0)系，表示捷联惯性导航系统开机运行时的导航坐标系，并在整个对准过程中相对于惯性空间保持静止；

初始载体坐标系b(0)系，表示捷联惯性导航系统开机运行时的载体坐标系，并在整个对准过程中相对于惯性空间保持静止；

结合李群的性质和捷联惯性导航系统的输出，建立线性的李群乘性卡尔曼滤波初始对准模型：

根据捷联惯性导航系统的特点，可以将运动状态下的对准问题转化为载体的姿态估计问题，姿态变换矩阵代表导航坐标系n系和载体坐标系b系之间的旋转，该矩阵是一个3×3的正交矩阵且行列式等于1，刚好符合李群中的三维特殊正交群SO(3)的性质，构成了三维旋转群SO(3)：

其中，R∈SO(3)表示用来代表姿态变换矩阵的三维旋转群SO(3)中的元素，

运动状态下的对准问题转化为李群描述下载体的姿态变换矩阵R的估计问题；根据基于李群描述的实时姿态矩阵分解链式法则，将待求的姿态矩阵

其中，t表示时间，

根据运动学特性和李群微分方程，姿态矩阵

其中，

将公式(3)、(4)离散化，可以得到姿态矩阵

由公式(2)-(7)可得，

根据捷联惯性导航中IMU和GPS的输出，可以构造出两个速度矢量，且其与基于李群描述的初始姿态矩阵

其中，β和α分别表示载体在n系下构造的速度矢量和b系下的构造的速度矢量，它们可通过下式求得：

其中v

将常值漂移，随机漂移和噪声考虑后，将(10)改写为：

其中

考虑初始姿态矩阵

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

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

由于选择了误差姿态矩阵η作为状态进行估计，所以

式中，

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

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

步骤(3)：根据上述误差状态的对准模型，建立转轴空间上等效姿态滤波模型，并对

为了建立转轴空间上等效姿态滤波模型，将误差模型观测矢量变换为相应的轴角模型变化：

e

基于向量点乘

a·b＝a

由(19)，(17)得向量空间中的等效状态方程和观测方程：

根据李群上的误差定义，有

由于向量自身的叉乘，结果为零向量，

利用

结合公式(16)和(22)，单位转轴n表示为：

综上，用旋转向量表示的系统残差模型为：

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

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

在修正误差向量

在滤波的每一步中将

步骤(4)：求解导航系统所需的姿态矩阵

根据之前步骤中求解得到的姿态变化矩阵

与现有技术相比，本发明具有如下优点和有益效果：

(1)本发明通过李群代替四元数描述初始姿态矩阵，利用角轴模型使得整个滤波模型成为线性模型，避免了奇异值分解法的大失准角下的奇异点问题和传统四元数方法的非线性近似带来的精度问题；

(2)本发明通过李群误差旋转矩阵的定义和轴角旋转模型，利用误差李代数作为状态量进行估计并对误差进行补偿，由于误差状态向量小于一般的状态向量，状态相关误差得以减小，提高了对准精度。

附图说明

图1捷联惯性导航系统装置总体简图。

图2捷联惯性导航系统流程图。

图3轴角新息模型示意图。

图4李群状态相关滤波流程图。

图5李群状态相关滤波仿真实验图。

图6李群状态相关滤波实验结果图。

具体实施方式

本发明是基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态下初始对准方法设计，下面结合本发明系统流程图对本发明的具体实施步骤进行详细的描述：

本发明提供的基于李群乘性卡尔曼滤波的SINS捷联惯性导航系统运动状态下初始对准方法，首先获取传感器实时数据；对采集到的数据进行处理，基于李群微分方程，建立基于李群描述的线性对准系统模型；使用李群乘性卡尔曼滤波算法，估计得到基于李群描述的初始姿态矩阵

步骤1：SINS捷联惯性导航系统进行系统预热准备，启动系统，获得载体所在位置的经度λ、纬度L、当地重力加速度在导航系下的投影g

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

根据捷联惯性导航系统的特点，可以将运动状态下的对准问题转化为载体的姿态估计问题，姿态变换矩阵代表导航坐标系n系和载体坐标系b系之间的旋转，该矩阵是一个3×3的正交矩阵且行列式等于1，恰好符合李群中的三维特殊正交群SO(3)的性质，构成了三维旋转群SO(3)：

其中，R∈SO(3)表示用来代表姿态变换矩阵的三维旋转群SO(3)中的元素，

运动状态下的对准问题转化为李群描述下载体的姿态变换矩阵R的估计问题；根据基于李群描述的实时姿态矩阵分解链式法则，将待求的姿态矩阵

其中，t表示时间，

根据运动学特性和李群微分方程，姿态矩阵

其中，

将公式(30)、(31)离散化，可以得到姿态矩阵

由公式(29)-(34)可得，

根据捷联惯性导航中IMU和GPS的输出，可以构造出两个速度矢量，且其与基于李群描述的初始姿态矩阵

其中，β和α分别表示载体在n系下构造的速度矢量和b系下的构造的速度矢量，它们可通过下式求得：

其中v

将常值漂移，随机漂移和噪声考虑后，将(37)改写为：

其中

考虑初始姿态矩阵

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

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

由于选择了误差姿态向量

式中，

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

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

步骤3：根据上述误差状态的对准模型，根据角轴将误差旋转群的元素映射到向量空间，并利用乘性卡尔曼滤波对其进行估计，再将误差补偿求得真实姿态R。

建立转轴空间上等效姿态滤波模型，将误差模型观测矢量变换为相应的轴角模型变化：

e

其中e

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

对(47)向量部分进行简化运算：有：

由于向量自身的叉乘，结果为零向量，所以，

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

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

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

在修正误差状态

在滤波的每一步中将

步骤4：求解姿态矩阵

求解导航系统所需的姿态矩阵

根据之前步骤中求解得到的姿态变化矩阵

本发明的有益效果如下：

(1)在以下的仿真条件下，对该方法进行仿真实验：

步骤(1)中，模拟运动状态下载体收到风浪影响，其航向角ψ、俯仰角θ、横滚角γ作周期变化，姿态变化情况如下：

运动载体的速度变化如下：

步骤(1)中，初始地理位置：东经118°，北纬40°；

步骤(1)中，传感器输出频率为100Hz；

步骤(1)中，陀螺仪漂移：三个方向轴上的陀螺常值漂移为0.01°/h，随机漂移为0.005°/h；

步骤(1)中，加速度计零位偏置：三个方向轴上的加速度计常值偏置为1.5×10

步骤(2)中，地球自转角速率7.2921158e

步骤(2)中，时间间隔T为0.02s；

步骤(3)中，李群最优估计算法初始值

方法仿真结果如下：

进行了600s仿真，以姿态角的估计误差作为衡量指标，仿真结果如图(3)所示。从图中可以看出，俯仰姿态在90s左右完成对准，收敛到0.18′；横滚姿态在100s左右完成对准，收敛到0.4′；航向姿态在85s左右完成对准，收敛到1.68′。由仿真结果可知，本方法可以快速有效的完成运动状态下对准任务。

本发明采用李群描述代替传统四元数描述实现对SINS姿态变换的计算，利用李群和捷联惯导系统的特性进行了取代四元数描述方法的新系统分析，并根据李群中特殊正交群的性质定义的误差旋转矩阵，建立了基于误差状态的线性初始对准模型，利用等效旋转角轴模型将误差映射到向量空间进行滤波。最后将误差补偿，得到姿态矩阵的最优估计，从而实现了SINS的一步直接自对准；本发明利用向量运算性质及拉格朗日公式等将状态量映射到向量空间进行滤波，避免了近似问题，有效提高了对准精度。

以上所述仅是本发明的优选实施方式，并不用于限制本发明。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。