讀書報告-基本原理

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

圖1-1磁傳感器三維觀測值的橢球分布Fig.1-1Ellipsoidaldistributionofmagnetometer3Dmeasurementvalues橢球擬合誤差補(bǔ)償方法的實質(zhì)為利用部分實測數(shù)據(jù)進(jìn)行橢球擬合，確定擬合橢球體的中心位置和形狀參數(shù)，然后用這些參數(shù)求取誤差參數(shù)P和C。橢球是一個特殊的二次曲面，其存在10個方程參數(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ù)距離。橢球擬合時選擇磁力儀輸出值到橢球面距離的平方和最小為判斷準(zhǔn)則，即：min||F(min||F(Z,v)|=minZtDtDZs.t4bb—b2>0(1-6)式中：x2y2z2xyyzxzxyz1

1 1 1111111111x2y2z2xyyzxzxyz1

D= 222222222222_x2y2z2xyyzxzxyz1

nnnnnnnnnnnn根據(jù)最小二乘估計法得到估計值Z，進(jìn)而求出矩陣A。對稱矩陣A可以表示為A=UtSAU，其中U為正交矩陣，SA為A的特征值組成的對角陣。因此根據(jù)A的表達(dá)式A=((P-1)tP-1)/|件『，可得誤差參數(shù)P的計算式如下：p=*』u^Su)1 (1-7)誤差參數(shù)c為橢球的中心坐標(biāo)。以上為三軸磁強(qiáng)計誤差補(bǔ)償系數(shù)估計值p和c的計算方法。單個三軸磁強(qiáng)計系統(tǒng)誤差的補(bǔ)償方法為：選定實驗區(qū)域，對背景磁場進(jìn)行觀測，得到背景磁場的模。變化需要補(bǔ)償三軸磁強(qiáng)計的姿態(tài)角，記錄磁力儀輸出值。姿態(tài)的選取盡量空間分布均勻。根據(jù)橢球擬合方法，利用磁力儀輸出值數(shù)據(jù)求取誤差參數(shù)估計值P和C。利用誤差參數(shù)估計值P和C，根據(jù)誤差補(bǔ)償公式(1-1)，對三軸磁強(qiáng)計的輸出值進(jìn)行誤差補(bǔ)償。上述方法是一種線下校正方法，可以在一定程度上滿足飛行器的精度要求。然而對于一些嚴(yán)酷的飛行環(huán)境，比如起飛降落或者在與地面校正時不同的大氣層飛行，飛行器的磁場環(huán)境也隨之改變，線下校正失去了原有的精度，因此對于磁強(qiáng)計的實時校正是非常必要的。2.基于滾動時域估計的姿態(tài)確定算法飛行器的姿態(tài)確定是導(dǎo)航的基本問題之一。陀螺儀是目前使用最廣泛地導(dǎo)航傳感器。有著無源、無輻射、精度高、自主導(dǎo)航等優(yōu)點，但是隨著時間的推移存在積累誤差。磁強(qiáng)計雖然沒有陀螺精度高，但是它不存在積累誤差，可以提供豐富的姿態(tài)信息。同時，它也是自主導(dǎo)航的方式之一，隱蔽性強(qiáng)。基于這些特點，慣性地磁導(dǎo)航系統(tǒng)具有巨大的軍事意義，是無人機(jī)導(dǎo)航方式的首選。2.1滾動時域估計基本原理2.1.1全信息滾動時域估計給定如下離散化的系統(tǒng)方程:(2-1)考慮一類如(2-1)形式的非線性Markov離散系統(tǒng)。從概率的角度來看，Bayes狀態(tài)的MAP(極大后驗估計)問題可以描述為：給定當(dāng)前和過去的測量y全"，y，…,y｝及其概率分布，尋找狀態(tài)x=｛x,x，…,x｝的概率分布，以獲得最1:k 12k 0:k12k大可能的估計狀態(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)的先驗信息。假設(shè)測量噪聲vk是相互獨立的，則可獲得如下的關(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ù)對數(shù)的性質(zhì)，可得如下等式:maxp(x|y)maxp(x|y)=arg{x0,x1，...,xk}=argmax(r0,x1,...,xki=0從而，將狀態(tài)估計問題轉(zhuǎn)化為優(yōu)化問題。為分析方便，特作如下假設(shè):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^；系統(tǒng)初始狀態(tài)x的先驗估計狀態(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于是，問題可以描述為:argmaxp(x|y)=argmaxJ{ }0：k1:k { }lx,x,...,xJ lx,x,...,xJ0,1,,k 0,1,，k可以證明，對于乘性噪聲的系統(tǒng)，存在同樣的結(jié)論，即問題可以描述為: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時刻式(2-2)的優(yōu)化解存在，記為￡*。顯然，k時刻的狀態(tài)估計值由ii=0初始時刻的先驗估計狀態(tài)x和W｝k-1確定。0ii=0上述問題使用了從零時刻到當(dāng)前時刻k的全部測量數(shù)據(jù)&,J，…,J｝，可以稱1 2k其為全信息估計(FullInformationEstimation)o顯然，隨著時間的增大，優(yōu)化問題處理的數(shù)據(jù)將越來越多，計算負(fù)荷也隨之增加，求解會非常困難，甚至不可行。因此，全信息估計在實際應(yīng)用中存在求解的可行性問題。為解決這一問題，限制優(yōu)化問題的維數(shù)是必要的，這促使了滾動時域估計方法的產(chǎn)生和發(fā)展。2.1.2滾動時域估計原理引入固定時域m<k，并將優(yōu)化問題的計算時域分為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如果考慮一種特殊情況，即一個延遲步長內(nèi)的估計問題，則問題可以描述為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時刻的估計的狀態(tài)值和協(xié)方差矩陣。而且，根據(jù)系統(tǒng)的Markov特性和優(yōu)化理論(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*)為性能指標(biāo)函數(shù)J (x)的近似。2k—mk—mk—m k—mk—m opt[k—m]k—m這里采用的是擴(kuò)展卡爾曼濾波的協(xié)方差陣更新公式來近似代價函數(shù)Jo畔m](xkm)的。這里x*和P*并不表示在t時刻狀態(tài)的后驗估計值x和P，它們是用k—m k—m k—m k—m k—m來表示對j頃訴m]Gkm)合理的近似，其具體的求解見后文。需要說明的是，時域步數(shù)m使得xk不敏感于J°畔m]匕「的近似精度，同時，對Jo畔m]4m)的近似也更加精確。但對時域步數(shù)m的增加也是有限制的，時域步數(shù)越大，計算量也就隨之增加。所以，需要在估計精度、收斂性及計算量之間進(jìn)行這種合理處理。令mtarget為預(yù)先設(shè)定的期望時域步數(shù)，則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)假定從測量中得到的所有信息已經(jīng)包含在先驗估計x(-)中。方程(2-6)給出了等式約束。因此MHE的輸出就是x^:時域中最后一個時刻的估計狀態(tài)向量。在該論文中假定不存在不等式約束。因此這就是一個帶權(quán)重的最小二乘問題，在該問題中權(quán)重就是方差陣的逆：W=R-1m,i iW■=Q-1W=(P(-))-1ai我們米用如下標(biāo)記法：aiwa]=ia『w把方程(2-4)、方程(2-5)帶入到代價函數(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問題轉(zhuǎn)化成了一個最小二乘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問題的輸入變量，例如求解當(dāng)前狀態(tài)需要提供的所有變量。這個非線性最小二乘問題可以用高斯牛頓迭代解方法解決。在每次的迭代中，通過前一個解x(如果前一個解得不到可以用最優(yōu)估計替代)線性化量測和狀態(tài)推演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通過在全部的時域內(nèi)引入狀態(tài)偏離軌跡AX=[Axrk一L,Axt ，…,Axt]t，該優(yōu)化問k—L+1 k題可以重寫為minimizeAxi1￡2i=k—L+1AAX-Bmwm,i+技IIi=k—LAAX-BI2+」|AAX-B2a d2(2-9)waSubjecttoAqAX―氣其中，和量測有關(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)推演代價函數(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與最終代價有關(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^)」線性化后的代價函數(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是一個常數(shù)矩陣，G由方程(2-10)中相應(yīng)的項確定。這樣就得到一個帶有線性約束的最小二乘目標(biāo)函數(shù)。帶約