CN115855038B - 一种短时高精度姿态保持方法 - Google Patents

Abstract

本发明公开一种短时高精度姿态保持方法，包括：获取陀螺仪输出和加速度计输出；基于加速度计输出和当地重力加速度，得到自身初始姿态信息；基于自身初始姿态信息和陀螺仪输出，得到航向角，作为航向输出；基于加速度计输出和当地重力加速度，得到俯仰角和横揺角，并进行低通滤波处理，将低通滤波处理后的俯仰角和横揺角作为水平姿态输出，实现姿态测量。本发明能够在不依赖任何外界信息的条件下快速完成初始姿态的确定，后续姿态测量也无需接受任何外界信息，抗干扰能力强。经过剧烈角运动后仍能实现高精度的姿态测量，提高姿态计算的精度，实现较低成本条件下的高精度姿态测量。

Description

一种短时高精度姿态保持方法

技术领域

本发明涉及惯性***及惯性测量领域，特别涉及一种短时高精度姿态保持方法。

背景技术

捷联惯导是作战飞机的关键导航设备，其初始对准的精度和速度决定着作战飞机起飞前的准备时间，现有的惯导自对准方法普遍存在时间长的问题，不利于战机的快速启动。为此，人们开始采用高精度激光测距辅助设备结合机载标志点和外界参考源对战机航向进行快速测量。在此过程中，激光测距辅助设备自身的姿态精度关系到战机航向计算的准确性。考虑到激光测距辅助设备在工作时的非移动特性，区别于现有的捷联惯导***，开发新的姿态基准方法，实现较低成本条件下的高精度姿态测量。

发明内容

为解决上述现有技术中所存在的问题，本发明提供一种短时高精度姿态保持方法，经过剧烈角运动后仍能实现高精度的姿态测量，提高姿态计算的精度，实现较低成本条件下的高精度姿态测量。

为了实现上述技术目的，本发明提供了一种短时高精度姿态保持方法，包括：

获取陀螺仪输出和加速度计输出；

基于所述加速度计输出和当地重力加速度，得到自身初始姿态信息；

基于所述自身初始姿态信息和所述陀螺仪输出，得到航向角，作为航向输出；

基于所述加速度计输出和所述当地重力加速度，得到俯仰角和横揺角，并对所述俯仰角和所述横揺角进行低通滤波处理，将所述低通滤波处理后的所述俯仰角和所述横揺角作为水平姿态输出，实现姿态测量。

可选地，所述陀螺仪输出为：

其中，b为载体坐标系；i为惯性坐标系；和/>分别为X轴、Y轴和Z轴的陀螺仪角速度输出，T为转置。

可选地，所述加速度计输出为：

其中，和/>分别表示X轴、Y轴和Z轴的加速度计比力输出。

可选地，所述自身初始姿态信息的获取过程为：

计算所述当地重力加速度的矢量在初始时刻导航惯性系的投影；

计算所述加速度计输出的比力在初始时刻载体惯性系的投影；

基于所述当地重力加速度的矢量在初始时刻导航惯性系的投影和所述加速度计输出的比力在初始时刻载体惯性系的投影，采用双矢量定姿法，得到所述自身初始姿态信息。

可选地，所述航向角的获取过程为：

基于所述自身初始姿态信息和所述陀螺仪输出，构建四元数微分方程；

基于所述四元数微分方程，采用四阶龙格-库塔法求解，得到所述航向角。

可选地，所述四元数微分方程为：

式中，为姿态四元数的导数；W为跟四元数运算相关的四维反对称阵操作；/>为在捷联惯导相对于导航坐标系的转动角速度在载体坐标系下的投影；Q为姿态四元数。

可选地，所述四阶龙格-库塔法的计算公式为：

式中，t_k表示采样时刻；h表示采样间隔；k₁-k₄为四元数微分方程在不同时刻的平均斜率。

可选地，所述低通滤波处理的参数设置为：通带截止频率为1HZ，阻带截止频率为10HZ，通带增益为1dB，阻带增益为-80dB。

可选地，所述俯仰角和所述横揺角为：

式中，θ为俯仰角；γ为横摇角；和/>分别为X轴、Y轴和Z轴低通滤波处理后的加速度计输出；g为当地重力加速度。

本发明具有如下技术效果：

本发明能够在不依赖任何外界信息的条件下快速完成初始姿态的确定，后续姿态测量也无需接受任何外界信息，抗干扰能力强。经过剧烈角运动后仍能实现高精度的姿态测量，提高姿态计算的精度，实现较低成本条件下的高精度姿态测量。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例短时高精度姿态保持方法的流程框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明公开一种短时高精度姿态保持方法，包括：

获取陀螺仪输出和加速度计输出；

陀螺仪输出为：加速度计输出为其中b表示载体坐标系，i表示惯性坐标系。/>和/>分别表示X轴、Y轴和Z轴的陀螺仪角速度输出；/> 和/>分别表示X轴、Y轴和Z轴的加速度计比力输出。

基于加速度计输出和当地重力加速度，进入初始对准阶段得到自身初始姿态信息；

首先定义两个惯性坐标系：(1)初始时刻载体惯性系b₀：与初始对准开始时刻的载体坐标系(b系)重合，相对于惯性空间保持不动。(2)初始时刻导航惯性系n₀：与初始对准开始时刻的导航坐标系(n系)重合，相对于惯性空间保持不动。初始对准的目的是求解初始时刻载体惯性系b₀与初始时刻导航惯性系n₀的姿态变换关系，即

首先，计算当地重力加速度矢量在n₀系的投影为：

其中，为当地重力加速度矢量在n₀系的数值；gⁿ为常矢量，gⁿ＝[0 0 -g]^T，g表示当地重力加速度的数值，而：

其中，L表示当地地理纬度，ω_ie表示地区自转角速度。由于为常值，即n系相对于n₀系为定轴转动，由上式可解得：

其中，t表示初始对准进行的时间，I表示单位四元数。所以有：

首先，计算加速度计输出的比力在b₀系的投影：

其中：

为陀螺仪的测量值，姿态阵初始值/>利用姿态更新算法可求得实时姿态阵/>

最后，通过姿态变换关系建立当地重力加速度与加速度计输出比力量测之间的关系，得：

式中，f^b为加速度计的输出。理论上，只要获得两个时刻的当地重力加速度及加速度计输出比力测量值，就可使用双矢量定姿法求解但是，为了降低加速度计噪声干扰的影响，在初始对准过程中对式(8)积分，即：

其中，0＜t₁＜t₂且通常取t₁＝t₂/2。根据双矢量定姿法，可求得：

由于是对准初始时刻的姿态阵，为了得到粗对准结束时刻的姿态矩阵，可使用如下公式进行计算：

其中，和/>分别由式(4)和(7)确定。

基于自身初始姿态信息和陀螺仪输出，得到航向角，作为航向输出；

根据刚体转动理论，从载体坐标系(b系)转换到导航坐标系(n系)的四元数所对应的的微分方程如下：

式中，为姿态四元数的导数；W为跟四元数运算相关的四维反对称阵操作；/>为在捷联惯导相对于导航坐标系的转动角速度在载体坐标系下的投影；Q为姿态四元数。静基座条件下/>

对于四元数微分方程，采用四阶龙格-库塔法求解，初始时刻的四元数根据初始对准获得的姿态矩阵确定，姿态阵和四元数的转换关系为：

四阶龙格-库塔法的计算公式如下：

式中，t_k表示采样时刻；h表示采样间隔。

根据四元数和欧拉角的关系，给出航向角ψ的计算结果：

式(17)获得的航向角直接作为航向输出。

基于加速度计输出和当地重力加速度，得到俯仰角和横揺角，并对俯仰角和横揺角进行低通滤波处理，将低通滤波处理后的俯仰角和横揺角作为水平姿态输出，实现姿态测量；

采用加速度计输出和重力矢量直接计算水平姿态角，因此加速度计的噪声会直接影响计算精度，为此，本发明采用IIR数字低通滤波器对俯仰角和横揺角进行低通滤波处理。考虑滤波效果及延迟大小，最终设计的滤波器参数如下：

通带截止频率为1HZ，阻带截止频率为10HZ，通带增益为1dB，阻带增益为-80dB。其对应的离散型传递函数的形式为

将式(18)所示的离散型传递函数转化为差分方程就可以对每个时刻的加速度计输出进行处理，得到处理后的加速度计输出

根据低通滤波处理后的加速度计输出与重力加速度矢量的关系可以得到：

式(19)展开可以得到：

式中，θ为俯仰角；γ为横摇角；和/>分别为X轴、Y轴和Z轴低通滤波处理后的加速度计输出；g为当地重力加速度。

由式(20)直接得到水平(俯仰角和横揺角)的计算公式：

将式(17)得到的航向角ψ与式(21)得到的俯仰角和横揺角作为姿态基准***的输出。

以上显示和描述了本发明的基本原理、主要特征和优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (1)

1.一种短时高精度姿态保持方法，其特征在于，包括以下步骤：

获取陀螺仪输出和加速度计输出；

基于所述加速度计输出和当地重力加速度，得到自身初始姿态信息；

基于所述自身初始姿态信息和所述陀螺仪输出，得到航向角，作为航向输出；

基于所述加速度计输出和所述当地重力加速度，得到俯仰角和横揺角，并对所述俯仰角和所述横揺角进行低通滤波处理，将所述低通滤波处理后的所述俯仰角和所述横揺角作为水平姿态输出，实现姿态测量；

根据四元数和欧拉角的关系，给出航向角ψ的计算结果：

所述陀螺仪输出为：

其中，b为载体坐标系；i为惯性坐标系；和/>分别为X轴、Y轴和Z轴的陀螺仪角速度输出，T为转置；

所述加速度计输出为：

其中，和/>分别表示X轴、Y轴和Z轴的加速度计比力输出；

所述自身初始姿态信息的获取过程为：

计算所述当地重力加速度的矢量在初始时刻导航惯性系的投影；

计算所述加速度计输出的比力在初始时刻载体惯性系的投影；

基于所述当地重力加速度的矢量在初始时刻导航惯性系的投影和所述加速度计输出的比力在初始时刻载体惯性系的投影，采用双矢量定姿法，得到所述自身初始姿态信息；

所述航向角的获取过程为：

基于所述自身初始姿态信息和所述陀螺仪输出，构建四元数微分方程；

基于所述四元数微分方程，采用四阶龙格-库塔法求解，得到所述航向角；

所述四元数微分方程为：

式中，为姿态四元数的导数；W为跟四元数运算相关的四维反对称阵操作；/>为在捷联惯导相对于导航坐标系的转动角速度在载体坐标系下的投影；Q为姿态四元数；

所述四阶龙格-库塔法的计算公式为：

式中，t_k表示采样时刻；h表示采样间隔；k₁-k₄为四元数微分方程在不同时刻的平均斜率；

所述俯仰角和所述横揺角为：

式中，θ为俯仰角；γ为横摇角；和/>分别为X轴、Y轴和Z轴低通滤波处理后的加速度计输出；g为当地重力加速度；

通过姿态变换关系建立当地重力加速度与加速度计输出比力量测之间的关系，得：

式中，f^b为加速度计的输出；为了降低加速度计噪声干扰的影响，在初始对准过程中对下式积分，为实时姿态阵，/>为当地重力加速度矢量在n₀系的投影；

即：

其中，0＜t₁＜t₂且通常取t₁＝t₂/2；根据双矢量定姿法，可求得：

所述低通滤波处理的参数设置为：通带截止频率为1HZ，阻带截止频率为10HZ，通带增益为1dB，阻带增益为-80dB；其对应的离散型传递函数的形式为：

将上式所示的离散型传递函数转化为差分方程就可以对每个时刻的加速度计输出进行处理，得到处理后的加速度计输出