一种基于H∞和CKF混合滤波的机载分布式POS传递对准方法与流程

本发明涉及一种基于H∞和CKF混合滤波的机载分布式POS(DistributedPositionandOrientationSystem,分布式位置姿态测量系统)传递对准方法，可以应用于机载或舰载分布式POS系统中子IMU的位置、速度、姿态求解，特别适用于机体存在挠曲变形且难以建模的情况。
背景技术：
：高精度POS由惯性测量单元(InertialmeasurementUnit,IMU)、导航计算机系统(POSComputerSystem，PCS)和GPS组成。高精度POS可以为高分辨率航空遥感系统提供高频、高精度的时间、空间及精度信息，通过运动误差补偿提高成像精度和效率，是实现高分辨率成像的关键。但是由于对地观测载荷的需求牵引，如集成高分辨测绘相机、全谱段成像光谱仪、SAR于同一载体的多任务载荷，机载分布式阵列天线SAR和柔性多基线干涉SAR以及舰载稀疏阵列成像雷达等，多个或多种载荷安装在飞机不同位置，采用传统的单一POS系统无法实现多点的高精度位置姿态测量以及各载荷数据的时间统一。同时对于集成多个载荷的航空遥感系统和阵列载荷，由于飞机机体和柔性杆臂的挠曲变形、振动等因素，单个POS无法测量分布在飞机不同位置多个载荷的位置速度姿态信息。因此迫切需要建立高精度分布式时空基准系统，为高性能航空遥感系统中所有载荷提供高精度的时间、空间信息。决定分布式POS测量精度的一个关键问题是子IMU传递对准精度。现有的传递对准方法(公开号1：CN102621565公开号2：CN103913181)需要建立挠曲变形模型进而将变形角扩展到滤波器中。然而分布式POS系统结构复杂，安装环境多样，由于机体收空气气流、机翼颤动及发动机噪声等因素影响使机体产生挠曲变形，同时由于载荷弹性安装、载荷变更等原因也会增加主子IMU之间挠曲变形程度，导致挠曲变形模型的不确定性，而传统的建模及滤波已不能保证系统的精度和稳定性。CKF是一种新近提出的滤波估计算法，它采用一组等权值的Cubature点集来解决贝叶波的积分问题，为非线性估计问题提供了一种新的实现方式和程度。而H∞滤波方法具有很强的抗干扰能力，特别是系统存在有色噪声或统计特性未知的干扰时，具有很好的鲁棒性，而基本的H∞鲁棒滤波仍然是一种线性方法。因此，在挠曲变形模型未知且时变的情况下，需要提出一种抗干扰能力强稳定性好精度高的非线性滤波方法。技术实现要素：本发明的技术解决问题是：克服现有技术的不足，提出了一种基于H∞和CKF混合滤波的机载分布式POS传递对准方法，解决了在挠曲变形难以建模的情况下，无法准确估计子系统位置速度姿态信息的问题，具有精度高、抗干扰能力强的优点。本发明技术解决方案是：一种基于H∞和CKF混合滤波的机载分布式POS传递对准方法，实现步骤如下：(1)考虑到挠曲变形难以建模且时变，将挠曲变形误差纳入到量测噪声中，建立系统的速度匹配量测方程和姿态匹配量测方程，得到分布式位置姿态测量系统(分布式POS)的传递对准模型。(2)以卡尔曼滤波为基础，利用三自由度Spherical-Radial求容积规则，采用一组等权值的容积点设计非线性滤波算法即容积卡尔曼滤波算法CKF；(3)利用H∞滤波良好的鲁棒自适应能力达到减弱和消除挠曲变形和动态杆臂的目的，设计H∞滤波算法；(4)结合步骤(2)CKF滤波和步骤(3)H∞滤波的特点和方法，设计H∞与CKF混合滤波算法，将H∞滤波方法应用到非线性方程中。使用该混合滤波算法对步骤(1)建立的传递对准模型中的量测值进行估计，可以得到更加准确的子惯性测量单元位置速度姿态信息，完成传递对准。步骤(1)所述的传递对准模型包括速度匹配量测方程和姿态匹配量测方程；(1)速度匹配量测方程Zv=δv-(δvmn+δvLAn+δvsn)]]>式中，δv为速度误差，为杆臂速度计算误差，为子惯导速度急速误差不必列入状态，作为白噪声处理；(2)姿态匹配量测方程设主惯导输出姿态矩阵为子惯导输出姿态矩阵为则姿态匹配量测阵可构造为：ZDCM=C^bmm(C^bsm)T=C^bmmC^nbs]]>由于C^nbs=CnbsCpn]]>式中，为主惯导的姿态误差角，视为白噪声，表示构成的反对称矩阵。同时假设主子惯导之间存在误差角φ＝(θ,γ,ψ)T，这里μ为主子惯导之间安装误差角，λf为挠曲变形角，为白噪声，则主子惯导的姿态转化矩阵为：可得由于挠曲变形模型的不确定性，不将挠曲变形角作为状态，θ,γ,ψ由φ＝μ求取，将挠曲变形归入到量测噪声中；由主子惯导姿态阵乘积得到相对姿态阵，并根据欧拉角与方向余弦阵的关系，得到姿态匹配量测方程为：Za=arcsin(ZDCM(3,2))arctan(-ZDCM(3,1)ZDCM(3,3))arctan(-ZDCM(1,2),ZDCM(2,2))+vθ]]>式中vm表示白噪声和挠曲变形干扰的综合作用。步骤(2)中，以卡尔曼滤波为基础，利用三自由度Spherical-Radial求容积规则，采用一组等权值的容积点设计非线性滤波算法即容积卡尔曼滤波算法CKF。利用三自由度Spherical-Radial求容积规则，采用一组等权值的容积点设计非线性滤波算法即容积卡尔曼滤波算法CKF，具体如下：考虑非线性离散状态空间模型：Xk=F(Xk-1)+wkzk=H(Xk)+Vk]]>式中Xk∈Rn和zk∈Rn分别为状态向量和量测向量，F和H为系统状态和量测的非线性转换；过程噪声wk和量测噪声Vk为互不相关的高斯白噪声，且均值和协方差矩阵为：E[wk]=0,E[wkwjT]=QδkjE[Vk]=0,E[VkVjT]=Rδkj]]>式中，δkj为Kronecker-Delta函数；将上述非线性逼近及求积分计算的思想应用于高斯贝叶斯估计，即可得到CKF滤波算法，具体步骤如下：a.时间预测①计算容积点②容积点传播③估计预测均值和协方差阵x^k-=12nΣi=12nXi,k*,Pk-=12nΣi=12nXi,k*(Xi,k*)T-x^k-(x^k-)T+Qk-1]]>b.量测更新①计算容积点②容积点传播Zi,k＝h(Xi,k,uk),i＝1,…,2n③计算量测预测值、新息方差和协方差阵z^k-=12nΣi=12nZi,k,Pzz,k=12nΣi=12nZi,kZi,kT-z^k-(z^k-)T+Rk,Pxz,k=12nΣi=12nXi,kZi,kT-x^k-(z^k-)T]]>④计算量测更新其中步骤(3)所述的H∞滤波算法可以描述为：给定正数γ>0，寻找次优H∞估计使得||Tk(Ff)||<γ，及满足其中P0为初始时刻状态变量的估计误差，Wk,Vk分别为k时刻的系统噪声序列和观测噪声序列，ek为滤波误差。此时，H∞滤波问题有解的充分必要条件为：Pk-1+HkTHk-γ-2LkTLk>0]]>得到H∞滤波递推公式为：Rk=I00-γ2I+HkGkPkHkTGkT]]>Pk+1=Fk+1/kPkFk+1/kT+Qk+1-Fk+1/kPkHk+1TGk+1TRk-1Hk+1Gk+1PkFk+1/kT]]>Kk+1=Pk+1Hk+1T(I+Hk+1Pk+1Hk+1T)-1]]>X^k+1=Fk+1/kX^k+Kk+1(Yk+1-Hk+1Fk+1/kX^k)]]>式中，Rk为k时刻观测噪声的估计方差阵，Kk+1为k+1时刻增益矩阵，为k时刻状态向量估计值。步骤(4)所述的混合滤波算法是将容积卡尔曼滤波方法应用于H∞滤波器，首先对线性H∞滤波方法中实现状态估计协方差阵递推的Riccati方程进行转换，状态协方差阵Pk的H∞鲁棒更新形式写成：Pk=Pk/k-1-Pxz,kPk/k-1Pzz,k-Rk+IPxz,kTPxz,kPk/k-1-γ2I-1Pxz,kTPk/k-1T]]>式中Pxz,k为k时刻状态预测和量测预测的协方差阵，Pzz,k为k时刻观测向量z的一步预测方差阵，下标k/k-1表示由k-1求得k时刻的一步预测。具体实现步骤如下：(1)H∞和CKF混合滤波进行初始化X^0=E(X0),P0=E(X0X0T)]]>(2)时间更新Pk-1|k-1Sk-1|k-1Sk-1|k-1T,Xi,k-1|k-1=Sk-1|k-1ξi+Xk-1|k-1,]]>Xk-1|k-1*=F(Xi,k-1|k-1),X^i,k-1|k-1=Σi=12nXi,k-1|k-1*,]]>Pxx,k-1|k-1=Σi=12nwi(Xi,k-1|k-1*-X^k-1|k-1)(Xi,k-1|k-1*-X^k-1|k-1)T+Qk-1,]]>Pxx,k|k-1=Sk|k-1Sk|k-1T,Xi,k|k-1=Sk|k-1ξi+X^k|k-1]]>Zi,k|k-1=H(Xi,k|k-1),z^k|k-1=Σi=12nwiZi,k|k-1,]]>Pzz,k|k-1=Σi=12nwi(Zi,k|k-1-z^k|k-1)(Zi,k|k-1-z^k|k-1)T+I,]]>Pxz,k|k-1=Σi=12nwi(Xi,k|k-1*-Xk|k-1)(Zi,k|k-1-z^k|k-1)T.]]>(3)量测更新Kk=Pxz,k|k-1Pzz,k|k-1-1,X^k|k=Xk|k-1+Kk(zk-z^k|k-1),]]>Re,k=I00-γ2I+Σi=12nwi(zi,k|k-1-z^k|k-1)(zi,k|k-1-z^k|k-1)TPxz,k|k-1TPxz,k|k-1I,]]>Pxz,k=Pk-1|k-1-Pk-1|k-1[HkTLkT]Re,k-1HkLkPk-1|k-1T.]]>通过该混合滤波对建立的传递对准模型中的量测值进行滤波估计，估计出子系统的位置误差、速度误差和姿态误差，利用上述误差修正子IMU捷联解算结果，得到更准确的子系统位置速度姿态信息和主/子IMU之间的相对空间关系。本发明与现有技术相比的优点在于：本发明针对传递对准滤波算法问题，采用H∞与CKF混合滤波的方法。由于在传递对准过程中，由于主子IMU之间的挠曲变形难以准确建模，这会导致滤波的精度和稳定性下降。而H∞滤波方法具有很强的抗干扰力，特别是系统存在有色噪声或统计特性未知的干扰时，具有很好的鲁棒性，因此，在CKF滤波中运用H∞滤波思想，以解决在挠曲变形难以建模时大失准角下传递对准的非线性滤波问题。附图说明图1为本发明的系统流程框图；图2为本发明的滤波算法图。具体实施方式如图1所示，本发明的具体实施步骤如下：1.建立系统的量测方程，将挠曲变形误差纳入到量测噪声，具体实时步骤如下：(1)速度匹配量测方程Zv=δv-(δvmn+δvLAn+δvsn)]]>式中，δv为速度误差，为杆臂速度计算误差，为子惯导速度急速误差不必列入状态，作为白噪声处理。(2)姿态匹配量测方程设主惯导输出姿态矩阵为子惯导输出姿态矩阵为则姿态匹配量测阵可构造为ZDCM=C^bmm(C^bsm)T=C^bmmC^nbs]]>由于C^nbs=CnbsCpn]]>式中，为主惯导的姿态误差角，视为白噪声，表示构成的反对称矩阵。同时假设主子惯导之间存在误差角φ＝(θ,γ,ψ)T，这里μ为主子惯导之间安装误差角，λf为挠曲变形角，为白噪声，则主子惯导的姿态转化矩阵为：Cbmbs=C2(γ)C1(θ)C3(ψ)]]>可得由于挠曲变形模型的不确定性，不将挠曲变形角作为状态，θ,γ,ψ由φ＝μ求取，将挠曲变形归入到量测噪声中。由主子惯导姿态阵成绩得到相对姿态阵，并根据欧拉角与方向余弦阵的关系，得到姿态匹配量测方程为：Za=arcsin(ZDCM(3,2))arctan(-ZDCM(3,1),ZDCM(3,3))arctan(-ZDCM(1,2),ZDCM(2,2))+vθ]]>式中vm表示白噪声和挠曲变形干扰的综合作用。2.建立系统传递对准模型设状态向量为状态空间模型为通过速度匹配量测方程和姿态匹配量测方程建立传递对准量测方程：Z=ZvaZangle=δvnarcsin(ZDCM(3,2))arctan(-ZDCM(3,1),ZDCM(3,3))arctan(-ZDCM(1,2),ZDCM(2,2))+vnvθ.]]>3.CKF算法实现步骤如下：(1)基于高斯假设的贝叶斯估计基本框架，将非线性滤波归结为非线性函数与高斯概率密度乘积的积分求解问题，即：I(f)=∫Rnf(X)e-XTXdX]]>式中:I(f)为所求积分；X为滤波估计状态向量，f(X)为求积非线性函数；Rn为积分区间。(2)采用3自由度Spherical-Radial求容积规则，采用一组2n个等权值的容积点(cubaturepoint)来实现非线性逼近，即：式中考虑如下的非线性离散状态空间模型:Xk=F(Xk-1)+wkzk=H(Xk)+Vk]]>式中：Xk∈Rn和zk∈Rn分别为状态向量和量测向量；F和H为系统状态和量测的非线性转换；过程噪声wk和量测噪声Vk为互不相关的高斯白噪声，且均值和协方差矩阵分别为：E[wk]=0,E[wkwjT]=QδkjE[Vk]=0,E[VkVjT]=Rδkj]]>式中δkj为Kronecker-Delta函数。(3)将非线性逼近及求积分计算的思想应用于高斯贝叶斯估计，即可得到CKF滤波算法:a.时间预测①计算容积点②容积点传播③估计预测均值和协方差阵x^k-=12nΣi=12nXi,k*,]]>Pk-=12nΣi=12nXi,k*(Xi,k*)T-x^k-(x^k-)T+Qk-1]]>b.量测更新①计算容积点②容积点传播Zi,k＝h(Xi,k,uk),i＝1,…,2n③计算量测预测值、新息方差和协方差阵z^k-=12nΣi=12nZi,k,]]>Pzz,k=12Σi=12nZi,kZi,kT-z^k-(z^k-)T+Rk,]]>Pxz,k=12nΣi=12nXi,kZi,kT-x^k-(z^k-)T]]>④计算量测更新其中Pk+=Pk--KkPzz,kKkT]]>4.H∞滤波的实现步骤如下:(1)考虑离散时间非线性动态系统xk＝f(xk-1)+wk-1zk＝h(xk)+vk式中xk,zk分别为k时刻系统状态向量和量测向量；f(),h()分别为系统状态转移模型非线性函数和量测模型非线性函数；wk-1,vk分别为过程噪声向量和观测噪声向量，其协方差阵分别为Qk-1,Rk，进而定义代价函数：J=Σk=1N||xk-x^k||2||x0-x^0||Po-12+Σk=1N(||wk||Qk-12+||vk||Rk-12)]]>||x0-x^0||P0-12=(x0-x^0)TP0-1(x0-x^0)]]>式中，x0为系统初始状态，其方差为P0，为x0对应的估计值；和的计算过程与类似。(2)在各种可能条件下，使估计误差的能量经输入噪声能量和初始误差能量归一化后达到最小，即寻求输出xk使得通常可以寻求次优迭代算法，设定一个门限值γ满足如下Riccati不等式：Pk-1+HkTHk-γ-2LkTLk>0]]>式中Pk为k时刻状态向量的方差阵；Hk为量测矩阵；Lk为约束方程系数矩阵,一般设为单位矩阵I。在H∞滤波器中，参数γ控制状态估计在最不利条件下的估计误差，约束水平γ越小，则系统的鲁棒性越强，约束水平γ越大，则H∞滤波的特性越接近于标准卡尔曼滤波。5.H∞和CKF混合滤波算法的实现过程如下：(1)将容积卡尔曼滤波方法应用于H∞滤波器，首先对线性H∞滤波方法中实现状态估计协方差阵递推的Riccati方程进行转换,由于状态协方差阵Pk的H∞鲁棒更新形式可以写成：式中Pxz,k为k时刻状态预测和量测预测的协方差阵，Pzz,k为k时刻观测向量z的一步预测方差阵，下标k/k-1表示由k-1求得k时刻的一步预测。(2)H∞和CKF混合滤波进行初始化X^0=E(X0),P0=E(X0X0T)]]>(3)时间更新Pxx,k|k-1=Sk|k-1Sk|k-1T,Xi,k-1|k-1=Sk|k-1ξi+X^k|k-1,]]>Xk-1|k-1*=F(Xi,k-1|k-1),X^i,k-1|k-1=Σi=12nXi,k-1|k-1*,]]>Pxx,k-1|k-1=Σi=12nwi(Xi,k-1|k-1*-X^k-1|k-1)(Xi,k-1|k-1*-X^k-1|k-1)T+Qk-1,]]>Pxx,k|k-1=Sk|k-1Sk|k-1T,Xi,k|k-1=Sk|k-1ξi+X^k|k-1,]]>Zi,k|k-1=H(Xi,k|k-1),z^k|k-1=Σi=12nwiZi,k|k-1,]]>Pzz,k|k-1=Σi=12nwi(Zi,k|k-1-z^k|k-1)(Zi,k|k-1-z^k|k-1)T+I,]]>Pxz,k|k-1=Σi=12nwi(Xi,k|k-1*-Xk|k-1)(Zi,k|k-1-z^k|k-1)T.]]>(3)量测更新Kk=Pxz,k|k-1Pzz,k|k-1-1,X^k|k=Xk|k-1+Kk(zk-z^k|k-1),]]>Re,k=I00-γ2I+Σi=12nwi(zi,k|k-1-z^k|k-1)(zi,k|k-1-z^k|k-1)TPxz,k|k-1TPxz,k|k-1I,]]>Pxz,k=Pk-1|k-1-Pk-1|k-1[HkTLkT]Re,k-1HkLkPk-1|k-1T.]]>通过该混合滤波对传递对准模型中的量测值进行滤波估计，估计出子系统的位置误差、速度误差和姿态误差，利用上述误差修正子IMU捷联解算结果，得到更准确的子系统位置速度姿态信息。当前第1页1 2 3