1.一种飞行器的强抗扰高度状态估计方法,其特征在于,所述方法包括以下步骤:步骤S1、获取传感器的数据并估计传感器时延,再对齐所述传感器时延,获得传感器观测量;
步骤S2、根据所述传感器观测量,估计所述传感器的误差方差;
步骤S3、计算所述传感器观测量的二次微分,并计算所述二次微分与惯性测量加速度的相关度;
步骤S4、将所述传感器观测量和所述相关度进行数据加权融合,计算飞行器距离地面的相对高度;
步骤S5、获取当前的绝对高度,分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理,再采用高度观测量切换机制,切换高度观测量计算飞行器的高度,最后用一阶卡尔曼滤波算法估计飞行器离地高度。
2.如权利要求1所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述步骤S1中,所述传感器时延表示传感器采集到数据后,处理并发送数据需要预设一定时间,处理器接收到的所述数据与实际数据的时间差值。
3.如权利要求2所述的飞行器的强抗扰高度状态估计方法,其特征在于,定义某个所述传感器输出的当前时刻的数据为z(k),以及所述传感器输出的历史数据为z(k+1),z(k+
2),…z(k+n),其中,n为整数,经过所述传感器延迟对齐后的数据为所述传感器观测量,所述传感器观测量由表达式(1)所示,所述传感器时延由表达式(2)所示:Zdelay=z(k+Td)…(1);
Td=Tdelay/ΔT…(2);
其中,Tdelay为相对于时延最大的传感器的时延,ΔT为传感器的采样周期。
4.如权利要求3所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述步骤S2具体包括以下子步骤:根据所述当前时刻的数据和所述历史数据;设置所述当前时刻的数据为z(0),所述历史数据为z(k),其中,k=1,2,…n;
估计所述传感器所测得的实际距离,所述实际距离如表达式(3)所示:Yest(k)=b+ka,k=0,1,2…n…(3);
计算所述传感器的误差,所述误差如表达式(4)所示:Err(k)=Yest(k)‑Z(k),k=0,1,2…n…(4);
计算所述误差的方差,并带入到卡尔曼滤波算法中,如表达式(5)所示:
2 2
Variance=E(Err)‑E(Err) …(5);
其中,Yest表达式中的系数a和系数b分别由以下表达式(6)和表达式(7)计算:
5.如权利要求4所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述步骤S3具体包括以下子步骤:根据所述当前时刻的数据为z(0),所述历史数据为z(k),k=0,1,2,…n‑1,对所述传感器观测量一次微分计算如下:再记录所述传感器观测量的一次微分的序列值z1(k),k=0,1,2,…n‑1,并计算所述传感器观测量的二次微分:记录下所述传感器观测量的二次微分序列值,计算并记录惯性坐标系下天向加速度的序值:z2(k)(k=0,1,2,…n‑1)…(10);
T
az=Cbnaiz(iz=[0 0 1])…(11);
az(k)(k=0,1,2,…n‑1)…(12);
其中,a为加速度计三轴的值,Cbn为从飞行器坐标系到惯性坐标系的转换矩阵;最后计算观测量二次微分与无人机天向加速度的相关函数:所述相关函数
6.如权利要求1所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述步骤S4中,所述相对高度的计算方法具体如下:预设飞行器搭载n个测距传感器,每个所述测距传感器的观测噪声方差为Ri,每个测距传感器的二次微分项与天向加速度的相关系数为Gi,每个所述测距传感器的观测量为Zi,其中,i=1,2,…n,n为正整数;
则每个所述测距传感器的加权系数Ki,i=1,2,…n,计算如下:令
则
将所述观测量进行加权融合,计算所述飞行器距离地面的相对高度:r_H=K1Z1+K2Z2+…+KnZn…(16);
其中,H为所述飞行器的高度。
7.如权利要求6所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述步骤S5具体包括以下子步骤:先定义卡尔曼滤波状态方程:
其中,h为飞行器高度状态,v为飞行器天向速度状态,abiase为天向加速度Az的偏置误差,Δt为采样周期;
定义卡尔曼滤波观测方程:
将卡尔曼滤波状态方程和卡尔曼滤波观测方程得到卡尔曼滤波算法;
再用一阶卡尔曼滤波算法融合所述测距传感器的观测量与所述天向加速度数据,估计无人机的距地距离;
其中,一阶尔曼滤波的状态方程和观测方程如下:其中,r‑H为所述飞行器的相对高度,Vz为由三阶卡尔曼滤波算法估计的无人机天向速度。
8.如权利要求7所述的飞行器的强抗扰高度状态估计方法,其特征在于,所述高度观测量切换机制具体为:步骤S51、预设高度切换阈值;
步骤S52、判断所述相对高度是否小于所述高度切换阈值;若是,则对所述相对高度标志位置1;若否,则对所述相对高度标志位置0;其中,所述位置1和位置0为赋值;
步骤S53、判断所述相对高度标志位置是否发声跳变,若是,则记录当前的相对高度、绝对高度和所述飞行器的高度;若否,则返回所述步骤S52。
9.一种飞行器的强抗扰高度状态估计系统,其特征在于,所述系统包括:获取模块,用于获取传感器的数据并估计传感器时延,再对齐所述传感器时延,获得传感器观测量;
估计模块,用于根据所述传感器观测量,估计所述传感器的误差方差;
微分计算模块,用于计算所述传感器观测量的二次微分,并计算所述二次微分与惯性测量加速度的相关度;
高度计算模块,用于将所述传感器观测量和所述相关度进行数据加权融合,计算飞行器距离地面的相对高度;
处理模块,用于获取当前的绝对高度,分别对所述相对高度和所述绝对高度用三阶卡尔曼滤波算法进行滤波处理,再采用高度观测量切换机制,切换高度观测量计算飞行器的高度,最后用一阶卡尔曼滤波算法估计飞行器离地高度。