CN107588771B - 基于李群描述的捷联惯性导航解算方法 - Google Patents

Abstract

本发明公开了基于李群描述的捷联惯性导航解算方法，采用李群描述代替传统四元数算法中的四元数描述进行捷联解算，将姿态矩阵和载体速度构建成SE(3)群，通过对惯性敏感器件采集到的数据积分用来对SE(3)群进行迭代更新。本发明直接对姿态矩阵进行计算，可以有效避免传统四元数捷联解算过程中由于四元数描述而产生的非唯一性问题和归一化计算过程，并且省略了传统四元数捷联解算过程中四元数和姿态矩阵之间相互转换的计算过程，避免了转换计算所带来的不可避免的误差，在确保解算精度的同时减少了计算量，在实际工程中具有良好的应用前景。

Description

基于李群描述的捷联惯性导航解算方法

技术领域

本发明提出了一种基于李群描述的捷联惯性导航解算方法，该方法属于导航方法及应用技术领域。

背景技术

所谓导航，就是正确地引导载体沿着预定的航线、以要求的精度、在指定的时间内将载体引导至目的地的过程。惯性导航系统根据自身传感器的输出，以牛顿第二定律为理论基础，对载体的各项导航参数进行解算。它是一种自主式的导航系统，在工作时不依靠外界信息，也不向外界辐射任何能量，隐蔽性好、抗扰性强，能够全天时、全天候为载体提供完备的运动信息。

早期的惯导系统以平台惯导为主，随着惯性器件的成熟和计算机技术的发展，上世纪60年代开始出现了惯性器件与载体直接固联的捷联惯导系统。与平台惯导相比，捷联惯导系统省去了复杂的实体稳定平台，具有成本低、体积小、重量轻、可靠性高等优点。近年来，捷联惯导系统日趋成熟，精度逐步提高，应用范围也逐渐扩大。捷联式惯性导航技术将陀螺仪和加速度计直接安装在载体上，得到载体系下的加速度和角速度，通过导航计算机将测得的数据转换至导航坐标系完成导航，它不需要实体的稳定平台，成本低、体积小、重量轻、可靠性高。

捷联式惯性导航系统进入导航任务后，通过采集加速度计和陀螺仪这类惯性敏感器件的输出信息量，以初始对准得到的导航信息为基准，进行更新迭代计算，得到当前的载体导航信息。传统的捷联解算方法使用四元数来计算载体姿态信息，四元数的表示方法弥补了欧拉角的不足，计算过程中不存在奇异点的问题，但是四元数存在非唯一性，每一组姿态角对应不止一个四元数，并且四元数描述方式在计算中需要不断向姿态阵转换和归一化，这难以避免的存在计算误差，导致姿态解算存在偏差。

针对上述问题，本发明用李群描述代替四元数描述实现捷联惯导的解算过程，将姿态信息和速度信息构造成符合SE(3)群的矩阵进行迭代更新。由于SE(3)群是紧的，该描述方式有效避免了姿态解算中的奇异值问题和非唯一问题。用李群描述可以直接得到载体的姿态矩阵，不需要转化过程，相比于四元数描述计算更加简单。在计算过程中不需要归一化处理，并且没有精度上的损失。

发明内容

基于李群描述的捷联惯性导航解算方法是应用李群替代传统的四元数方法实现捷联惯导的解算过程，将姿态矩阵和载体速度构造SE(3)群，通过惯性元件的积分计算来对SE(3)群进行迭代更新，求出下一时刻的姿态矩阵和速度信息。本发明能实时的反映载体在运动过程中的姿态和速度的变化，确保精度的同时减少了计算量，在实际工程中具有良好的应用前景。

本方法的详细描述中坐标系定义如下：地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，原点选取地球中心，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在下述的坐标系下进行，位于舰载机重心为原点，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；

根据现有的捷联惯性导航解算方法中的问题，提出基于李群的捷联惯性导航解算方法；

为实现方法流程，采用的技术方案为基于李群描述的捷联惯性导航解算方法，该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得初始对准得到的载体所在位置的经度λ、纬度L，姿态航向角H，俯仰角P，横滚角R，以及载体东向速度v^e，北向速度vⁿ，天向速度v^u基本信息，采集惯性测量单元IMU中陀螺的输出角度信息

和加速度计的输出信息f^b；

(2)对采集到的陀螺和加速度计的数据进行处理，应用李群方法解算姿态矩阵与机体在n系下的速度；

将导航的姿态矩阵和机体在n系下的速度用一个4×4的正交变换矩阵来表示；该矩阵符合李群的特殊欧式群SE(3)的性质，构成了SE(3)群：

其中，R∈SO(3)对应了特定的导航姿态矩阵，

表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式，T∈SE(3)对应了包括姿态速度的变换矩阵，

表示4×4的向量空间，t表示平动矢量，

表示3×1的向量空间；

机体姿态与速度位置的求解问题转化为对变换矩阵的更新问题。根据李群的微分方程：

其中，ξ是一个六维向量，前三维为平移记做ρ，即速度信息，由三轴加速度计能够测量，后三维为旋转，记做φ，即角速度信息，由三轴陀螺仪可以测量，符号^是将六维向量转换成四维矩阵的运算，运算法则如下：

其中φ_×表示将三维向量转换成一个反对称矩阵的运算，运算规则如下：

在实际解算中，需要将李群微分方程离散化之后，再进行迭代更新，离散化结果如下：

T_k+1＝T_k exp(ξ^) (6)

其中T_k是k时刻的变换矩阵，exp(ξ^)是一个矩阵的指数，将矩阵ξ^分块，先计算exp(φ_×)部分，对于任意矩阵A的指数写成一个泰勒展开：

对于exp(φ_×)部分，也按照这种方式进行展开：

由于φ是三维向量，定义该三维向量的模值和方向分别记做θ和a，即φ＝θa，a是一个长度为1的方向向量，对于a_×，有以下两条性质：

a_×a_×＝aa^T-I (9)

a_×a_×a_×＝-a_× (10)

根据(9)和(10)式，将(8)式展开计算：

最后得到式(11)：

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

式(11)与表示旋转的罗德里格斯公式相似，即exp(θa_×)是一个旋转矩阵，该旋转矩阵代表k时刻的姿态矩阵R_k与k+1时刻的姿态矩阵R_k+1之间的转动关系，记做ΔR，即

R_k+1＝R_k exp(φ_×)＝R_kΔR (12)

计算平动部分ρ的指数形式，平动部分对应的是k时刻至k+1时刻的速度变化量。由于速度信息由加速度计提供，得到的速度信息是在k+1时刻机体坐标系下的信息。在计算载体位置时，需要用到的是导航坐标系，即地理坐标系下的速度信息，故需在指数计算的过程中对速度信息左乘ΔR来调整。即平动部分ρ的指数形式为ΔRρ。综上得到exp(ξ^)的展开形式：

则变换矩阵的迭代更新方程为：

所以在知道载体的初始姿态位速信息，之后根据加速度计和陀螺仪的信息，利用变换矩阵的迭代更新方程，就计算出之后每个时刻的载体导航信息。

采用李群描述代替传统的四元数描述进行捷联解算，避免了四元数向姿态矩阵转换的复杂计算，并且变换矩阵和载体导航信息是一一对应的关系，避免了四元数表示的非唯一性问题。

附图说明

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

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

图3：导航坐标系到机体坐标系的一般运动；

图4：李群描述的捷联惯性导航解算方法流程图；

图5：仿真数据结果图；

图6：实验数据结果图；

图7：上位机采集真实导航信息页面示意图；

具体实施方式

本发明是基于李群描述的捷联惯性导航系统设计，下面结合本发明系统流程图，对本发明的具体实施步骤进行详细的描述：

步骤1：系统准备阶段，导航系统进行初始对准过程，获取载体的初始位置经度λ、纬度L、姿态航向角姿态航向角H，俯仰角P，横滚角R，姿态矩阵

以及载体东向速度v^e，北向速度vⁿ，天向速度v^u基本信息；

步骤2：构造k＝0时刻的SE(3)阵：

其中

采集惯性测量单元IMU中陀螺的输出角速度信息

和加速度计的输出信息f^b；

步骤3：利用初始时刻的导航信息，计算导航系相对于惯性系的旋转角速度在机体系下的投影

计算公式如下：

其中，WIE是地球自转角速度，R_n是地球半短轴长度，R_e是地球半长轴长度；由此可得到机体系相对于导航系的角速度在机体系下的投影

步骤4：构造六维矢量ξ：

其中，

是这一个采样周期比力对时间的积分，

是这一个采样周期

对时间的积分；

步骤5：根据(6)和(13)计算出k＝1时刻的变换矩阵：

步骤6：去除重力加速度和有害加速度对速度的影响：

其中gⁿ是重力加速度在导航系下的投影，g是当地重力加速度量纲，v(k+1)即k+1时刻的载体速度信息；利用该速度信息，更新k+1时刻载体的经纬度：

根据求得的k+1时刻姿态矩阵

解算出姿态角P、R、H；至此，k+1时刻所有导航信息均已解算完成；

步骤7：以k+1时刻的信息为初始信息，重复步骤2至步骤6。

本发明的有益效果如下：

(1)在以下仿真环境下，对该方法进行仿真实验：

模拟飞机以100(m/s)的速度飞行，姿态角随机变化；

初始地理位置：东经118°，北纬32°；

陀螺漂移：三个方向轴上的陀螺常值漂移为0.2°/h，随机漂移为0.05°/h

加速度计零偏：三个方向轴上的加速度计常值偏置为

随机偏置为

常数设置

地球半径：EARTH_RADII＝6.378165e+6(米)；

圆周率：PAI＝3.141592653589798；

地球自转角速度：WIE＝15.041088*PAI/180.0/3600.0(弧度/秒)；

重力加速度：GRAV_CONS＝9.8(米/秒²)；

方法仿真结果如下：

进行了60s仿真，仿真结果如图5所示，可以看出李群描述方法可以很好的完成捷联惯性导航解算过程，并且相比于四元数法精度没有损失；

(2)通过真实实验对本发明提出的基于李群描述的捷联惯导解算系统进行验证。真实试验中，不提任何供外界辅助信息，系统装置放置在车上，有人员上下车、开关车门、对车进行晃动等干扰。实验历时600s，试验地点在北京工业大学羽毛球馆南广场位置。上位导航计算机控制导航系统，以100HZ的数据更新速率，115200bps的波特率，采集航向精度达0.1度、姿态精度达0.05度的实际三轴姿态信息，其上位机采集页面示意图如图7。解算获得的载体姿态信息与本步骤中得到的高精度真实载体姿态信息做比较，证明本方法和系统的可行性和有效性。

实验结果如下：

截取60s的实际数据，结果如图6所示。可以看出在真实实验的情况下李群描述依然可以完成解算任务，并且误差相比于四元数法精度没有损失。

Claims (2)

1.基于李群描述的捷联惯性导航解算方法，本方法的详细描述中坐标系定义如下：地球坐标系e系，选取地球中心为原点，X轴位于赤道平面内，从地心指向本初子午线，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系，随地球自转而转动；地心惯性坐标系i系，原点选取地球中心，X轴位于赤道平面内，从地心指向春分点，Z轴从地心指向地理北极，X轴、Y轴和Z轴构成右手坐标系；导航坐标系n系，即导航基准的坐标系，导航相关运算都在此的坐标系下进行，以舰载机重心为原点，X轴指向东向E，Y轴指向北向N，Z轴指向天向U；载体坐标系b系，原点位于舰载机重心，X轴、Y轴、Z轴分别沿舰载机机体横轴指向右、沿纵轴指向前、沿立轴指向上；

其特征在于：该方法通过下述流程实现，

(1)捷联惯导系统进行预热准备，启动系统，获得初始对准得到的载体所在位置的经度λ、纬度L，姿态航向角H，俯仰角P，横滚角R，以及载体东向速度v_e，北向速度v_n，天向速度v_u基本信息，采集惯性测量单元IMU中陀螺的输出角速度信息

和加速度计的输出信息f^b；

(2)对采集到的陀螺和加速度计的数据进行处理，应用李群方法解算姿态矩阵与机体在n系下的速度；

将导航的姿态矩阵和机体在n系下的速度用一个4×4的正交变换矩阵来表示；该矩阵符合李群的特殊欧式群SE(3)的性质，构成了SE(3)群：

其中，R∈SO(3)对应了特定的导航姿态矩阵，

表示3×3的向量空间，上标T表示矩阵的转置，I表示三维单位矩阵，det(R)表示为矩阵R的行列式，T∈SE(3)对应了包括姿态速度的变换矩阵，

表示4×4的向量空间，t表示平动矢量，

表示3×1的向量空间；

机体姿态与速度位置的求解问题转化为对变换矩阵的更新问题；根据李群的微分方程：

其中，ξ是一个六维向量，前三维为平移记做ρ，即速度信息，由三轴加速度计测量，后三维为旋转，记做φ，即角速度信息，由三轴陀螺仪测量，符号^是将六维向量转换成四维矩阵的运算，运算法则如下：

其中φ_×表示将三维向量转换成一个反对称矩阵的运算，运算规则如下：

在实际解算中，需要将李群微分方程离散化之后，再进行迭代更新，离散化结果如下：

T_k+1＝T_kexp(ξ^) (6)

其中T_k是k时刻的变换矩阵，exp(ξ^)是一个矩阵的指数，将矩阵ξ^分块，先计算exp(φ_×)部分，对于任意矩阵A的指数写成一个泰勒展开：

对于exp(φ_×)部分，也按照这种方式进行展开：

由于φ是三维向量，定义该三维向量的模值和方向分别记做θ和a，即φ＝θa，a是一个长度为1的方向向量，对于a_×，有以下两条性质：

a_×a_×＝aa^T-I(9)

a_×a_×a_×＝-a_×(10)

根据(9)和(10)式，将(8)式展开计算：

最后得到式(11)：

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

式(11)与表示旋转的罗德里格斯公式相似，即exp(θa_×)是一个旋转矩阵，该旋转矩阵代表k时刻的姿态矩阵R_k与k+1时刻的姿态矩阵R_k+1之间的转动关系，记做ΔR，即

R_k+1＝R_kexp(φ_×)＝R_kΔR(12)

计算平动部分ρ的指数形式，平动部分对应的是k时刻至k+1时刻的速度变化量；由于速度信息由加速度计提供，得到的速度信息是在k+1时刻机体坐标系下的信息；在计算载体位置时，需要用到的是导航坐标系，即地理坐标系下的速度信息，故需在指数计算的过程中对速度信息左乘ΔR来调整；即平动部分ρ的指数形式为ΔRρ；综上得到exp(ξ^{^})的展开形式：

则变换矩阵的迭代更新方程为：

2.根据权利要求1所述的基于李群描述的捷联惯性导航解算方法，其特征在于：

步骤1：系统准备阶段，导航系统进行初始对准过程，获取载体的初始位置经度λ、纬度L、姿态航向角H，俯仰角P，横滚角R，姿态矩阵

以及载体东向速度v_e，北向速度v_n，天向速度v_u基本信息；

步骤2：构造k＝0时刻的SE(3)阵：

其中

采集惯性测量单元IMU中陀螺的输出角速度信息

和加速度计的输出信息f^b；

步骤3：利用初始时刻的导航信息，计算导航系相对于惯性系的旋转角速度在机体系下的投影

计算公式如下：

其中，WIE是地球自转角速度，R_n是地球半短轴长度，R_e是地球半长轴长度；由此可得到机体系相对于导航系的角速度在机体系下的投影

步骤4：构造六维向量ξ：

其中，

是这一个采样周期比力对时间的积分，

是这一个采样周期

对时间的积分；

步骤5：根据(6)和(13)计算出k＝1时刻的变换矩阵：

步骤6：去除重力加速度和有害加速度对速度的影响：

其中gⁿ是重力加速度在导航系下的投影，g是当地重力加速度量纲，v(k+1)即k+1时刻的载体速度信息；利用该速度信息，更新k+1时刻载体的经纬度：

根据求得的k+1时刻姿态矩阵

解算出姿态角P、R、H；至此，k+1时刻所有导航信息均已解算完成；

步骤7：以k+1时刻的信息为初始信息，重复步骤2至步骤6。

Images