讀書報(bào)告-基本原理

博士研究生讀書報(bào)告報(bào)告題目： 慣性/地磁組合導(dǎo)航技術(shù)研究生姓名： 黃靖麗學(xué)科、專業(yè)： 控制科學(xué)與工程研究方向：指導(dǎo)教師：副指導(dǎo)教師：日期： 二。一四年七月十六日慣性/地磁組合導(dǎo)航技術(shù)摘要：本文主要介紹了三軸磁強(qiáng)計(jì)校正技術(shù)以及基于滾動(dòng)時(shí)域估計(jì)的飛行器姿態(tài)估計(jì)方法。并提出在姿態(tài)估計(jì)的同時(shí)進(jìn)行磁強(qiáng)計(jì)校正，保證了飛行器在不同磁環(huán)境下都能夠進(jìn)行精確地導(dǎo)航。關(guān)鍵詞：磁強(qiáng)計(jì)在線校正；滾動(dòng)時(shí)域；姿態(tài)估計(jì)1.三軸磁強(qiáng)計(jì)校正技術(shù)基于橢球補(bǔ)償?shù)淖钚《怂惴ㄊ悄壳靶Ｕ艔?qiáng)計(jì)使用最廣泛地方法之一，下面介紹該方法的基本原理。用三軸磁強(qiáng)計(jì)進(jìn)行測(cè)量時(shí)，磁力儀的背景磁場(chǎng)Bb為磁力儀的實(shí)際輸出值Br。根據(jù)單三軸磁強(qiáng)計(jì)的系統(tǒng)誤差數(shù)學(xué)模型式，實(shí)際輸出值Br和背景磁場(chǎng)Bb可以表示為：Br=Bb=P-1(Bm—C) (1-1)上式為三軸磁強(qiáng)計(jì)的系統(tǒng)誤差補(bǔ)償公式，當(dāng)已知誤差參數(shù)P和C時(shí)，可以利用此誤差補(bǔ)償公式實(shí)現(xiàn)對(duì)磁力儀實(shí)際輸出值的系統(tǒng)誤差補(bǔ)償。系統(tǒng)誤差補(bǔ)償?shù)闹饕ぷ鳛橄到y(tǒng)誤差參數(shù)P和C的計(jì)算。下面通過(guò)橢球擬合方法求取系統(tǒng)誤差參數(shù)P和C。當(dāng)背景磁場(chǎng)恒定時(shí)，可將背景磁場(chǎng)的模視為常量，即(Bb)t(Bb)=const。三軸磁強(qiáng)計(jì)不同姿態(tài)下的實(shí)際輸出值滿足以下二次曲面方程：(Bm—C)t((P-1)TP—1)(Bm—C)=IBb112 (1-2)設(shè)矩陣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)為一個(gè)橢球曲面方程，這表明三軸磁強(qiáng)計(jì)的實(shí)際輸出值Bm(Bm,Bm,Bm)在一個(gè)橢球面上，如圖1-1所示。橢球的中心坐標(biāo)為系統(tǒng)誤差參數(shù)C。

圖1-1磁傳感器三維觀測(cè)值的橢球分布Fig.1-1Ellipsoidaldistributionofmagnetometer3Dmeasurementvalues橢球擬合誤差補(bǔ)償方法的實(shí)質(zhì)為利用部分實(shí)測(cè)數(shù)據(jù)進(jìn)行橢球擬合，確定擬合橢球體的中心位置和形狀參數(shù)，然后用這些參數(shù)求取誤差參數(shù)P和C。橢球是一個(gè)特殊的二次曲面，其存在10個(gè)方程參數(shù)，設(shè)方程如下：F(3)=bx2+by2+bz2+bxy+byz+bxz+bx+by+bz+b=0 (1-5)12 345 6789 10式中：Z=[bbbbbbbbbb]T為需要求取的參數(shù)向量；12345678910v=[x2y2z2xyyz1磁力儀輸出值組成的向量，F(xiàn)?v)為磁力儀輸出值(x,y,z)到橢球面F(Z,v)=0的代數(shù)距離。橢球擬合時(shí)選擇磁力儀輸出值到橢球面距離的平方和最小為判斷準(zhǔn)則，即：min||F(min||F(Z,v)|=minZtDtDZs.t4bb—b2>0(1-6)式中：x2y2z2xyyzxzxyz1

1 1 1111111111x2y2z2xyyzxzxyz1

D= 222222222222_x2y2z2xyyzxzxyz1

nnnnnnnnnnnn根據(jù)最小二乘估計(jì)法得到估計(jì)值Z，進(jìn)而求出矩陣A。對(duì)稱矩陣A可以表示為A=UtSAU，其中U為正交矩陣，SA為A的特征值組成的對(duì)角陣。因此根據(jù)A的表達(dá)式A=((P-1)tP-1)/|件『，可得誤差參數(shù)P的計(jì)算式如下：p=*』u^Su)1 (1-7)誤差參數(shù)c為橢球的中心坐標(biāo)。以上為三軸磁強(qiáng)計(jì)誤差補(bǔ)償系數(shù)估計(jì)值p和c的計(jì)算方法。單個(gè)三軸磁強(qiáng)計(jì)系統(tǒng)誤差的補(bǔ)償方法為：選定實(shí)驗(yàn)區(qū)域，對(duì)背景磁場(chǎng)進(jìn)行觀測(cè)，得到背景磁場(chǎng)的模。變化需要補(bǔ)償三軸磁強(qiáng)計(jì)的姿態(tài)角，記錄磁力儀輸出值。姿態(tài)的選取盡量空間分布均勻。根據(jù)橢球擬合方法，利用磁力儀輸出值數(shù)據(jù)求取誤差參數(shù)估計(jì)值P和C。利用誤差參數(shù)估計(jì)值P和C，根據(jù)誤差補(bǔ)償公式(1-1)，對(duì)三軸磁強(qiáng)計(jì)的輸出值進(jìn)行誤差補(bǔ)償。上述方法是一種線下校正方法，可以在一定程度上滿足飛行器的精度要求。然而對(duì)于一些嚴(yán)酷的飛行環(huán)境，比如起飛降落或者在與地面校正時(shí)不同的大氣層飛行，飛行器的磁場(chǎng)環(huán)境也隨之改變，線下校正失去了原有的精度，因此對(duì)于磁強(qiáng)計(jì)的實(shí)時(shí)校正是非常必要的。2.基于滾動(dòng)時(shí)域估計(jì)的姿態(tài)確定算法飛行器的姿態(tài)確定是導(dǎo)航的基本問(wèn)題之一。陀螺儀是目前使用最廣泛地導(dǎo)航傳感器。有著無(wú)源、無(wú)輻射、精度高、自主導(dǎo)航等優(yōu)點(diǎn)，但是隨著時(shí)間的推移存在積累誤差。磁強(qiáng)計(jì)雖然沒(méi)有陀螺精度高，但是它不存在積累誤差，可以提供豐富的姿態(tài)信息。同時(shí)，它也是自主導(dǎo)航的方式之一，隱蔽性強(qiáng)?；谶@些特點(diǎn)，慣性地磁導(dǎo)航系統(tǒng)具有巨大的軍事意義，是無(wú)人機(jī)導(dǎo)航方式的首選。2.1滾動(dòng)時(shí)域估計(jì)基本原理2.1.1全信息滾動(dòng)時(shí)域估計(jì)給定如下離散化的系統(tǒng)方程:(2-1)考慮一類如(2-1)形式的非線性Markov離散系統(tǒng)。從概率的角度來(lái)看，Bayes狀態(tài)的MAP(極大后驗(yàn)估計(jì))問(wèn)題可以描述為：給定當(dāng)前和過(guò)去的測(cè)量y全"，y，…,y｝及其概率分布，尋找狀態(tài)x=｛x,x，…,x｝的概率分布，以獲得最1:k 12k 0:k12k大可能的估計(jì)狀態(tài)匕,x,...,x}，即UBXp(x\y)。0 1k 0:k 1:ky,...,%}

依據(jù)Markov系統(tǒng)的鏈性質(zhì)，可將狀態(tài)的聯(lián)合概率表示為：p(x,x,...,x)=p(x)H1p(xx)01k 0 i+1ii=0其中p(x0)表示關(guān)于系統(tǒng)初始狀態(tài)的先驗(yàn)信息。假設(shè)測(cè)量噪聲vk是相互獨(dú)立的，則可獲得如下的關(guān)系：p(y-h(x)))=p(y-h(x))0：ki=1根據(jù)Bayes準(zhǔn)則，條件概率分布密度可表示為：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根據(jù)對(duì)數(shù)的性質(zhì)，可得如下等式:maxp(x|y)maxp(x|y)=arg{x0,x1，...,xk}=argmax(r0,x1,...,xki=0從而，將狀態(tài)估計(jì)問(wèn)題轉(zhuǎn)化為優(yōu)化問(wèn)題。為分析方便，特作如下假設(shè):argmaxlogp(x0Jy1：k)%,...,xk}(x)))+logp(x)ii 00:k… { }飪0，x1，...，X」Z1logp(x x)+logp(y一hi+1i v,ii擾動(dòng)”k是相互獨(dú)立的；fk(xk,wk)=f(x「+w^；系統(tǒng)初始狀態(tài)x的先驗(yàn)估計(jì)狀態(tài)滿足均值為x、協(xié)方差為P的正態(tài)分布;TOC\o"1-5"\h\z0 0 0系統(tǒng)噪聲wk、vk是零均值，協(xié)方差分別為Qk和R的高斯白噪聲。根據(jù)上面假設(shè)，得到如下的性能指標(biāo)函數(shù)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于是，問(wèn)題可以描述為:argmaxp(x|y)=argmaxJ{ }0：k1:k { }lx,x,...,xJ lx,x,...,xJ0,1,,k 0,1,，k可以證明，對(duì)于乘性噪聲的系統(tǒng)，存在同樣的結(jié)論，即問(wèn)題可以描述為: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設(shè)在第k時(shí)刻式(2-2)的優(yōu)化解存在，記為￡*。顯然，k時(shí)刻的狀態(tài)估計(jì)值由ii=0初始時(shí)刻的先驗(yàn)估計(jì)狀態(tài)x和W｝k-1確定。0ii=0上述問(wèn)題使用了從零時(shí)刻到當(dāng)前時(shí)刻k的全部測(cè)量數(shù)據(jù)&,J，…,J｝，可以稱1 2k其為全信息估計(jì)(FullInformationEstimation)o顯然，隨著時(shí)間的增大，優(yōu)化問(wèn)題處理的數(shù)據(jù)將越來(lái)越多，計(jì)算負(fù)荷也隨之增加，求解會(huì)非常困難，甚至不可行。因此，全信息估計(jì)在實(shí)際應(yīng)用中存在求解的可行性問(wèn)題。為解決這一問(wèn)題，限制優(yōu)化問(wèn)題的維數(shù)是必要的，這促使了滾動(dòng)時(shí)域估計(jì)方法的產(chǎn)生和發(fā)展。2.1.2滾動(dòng)時(shí)域估計(jì)原理引入固定時(shí)域m<k，并將優(yōu)化問(wèn)題的計(jì)算時(shí)域分為G<ivk-m｝和*-m<i<k-1｝兩部分，則性能指標(biāo)函數(shù)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如果考慮一種特殊情況，即一個(gè)延遲步長(zhǎng)內(nèi)的估計(jì)問(wèn)題，則問(wèn)題可以描述為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號(hào)?-1-xk-1-x)+const其中，xk1、Pk1分別為在k-1時(shí)刻的估計(jì)的狀態(tài)值和協(xié)方差矩陣。而且，根據(jù)系統(tǒng)的Markov特性和優(yōu)化理論(theprincipleofoptimality)，問(wèn)題最后可以描述為

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上述問(wèn)題即為滾動(dòng)時(shí)域估計(jì)問(wèn)題。圖2圖2滾動(dòng)時(shí)域估計(jì)原理其中，上(-x*)P*)】《-x*)為性能指標(biāo)函數(shù)J (x)的近似。2k—mk—mk—m k—mk—m opt[k—m]k—m這里采用的是擴(kuò)展卡爾曼濾波的協(xié)方差陣更新公式來(lái)近似代價(jià)函數(shù)Jo畔m](xkm)的。這里x*和P*并不表示在t時(shí)刻狀態(tài)的后驗(yàn)估計(jì)值x和P，它們是用k—m k—m k—m k—m k—m來(lái)表示對(duì)j頃訴m]Gkm)合理的近似，其具體的求解見后文。需要說(shuō)明的是，時(shí)域步數(shù)m使得xk不敏感于J°畔m]匕「的近似精度，同時(shí)，對(duì)Jo畔m]4m)的近似也更加精確。但對(duì)時(shí)域步數(shù)m的增加也是有限制的，時(shí)域步數(shù)越大，計(jì)算量也就隨之增加。所以，需要在估計(jì)精度、收斂性及計(jì)算量之間進(jìn)行這種合理處理。令mtarget為預(yù)先設(shè)定的期望時(shí)域步數(shù)，則m(k)=mink，m^突｝，這也是為求解MHE問(wèn)題的方便性考慮的。2.2帶有等式約束的非線性滾動(dòng)時(shí)域估計(jì)問(wèn)題

??? 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)假定從測(cè)量中得到的所有信息已經(jīng)包含在先驗(yàn)估計(jì)x(-)中。方程(2-6)給出了等式約束。因此MHE的輸出就是x^:時(shí)域中最后一個(gè)時(shí)刻的估計(jì)狀態(tài)向量。在該論文中假定不存在不等式約束。因此這就是一個(gè)帶權(quán)重的最小二乘問(wèn)題，在該問(wèn)題中權(quán)重就是方差陣的逆：W=R-1m,i iW■=Q-1W=(P(-))-1ai我們米用如下標(biāo)記法：aiwa]=ia『w把方程(2-4)、方程(2-5)帶入到代價(jià)函數(shù)(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問(wèn)題轉(zhuǎn)化成了一個(gè)最小二乘NLP問(wèn)題r=(x,x,...,x)k k-Lk-L+1 k問(wèn)題(2-7)可以描述成min ||J(^;D^)|2 (2-8)s.t.lD)=0這里0'代表一組MHE問(wèn)題的輸入變量，例如求解當(dāng)前狀態(tài)需要提供的所有變量。這個(gè)非線性最小二乘問(wèn)題可以用高斯牛頓迭代解方法解決。在每次的迭代中，通過(guò)前一個(gè)解x(如果前一個(gè)解得不到可以用最優(yōu)估計(jì)替代)線性化量測(cè)和狀態(tài)推演i方程，其中Ax.=x.-x.是對(duì)真實(shí)值的偏離：

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通過(guò)在全部的時(shí)域內(nèi)引入狀態(tài)偏離軌跡AX=[Axrk一L,Axt ，…,Axt]t，該優(yōu)化問(wèn)k—L+1 k題可以重寫為minimizeAxi1￡2i=k—L+1AAX-Bmwm,i+技IIi=k—LAAX-BI2+」|AAX-B2a d2(2-9)waSubjecttoAqAX―氣其中，和量測(cè)有關(guān)的矩陣和向量是氣=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和狀態(tài)推演代價(jià)函數(shù)相關(guān)的矩陣和向量是堂(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與最終代價(jià)有關(guān)的矩陣和向量是A=L1,0,...,0]k—L-xk:L]

W=(P(-))-1a k一L最后，與等式約束有關(guān)的矩陣和向量是A=diagB=11(x^l)T，-1A=diagB=11(x^l)T，-1(xkl1)t,...,—l(x^)」線性化后的代價(jià)函數(shù)可以寫成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è)常數(shù)矩陣，G由方程(2-10)中相應(yīng)的項(xiàng)確定。這樣就得到一個(gè)帶有線性約束的最小二乘目標(biāo)函數(shù)。帶約