读书报告-基本原理

博士研究生读书报告报告题目： 惯性/地磁组合导航技术研究生姓名： 黄靖丽学科、专业： 控制科学与工程研究方向：指导教师：副指导教师：日期： 二。一四年七月十六日惯性/地磁组合导航技术摘要：本文主要介绍了三轴磁强计校正技术以及基于滚动时域估计的飞行器姿态估计方法。并提出在姿态估计的同时进行磁强计校正，保证了飞行器在不同磁环境下都能够进行精确地导航。关键词：磁强计在线校正；滚动时域；姿态估计1.三轴磁强计校正技术基于椭球补偿的最小二乘算法是目前校正磁强计使用最广泛地方法之一，下面介绍该方法的基本原理。用三轴磁强计进行测量时，磁力仪的背景磁场Bb为磁力仪的实际输出值Br。根据单三轴磁强计的系统误差数学模型式，实际输出值Br和背景磁场Bb可以表示为：Br=Bb=P-1(Bm—C) (1-1)上式为三轴磁强计的系统误差补偿公式，当已知误差参数P和C时，可以利用此误差补偿公式实现对磁力仪实际输出值的系统误差补偿。系统误差补偿的主要工作为系统误差参数P和C的计算。下面通过椭球拟合方法求取系统误差参数P和C。当背景磁场恒定时，可将背景磁场的模视为常量，即(Bb)t(Bb)=const。三轴磁强计不同姿态下的实际输出值满足以下二次曲面方程：(Bm—C)t((P-1)TP—1)(Bm—C)=IBb112 (1-2)设矩阵a=S-1)TP—1V|\BbI2，则上式可表示为(1-3)(1-4)(Bm—C)TA(Bm—C)=1(1-3)(1-4)b1

b"

bb1

b"

bJ2bpbJ2

b2 "bj2b3式(1-3)为一个椭球曲面方程，这表明三轴磁强计的实际输出值Bm(Bm,Bm,Bm)在一个椭球面上，如图1-1所示。椭球的中心坐标为系统误差参数C。

图1-1磁传感器三维观测值的椭球分布Fig.1-1Ellipsoidaldistributionofmagnetometer3Dmeasurementvalues椭球拟合误差补偿方法的实质为利用部分实测数据进行椭球拟合，确定拟合椭球体的中心位置和形状参数，然后用这些参数求取误差参数P和C。椭球是一个特殊的二次曲面，其存在10个方程参数，设方程如下：F(3)=bx2+by2+bz2+bxy+byz+bxz+bx+by+bz+b=0 (1-5)12 345 6789 10式中：Z=[bbbbbbbbbb]T为需要求取的参数向量；12345678910v=[x2y2z2xyyz1磁力仪输出值组成的向量，F«v)为磁力仪输出值(x,y,z)到椭球面F(Z,v)=0的代数距离。椭球拟合时选择磁力仪输出值到椭球面距离的平方和最小为判断准则，即：min||F(min||F(Z,v)|=minZtDtDZs.t4bb—b2>0(1-6)式中：x2y2z2xyyzxzxyz1

1 1 1111111111x2y2z2xyyzxzxyz1

D= 222222222222_x2y2z2xyyzxzxyz1

nnnnnnnnnnnn根据最小二乘估计法得到估计值Z，进而求出矩阵A。对称矩阵A可以表示为A=UtSAU，其中U为正交矩阵，SA为A的特征值组成的对角阵。因此根据A的表达式A=((P-1)tP-1)/|件『，可得误差参数P的计算式如下：p=*』u^Su)1 (1-7)误差参数c为椭球的中心坐标。以上为三轴磁强计误差补偿系数估计值p和c的计算方法。单个三轴磁强计系统误差的补偿方法为：选定实验区域，对背景磁场进行观测，得到背景磁场的模。变化需要补偿三轴磁强计的姿态角，记录磁力仪输出值。姿态的选取尽量空间分布均匀。根据椭球拟合方法，利用磁力仪输出值数据求取误差参数估计值P和C。利用误差参数估计值P和C，根据误差补偿公式(1-1)，对三轴磁强计的输出值进行误差补偿。上述方法是一种线下校正方法，可以在一定程度上满足飞行器的精度要求。然而对于一些严酷的飞行环境，比如起飞降落或者在与地面校正时不同的大气层飞行，飞行器的磁场环境也随之改变，线下校正失去了原有的精度，因此对于磁强计的实时校正是非常必要的。2.基于滚动时域估计的姿态确定算法飞行器的姿态确定是导航的基本问题之一。陀螺仪是目前使用最广泛地导航传感器。有着无源、无辐射、精度高、自主导航等优点，但是随着时间的推移存在积累误差。磁强计虽然没有陀螺精度高，但是它不存在积累误差，可以提供丰富的姿态信息。同时，它也是自主导航的方式之一，隐蔽性强。基于这些特点，惯性地磁导航系统具有巨大的军事意义，是无人机导航方式的首选。2.1滚动时域估计基本原理2.1.1全信息滚动时域估计给定如下离散化的系统方程:(2-1)考虑一类如(2-1)形式的非线性Markov离散系统。从概率的角度来看，Bayes状态的MAP(极大后验估计)问题可以描述为：给定当前和过去的测量y全"，y，…,y｝及其概率分布，寻找状态x=｛x,x，…,x｝的概率分布，以获得最1:k 12k 0:k12k大可能的估计状态匕,x,...,x}，即UBXp(x\y)。0 1k 0:k 1:ky,...,%}

依据Markov系统的链性质，可将状态的联合概率表示为：p(x,x,...,x)=p(x)H1p(xx)01k 0 i+1ii=0其中p(x0)表示关于系统初始状态的先验信息。假设测量噪声vk是相互独立的，则可获得如下的关系：p(y-h(x)))=p(y-h(x))0：ki=1根据Bayes准则，条件概率分布密度可表示为：0:k"1:k)=p^yJx0:k"1:kp(y1/xx)-Hp(y-h(x))i Vj iiii=1P(x.k九)Xp(x)Hp(xP(x.k九i=0根据对数的性质，可得如下等式:maxp(x|y)maxp(x|y)=arg{x0,x1，...,xk}=argmax(r0,x1,...,xki=0从而，将状态估计问题转化为优化问题。为分析方便，特作如下假设:argmaxlogp(x0Jy1：k)%,...,xk}(x)))+logp(x)ii 00:k… { }饪0，x1，...，X」Z1logp(x x)+logp(y一hi+1i v,ii扰动”k是相互独立的；fk(xk,wk)=f(x「+w^；系统初始状态x的先验估计状态满足均值为x、协方差为P的正态分布;TOC\o"1-5"\h\z0 0 0系统噪声wk、vk是零均值，协方差分别为Qk和R的高斯白噪声。根据上面假设，得到如下的性能指标函数J=1Z12i=0!(x-x)TP-1(x-x)20 0 0 0 0\o"CurrentDocument"tQ-1w + [yJ=1Z12i=0!(x-x)TP-1(x-x)20 0 0 0 0iii i+1 i+1i+1 i+1i+1 i+1 i+1于是，问题可以描述为:argmaxp(x|y)=argmaxJ{ }0：k1:k { }lx,x,...,xJ lx,x,...,xJ0,1,,k 0,1,，k可以证明，对于乘性噪声的系统，存在同样的结论，即问题可以描述为:argmaxp(x0argmaxp(x0ky1k)=argmaxj(2-2)匕0,x1,...,xk}匕0,x1,...,xk}满足:x=f(x,w),j=h(x)+v,i=0,1,2,...,k-1i+1 iii i+1 i+1i+1 i+1设在第k时刻式(2-2)的优化解存在，记为£*。显然，k时刻的状态估计值由ii=0初始时刻的先验估计状态x和W｝k-1确定。0ii=0上述问题使用了从零时刻到当前时刻k的全部测量数据&,J，…,J｝，可以称1 2k其为全信息估计(FullInformationEstimation)o显然，随着时间的增大，优化问题处理的数据将越来越多，计算负荷也随之增加，求解会非常困难，甚至不可行。因此，全信息估计在实际应用中存在求解的可行性问题。为解决这一问题，限制优化问题的维数是必要的，这促使了滚动时域估计方法的产生和发展。2.1.2滚动时域估计原理引入固定时域m<k，并将优化问题的计算时域分为G<ivk-m｝和*-m<i<k-1｝两部分，则性能指标函数J可表示为:2i=0TOC\o"1-5"\h\ztQ-1w + [j -h (x )]fR-1 [y -h (x )2i=0iii i+1 i+1 i+1 i+1 i+1 i+1 i+1—(x-x)TP-1(x-x)20 0 0 0 0\o"CurrentDocument"=—如^wtQ-1w+[y - h(x )LR-1[y -h(x )1L2 iii i+1 i+1 i+1 i+1 i+1 i+1 i+1i=k-m"-E^WtQ-1w + [y 一h (x )]R-1 [y 一h (x )1L2 iii i+1 i+1 i+1 i+1 i+1 i+1 i+1i=0—(x一x)TP-1(x一x)20 0 0 0 0=—如^wtQ-1w +[y 一 h(x)LR-1[y -h(x)1L2 iiii+1 i+1 i+1 i+1 i+1 i+1 i+1i=k-m )opt[k-m] k-m如果考虑一种特殊情况，即一个延迟步长内的估计问题，则问题可以描述为J=—wt Q-1w+」[y -h (x )LR-1 [y -h(x)]+J (x )2 k-1 k-1 k-1 2kkkkkkk opt[k-1] k-1EKF滤波就是将Jt[…近似表示为Jopt[k-Jopt[k-1]«-1号«-1-xk-1-x)+const其中，xk1、Pk1分别为在k-1时刻的估计的状态值和协方差矩阵。而且，根据系统的Markov特性和优化理论(theprincipleofoptimality)，问题最后可以描述为

i+1 i+1TOC\o"1-5"\h\zJ=—t1 ^WtQ-iw + [y —h (x )1R-i [y 一h (x )〕Li+1 i+12 iii i+1 i+1i+1 i+1i+1 '' ''i=-m( 一x*k-m k-m k一mminjminjk-m，Wk-m，…，Wk_jk一m'Wk-m,•••,Wk-J*%

lx满足:x=f(x,w),i=k一m,・・・,k一1上述问题即为滚动时域估计问题。图2图2滚动时域估计原理其中，上(-x*)P*)】《-x*)为性能指标函数J (x)的近似。2k—mk—mk—m k—mk—m opt[k—m]k—m这里采用的是扩展卡尔曼滤波的协方差阵更新公式来近似代价函数Jo畔m](xkm)的。这里x*和P*并不表示在t时刻状态的后验估计值x和P，它们是用k—m k—m k—m k—m k—m来表示对j顷诉m]Gkm)合理的近似，其具体的求解见后文。需要说明的是，时域步数m使得xk不敏感于J°畔m]匕「的近似精度，同时，对Jo畔m]4m)的近似也更加精确。但对时域步数m的增加也是有限制的，时域步数越大，计算量也就随之增加。所以，需要在估计精度、收敛性及计算量之间进行这种合理处理。令mtarget为预先设定的期望时域步数，则m(k)=mink，m^突｝，这也是为求解MHE问题的方便性考虑的。2.2带有等式约束的非线性滚动时域估计问题

••• 1"□,minimize乙vtR-1v+2iX-L，,,,，％ i=k-L+1Vk-L+1，…,VkWk-L，…，Wk-11& 1,£^wtQ-1w+(x(-)1& 1,£^wtQ-1w+(x(-)-x)t(P(-))-1(x(-)i=k—L-气-l)(2-3)Xi=f(Xi,u)+wfori=k-L,…k-1j=h(x)+vfori=k一L+1,…kl(x.)=0fori=k-L,…k(2-4)(2-5)(2-6)假定从测量中得到的所有信息已经包含在先验估计x(-)中。方程(2-6)给出了等式约束。因此MHE的输出就是x^:时域中最后一个时刻的估计状态向量。在该论文中假定不存在不等式约束。因此这就是一个带权重的最小二乘问题，在该问题中权重就是方差阵的逆：W=R-1m,i iW■=Q-1W=(P(-))-1ai我们米用如下标记法：aiwa]=ia『w把方程(2-4)、方程(2-5)带入到代价函数(2-3)中，得到下式:minimize—£||j一h(x)||2+上如||x一f(x,u)||2+ x(-)一x2 (2-7)x. 2・kL1i Wmi 2•kL i+1 i Wpi 2“ k-L 顼吧Subjecttol(x)=0fori=k-L,…k这样就把MHE问题转化成了一个最小二乘NLP问题r=(x,x,...,x)k k-Lk-L+1 k问题(2-7)可以描述成min ||J(^;D^)|2 (2-8)s.t.lD)=0这里0'代表一组MHE问题的输入变量，例如求解当前状态需要提供的所有变量。这个非线性最小二乘问题可以用高斯牛顿迭代解方法解决。在每次的迭代中，通过前一个解x(如果前一个解得不到可以用最优估计替代)线性化量测和状态推演i方程，其中Ax.=x.-x.是对真实值的偏离：

minimizeAxi1£2i=k—L+1dh(x)y-h(x)—ii dxiwm,ii+1df(minimizeAxi1£2i=k—L+1dh(x)y-h(x)—ii dxiwm,ii+1df(%,七)Axdx，iSubjectto+2Ixk:L-xk—L-Axk—Lwwal(x)+°'WAx=0fori=k-L,.••kii dx 》i通过在全部的时域内引入状态偏离轨迹AX=[Axrk一L,Axt ，…,Axt]t，该优化问k—L+1 k题可以重写为minimizeAxi1£2i=k—L+1AAX-Bmwm,i+技IIi=k—LAAX-BI2+」|AAX-B2a d2(2-9)waSubjecttoAqAX―气其中，和量测有关的矩阵和向量是气=diag-'dh(x )dh(x ) k~L11—, k~L+2—dx dxk—L+1 k—L+2dh(x)'dx )kw=diag)-yk一L>MJ一l2)-y-1 ,R-1 ,…，R—1)k—L+1 k—L+2 kk-L+1k-1和状态推演代价函数相关的矩阵和向量是堂(XL，uL)

dxk一L6f(x kL11~kL11dxk一L+1df(xk1)dxk—1fUk-L k-L)-Xk-L+1IG(xk-L+1k-1+1w=diag(Q-1,Q-1，…，Q-1)p k-Lk-L+1 k-1与最终代价有关的矩阵和向量是A=L1,0,...,0]k—L-xk:L]

W=(P(-))-1a k一L最后，与等式约束有关的矩阵和向量是A=diagB=11(x^l)T，-1A=diagB=11(x^l)T，-1(xkl1)t,...,—l(x^)」线性化后的代价函数可以写成oxk/—J^L——kL,—kL11——kLoxk/. Ox Ox' k-L k-L+1J(AX)=1AXt(atWA+AtWA+AtWAXx2 mmmPPPaaa(2-10)AtWA+AtWA+AtWAXx+^C

mmmPPPaaa 2(2-10)1[aXtAX-2GAX+C]2其中，C是一个常数矩阵，G由方程(2-10)中相应的项确定。这样就得到一个带有线性约束的最小二乘目标函数。带约