本发明涉及一种针对动力学指令丢包的无人机复合非高斯姿态估计方法。针对内嵌动力学的无人机惯性/偏振组合定姿系统，考虑动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声的影响，首先，建立内嵌动力学的无人机惯性/偏振组合定姿系统模型，并对动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声等复合非高斯干扰进行精细表征，建立丢包指示因子的分层概率模型；其次，设计含有阵风干扰补偿和丢包指示因子后验分布跟踪的复合非高斯姿态估计方法，实现复合非高斯干扰情形下无人机姿态后验分布的实时估计。本发明能够改善无人机惯性/偏振组合定姿系统对复合非高斯干扰的辨识和补偿能力，提高复杂未知环境与间歇通讯条件下无人机姿态解算的精度与可靠性，可应用于各类旋翼无人机的室外自主导航任务。
背景技术：
1、对于无人机惯性/偏振组合定姿系统而言，粒子滤波是目前应用最为广泛的一类算法，它不依赖于高斯假设，理论上可以达到任意高精度。然而，现有粒子滤波算法大多依赖于精确已知的模型信息，难以处理阵风干扰和未知动力学指令丢包概率带来的状态转移分布不确定性。因此，有必要通过改进现有粒子滤波算法，提升无人机惯性/偏振组合定姿系统对阵风干扰和未知动力学指令丢包概率的适应能力。
2、上述无人机惯性/偏振组合定姿系统滤波解算问题可以归结为动态干扰和未知参数情形下的改进粒子滤波算法设计问题。目前，与之相关的研究结果较少，已有结果主要包括鲁棒粒子滤波和参数自适应粒子滤波。其中，鲁棒粒子滤波仅能处理具有动态特性的干扰信号，无法对模型参数(如未知动力学指令丢包概率)进行自适应辨识；参数自适应粒子滤波能够对未知模型参数进行实时更新，但面对动态干扰信号(如阵风干扰)时估计性能难以保证。
技术实现思路
1、本发明要解决的问题是：考虑无人机惯性/偏振组合定姿系统中的未知动力学指令丢包概率、阵风干扰、惯性器件误差和偏振测量噪声等复合非高斯干扰，克服现有粒子滤波算法在复合非高斯干扰情形下精度和鲁棒性不足的问题，提出针对动力学指令丢包的无人机的复合非高斯滤波器设计方法。该方法基于内嵌动力学的无人机惯性/偏振组合定姿系统模型，对动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声等复合非高斯干扰进行精细表征，建立丢包指示因子的分层概率模型，并设计了含有阵风干扰补偿和丢包指示因子后验分布跟踪的复合非高斯姿态估计方法，实现复合非高斯干扰情形下无人机姿态后验分布的实时估计。
2、本发明的技术解决方案为：一种针对动力学指令丢包的无人机复合非高斯姿态估计方法，其实现步骤如下：
3、第一步：建立内嵌动力学的无人机惯性/偏振组合定姿系统模型，并对动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声等复合非高斯干扰进行精细表征，建立丢包指示因子的分层概率模型：
4、首先，建立状态方程如下：
5、
6、其中，φ(t)、θ(t)、ψ(t)分别为无人机在导航坐标系“n-系”下的滚转角、俯仰角与偏航角；p(t)、q(t)、r(t)分别为无人机在机体坐标系“b-系”下的角速度在x轴、y轴和z轴的分量；mx(t)、my(t)、mz(t)分别为无人机力矩在x轴、y轴和z轴的分量；ixx、iyy、izz为无人机的转动惯量，通常由无人机说明书给出；wp(t)、wq(t)、wr(t)均为零均值、方差已知的高斯噪声项；γ(t)为丢包指示因子，取值为1或0，其中1代表动力学指令正常，0代表动力学指令发生丢包；dp(t)、dq(t)、dr(t)分别为阵风干扰在“b-系”x轴、y轴和z轴的分量，定义d(t)＝[dp(t)dq(t)dr(t)]t，将其动态特性描述为如下的一阶马尔科夫过程：
7、
8、其中，为阵风干扰子系统的状态变量，v＝diag{[ap 0],[aq 0],[ar 0]}，且σp、σq、σr为谐波干扰的角频率，ap、aq、ar为谐波干扰的幅值；n(t)为均值为零、方差已知的高斯噪声项；
9、其次，建立陀螺仪与加速度计量测方程如下：
10、
11、其中，分别为由陀螺仪测量得到的x轴、y轴和z轴角速度分量；分别为加速度计测量得到的x轴和y轴加速度分量；g为当地重力加速度；vp(t)、vq(t)、vr(t)、vx(t)、vy(t)为惯性导航传感器量测噪声，服从均值为零、方差已知的高斯分布；
12、接着，建立偏振传感器量测方程如下：
13、
14、其中，为偏振传感器实时量测值；vpol(t)为偏振传感器量测噪声，服从高斯分布；ki为已知的偏振传感器光强增益系数；kd为已知的偏振传感器光路耦合系数；a为已知的太阳方向角；为已知的偏振检测角；iin为已知的入射光强系数；
15、进一步，将式(1)、(2)、(3)、(4)进行联立得到：
16、
17、其中，x(t)＝[φ(t)θ(t)ψ(t)p(t)q(t)r(t)]t，u(t)＝[000mx(t)my(t)mz(t)]t，w(t)＝[000wp(t)wq(t)wr(t)]t，v(t)＝[vp(t)vq(t)vr(t)ax(t)ay(t)iout(t)]t；与分别为由公式(1)与公式(3)中只与x(t)有关的项构成的非线性系统模型与量测方程；
18、最后，对式(5)所描述的模型进行离散化，得到内嵌动力学的无人机惯性/偏振组合定姿系统离散状态空间模型如下：
19、
20、其中，wk＝i+wts，vk＝v；非线性系统模型与量测方程f(·)和h(·)可分别根据和以及采样时间间隔ts计算得到；wk、nk和vk为高斯随机噪声项，满足：wk～n(0,σw,k)，nk～n(0,σn,k)，和vk～n(0,σv,k)，且σn,k、σw,k和σv,k均是已知的；γk为第k时刻的丢包指示变量，其概率分布服从如下分层概率模型：
21、γk～bern(γk；1-τk),τk～beta(τk；αk,βk) (7)
22、其中，bern(p)表示成功概率为p的伯努利分布；τk为未知的丢包概率，服从参数为αk与βk的beta分布；
23、第二步：针对系统结构参数已知的情形，设计含有阵风干扰补偿和丢包指示因子后验分布跟踪的复合非高斯姿态估计方法，实现复合非高斯干扰情形下无人机姿态后验分布的实时估计：
24、在第k时刻，首先根据第k-1时刻得到的粒子集以及对应于每个粒子的参数估计值计算第k时刻对应于每个粒子的参数预测值如下：
25、
26、其中，ρ为预先指定的遗忘因子，满足0＜ρ＜1；
27、接着，利用上述参数预测值及各参数分布的分布类型，采样第k时刻对应于每个粒子的参数：
28、
29、其中，n为预先指定的粒子数目；n(·；μ,σ)表示均值为μ、方差为σ的高斯分布；e[·]表示对随机变量求期望；然后，结合第k-1时刻滤波器计算得到的粒子集以及系统模型(6)，生成粒子集
30、
31、其次，对每个粒子定义e<0>[γk]＝ei-[γk]，e<0)[log(1-τk)]＝ei-[log(1-τk)]，
32、e(0)[logτk]＝ei-[logτk]，其中ei-[γk]、ei-[log(1-τk)]和ei-[logτk]由下式计算得到：
33、
34、其中，ψ(·)表示反gamma函数；然后，对j＝1,2,...,nmax，循环执行如下计算步骤：
35、
36、其中，nmax为预先指定的最大迭代次数；为对应于第i个粒子的虚拟量测量；λ<j>为用于保证pr<j>(γk＝1)+pr<j>(γk＝0)＝1成立的归一化因子；循环结束后，将参数估计值更新为
37、
38、接下来，对i＝1,2,...,n，计算粒子所对应的权重如下：
39、
40、其中，wk,i为粒子权重；根据权重生成重采样序列{r1,r2,...,rn}，其中ri表示重采样后第i个粒子的序号，满足其中l(x)为指示函数，满足根据重采样序列，对粒子及其关联的参数进行更新如下：
41、
42、最后，输出粒子集丢包指示变量估计值和参数估计值和作为第k+1时刻滤波器的输入变量。
43、本发明与现有技术相比的优点在于：
44、(1)本发明以无人机仿生自主组合导航系统为研究对象，引入加速度计、陀螺仪与偏振传感器实现对无人机欧拉角与角速度的观测，同时考虑对四旋翼无人机姿态动力学稳定性影响较大的动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声，基于物理实际建立了内嵌动力学的无人机惯性/偏振组合定姿系统模型，并对动力学指令丢包、阵风干扰、惯性器件误差和偏振测量噪声等复合非高斯干扰进行了精细表征，建立了丢包指示因子的分层概率模型；
45、(2)本发明采用复合非高斯滤波器同时实现阵风干扰补偿、动力学指令丢包指示因子概率分布的在线跟踪与四旋翼无人机姿态后验分布的实时估计，克服了现有鲁棒惯性/偏振组合定姿滤波算法在动态干扰(阵风干扰)与未知随机参数(动力学指令丢包指示因子)情形下估计精度与鲁棒性不足的问题。