基于malabsymboliccommae的視覺(jué)導(dǎo)航信息估計(jì)

基于malabsymboliccommae的視覺(jué)導(dǎo)航信息估計(jì)

0個(gè)及多個(gè)的情況下航天器位置與姿態(tài)的可疑性由于cd技術(shù)的發(fā)展，全球信息導(dǎo)航和視覺(jué)導(dǎo)航受到了高度重視，并在國(guó)內(nèi)外得到了廣泛的研究。本文所研究的視覺(jué)導(dǎo)航是從星光導(dǎo)航發(fā)展而來(lái)的,從CCD相機(jī)測(cè)得的燈標(biāo)光線矢量來(lái)確定飛行器或車輛的姿態(tài)與位置。究竟一次最少用多少個(gè)光線矢量能確定出飛行器的姿態(tài)與位置?當(dāng)測(cè)量光線矢量有一個(gè)、兩個(gè)、三個(gè)及多個(gè)的情況下飛行器的位置與姿態(tài)的可觀性如何?本文就是要用基于MatlabSymbolicComputation的方法來(lái)研究這一問(wèn)題。文獻(xiàn)提出了用光線矢量確定飛行器位置與姿態(tài)的最優(yōu)估計(jì)方法并給出了估計(jì)誤差協(xié)方差的公式,證明了所得的估計(jì)誤差協(xié)方差陣為Cramer-Rao下界,即所得的估計(jì)器是最優(yōu)的。文獻(xiàn)從最優(yōu)估計(jì)的誤差協(xié)方差陣入手,從理論上分析了在一個(gè)、兩個(gè)、三個(gè)及多個(gè)燈標(biāo)情況下飛行器的位置與姿態(tài)的六個(gè)狀態(tài)變量的可觀性,這歸結(jié)為對(duì)于誤差協(xié)方差矩陣的秩、特征值、特征向量和跡的分析。本文在文獻(xiàn)、的基礎(chǔ)上,用MatlabSymbolicComputation工具箱對(duì)誤差協(xié)方差陣的秩、特征值、特征向量和跡直接進(jìn)行了分析,得到了一些有用的結(jié)論。不同于文獻(xiàn)的是,本文中的推導(dǎo)大多是用計(jì)算機(jī)進(jìn)行符號(hào)運(yùn)算得到的,提高了速度,簡(jiǎn)化了推導(dǎo)過(guò)程,并且在2.3中使用了不同于文獻(xiàn)2的簡(jiǎn)便方法推導(dǎo)誤差陣的跡。1機(jī)時(shí)間的轉(zhuǎn)換矩陣定義CCD傳感器坐標(biāo)系的z軸為光軸方向且指向外,定義像空間為(x,y,z),目標(biāo)的空間坐標(biāo)為(X,Y,Z)(如文獻(xiàn)圖1所示),則在測(cè)量誤差為零時(shí)燈標(biāo)在像平面投影的位置為xi=-fa11(Xi-Xc)+a12(Yi-Yc)+a13(Ζi-Ζc)a31(Xi-Xc)+a32(Yi-Yc)+a33(Ζi-Ζc)i=1?2???Νyi=-fa21(Xi-Xc)+a22(Yi-Yc)+a23(Ζi-Ζc)a31(Xi-Xc)+a32(Yi-Yc)+a33(Ζi-Ζc)i=1?2???Ν其中N是燈標(biāo)的個(gè)數(shù),(xi,yi)是第i個(gè)燈標(biāo)在像平面投影的位置,(Xi,Yi,Zi)是第i個(gè)燈標(biāo)在目標(biāo)空間坐標(biāo)系的位置且為已知,(Xc,Yc,Zc)是CCD相機(jī)鏡頭在目標(biāo)空間坐標(biāo)系的位置且為未知,f是已知的鏡頭焦距,A為從目標(biāo)空間坐標(biāo)系轉(zhuǎn)換倒像平面坐標(biāo)系的轉(zhuǎn)換矩陣,aij是A的第i行第j列的元素。測(cè)量誤差為零時(shí)的測(cè)量方程為bi=Arii=1?2???Ν其中bi=1√f2+x2i+y2i[-xi-yif]ri=1√(Xi-Xc)2+(Yi-Yc)2+(Ζi-Ζc)2[Xi-XcYi-YcΖi-Ζc]ri是鏡頭到燈標(biāo)的單位向量,bi是相應(yīng)的測(cè)量向量。當(dāng)測(cè)量噪聲出現(xiàn)時(shí),第i個(gè)觀測(cè)方程為～bi=Ari+wiwΤiAri=0其中～bi是第i個(gè)含有誤差的測(cè)量,傳感器誤差wi可近似為Gaussian白噪聲,且滿足E{wi}=0?E{wiwΤj}=σ2i[Ι-(Ari)(Ari)Τ]這里I為單位陣。用測(cè)量矢量來(lái)確定飛行器的姿態(tài)和位置就是要找到姿態(tài)矩陣A和位置向量p?(Xc,Yc,Zc)T,以極小化損失函數(shù)J(?A??p)=12Ν∑i=1σ-2i∥～bi-Ari∥2姿態(tài)陣的真值A(chǔ)與估計(jì)值?A的近似關(guān)系為A=e-[δα×]?A≈(Ι3×3-[δα×])?A其中δα=(δθδψδγ)T,δθ、δψ、δγ是姿態(tài)矩陣A的三個(gè)歐拉角的估計(jì)誤差角。其中[a×]的定義為:[a×]=[0-a3a2a30-a1-a2a10],對(duì)于向量a,b,a×b=[a×]b。引理1:設(shè)參數(shù)向量x=(δθδψδγXcYcZc)T,其誤差協(xié)方差陣P=E{xxT}-E{x}ET{x},則可導(dǎo)出最優(yōu)誤差方差P為Ρ=[-Ν∑i=1σ-2i[Ari×]2Ν∑i=1σ-2iζ-1/2iA[ri×]Ν∑i=1σ-2iζ-1/2i[ri×]ΤAΤ-Ν∑i=1σ-2iζ-1i[ri×]2]-1其中ri=(ai1ai2ai3)T,并且‖ri‖=1,ζi=(xi-xc)2+(yi-yc)2+(zi-zc)2。因?yàn)锳是姿態(tài)矩陣,故AAT=ATA=I,從而有[Ar×]=A[r×]T,[Ar×]2=A[r×]2AT。設(shè)?Μ=[A03×303×3Ι3×3],則有Q?～ΜΤΡ-1～Μ=[-Ν∑i=1σ-2i[ri×]2Ν∑i=1σ-2iζ-1/2i[ri×]-Ν∑i=1σ-2iζ-1/2i[ri×]-Ν∑i=1σ-2iζ-1i[ri×]2]=Ν∑i=1Qi其中Qi=[-σ-2i[ri×]2σ-2iζ-1/2i[ri×]-σ2iζ-1/2i[ri×]-σ-2iζ-1i[ri×]2]并且用到[ri×]T=-[ri×],從而可知誤差協(xié)方差陣的秩和姿態(tài)矩陣A無(wú)關(guān)。若要P存在,6×6的矩陣Q的秩應(yīng)為6。下面我們就來(lái)討論不同的觀測(cè)矢量時(shí)Q的秩、特征值、特征向量和跡。2概觀分析2.1raken在單個(gè)燈標(biāo)即N=1時(shí),r=(a1a2a3)T。利用MatlabSymbolicComputation中求秩函數(shù)rank(·)仿真得:對(duì)任意的a1、a2、a3,只要滿足a21+a22+a23=1,則有rank(Q)=2只有兩個(gè)變量是能觀測(cè)的,它們是姿態(tài)和位置的組合。在2個(gè)燈標(biāo)即N=2時(shí),r1=(a11a12a13)T,r2=(a21a22a23)T。利用MatlabSymbolicComputation中求秩函數(shù)rank(·)仿真得:對(duì)任意的a1i、a2i(i=1,2),只要滿足a211+a212+a213=1和a221+a222+a223=1,則rank(Q)=4只有4個(gè)變量是能觀測(cè)的,它們是姿態(tài)和位置的組合。在3個(gè)燈標(biāo)即N=3時(shí),r1=(a11a12a13)T,r2=(a21a22a23)T,r3=(a31a32a33)T。利用MatlabSymbolicComputation中求秩函數(shù)rank(·)仿真得:當(dāng)a11=a23=a33=0且a22a31≠a21a32(滿足a2i1+a2i2+a2i3=1,i=1,2,3)時(shí),則有rank(Q)=6也即rank(P)=6,即有6個(gè)變量是能觀測(cè)的,這正是我們期望的。aij的其它情形可進(jìn)行類似的仿真分析。從上面的分析直覺(jué)可得出:隨著觀測(cè)點(diǎn)的增加,rank(P)=6的概率也在增加。2.2實(shí)驗(yàn)2.3特征值檢驗(yàn)這里只考慮單個(gè)燈標(biāo)情形,多個(gè)燈標(biāo)的情形類似,只不過(guò)利用MatlabSymbolicComputation時(shí)的計(jì)算量增加了。利用MatlabSymbolicComputation中求矩陣的特征值和特征向量的函數(shù)eig(·)仿真得:對(duì)任意的a1、a2、a3(a21+a22+a23=1),誤差方差陣的逆矩陣Q的特征值為s1=s2=s3=s4=0?s5=s6=σ-2(1+ζ-1)在MatlabSymbolicComputation中假定所有符號(hào)是非零的,而實(shí)際情況并非如此,下面分幾種情形來(lái)討論s5和s6的特征向量V1和V2。(1)/3aa3V1=[a1/a21-(a21+a21)/(a2a3)ζ-1/2/a3-ζ-1/2a1/(a2a3)0]ΤV2=[ζ1/2a2/(a21+a22)-ζ1/2a1/(a21+a22)0-a1a3/(a21+a22)-a2a3/(a21+a22)1]Τ(2)當(dāng)a1。0，a2和a3之間為0時(shí)，對(duì)應(yīng)于s3和a5的特征向量為V1=[01-a2/a3ζ-1/2/a300]ΤV2=[1000-a3/ζ1/2a2/ζ1/2]Τ(3)當(dāng)a2。0，a1和a3之間為0時(shí)，對(duì)應(yīng)于s2和a5的特征向量為V1=[-a3/a1010ζ-1/2/a10]ΤV2=[010a3/ζ1/20-a1/ζ1/2]Τ(4)如果3a.0，a1和a2之間為0，則對(duì)應(yīng)于s3和a5的特征向量為V1=[1-a1/a2000ζ-1/2/a2]ΤV2=[00ζ1/2/a1-a2/a110]Τ(5)當(dāng)a1。a2。0，a1。1時(shí)，s和a6的特征向量為V1=[1000?ζ-1/20]ΤV2=[010?ζ1/200]Τ(6)當(dāng)a1。0和a2。1時(shí)，s和s3的特征向量為V1=[00?ζ1/2100]ΤV2=[10000±ζ1/2]Τ(7)當(dāng)a2。0，a1。1時(shí)，s和s3的特征向量為V1=[00±ζ1/2010]ΤV2=[0?ζ1/20001]Τ2.3誤差方差陣命題1i+1個(gè)燈標(biāo)情形的誤差方差陣Pi+1的跡小于i個(gè)燈標(biāo)情形的誤差方差陣Pi的跡,即tr(Ρi+1)＜tr(Ρi)這里tr(·)表示矩陣的跡。證:用遞推法。在3個(gè)燈標(biāo)情形中,設(shè)誤差方差陣為P3,Ρ3=?Μ(3∑i=1Qi)-1?ΜΤ;在4個(gè)燈標(biāo)情形中,設(shè)誤差方差陣為P4,ˉΡ4=?Μ(4∑i=1Qi)-1?ΜΤ。設(shè)R?3∑i=1Qi?Ν?4∑i=1Qi=R+Q4,應(yīng)用矩陣求逆公式(A+BCD)-1=A-1-A-1B(C-1+DA-1B)-1DA-1得Ν-1=R-1-ΔΝ其中ΔN=R-1(Q-14+R-1)-1R-1。因?yàn)棣為半正定陣,故tr(ΔN)>0,進(jìn)而tr(N-1)<tr(R-1)得。根據(jù)上面的討論有tr(Ρ4)＜tr(Ρ3)進(jìn)而有一般情形tr(ˉΡi+1)＜tr(ˉΡi)3估計(jì)姿