本发明涉及一种基于四环惯导平台的载体姿态角速度测量方法。
背景技术：
随着科技的不断进步，对惯性导航系统提出了更高的要求。四环惯导平台是高精度全姿态平台式惯导系统，但四环惯导平台输出的是相对于惯性空间的相对角度信息，无法输出用于载体姿态控制的载体姿态角速度信息。
目前，空间稳定型惯导平台(四环惯导平台如图1所示，包括台体、内框、外框和随动框)工作时，平台台体相对于惯性空间稳定，四环惯导平台只能获取四个框架的角信息(即四个框架旋转轴的相对转角)，没有直接输出角速度的部件，无法直接输出载体角速度信息，需要载体角速度信息时必须需配备速率陀螺或速率组合装置，这样一来，就增加了四环惯导平台的体积重量，同时增加了整个系统的成本。
技术实现要素：
为了解决四环惯导平台配备速率陀螺或速率组合装置获取载体角速度的方式带来的系统体积重量大，且成本较高的问题，本发明提供了一种基于四环惯导平台的载体姿态角速度测量方法，能够在四环惯导平台无需配备速率陀螺或速率组合装置时也可获取高精度载体角速度。
本发明的具体技术方案是：
本发明提供了一种基于四环惯导平台的载体姿态角速度测量方法，包括以下步骤：
步骤1：在惯导平台计算机里创建角偏差补偿的双自适应跟踪微分运算模块；
所述角偏差补偿的双自适应跟踪微分运算模块的输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角；其输出为四个框架角速度；
该模块具体包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；
步骤2：在惯导平台计算机里创建角速度转换矩阵实时计算模型；
所述角速度转换矩阵实时计算模型为：
式中：βyk、βxk、βzk、βsk分别为台体框架角、内框框架角、外框框架角和随动框框架角；ck为角速度转换矩阵；
步骤3：通过惯导平台计算机实时采集平台框架角传感器角度信息，分别获取实时的台体框架角、内框框架角、外框框架角和随动框框架角；
步骤4：将四个框架角输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，分别得到台体框架角速度、内框框架角速度、外框框架角速度和随动框框架角速度；
步骤5：将步骤4得到的台体框架角速度
步骤6：将步骤3实时获取的台体框架角、内框框架角、外框框架角和随动框框架角代入步骤2的角速度转换矩阵计算模块中，得到实时的角速度转换矩阵ck；
步骤7：将步骤5获取的当前时刻角速度转换矩阵ck乘以步骤6中获得的框架角速度列向量ωk，从而得到载体角速度列向量ωb。
具体来说，上述步骤4中四个框架角中任一框架角速度的具体处理过程为：
任一框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号
具体来说，上述所述第一自适应跟踪微分器和第二自适应跟踪微分器均包括初始化赋值和实时计算两部分；
a：初始化赋值：
a1：初始化当前时刻跟踪信号βt(k)和上一时刻跟踪信号βt(k-1)为当前输入信号；
a2：初始化当前时刻微分信号
a3：初始化当前时刻微分偏差信号
a4：初始化当前时刻快速因子计算使用的常量r1和r2，r1为20000，r2为12000；
a5：初始化当前时刻滤波因子计算使用的常量h01和h02，h01为3000，h02为0.004；
a6：初始化当前时刻快速因子r(k)和滤波因子h0(k)为0；
a7：初始化计算周期h为0.0005；
b：实时计算过程如下：
b1：计算自适应快速因子
式中：r(k)为当前时刻快速因子；
b2：计算自适应滤波因子
式中：h0(k)为当前时刻滤波因子；
b3:计算当前时刻跟踪信号
h为计算周期；
b4：计算当前时刻微分信号
fst(βt(k)-β(k),
式中：d、d0、y、a0、a为计算过程的中间量；x1、x2、r、h为输入量，且x1、x2、r、h的取值分别对应βt(k)-β(k)、
5)计算当前微分偏差信号
具体来说，上述步骤2中所述角速度转换矩阵实时计算模型的构建过程如下：
定义四环惯导平台的相关坐标系；
其中，oxpypzp为台体坐标系，
载体、随动框架、外框架与内框架绕台体轴yp转过βyk时，有：
式中：ωxp、ωyp、ωzp分别为台体绕xp、yp、zp轴的绝对角速度；ωxp1、ωyp1、ωzp1分别为载体、随动框架、外框架和内框架一起绕xp1、yp1、zp1轴的绝对角速度；
载体、随动框架与外框架绕内环轴xp1转过βxk角时，有:
式中：ωxp2、ωyp2、ωzp2分别为载体、随动框架、和外框架一起绕xp2、yp2、zp2轴的绝对角速度；
载体、随动框架一起绕外环轴zp2转过βzk角时，有:
式中：ωxp3、ωyp3、ωzp3分别为载体、随动框架一起绕xp3、yp3、zp3轴的绝对角速度；
载体绕随动环轴xp3转过βsk角时，有：
式中：ωxb、ωyb、ωzb分别为载体、随动框架、和外框架一起绕xb、yb、zb轴的绝对角速度；
其中：
最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：
其中：
角速度转换矩阵实时计算模型为：
具体来说，上述步骤7的具体计算过程为：
由于四环惯导平台台体相对惯性空间静止，故台体角速度[ωxpωypωzp]t为零，因此四环惯导平台载体角速度的计算关系为：
本发明的优点在于：
1、本发明通过构建角偏差补偿的双自适应跟踪微分运算模块和角速度转换矩阵实时计算模型实现了空间稳定型四环惯性平台实时输出载体姿态角速度信息供控制系统使用，无需载体再安装其他用于输出姿态角速度的设备，减小了四环惯导平台的体积重量以及整个系统的成本。
2、本发明采用角偏差补偿的双自适应跟踪微分算法，提高框架角速度的计算精度。
3、本发明采用自适应跟踪微分器，并对其中重要参数的滤波因子和快速因子进行在线自适应调整，提高跟踪微分算法对大过载高动态环境的适应能力。
附图说明
图1为四环惯导平台框架结构示意图；
图2为角偏差补偿的双自适应跟踪微分运算模块的示意图；
图3为四环惯导平台各框架坐标系的关系示意图；
图4为载体角速度测量方法原理图。
具体实施方式
下面结合附图对本发明的方法作进一步的描述：
1、在惯导平台计算机里创建构建角偏差补偿的双自适应跟踪微分运算模块
为了兼顾框架角信号的跟随性和稳定性，利用跟踪微分器输出的速率偏差信号来补偿微分信号(框架角速率信号)滞后的方法，即需要建立角偏差补偿的双自适应跟踪微分运算模块，该模块如图2所示，包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；该模块输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角，其输出为四个框架角速度；
其中，第一自适应跟踪微分器和第二自适应跟踪微分器均是为了得到输入信号的微分信号，是角偏差补偿的双自适应跟踪微分运算模块的关键运算器，均包括初始化赋值和实时计算两部分；
设实时的输入信号为β(k)，输出信号为
a：初始化赋值：
a1：初始化当前时刻跟踪信号βt(k)和上一时刻跟踪信号βt(k-1)为当前输入信号；
a2：初始化当前时刻微分信号
a3：初始化当前时刻微分偏差信号
a4：初始化当前时刻快速因子计算使用的常量r1和r2，r1为20000，r2为12000；
a5：初始化当前时刻滤波因子计算使用的常量h01和h02，h01为3000，h02为0.004；
a6：初始化当前时刻快速因子r(k)和滤波因子h0(k)为0；
a7：初始化计算周期h为0.0005；
b：实时计算过程如下：
b1：计算自适应快速因子
式中：r(k)为当前时刻快速因子；
r1和r2为快速因子计算所需常量；
b2：计算自适应滤波因子
式中：h0(k)为当前时刻滤波因子；
h01和h02为滤波因子计算所需常量；
b3：计算当前时刻跟踪信号
式中：βt(k)为当前时刻跟踪信号；
βt(k-1)为上一时刻跟踪信号；
h为计算周期；
b4：当前时刻微分信号计算
式中：
h为计算周期；
βt(k)为当前时刻跟踪信号；
β(k)为当前时刻输入信号；
r(k)为当前时刻快速因子；
h0(k)为当前时刻滤波因子；
fst(βt(k)-β(k),
最速控制综合函数计算见下式：
式中：d、d0、y、a0、a为计算过程的中间量；x1、x2、r、h为输入量，且x1、x2、r、h的取值分别对应βt(k)-β(k)、
b5：计算当前微分偏差信号
式中：
由自适应跟踪微分器算法可知，当框架角偏差e(t)趋于0时，第一自适应跟踪微分器即可完全跟踪系统输入的框架角信号β(t)，此时第一自适应跟踪微分器输出的微分信号β1(t)即为框架角的微分信号
当框架角偏差e(t)不为零时，
分别将四个框架角信息输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，本实施例中以台体轴框架角为例说明角偏差补偿的双自适应跟踪微分运算模块的处理过程：
台体轴框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号
2、在惯导平台计算机里创建角速度转换矩阵实时计算模型；
四环惯导平台相关坐标系定义见图3所示。图中oxpypzp为台体坐标系，
载体、随动框架、外框架与内框架绕台体轴yp转过βyk时，有：
式中：ωxp、ωyp、ωzp分别为台体绕xp、yp、zp轴的绝对角速度；ωxp1、ωyp1、ωzp1分别为载体、随动框架、外框架和内框架一起绕xp1、yp1、zp1轴的绝对角速度；
载体、随动框架与外框架绕内环轴xp1转过βxk角时，有:
式中：ωxp2、ωyp2、ωzp2分别为载体、随动框架、和外框架一起绕xp2、yp2、zp2轴的绝对角速度；
载体、随动框架一起绕外环轴zp2转过βzk角时，有:
式中：ωxp3、ωyp3、ωzp3分别为载体、随动框架一起绕xp3、yp3、zp3轴的绝对角速度。
载体绕随动环轴xp3转过βsk角时，有：
式中：ωxb、ωyb、ωzb分别为载体、随动框架、和外框架一起绕xb、yb、zb轴的绝对角速度。
其中：
最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：
其中：
角速度转换矩阵实时计算模型为：
由于四环惯导平台台体相对惯性空间静止，故台体角速度[ωxpωypωzp]t为零，因此四环惯导平台载体角速度的计算关系为：
3、四环惯导平台的载体角速度具体测量过程
如图4所示，βy、βx、βz和βs分别为实时获取的台体框架角、内框框架角、外框框架角和随动框框架角；
根据公式(13)将角速度转换矩阵ck和框架角速度列向量ωk相乘从而得到载体角速度列向量ωb。
1.一种基于四环惯导平台的载体姿态角速度测量方法，其特征在于：包括以下步骤：
步骤1：在惯导平台计算机里创建角偏差补偿的双自适应跟踪微分运算模块；
所述角偏差补偿的双自适应跟踪微分运算模块的输入为四环惯导平台的四个框架角，分别为台体框架角、内框框架角、外框框架角和随动框框架角；其输出为四个框架角速度；
该模块具体包括第一自适应跟踪微分器、第一比较器、第二自适应跟踪微分器以及第二比较器；
步骤2：在惯导平台计算机里创建角速度转换矩阵实时计算模型；
所述角速度转换矩阵实时计算模型为：
式中：βyk、βxk、βzk、βsk分别为台体框架角、内框框架角、外框框架角和随动框框架角；ck为角速度转换矩阵；
步骤3：通过惯导平台计算机实时采集四环惯导平台中角传感器角度信息，分别获取实时的台体框架角、内框框架角、外框框架角和随动框框架角；
步骤4：将四个框架角输入至角偏差补偿的双自适应跟踪微分运算模块中进行处理，分别得到台体框架角速度、内框框架角速度、外框框架角速度和随动框框架角速度；
步骤5：将步骤4得到的台体框架角速度
步骤6：将步骤3实时获取的台体框架角、内框框架角、外框框架角和随动框框架角代入步骤2的角速度转换矩阵计算模块中，得到实时的角速度转换矩阵ck；
步骤7：将步骤5获取的当前时刻角速度转换矩阵ck乘以步骤6中获得的框架角速度列向量ωk，从而得到载体角速度列向量ωb。
2.根据权利要求1所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤4中四个框架角中任一框架角速度的具体处理过程为：
任一框架角β(t)进入第一自适应跟踪微分器，得到第一自适应跟踪微分器的跟踪信号β′(t)和微分信号
3.根据权利要求2所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：
所述第一自适应跟踪微分器和第二自适应跟踪微分器均包括初始化赋值和实时计算两部分；
a：初始化赋值：
a1：初始化当前时刻跟踪信号βt(k)和上一时刻跟踪信号βt(k-1)为当前输入信号；
a2：初始化当前时刻微分信号
a3：初始化当前时刻微分偏差信号
a4：初始化当前时刻快速因子计算使用的常量r1和r2，r1为20000，r2为12000；
a5：初始化当前时刻滤波因子计算使用的常量h01和h02，h01为3000，h02为0.004；
a6：初始化当前时刻快速因子r(k)和滤波因子h0(k)为0；
a7：初始化计算周期h为0.0005；
b：实时计算过程如下：
b1：计算自适应快速因子
式中：r(k)为当前时刻快速因子；
b2：计算自适应滤波因子
式中：h0(k)为当前时刻滤波因子；
b3:计算当前时刻跟踪信号
h为计算周期；
b4：计算当前时刻微分信号
式中：d、d0、y、a0、a为计算过程的中间量；x1、x2、r、h为输入量，且x1、x2、r、h的取值分别对应βt(k)-β(k)、
5)计算当前微分偏差信号
4.根据权利要求1所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤2中所述角速度转换矩阵实时计算模型的构建过程如下：
定义四环惯导平台的相关坐标系；
其中，oxpypzp为台体坐标系，
载体、随动框架、外框架与内框架绕台体轴yp转过βyk时，有：
式中：ωxp、ωyp、ωzp分别为台体绕xp、yp、zp轴的绝对角速度；ωxp1、ωyp1、ωzp1分别为载体、随动框架、外框架和内框架一起绕xp1、yp1、zp1轴的绝对角速度；
载体、随动框架与外框架绕内环轴xp1转过βxk角时，有:
式中：ωxp2、ωyp2、ωzp2分别为载体、随动框架、和外框架一起绕xp2、yp2、zp2轴的绝对角速度；
载体、随动框架一起绕外环轴zp2转过βzk角时，有:
式中：ωxp3、ωyp3、ωzp3分别为载体、随动框架一起绕xp3、yp3、zp3轴的绝对角速度；
载体绕随动环轴xp3转过βsk角时，有：
式中：ωxb、ωyb、ωzb分别为载体、随动框架、和外框架一起绕xb、yb、zb轴的绝对角速度；
其中：
最终得到载体角速度与四框架平台台体角速度、框架角速度之间的一般关系：
其中：
5.根据权利要求4所述的基于四环惯导平台的载体姿态角速度测量方法，其特征在于：所述步骤7的具体计算过程为：
由于四环惯导平台台体相对惯性空间静止，故台体角速度[ωxpωypωzp]t为零，因此四环惯导平台载体角速度的计算关系为：
