[发明专利]一种基于联邦滤波的机载分布式POS实时传递对准方法

摘要

一种基于联邦滤波的机载分布式POS实时传递对准方法。机载分布式POS由一个高精度主POS和多个子IMU组成。针对机载分布式POS实时传递对准对实时性要求高的问题，采用基于分散结构的联邦滤波进行传递对准。首先，进行联邦滤波器结构设计，包括两个子滤波器和一个主滤波器；其次，建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；再次，由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置；最后，利用重置后的误差状态量计算出更加准确的子IMU的位置、速度和姿态。

主权项

一种基于联邦滤波的机载分布式POS实时传递对准方法，其特征在于具体步骤为：(1)进行联邦滤波器结构设计，包括两个子滤波器和一个主滤波器；在子滤波器1中子IMU和主POS采用速度匹配方式进行局部滤波得到局部状态估计值和协方差阵Pv；在子滤波器2中子IMU与主POS采用姿态匹配方式进行局部滤波得到局部状态估计值和协方差阵Pa；最后将各子滤波器公共状态的估计值和协方差阵Pcv和Pca送入主滤波器并与主滤波器的状态估计值和协方差阵Pm进行融合，得到全局最优估计值和协方差阵Pg；(2)建立两个子滤波器的数学模型，并进行卡尔曼滤波，获得局部估计；1)选取子滤波器的状态变量相关参考坐标系的定义包括：记i为地心惯性坐标系；e为地球坐标系；主POS和子IMU导航坐标系均为东北天地理坐标系，分别用n和n1表示；载体坐标系原点为载体重心，x轴沿载体横轴向右，y轴沿载体纵轴向前，z轴沿载体竖轴向上，该坐标系固定在载体上，称为右前上载体坐标系，用bm和b分别代表主POS和子IMU的载体坐标系；①子滤波器1的状态变量Xv=[φEv,φNv,φUv,δVE,δVN,δVU,ϵxv,ϵyv,ϵzv,▿x,▿y,▿z]T]]>其中，和分别为子IMU在子滤波器1中的东向、北向和天向失准角，下标E、N和U分别表示东向、北向和天向；δVE、δVN和δVU分别为子IMU的东向、北向和天向速度误差；和分别为子IMU在子滤波器1中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；和分别为子IMU载体坐标系x轴、y轴和z轴的加速度计常值偏置；②子滤波器2的状态变量由于安装误差角和弹性变形角对子IMU的姿态角精度影响较大，所以在子滤波器2中将安装误差角和弹性变形角以及弹性变形角速率均扩充为状态变量，子滤波器2的状态变量Xa共15维，具体为；Xa=[(φEa,φNa,φUa,ϵxa,ϵya,ϵza,ρx,ρy,ρz,θx,θy,θz,θ·x,θ·y,θ·z]T]]>其中，和分别为子IMU在子滤波器2中的东向、北向和天向失准角；和分别为子IMU在子滤波器2中的载体坐标系x轴、y轴和z轴的陀螺常值漂移；ρx、ρy和ρz分别为子IMU相对于主POS的安装误差角在子IMU载体坐标系x轴、y轴和z轴上的分量；θx、θy和θz分别为子IMU相对于主POS的弹性变形角在子IMU载体坐标系x轴、y轴和z轴上的分量，和分别为对应的弹性变形角速率；2)建立子滤波器的数学模型①建立子滤波器1的数学模型a)状态方程子滤波器1的系统状态方程如下：X·v=FvXv+Gvωv]]>其中，Fv、Gv和ωv分别为子滤波器1的状态转移矩阵、系统噪声驱动阵和系统噪声向量；和分别为子IMU载体坐标系x轴、y轴、z轴陀螺和加速度计的随机误差，不包括随机常值误差，其方差阵Qv包含Qav和Qcv两部分，其中Qcv＝1/β1Q，β1＝1/3，Q为陀螺随机误差噪声方差阵，根据陀螺的噪声水平进行选取，Qav为加速度计随机误差噪声方差阵，根据加速度计的噪声水平进行选取；根据对子滤波器1中状态变量的选取可知，子滤波器1中的误差方程包括姿态误差方程、速度误差方程和惯性仪表误差方程，分别为：φ·vn1=-ωin1n1×φvn1+δωin1n1+Cbn1ϵvb]]>δV·n1=fn1×φvn1-(2δωien1+δωen1n1)×Un1-(2ωien1+ωen1n1)×δVn1+Cbn1▿b]]>ϵ·v=0]]>▿·=0]]>其中，为子IMU导航坐标系相对于惯性坐标系的旋转角速度在其导航坐标系中的投影；为的计算误差；为子IMU载体坐标系到其导航坐标系的转换矩阵；为陀螺随机误差，包括随机常值漂移和白噪声漂移；VE、VN和VU分别为子IMU导航坐标系的东向、北向和天向速度；fE、fN和fU分别为子IMU在其导航坐标系中东向、北向和天向加速度；为地球坐标系相对惯性坐标系的旋转角速度在子IMU导航坐标系的投影；为子IMU导航坐标系相对地球坐标系的旋转角速度在其导航坐标系的投影；和分别为和的计算误差；为加速度计随机误差，包括随机常值偏置和白噪声偏置；根据上述误差方程确定的Fv和Gv分别为：Fv=F1F2Cbn103×3F3F403×3Cbn103×303×303×303×303×303×303×303×3]]>F1=0ωiesinL+VEtanLRN+H-(ωiecosL+VERN+H)-(ωiesinL+VEtanLRN+H)0-VNRM+HωiecosL+VERN+HVNRM+H0]]>F2=0-1RM+HTVN(RM+H)21RN+HTωiesinLRM+H-(TVNωiesinL(RM+H)2+TVEtanL(RN+H)2)tanLRN+H(VEsec2LRN+H-ωiecosL)TRM+H-((VEsec2LRN+H-ωiecosL)TVN(RM+H)2+TVEtanL(RN+N)2)]]>F3=0-fUfNfU0-fE-fNfE0]]>F4=VNtanLRN+H-VURN+H2ωiesinL+VEtanLRN+H+A1TRM+H-2ωiecosL-VERN+H-A1TVN(RM+H)2+A2T-2(ωiesinL+VEtanLRN+H)-VURM+H+A3TRM+H-VNRM+H-A3TVN(RM+H)2+A4T2(ωiecosL+VERN+H)2VNRM+H+A5TRM+H-A5TVN(RM+H)2+A6T]]>Gv=Cbn103×303×3Cbn106×306×3]]>其中，ωie为地球自转角速度；RM和RN分别为地球沿子午圈和卯酉圈的主曲率半径；L和H分别为子IMU的纬度和高度；T为滤波周期；式中，A1=2ωieVNcosL+VEVNsec2LRN+H+2ωieVUsinL,A2=VEVU-VEVNtanL(RN+H)2,]]>A5＝‑2ωieVE sin L以及A6=-VE2+VN2(RN+H)2;]]>b)量测方程子滤波器1的量测方程为：Zv＝HxXv+vv其中，系统观测量为：Zv＝[δV′E δV′N δV′U]T，δV′E、δV′N和δV′U分别为主POS经杆臂速度补偿后与子IMU之间的东向、北向和天向速度之差，杆臂速度的计算公式为：Vrn=Cbmn(ωibmbm×rbm)+Cbmnr·bm]]>其中，为主POS载体坐标系到其导航坐标系的转换矩阵；为主POS载体坐标系相对于惯性坐标系的旋转角速度在其载体坐标系中的投影；为主、子系统之间的杆臂在主POS载体坐标系上的投影；系统量测噪声和分别为主POS东向、北向和天向速度的量测噪声，vv为零均值高斯白噪声，其方差阵Rv由主POS的速度精度决定，量测矩阵Hv为：Hv＝[03×3 I3×3 03×6]②建立子滤波器2的数学模型a)状态方程子滤波器2的系统状态方程如下：X·a=FaXa+Gaωa+BaUa]]>其中，Fa、Ga、ωa和Ba分别为子滤波器2的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值漂移，ηx、ηy和ηz分别为子IMU载体坐标系x轴、y轴和z轴弹性变形角二阶Markov过程白噪声，ωa对应的噪声方差阵Qa包括Qca和Qη两部分，其中Qca＝1/β2Q，β2＝1/3，Qη为二阶Markov过程白噪声的方差；Ua为已知的非随机函数，Ua＝[δVE δVN δVU]T，δVE、δVN和δVU为子滤波器1中估计出来的速度误差；根据对子滤波器2中状态变量的选取可知，子滤波器2中的误差方程包括姿态误差方程、陀螺误差方程、安装误差角方程和弹性变形角方程，其中姿态误差方程和陀螺误差方程表达式与子滤波器1中的姿态误差方程和陀螺误差方程相同，安装误差角方程和弹性变形角方程分别为：ρ·=0]]>θ··J+2βJθ·J+βJ2θJ=ηJ,J=x,y,z]]>其中，ρ＝[ρx ρy ρz]T；θJ用二阶Markov过程模型来描述；βJ＝2.146/τJ，τJ为二阶Markov过程相关时间；ηJ为零均值白噪声，其方差满足：QηJ=4βJ3σJ2]]>其中，σJ2为弹性变形角θJ的方差，根据上述误差方程确定的Fa、Ga和Ba分别为：Fa=F1Cbn103×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×303×3I3×303×303×303×3B1B2]]>B1=-βx2000-βy2000-βz2]]>B2=-2βx000-2βy000-2βz]]>Ga=Cbn103×303×303×306×306×303×3I3×3]]>Ba=F2012×3]]>其中，F1和F2的含义与子滤波器1状态转移矩阵中F1和F2相同；b)量测方程子滤波器2的量测方程为：Za＝HaXa+va其中，系统观测量为：Za＝[δψ δθ δγ]T，δψ、δθ和δγ分别为子IMU与主POS之间的航向角、俯仰角和横滚角之差；系统量测噪声为va＝[vδψ vδθ vδγ]T，vδψ、vδθ、vδγ分别为主POS航向角、俯仰角和横滚角的测量噪声，va取为零均值高斯白噪声，其方差阵Ra由主POS的姿态精度决定，量测矩阵Ha为：Ha＝[H1 03×3 H2 H3 03×3]H1=Ta(12)Ta(32)(Ta(12))2+(Ta(22))20-1-Ta(22)1-(Ta(32))2Ta(12)1-(Ta(32))20Ta(21)Ta(33)-Ta(31)Ta(23)(Ta(33))2+(Ta(31))2Ta(31)Ta(13)-Ta(11)Ta(33)(Ta(33))2+(Ta(31))20]]>H2=H3=Ta(12)Ta(23)-Ta(13)Ta(22)(Ta(12))2+(Ta(22))20Ta(11)Ta(22)-Ta(12)Ta(21)(Ta(12))2+(Ta(22))2Ta(33)1-(Ta(32))20-Ta(31)1-(Ta(32))2-Ta(31)Ta(32)(Ta(33))2+(Ta(31))21-Ta(32)Ta(33)(Ta(33))2+(Ta(31))2]]>其中，为主POS载体坐标系到其导航坐标系的转换矩阵，表示Ta的第j行、第l列元素；3)计算局部估计值根据子滤波器1和子滤波器2的数学模型分别进行卡尔曼滤波来求解各自的局部估计值，并将子滤波器1和子滤波器2中的公共状态变量，即子IMU的失准角和陀螺的常值漂移，估计值分别记为和对应协方差记为Pcv和Pca；(3)由主滤波器估计出全局滤波解，并对两个子滤波器和主滤波器进行重置；1)主滤波器进行卡尔曼滤波①选取主滤波器状态变量将子IMU的失准角和陀螺常值漂移作为主滤波器状态变量：Xm=[φEm,φNm,φUm,ϵxm,ϵym,ϵzm]T]]>②主滤波器数学模型由于主滤波器没有观测信息，所以没有观测方程只有状态方程，其状态方程为：X·m=FmXm+Gmωm+BmUm]]>其中，Fm、Gm、ωm和Bm分别为主滤波器的状态转移矩阵、系统噪声驱动矩阵、系统噪声向量和确定输入项驱动矩阵；和分别为子IMU载体坐标系x轴、y轴和z轴陀螺的随机误差，不包括随机常值误差，ωm取为零均值白噪声，其方差阵Qm＝1/βmQ，βm＝1/3；Um为已知的非随机函数，Um＝[δVE δVN δVU]T，δVE、δVN和δVU为子滤波器1中估计出来的速度误差；根据对主滤波器状态变量的选取可知，主滤波器中的误差方程包括姿态误差方程和陀螺误差方程，二者表达式均与子滤波器1的姿态误差方程和陀螺误差方程相同，因此Fm、Gm和Bm的表达式分别为：Fm=F1Cbn103×303×3]]>Gm=Cbn103×3]]>Bm=F203×3]]>其中，F1和F2的含义与子滤波器1状态转移矩阵中F1和F2相同；③主滤波器根据其数学模型进行卡尔曼滤波，由于主滤波器中只有状态方程，因此只进行时间更新，并将状态变量估计值和协方差阵分别记为和Pm；2)计算全局滤波解将主、子滤波器中的公共状态变量估计值和协方差阵按照下式进行融合，即可得到全局最优估计：Pg=(Pcv-1+Pca-1+Pm-1)-1X^g=Pg(Pcv-1X^cv+Pca-1X^ca+Pm-1X^m)]]>其中，和Pg分别表示全局状态变量估计值和协方差阵，是由子滤波器1和子滤波器2中局部状态变量估计值和协方差阵在主滤波器中进行融合后得到的；3)对主、子滤波器公共状态变量估计值和协方差阵进行重置利用全局滤波解对主、子滤波器中的失准角和陀螺常值漂移的估计值和协方差阵进行重置，如下式所示：X^cv=X^gPcv′-1=β1Pg-1]]>X^ca′=X^gPca′-1=β2Pg-1]]>X^m′=X^gPm′-1=βmPg-1]]>其中，和分别表示经过重置后子滤波器1、子滤波器2和主滤波器中的公共状态变量估计值；Pcv′、Pca′和Pm′分别表示经过重置后子滤波器1、子滤波器2和主滤波器中公共状态变量的协方差阵；(4)利用步骤(3)中重置后的误差状态量计算出更加准确的子IMU的位置、速度和姿态，不断重复步骤(2)和步骤(3)直至传递对准结束；1)速度修正VE′=VE-δVEVN′=VN-δVNVU′=VU-δVU]]>其中，VE′、VN′和VU′分别为修正后子IMU的东向、北向和天向速度；2)位置修正L′=L-δLλ′=λ-δλH′=H-δH]]>其中，λ为捷联解算得到的子IMU的经度；L′、λ′和H′分别为修正后子IMU的纬度、经度和高度；δL、δλ和δH分别为子IMU的纬度、经度和高度误差，由子滤波器1估计出的速度误差通过积分得到，表达式如下所示：δL=δVNTRM+H-VNT(RM+H)2δHδλ=TsecLRN+HδVE+TVEsecLtanLRN+HδL-TVEsecL(RN+H)2δHδH=δVUT]]>3)姿态修正利用重置后中的失准角来计算子IMU导航坐标系n1与计算导航坐标系n′1间的转换矩阵Cn1′n1=1-φU′aφN′aφU′a1-φE′a-φN′aφE′a1]]>修正后的转换矩阵为：Cbn1=Cn1′n1Cbn1′]]>其中，为子IMU进行捷联解算后得到的转换矩阵；利用修正后的转换矩阵计算子IMU的姿态角，包括航向角、俯仰角和横滚角；4)将子滤波器1和子滤波器2状态变量中的失准角和速度误差变量置零，并将修正后子IMU的速度、位置和姿态作为下一次解算初始值，重复步骤(2)和步骤(3)，直至循环结束完成传递对准。