基于malabsymboliccommae的视觉导航信息估计

基于malabsymboliccommae的视觉导航信息估计

0个及多个的情况下航天器位置与姿态的可疑性由于cd技术的发展，全球信息导航和视觉导航受到了高度重视，并在国内外得到了广泛的研究。本文所研究的视觉导航是从星光导航发展而来的,从CCD相机测得的灯标光线矢量来确定飞行器或车辆的姿态与位置。究竟一次最少用多少个光线矢量能确定出飞行器的姿态与位置?当测量光线矢量有一个、两个、三个及多个的情况下飞行器的位置与姿态的可观性如何?本文就是要用基于MatlabSymbolicComputation的方法来研究这一问题。文献提出了用光线矢量确定飞行器位置与姿态的最优估计方法并给出了估计误差协方差的公式,证明了所得的估计误差协方差阵为Cramer-Rao下界,即所得的估计器是最优的。文献从最优估计的误差协方差阵入手,从理论上分析了在一个、两个、三个及多个灯标情况下飞行器的位置与姿态的六个状态变量的可观性,这归结为对于误差协方差矩阵的秩、特征值、特征向量和迹的分析。本文在文献、的基础上,用MatlabSymbolicComputation工具箱对误差协方差阵的秩、特征值、特征向量和迹直接进行了分析,得到了一些有用的结论。不同于文献的是,本文中的推导大多是用计算机进行符号运算得到的,提高了速度,简化了推导过程,并且在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是灯标的个数,(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是相应的测量向量。当测量噪声出现时,第i个观测方程为∼bi=Ari+wiwΤiAri=0其中∼bi是第i个含有误差的测量,传感器误差wi可近似为Gaussian白噪声,且满足E{wi}=0‚E{wiwΤj}=σ2i[Ι-(Ari)(Ari)Τ]这里I为单位阵。用测量矢量来确定飞行器的姿态和位置就是要找到姿态矩阵A和位置向量p≜(Xc,Yc,Zc)T,以极小化损失函数J(ˆA‚ˆp)=12Ν∑i=1σ-2i∥∼bi-Ari∥2姿态阵的真值A与估计值ˆA的近似关系为A=e-[δα×]ˆA≈(Ι3×3-[δα×])ˆA其中δα=(δθδψδγ)T,δθ、δψ、δγ是姿态矩阵A的三个欧拉角的估计误差角。其中[a×]的定义为:[a×]=[0-a3a2a30-a1-a2a10],对于向量a,b,a×b=[a×]b。引理1:设参数向量x=(δθδψδγXcYcZc)T,其误差协方差阵P=E{xxT}-E{x}ET{x},则可导出最优误差方差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是姿态矩阵,故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×],从而可知误差协方差阵的秩和姿态矩阵A无关。若要P存在,6×6的矩阵Q的秩应为6。下面我们就来讨论不同的观测矢量时Q的秩、特征值、特征向量和迹。2概观分析2.1raken在单个灯标即N=1时,r=(a1a2a3)T。利用MatlabSymbolicComputation中求秩函数rank(·)仿真得:对任意的a1、a2、a3,只要满足a21+a22+a23=1,则有rank(Q)=2只有两个变量是能观测的,它们是姿态和位置的组合。在2个灯标即N=2时,r1=(a11a12a13)T,r2=(a21a22a23)T。利用MatlabSymbolicComputation中求秩函数rank(·)仿真得:对任意的a1i、a2i(i=1,2),只要满足a211+a212+a213=1和a221+a222+a223=1,则rank(Q)=4只有4个变量是能观测的,它们是姿态和位置的组合。在3个灯标即N=3时,r1=(a11a12a13)T,r2=(a21a22a23)T,r3=(a31a32a33)T。利用MatlabSymbolicComputation中求秩函数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中求矩阵的特征值和特征向量的函数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)得。根据上面的讨论有tr(Ρ4)＜tr(Ρ3)进而有一般情形tr(ˉΡi+1)＜tr(ˉΡi)3估计姿