基于malabsymboliccommae的視覺導航信息估計

基于malabsymboliccommae的視覺導航信息估計

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