CN112611378A - 一种基于四环惯导平台的载体姿态角速度测量方法 - Google Patents

Abstract

本发明公开了一种基于四环惯导平台的载体姿态角速度测量方法，该方法无需配备速率陀螺或速率组合装置时也可获取高精度载体角速度。该方法的主要实现步骤是：1、创建角偏差补偿的双自适应跟踪微分运算模块；2、创建角速度转换矩阵实时计算模型；3、采集四环惯导平台中角传感器角度信息，获取四环惯导平台的四个框架角；4、将四个框架角输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，得到四个框架角速度，并组成框架角速度列向量；5、将实时获取的四个框架角代入至角速度转换矩阵计算模型中，得到实时的角速度转换矩阵；6、将实时的角速度转换矩阵乘以框架角速度列向量，从而得到载体角速度列向量。

Description

一种基于四环惯导平台的载体姿态角速度测量方法

技术领域

本发明涉及一种基于四环惯导平台的载体姿态角速度测量方法。

背景技术

随着科技的不断进步，对惯性导航系统提出了更高的要求。四环惯导平台是高精度全姿态平台式惯导系统，但四环惯导平台输出的是相对于惯性空间的相对角度信息，无法输出用于载体姿态控制的载体姿态角速度信息。

目前，空间稳定型惯导平台(四环惯导平台如图1所示，包括台体、内框、外框和随动框)工作时，平台台体相对于惯性空间稳定，四环惯导平台只能获取四个框架的角信息(即四个框架旋转轴的相对转角)，没有直接输出角速度的部件，无法直接输出载体角速度信息，需要载体角速度信息时必须需配备速率陀螺或速率组合装置，这样一来，就增加了四环惯导平台的体积重量，同时增加了整个系统的成本。

发明内容

为了解决四环惯导平台配备速率陀螺或速率组合装置获取载体角速度的方式带来的系统体积重量大，且成本较高的问题，本发明提供了一种基于四环惯导平台的载体姿态角速度测量方法，能够在四环惯导平台无需配备速率陀螺或速率组合装置时也可获取高精度载体角速度。

本发明的具体技术方案是：

本发明提供了一种基于四环惯导平台的载体姿态角速度测量方法，包括以下步骤：

步骤1：在惯导平台计算机里创建角偏差补偿的双自适应跟踪微分运算模块；

所述角偏差补偿的双自适应跟踪微分运算模块的输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角；其输出为四个框架角速度；

该模块具体包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；

步骤2：在惯导平台计算机里创建角速度转换矩阵实时计算模型；

所述角速度转换矩阵实时计算模型为：

式中：β_yk、β_xk、β_zk、β_sk分别为台体框架角、内框框架角、外框框架角和随动框框架角；C_k为角速度转换矩阵；

步骤3：通过惯导平台计算机实时采集平台框架角传感器角度信息，分别获取实时的台体框架角、内框框架角、外框框架角和随动框框架角；

步骤4：将四个框架角输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，分别得到台体框架角速度、内框框架角速度、外框框架角速度和随动框框架角速度；

步骤5：将步骤4得到的台体框架角速度

内框框架角速度

外框框架角速度

和随动框框架角速度

组成框架角速度列向量ω_k，具体表达式为：

步骤6：将步骤3实时获取的台体框架角、内框框架角、外框框架角和随动框框架角代入步骤2的角速度转换矩阵计算模块中，得到实时的角速度转换矩阵C_k；

步骤7：将步骤5获取的当前时刻角速度转换矩阵C_k乘以步骤6中获得的框架角速度列向量ω_k，从而得到载体角速度列向量ω_b。

具体来说，上述步骤4中四个框架角中任一框架角速度的具体处理过程为：

任一框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号

任一框架角β(t)与第一自适应跟踪微分器的跟踪信号β′(t)进入第一比较器，相减得到差值e(t)，差值e(t)进入第二自适应跟踪微分器，得到第二自适应跟踪微分器的微分信号

第一自适应跟踪微分器的微分信号

和第二自适应跟踪微分器的微分信号

进入第二比较器，相加得到任一框架角速度

具体来说，上述所述第一自适应跟踪微分器和第二自适应跟踪微分器均包括初始化赋值和实时计算两部分；

A：初始化赋值：

A1：初始化当前时刻跟踪信号β_t(k)和上一时刻跟踪信号β_t(k-1)为当前输入信号；

A2：初始化当前时刻微分信号

和上一时刻跟踪信号

为0；

A3：初始化当前时刻微分偏差信号

和上一时刻微分偏差信号

为0；

A4：初始化当前时刻快速因子计算使用的常量r₁和r₂，r₁为20000，r₂为12000；

A5：初始化当前时刻滤波因子计算使用的常量h₀₁和h₀₂，h₀₁为3000，h₀₂为0.004；

A6：初始化当前时刻快速因子r(k)和滤波因子h₀(k)为0；

A7：初始化计算周期h为0.0005；

B：实时计算过程如下：

B1：计算自适应快速因子

式中：r(k)为当前时刻快速因子；

为上一时刻微分偏差信号；

B2：计算自适应滤波因子

式中：h₀(k)为当前时刻滤波因子；

B3:计算当前时刻跟踪信号

h为计算周期；

B4：计算当前时刻微分信号

fst(β_t(k)-β(k),

r(k),h₀(k))为当前时刻最速控制综合函数值，最速控制综合函数计算见下式；

式中：d、d₀、y、a₀、a为计算过程的中间量；x₁、x₂、r、h为输入量，且x₁、x₂、r、h的取值分别对应β_t(k)-β(k)、

r(k)、h₀(k)；fst为输出量；

5)计算当前微分偏差信号

具体来说，上述步骤2中所述角速度转换矩阵实时计算模型的构建过程如下：

定义四环惯导平台的相关坐标系；

其中，O_XpYpZp为台体坐标系，

为载体坐标系，

为内框架坐标系，

为外框架坐标系，

为随动框架坐标系，各框架绕其框架轴正向的转角由内向外分别定义为β_yk、β_xk、β_zk、β_sk；

载体、随动框架、外框架与内框架绕台体轴Y_p转过β_yk时，有：

式中：ω_xp、ω_yp、ω_zp分别为台体绕X_p、Y_p、Z_p轴的绝对角速度；ω_xp1、ω_yp1、ω_zp1分别为载体、随动框架、外框架和内框架一起绕X_p1、Y_p1、Z_p1轴的绝对角速度；

载体、随动框架与外框架绕内环轴X_p1转过β_xk角时，有:

式中：ω_xp2、ω_yp2、ω_zp2分别为载体、随动框架、和外框架一起绕X_p2、Y_p2、Z_p2轴的绝对角速度；

载体、随动框架一起绕外环轴Z_p2转过β_zk角时，有:

式中：ω_xp3、ω_yp3、ω_zp3分别为载体、随动框架一起绕X_p3、Y_p3、Z_p3轴的绝对角速度；

载体绕随动环轴X_p3转过β_sk角时，有：

式中：ω_xb、ω_yb、ω_zb分别为载体、随动框架、和外框架一起绕X_b、Y_b、Z_b轴的绝对角速度；

其中：

最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：

其中：

角速度转换矩阵实时计算模型为：

具体来说，上述步骤7的具体计算过程为：

由于四环惯导平台台体相对惯性空间静止，故台体角速度[ω_xp ω_yp ω_zp]^T为零，因此四环惯导平台载体角速度的计算关系为：

本发明的优点在于：

1、本发明通过构建角偏差补偿的双自适应跟踪微分运算模块和角速度转换矩阵实时计算模型实现了空间稳定型四环惯性平台实时输出载体姿态角速度信息供控制系统使用，无需载体再安装其他用于输出姿态角速度的设备，减小了四环惯导平台的体积重量以及整个系统的成本。

2、本发明采用角偏差补偿的双自适应跟踪微分算法，提高框架角速度的计算精度。

3、本发明采用自适应跟踪微分器，并对其中重要参数的滤波因子和快速因子进行在线自适应调整，提高跟踪微分算法对大过载高动态环境的适应能力。

附图说明

图1为四环惯导平台框架结构示意图；

图2为角偏差补偿的双自适应跟踪微分运算模块的示意图；

图3为四环惯导平台各框架坐标系的关系示意图；

图4为载体角速度测量方法原理图。

具体实施方式

下面结合附图对本发明的方法作进一步的描述：

1、在惯导平台计算机里创建构建角偏差补偿的双自适应跟踪微分运算模块

为了兼顾框架角信号的跟随性和稳定性，利用跟踪微分器输出的速率偏差信号来补偿微分信号(框架角速率信号)滞后的方法，即需要建立角偏差补偿的双自适应跟踪微分运算模块，该模块如图2所示，包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；该模块输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角，其输出为四个框架角速度；

其中，第一自适应跟踪微分器和第二自适应跟踪微分器均是为了得到输入信号的微分信号，是角偏差补偿的双自适应跟踪微分运算模块的关键运算器，均包括初始化赋值和实时计算两部分；

设实时的输入信号为β(k)，输出信号为

为输入信号β(k)的微分信号；

A：初始化赋值：

A1：初始化当前时刻跟踪信号β_t(k)和上一时刻跟踪信号β_t(k-1)为当前输入信号；

A2：初始化当前时刻微分信号

和上一时刻跟踪信号

为0；

A3：初始化当前时刻微分偏差信号

和上一时刻微分偏差信号

为0；

A4：初始化当前时刻快速因子计算使用的常量r₁和r₂，r₁为20000，r₂为12000；

A5：初始化当前时刻滤波因子计算使用的常量h₀₁和h₀₂，h₀₁为3000，h₀₂为0.004；

A6：初始化当前时刻快速因子r(k)和滤波因子h₀(k)为0；

A7：初始化计算周期h为0.0005；

B：实时计算过程如下：

B1：计算自适应快速因子

式中：r(k)为当前时刻快速因子；

r₁和r₂为快速因子计算所需常量；

为上一时刻微分偏差信号。

B2：计算自适应滤波因子

式中：h₀(k)为当前时刻滤波因子；

h₀₁和h₀₂为滤波因子计算所需常量；

为上一时刻微分偏差信号。

B3：计算当前时刻跟踪信号

式中：β_t(k)为当前时刻跟踪信号；

β_t(k-1)为上一时刻跟踪信号；

h为计算周期；

为上一时刻微分信号。

B4：当前时刻微分信号计算

式中：

为当前时刻微分信号；

为上一时刻微分信号；

h为计算周期；

β_t(k)为当前时刻跟踪信号；

β(k)为当前时刻输入信号；

r(k)为当前时刻快速因子；

h₀(k)为当前时刻滤波因子；

fst(β_t(k)-β(k),

r(k),h₀(k))为当前时刻最速控制综合函数值。

最速控制综合函数计算见下式：

式中：d、d₀、y、a₀、a为计算过程的中间量；x₁、x₂、r、h为输入量，且x₁、x₂、r、h的取值分别对应β_t(k)-β(k)、

r(k)、h₀(k)；fst为输出量；

B5：计算当前微分偏差信号

式中：

为当前时刻微分偏差信号；

为当前时刻微分信号；

为上一时刻微分信号。

由自适应跟踪微分器算法可知，当框架角偏差e(t)趋于0时，第一自适应跟踪微分器即可完全跟踪系统输入的框架角信号β(t)，此时第一自适应跟踪微分器输出的微分信号β₁(t)即为框架角的微分信号

当框架角偏差e(t)不为零时，

并不能准确反映框架角的微分信号

而与真实的

存在一定的偏差，通过引入第二自适应跟踪微分器，可以使得e′(t)趋于e(t)，从而双自适应跟踪微分器完全跟踪了输入的框架角信号，因而

近似等于

从而得到框架角的微分信号

分别将四个框架角信息输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，本实施例中以台体轴框架角为例说明角偏差补偿的双自适应跟踪微分运算模块的处理过程：

台体轴框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号

台体轴框架角β(t)与第一跟踪微分器的跟踪信号β′(t)进入第一比较器，相减得到差值e(t)，差值e(t)进入第二自适应跟踪微分器，得到第二自适应跟踪微分器的微分信号

第一自适应跟踪微分器的微分信号

和第二自适应跟踪微分器的微分信号

进入第二比较器，相加得到台体轴框架角速度

内框架轴、外框架轴和随动框架轴的角速度计算过程与台体轴框架角的计算过程类似。

2、在惯导平台计算机里创建角速度转换矩阵实时计算模型；

四环惯导平台相关坐标系定义见图3所示。图中O_XpYpZp为台体坐标系，

为载体坐标系，

为内框架坐标系，

为外框架坐标系，

为随动框架坐标系，各框架绕其框架轴正向的转角由内向外分别定义为β_yk、β_xk、β_zk、β_sk；

载体、随动框架、外框架与内框架绕台体轴Y_p转过β_yk时，有：

式中：ω_xp、ω_yp、ω_zp分别为台体绕X_p、Y_p、Z_p轴的绝对角速度；ω_xp1、ω_yp1、ω_zp1分别为载体、随动框架、外框架和内框架一起绕X_p1、Y_p1、Z_p1轴的绝对角速度；

载体、随动框架与外框架绕内环轴X_p1转过β_xk角时，有:

式中：ω_xp2、ω_yp2、ω_zp2分别为载体、随动框架、和外框架一起绕X_p2、Y_p2、Z_p2轴的绝对角速度；

载体、随动框架一起绕外环轴Z_p2转过β_zk角时，有:

式中：ω_xp3、ω_yp3、ω_zp3分别为载体、随动框架一起绕X_p3、Y_p3、Z_p3轴的绝对角速度。

载体绕随动环轴X_p3转过β_sk角时，有：

式中：ω_xb、ω_yb、ω_zb分别为载体、随动框架、和外框架一起绕X_b、Y_b、Z_b轴的绝对角速度。

其中：

最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：

其中：

角速度转换矩阵实时计算模型为：

由于四环惯导平台台体相对惯性空间静止，故台体角速度[ω_xp ω_yp ω_zp]^T为零，因此四环惯导平台载体角速度的计算关系为：

3、四环惯导平台的载体角速度具体测量过程

如图4所示，β_y、β_x、β_z和β_s分别为实时获取的台体框架角、内框框架角、外框框架角和随动框框架角；

和

分别为经角偏差补偿的双自适应跟踪微分运算模块处理后得到的台体框架角速度、内框框架角速度、外框框架角速度和随动框框架角速度，从而得到框架角速度列向量ω_k

根据公式(13)将角速度转换矩阵C_k和框架角速度列向量ω_k相乘从而得到载体角速度列向量ω_b。

Claims (5)

1.一种基于四环惯导平台的载体姿态角速度测量方法，其特征在于：包括以下步骤：

步骤1：在惯导平台计算机里创建角偏差补偿的双自适应跟踪微分运算模块；

所述角偏差补偿的双自适应跟踪微分运算模块的输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角；其输出为四个框架角速度；

该模块具体包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；

步骤2：在惯导平台计算机里创建角速度转换矩阵实时计算模型；

所述角速度转换矩阵实时计算模型为：

式中：β_yk、β_xk、β_zk、β_sk分别为台体框架角、内框框架角、外框框架角和随动框框架角；C_k为角速度转换矩阵；

步骤3：通过惯导平台计算机实时采集四环惯导平台中角传感器角度信息，分别获取实时的台体框架角、内框框架角、外框框架角和随动框框架角；

步骤4：将四个框架角输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，分别得到台体框架角速度、内框框架角速度、外框框架角速度和随动框框架角速度；

步骤5：将步骤4得到的台体框架角速度

内框框架角速度

外框框架角速度

和随动框框架角速度

组成框架角速度列向量ω_k，具体表达式为：

步骤6：将步骤3实时获取的台体框架角、内框框架角、外框框架角和随动框框架角代入步骤2的角速度转换矩阵计算模块中，得到实时的角速度转换矩阵C_k；

步骤7：将步骤5获取的当前时刻角速度转换矩阵C_k乘以步骤6中获得的框架角速度列向量ω_k，从而得到载体角速度列向量ω_b。

2.根据权利要求1所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤4中四个框架角中任一框架角速度的具体处理过程为：

任一框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号

任一框架角β(t)与第一自适应跟踪微分器的跟踪信号β′(t)进入第一比较器，相减得到差值e(t)，差值e(t)进入第二自适应跟踪微分器，得到第二自适应跟踪微分器的微分信号

第一自适应跟踪微分器的微分信号

和第二自适应跟踪微分器的微分信号

进入第二比较器，相加得到任一框架角速度

3.根据权利要求2所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：

所述第一自适应跟踪微分器和第二自适应跟踪微分器均包括初始化赋值和实时计算两部分；

A：初始化赋值：

A1：初始化当前时刻跟踪信号β_t(k)和上一时刻跟踪信号β_t(k-1)为当前输入信号；

A2：初始化当前时刻微分信号

和上一时刻跟踪信号

为0；

A3：初始化当前时刻微分偏差信号

和上一时刻微分偏差信号

为0；

A4：初始化当前时刻快速因子计算使用的常量r₁和r₂，r₁为20000，r₂为12000；

A5：初始化当前时刻滤波因子计算使用的常量h₀₁和h₀₂，h₀₁为3000，h₀₂为0.004；

A6：初始化当前时刻快速因子r(k)和滤波因子h₀(k)为0；

A7：初始化计算周期h为0.0005；

B：实时计算过程如下：

B1：计算自适应快速因子

式中：r(k)为当前时刻快速因子；

为上一时刻微分偏差信号；

B2：计算自适应滤波因子

式中：h₀(k)为当前时刻滤波因子；

B3:计算当前时刻跟踪信号

h为计算周期；

B4：计算当前时刻微分信号

为当前时刻最速控制综合函数值，最速控制综合函数计算见下式；

式中：d、d₀、y、a₀、a为计算过程的中间量；x₁、x₂、r、h为输入量，且x₁、x₂、r、h的取值分别对应β_t(k)-β(k)、

r(k)、h₀(k)；fst为输出量；

5)计算当前微分偏差信号

4.根据权利要求1所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤2中所述角速度转换矩阵实时计算模型的构建过程如下：

定义四环惯导平台的相关坐标系；

其中，O_XpYpZp为台体坐标系，

为载体坐标系，

为内框架坐标系，

为外框架坐标系，

为随动框架坐标系，各框架绕其框架轴正向的转角由内向外分别定义为β_yk、β_xk、β_zk、β_sk；

载体、随动框架、外框架与内框架绕台体轴Y_p转过β_yk时，有：

式中：ω_xp、ω_yp、ω_zp分别为台体绕X_p、Y_p、Z_p轴的绝对角速度；ω_xp1、ω_yp1、ω_zp1分别为载体、随动框架、外框架和内框架一起绕X_p1、Y_p1、Z_p1轴的绝对角速度；

载体、随动框架与外框架绕内环轴X_p1转过β_xk角时，有:

式中：ω_xp2、ω_yp2、ω_zp2分别为载体、随动框架、和外框架一起绕X_p2、Y_p2、Z_p2轴的绝对角速度；

载体、随动框架一起绕外环轴Z_p2转过β_zk角时，有:

式中：ω_xp3、ω_yp3、ω_zp3分别为载体、随动框架一起绕X_p3、Y_p3、Z_p3轴的绝对角速度；

载体绕随动环轴X_p3转过β_sk角时，有：

式中：ω_xb、ω_yb、ω_zb分别为载体、随动框架、和外框架一起绕X_b、Y_b、Z_b轴的绝对角速度；

其中：

最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：

其中：

5.根据权利要求4所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤7的具体计算过程为：

由于四环惯导平台台体相对惯性空间静止，故台体角速度[ω_xp ω_yp ω_zp]^T为零，因此四环惯导平台载体角速度的计算关系为：

Images