专利名称：利用景象匹配提高导航系统精度的方法及组合导航仿真系统的制作方法
技术领域：
本发明涉及一种提高导航系统导航精度的方法，尤其涉及一种利用景象匹配进行辅助导航从而提高导航精度的方法，同时涉及一种验证该方法的组合导航仿真系统。
背景技术：
精确制导技术的核心在于精确导航定位，目前，由于惯性/GPS组合导航系统和惯性/北斗双星组合导航系统都还存在技术上的局限性，不方便使用。而惯性/地形辅助导航系统以其特有的抗干扰和自主导航能力，在精确导航技术中的地位日益重要。
惯性/地形辅助导航系统分为高程匹配辅助和景象匹配辅助两类，其中高程匹配辅助导航系统尽管具有较强的自主性，但其利用地形高程信息进行辅助导航定位，对导航系统的修正能力有限，导航精度较低。而景象匹配辅助导航系统则是利用机载高分辨率成像雷达或光电图像传感器实时获取地面景物图像，并与机载计算机中预先存储的二维景象数字地图相比较，用于确定出飞行器位置，由于景象匹配定位的精度很高，因此可以利用这种精确的位置信息来消除惯导系统长时间工作的累计误差，以提高导航定位的精度和自主性。
景象匹配辅助导航系统中的景象匹配本质是多传感器景象匹配，而各传感器所获得的图像之间灰度不一致甚至相反，因此不宜采用目前常用的基于灰度的景象匹配方法，必须采用基于特征的景象匹配方法，因此特征点的选取成为关键。同时，由于实测图中存在噪声和几何畸变，因此，选用的景象匹配方法还必须具有一定的鲁棒性。其中，基于Hausdorff距离的边缘景象匹配方法因其计算的简便性而得到广泛的应用。它是由Daniel P.Huttenlocher等人在研究目标检测时提出的，其缺点是不能处理目标被遮掩和外部点存在的情形。同时，由于导航系统有其一定的误差漂移，造成用于匹配的实测图像和数字地图之间有不同程度的旋转和尺度变化，因此，这就要求景象匹配方法具有智能适应旋转和尺度变化的能力，而Hausdorff距离方法对此只能克服很小范围内的值的变化。因此，针对未知的旋转尺度变化，还不宜采用通常的Hausdorff距离方法。并且，景象匹配方法一般基于全局搜索法，耗时较大，不易满足景象匹配辅助导航系统的实时性的特殊要求。
在高精度惯导和景象匹配组合的景象匹配辅助导航系统中，针对景象匹配定位输出需要耗用不等的匹配计算时间，因此，景象匹配输出的定位信息具有固有的滞后效应，如果此时仅采用常规的卡尔曼滤波器进行组合滤波，会导致滤波精度不高甚至滤波发散。
在我国，如何利用景象匹配来有效的辅助导航，这一方面的研究正在进行中，因此基于景象匹配辅助导航系统的理论研究和计算机仿真技术就显得十分的重要。
发明内容
1、发明目的本发明的一个目的是提供一种利用景象匹配进行辅助导航，从而提高导航精度的方法，另一个目的是提供一种实现该方法、演示组合导航过程、验证该方法的计算机仿真系统。
2、技术方案为了达到上述的发明目的，本发明的利用景象匹配提高导航系统导航精度的方法包括下列步骤1.1组合导航系统中的地形景象匹配模块根据地形数字图片读入地形数据地形数字图片采用BMP格式4个字节的文件头，50个字节的文件信息头，图片的数据自下而上存储。首先读入文件头和文件信息头，其中文件信息头包含图片的宽度和高度值，依据该信息可以正确的读入图片数据。由于景象匹配方法需要的图片数据格式是自上而下的，所以需要把获得的图片数据进行反序之后再进行匹配；1.2根据当前惯性导航系统得到的经纬度，在步骤1.1中的地形图片中找到映射点；1.3以步骤1.2中所述的映射点为中心点，根据匹配时间的要求，从地形图片数据中截取和当前惯导误差范围相当大小的参考图的数据；匹配要求时间长，图片则截取大一些，匹配要求时间短，图片则截取小一些；因惯导系统存在一定的误差漂移，导致步骤1.2中的位置信息并不准确。以已含有误差的映射点为中心，上下左右放宽数倍当前惯导误差范围，以此截取出来的参考图中必然包含了正确的位置点。
1.4对实测图和参考图进行匹配，该步骤包括下列步骤1.4.1进行抗变形粗匹配设步骤1.4中的实测图上任意一点为ai(i＝1，2，...，n)，参考图上与之最匹配的点为bj(j＝1，2，...，m)，将实测图以ai为中心，进行对数极坐标变换，根据其他n-1个点在极径和极角映射图中分布的数据N来匹配参考图上的bj；为了增强匹配结果的抗干扰性，本步骤分别以欲匹配的点本身和它的4-邻域点为中心计算其相应的对数极坐标映射图，形成5组数据，共同参与匹配；本步骤通过下列公式实现笛卡儿坐标系下的图像中的某一点(x，y)到极坐标系下图像中的某一点(ρ，θ)的变换关系为ρ=log(x-xc)2+(y-yc)2]]>θ=arctan|y-ycx-xc|]]>其中，ρ为对数极坐标系中的极径，θ为极角。(xc，yc)为旋转发生的中心点。
设ρ∈[1，2，…，k]，令实测图极坐标变换得到的映射数组为BH1，参考图上任一点的映射数组为BH2，为了度量这两个数组之间的相似程度，选用相关系数方法Rc和平均绝对值方法Rm，公式如下Rc=Σi=1kBH1(i)gBH2(i)[Σi=1kBH12(i)]12[Σi=1kBH22(i)]12...(1)]]>Rm=1kΣi=1k|BH1(i)-BH2(i)|...(2)]]>当Rc达到最大值所对应的点和Rm达到最小值所对应的点一致时，此点就是参考图上与实测图ai最匹配的点；两幅图像在粗匹配后，将进行初步的配准，并在数字地图中裁减出相应大小的参考图，用于下一步的精匹配。
本步骤中，进行景象匹配时，并不是仅仅按照图像中心点来做变换，而选取了图像4-邻域的点共同参与，克服了由于未知小尺度变化造成的图像旋转中心点的偏移，有利于找到更准确的景象匹配点。
1.4.2利用基于分支点的加权Hausdorff多级匹配方法进行精匹配1.4.2.1提取分支点本步骤首先采用Canny算子获得步骤1.4所得的实测图和参考图的边缘特征二值图，再对所得的二值图像进行细化，先进行第一次扫描(1)从左到右，从上到下顺序扫描整幅图像，计算当前像素点P的8邻域像素点值；(2)令NC为P1、P2、P3、P4、P5、P6、P7、P8、P1中0值变1值的次数；(3)令NB＝P1+P2+P3+P4+P5+P6+P7+P8；(4)若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P7＝0且P1×P5×P7＝0(检测东南边界)；当P3或P5已标记时，若视P3、P5为0，依然有NC＝1(保证宽度为2的线条不断开)，当上述4个条件全部满足，则将该中心像素作标记；第二次扫描的基本步骤同第一次扫描，若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P5＝0且P3×P5×P7＝0(检测西北边界)；当P1或P7已标记时，若视P1、P7为0，依然有NC＝1，上述4个条件全部满足，则将该中心像素作标记；两次扫描完成后，去掉作标记的像素再重复第一、二次扫描，直到所有点都不可删除为止，即完成了二值图像细化；最后，再提取分支特征点分支点周围像素的值从0变到1或从1变到0的次数T为6次，判断公式如下T=Σi=18|Pi-Pi+1|,]]>其中，Pi＝0或1，P9＝P1。
由于边缘特征二值图中存在分支点，且这些分支点不易受噪声的干扰。在细化后的二值图中，提取这些分支点，并将其作为权值较大的特征点来计算加权Hausdorff距离，可以提高景象匹配的精度。
1.4.2.2运用加权Hausdorff精匹配方法进行匹配为了匹配被斑点噪声污染的图像，获得更加准确的目标匹配结果，结合传统Hausdorff距离定义，本发明采用加权Hausdorff距离WHD(Weighted Hausdorff Distance)的方法。具体公式如下按照公式采用加权Hausdorff距离WHD(Weighted Hausdorff Distance)的方法hWHD(A,B)=1NaΣa∈Aw(a)·d(a,B)]]>Σa∈Aw(a)=Na...(3)]]>其中，A、B是两个点集，Na是点集A中特征点的总数，a是属于A中的一个特征点，d(a，B)是点集A中的特征点a到点集B的距离，w(a)是此距离的权值，本发明为了提高景象匹配的准确性，采用以下的方法在边缘特征二值图像中，特征点分为一般特征点、分支点和欲排除的点三类，分别令其权值为w(c)、w(b)和w(e)。其中，w(e)为0；w(c)为1。将点集A中所有点到点集B的距离将按由小到大的顺序排序，若取其中序号为k的距离为dk(A，B)，则可通过调整k的大小，将点集A的一个部分与点集B进行匹配，就可排除由噪声点和漏检点引起的匹配影响，其中k＝f×Na，0≤f≤1。同时令A中分支点的个数为no，即可得分支点的权值w(b)=Na-kno+1...(4)]]>1.4.2.3小波多级实时景象匹配方法设给定用于匹配的数字地图为A，用于匹配的景物图像为B，设定多级匹配的级数为j，且j＝1，2，...J，其中J表示最大匹配级数，并规定以两倍抽取减小用于匹配的图像尺寸大小。定义Aj为数字地图A的第j级特征图像，同理定义Bj为数字地图B的第j级特征图像。景象匹配首先从第J级开始，即j＝J开始，则多级景象匹配步骤如下(1)利用小波变换提取数字地图A和景物图像B的第j级特征图像Aj和Bj；(2)利用加权Hausdorff距离对第j级特征图像Aj和Bj进行景象匹配，按如下方式进行处理①如果j＝J，则利用特征图像Aj和Bj逐一计算Aj上每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，则τj就反映了在该匹配位置点上的匹配精度，即匹配位置点相对于精确匹配位置点的距离；②如果j＜J，则仅利用特征图像Aj和Bj计算Aj上以点(2×xj+1，2×yj+1)为中心，以2×τj+1为半径的邻域内的每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，其含义同①。
(3)当j＝1时，此时获取的平移点(xj，yj)即为最终的匹配位置点，且此时的匹配参数τj即为最终的匹配定位参数；如果j≠1，则令j＝j-1，并重复上述(1)～(2)步。
该方法在以加权Hausdorff距离作为景象匹配相似性度量的基础上，结合小波变换的多分辨率思想来提取多尺度图像特征，从而实现了多级分层实时匹配，从而快速地获得精确的匹配位置。
1.5利用上述的匹配结果对惯导进行修正1.5.1建立组合导航系统的数学模型为了进一步提高导航精度，以满足长时间自主导航定位的需要，就必须采用组合导航技术。组合导航技术以惯导系统作为主导航系统，并用景象匹配定位系统作为辅助导航系统(定位误差不随时间积累)，通过一定的组合导航模型，从而大大提高导航精度。
本发明采用非等间隔并解决滞后的卡尔曼滤波器进行组合，则组合导航系统的状态方程为INS的误差方程，导航坐标系选为东北天地理坐标系。惯性导航系统的误差状态方程为X·(t)=F(t)X(t)+G(t)W(t)...(5)]]>
其中系统状态变量定义为X＝[φE，φN，φU，δvE，δvN，δvU，δL，δλ，δh，εbx，εby，εbz，εrx，εry，εrz，x，y，z]T其中φE，φN，φU为平台误差角；δvE，δvN，δvU为速度误差；δL，δλ，δh为纬度、经度和高度误差；εbx，εby，εbz，εrx，εry，εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；x，y，z为加速度计零偏。
在组合系统中，量测值有两组。一组为水平方向的量测值，由INS输出的航向角度、纬度和经度与景象匹配给出的航向角度、纬度和经度的差值构成，如式(6)。另一组为高度方向的量测值，由INS输出的高度和气压高度表输出的高度的差值构成，如式(7)。
Z1=ψI-ψs(LI-Ks)RM(λI-λs)RNcosL=φURMδLRNcosLδλ+V1V2V3=M100000M03×2M0000RM0M03×10M00000RNcosLMX+V1V2V3...(6)]]>式中V1为景象匹配输出时的航向角度误差，V2为东向位置误差，V3为北向的位置误差，其大小依赖于所采用的景象匹配定位方法的精度。
Z2＝[h1-he]＝δh+V2＝
X+V4(7)式中V4为气压高度表输出时的高度误差。
由上述式(6)和(7)可以获得组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(8)Z=Z1ΛZ2=φURMδLRNcosLδλΛδh=M100000M03×2M0000RM0M03×10M00000RMcosLMΛΛΛΛΛΛΛΛΛΛ01×8M1M01×9X+V1V2V3ΛV4...(8)]]>由此，建立了组合导航系统数学模型中的状态方程和量测方程。
1.5.2非等间隔及解决量测滞后的滤波方法得到步骤1.5.1中的数学模型后，还要考虑滤波技术。在惯性导航系统和景象匹配系统两个系统输出的基础上，利用滤波技术去估计系统的各种误差，再用误差的估值去校正系统，即可达到组合两个系统的目的。
由于景象匹配定位输出需要耗用不等的匹配计算时间，因此，景象匹配输出的定位信息具有固有的滞后效应。针对景象像匹配定位具有的不等间隔频率输出和量测信息滞后的特点，本发明提出了下述两种滤波方法。
首先，采用解决非等间隔量测的滤波方法进行滤波定义INS的计算周期为TINS，非等间隔量测周期为TSAR，状态方程的离散化周期为T，并令T＝N×TINS，且TSAR＝M×T，式中M、N均为正整数，这样的关系总可以通过合理的M、N选择而得到保证；下面以一个量测周期内的处理过程为例，介绍本发明设计的非等间隔卡尔曼滤波器。其中，tk为某次景象匹配的开始时刻，tk+1为该次景象匹配的结束时刻，tk(i)表示tk+i×T(i＝0，1，2...，M)时刻，如图27所示，从图27的时间关系图，可以清楚地看出，当没有量测信息的输出时，可以在每一个时刻点tk(i)(i＝0，1，2...，M-1)，利用系统状态转移矩阵的特性，只进行卡尔曼滤波器的时间更新；而在量测信息输出的时刻tk(M)，同时进行卡尔曼滤波器的时间更新和量测更新，从而可以解决非等间隔量测的滤波问题。
非等间隔卡尔曼滤波方法为在tk(i)(i＝0，1，2...，M-1)时刻，相应的卡尔曼滤波时间更新方程为X^k(i)=Φk(i),k(i-1)X^k(i-1)...(9)]]>Pk(i)＝Φk(i)，k(i-1)Pk(i-1)Φk(i)，k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T(10)在tk(M)时刻，相应的卡尔曼滤波时间更新和量测更新方程为X^k(M)|k(M-1)=Φk(M),k(M-1)X^k(M-1)...(11)]]>X^k(M)=X^k(M)|k(M-1)+Kk(M)[Zk(M)-Hk(M)X^k(M)|k(M-1)]...(12)]]>Pk(M)＝Φk(M)，k(M-1)Pk(M-1)Φk(M)，k(M-1)T+Γk(M-1)Qk(M-1)Γk(M-1)T(13)Kk(M)=Pk(M)|k(M-1)Hk(M)T(Hk(M)Pk(M)|k(M-1)Hk(M)T+Rk(M))-1...(14)]]>Pk(M)=(I-Kk(M)Hk(M))Pk(M)|k(M-1)(I-Kk(M)Hk(M))T+Kk(M)Rk(M)Kk(M)T...(15)]]>再采用解决量测滞后的滤波方法进行滤波利用上面提出的解决非等间隔量测的卡尔曼滤波方法，就可以很好地解决量测非等间隔的问题。不过在上面的滤波方法讨论过程中，并没有考虑景象匹配滞后效应的影响，从而会影响组合导航精度。为此，本部分主要是在上面提出的解决非等间隔卡尔曼滤波方法的基础上，提出一种解决量测滞后的方案。
从上述的非等间隔量测中的时间关系图27，可以看出，tk+1时刻的量测信息输出对应于tk时刻的INS状态，因此，可以利用一个数组A记录下tk(i)(i＝0，1，2...，M)时刻的所有INS状态信息，当在tk+1时刻有量测信息输出时，利用该量测信息和数组A中记录下来的tk时刻的INS状态信息进行卡尔曼滤波处理，获得tk时刻的卡尔曼滤波修正参数和相应的估计误差均方误差阵；然后利用系统状态转移矩阵的特性，与上述非等间隔卡尔曼滤波时间更新方程类似，计算出tk+1时刻的修正参数和相应的估计误差均方误差阵，并利用该修正参数对tk+1时刻的INS状态进行修正。
设在tk时刻，相应的卡尔曼滤波方程为X^k(0)|k(-1)=Φk(0),k(-1)X^k(-1)...(16)]]>X^k(0)=X^k(0)|k(-1)+Kk(0)[Zk(0)-Hk(0)X^k(0)|k(-1)]...(17)]]>Pk(0)＝Φk(0)，k(-1)Pk(-1)Φk(0)，k(-1)T+Γk(-1)Qk(-1)Γk(-1)T(18)Kk(0)=Pk(0)|k(-1)Hk(0)T(Hk(0)Pk(0)|k(-1)Hk(0)T+Rk(0))-1...(19)]]>
Pk(0)=(I-Kk(0)Hk(0))Pk(0)|k(-1)(I-Kk(0)Hk(0))T+Kk(0)Rk(0)Kk(0)T...(20)]]>在tk+1时刻的修正参数和相应的估计误差均方误差阵计算方程为X^k(M)=Πi=1MΦk(i),k(i-1)X^k(i-1)...(21)]]>Pk(M)=Πi=1M[Φk(i),k(i-1)Pk(i-1)Φk(i),k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T]...(22)]]>至此，完成了一个航迹点的景象匹配辅助导航系统的全过程，当出现新的航迹点时，再进行上述景象匹配方法，不断地循环下去则完成利用景象匹配提高导航系统导航精度的目的。
步骤1.4.2.1中的利用基于分支点的加权Hausdorff多级匹配方法进行精匹配步骤中，在进行分支点的提取前，可先进行去噪处理，具体步骤如下计算得到当前像素点P的8邻域像素点值；设标志位flag1＝P1+P3+P5+P7，flag2＝(P1+P2+P3)(P5+P6+P7)+(P3+P4+P5)(P7+P8+P1)；若P＝0且flag1≥3，即P的4邻域中有3个以上的值为1的点，则P＝1；否则，若P＝1且flag2＝0，则P＝0。
本发明的利用景象匹配提高导航系统导航精度的方法的组合导航仿真系统包括航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块，其中，本系统接受用户指令后，在航迹规划模块中选择并打开地形数字图片文件并在图像显示模块进行显示，然后，设置地形图片的地理位置，再选择地形区域中的航路点，获得该地形区域的航路点信息，完成航迹规划；航迹规划模块的输出信号输入飞行控制模块，在飞行控制模块中，首先利用公式head＝a tan(RNcos Lδλ/RMδL)其中head为航向角，a为航向角变化率，L为经度，λ为纬度，RN为卯酉圈半径，RM为子午圈半径，计算出当前航迹点和下一个航迹点之间的航向角，与当前导航输出的航向角进行比较，根据航向差值，调整航迹发生器IMU仿真中的航向角速率仿真值，从而实现对航迹的控制；航迹发生器的输出一路输入惯性导航模块，惯性导航模块通过航迹发生器仿真的IMU数据，通过惯性导航算法，获得惯性导航数据；另一路输入地形景象匹配模块，其中的组合导航系统需要使用航迹发生器仿真的IMU数据，获得组合导航数据，同时，实测图和参考图进行匹配，输出匹配结果，利用滤波算法，修正惯性导航结果，并将此过程的数据进行保存；地形景象匹配模块的输出数据输入到图像显示模块，此模块动态显示实测图和参考图，对导航结果进行可视化显示，并三维显示飞行器的飞行状态。
图像显示模块包括二维航迹局部显示模块、二维航迹全局显示模块、三维导航显示模块和可视化数据显示模块。二维航迹显示模块的主要目的是对真实航迹和惯性导航输出航迹进行可视化显示，从两个航迹的变化直观的表现地形景象匹配的结果和对惯性导航的修正。其中的二维航迹局部显示模块使导弹进入地形区域后，用户能够直观的看到航迹按照航迹规划模块设置的航迹飞行，动态的反映了导弹的运动过程。二维航迹全局显示模块使用户能够直观的看到整个导航过程的航迹大体变化过程。三维导航显示模块用于显示飞行器的飞行状态。可视化数据显示模块用于显示导航数据和匹配结果的数据。
如图16所示，在仿真过程中，本系统动态显示飞行器的航迹；在飞行控制点对航迹进行控制；在匹配区，对实测图和参考图进行匹配，输出匹配结果，修正惯性导航结果，同时动态显示实测图和参考图，并且能够可视化显示导航结果，三维显示飞行器的飞行状态。
(1)图像显示模块实现二维航迹局部显示、二维航迹全局显示、三维导航显示和可视化数据显示，如图18所示，使用分割窗口的方法对四个窗口的位置和显示进行管理局部显示实现本发明对航迹进行了局部显示。实现的方法为将分辨率disting设置为一个合适的分辨率，根据实际需要，本文默认设置为0.0001，即能将航迹分辨到10米的动态变化。用户可以根据自己的需要，调用设置分辨率对话框CSetDistinguishDlg进行修改。
全局显示实现实现的方法为将分辨率disting设置为一个合适的分辨率，根据实际需要，本文默认设置为0.01，即能将分辨1000米的动态航迹变化。用户可以根据自己的需要，调用设置分辨率对话框CSetDistinguishDlg进行修改。这样虽然分辨率降低了，但是用户能够直观的看到整个导航过程的航迹大体变化过程。
地形图片显示实现首先读入地形配置文件中的地形图片文件路径和地形经纬度，文件格式如表9所示。然后将当前航迹的经纬度与地形图片的经纬度进行比较，根据公式1判断航迹是否进入地形区域，ΔL1＞0、ΔL2＜0、ΔL3＞0及ΔL4＜0时表示进入了地形区域。如果进入地形区域，计算地形的经纬度在二维航迹局部显示坐标系中的位置，最后采用CDib类对地形图片进行显示。
ΔL1=longtitude_track-longtitude_terrain_littleΔL2=longtitude_track-longtitude_terrain_bigΔL3=latitude_track-latitude_terrain_littleΔL4=latitude_track-latitude_terrain_big]]>(2)航迹规划模块采用Visual C++H6.0中的MFC对话框类来实现航迹规划模块，利用MFC的CFileDialog类选择地形文件，利用CDib类加载和显示地形图片，在航迹规划模块中(图19)，首先，通过航迹规划模块的文件选择对话框打开所需要的地形数字图片文件，然后可以选择普通和边缘两种模式对地形图片进行显示，其中，普通模式显示地形的8位灰度原始图；边缘显示主要是利用景象匹配方法中的提取图片边缘图方法实现，通过边缘图，可以直观的了解到地形图片中哪些区域比较适合进行景象匹配。调用地形设置对话框设置地形图片的地理位置之后(地形经纬度)，用户可以通过鼠标选择地形区域中的航路点，程序通过计算获得该地形区域的航路点信息，然后通过表9格式的航迹配置文件格式将所需要的航迹信息写入文件。实现了对飞行航迹的规划。
(3)飞行控制模块实现首先在航迹控制点，程序从航迹配置文件中读入下一个航路点的航迹信息。然后利用head＝a tan(RNcos Lδλ/RMδL)计算出当前航迹点和下一个航迹点之间的航向角，与当前导航输出的航向角进行比较。根据航向差值，调整航迹发生器IMU仿真中的航向角速率仿真值，从而实现对航迹的控制。根据仿真的需要，本发明对航向角速率的最大控制值为30度/秒。如果当前航迹点和下一个航迹点之间的航向角与当前导航输出的航向角的差值大于30度，则按30度/秒的航向变化率进行控制；否则按照两者的差值大小作为控制值进行航向转向控制。由于航向控制过程需要经过多个航路点，所以对接下来的飞行航迹点进行循环修正，直到两者差值在控制范围之内为止。本发明根据实际情况出发，默认的航向差值范围为-0.01～0.01度之间。飞行控制模块的实现流程图如图23所示。
(4)景象匹配模块的实现根据上述利用景象匹配提高导航系统精度的方法可实现本模块。
3、有益效果通过本发明的方法可以大幅度提高惯导系统的导航精度，下面结合本方法中相应步骤进行说明(1)本发明的方法的步骤1.4.1采用了抗变形的粗匹配方法，对该方法优点的说明如下多传感器景象匹配，必然存在由于拍摄角度不同、飞行器航向偏差等原因造成的参考图和实测图之间的旋转尺度变化，因此，如何在有未知几何变形的情况下，仍能准确匹配到图像的中心位置，是景象匹配得以成功应用的关键。抗变形的粗匹配方法定量分析了景象匹配过程中惯性导航系统漂移和无线电气压高度表测量误差对实测图的旋转和尺度所造成的影响。引入了对数极坐标变换，基于图像边缘特征提取，改进了传统匹配中心点的方法，提出了一种结合中心点的4-邻域点共同参与计算的抗旋转和小尺度变化的景象匹配方法。该方法在主要考虑旋转对景象匹配影响的基础上，仍能克服小量尺度变化的影响。仿真结果表明，本方法能很好的满足导航系统准确性和鲁棒性的要求。在只有旋转角度的情况下，定位精度达到1个象素以内。同时，在有尺度变化正负1.1倍以内时，此方法在图像旋转角度达10度时仍能正确匹配定位，且定位误差最大为3个象素，已经能够完全满足补偿中等精度惯性导航系统误差漂移的要求。
为了验证本方法的有效性，利用SAR图像为例进行匹配仿真实验，分别测试了只有旋转变化、只有尺度变化、旋转和尺度共同变化下景象匹配的准确性。景象匹配方法采用MATLAB 7.0语言编写，在主频为Pentium 2.80GHz、内存512M的PC机上进行测试。在景象匹配过程中，根据图像的大小，ρ被分成30份，θ被分成12份(在下文的实施方式中，有具体的参数说明)。采用了部分Hausdorff距离(PHD)方法作为对比分析。选用的SAR图像实测图、参考图像和匹配结果图见图1。具体对比表格见表1。
表1 旋转尺度变化对景象匹配结果影响表
以中等精度惯性导航系统为例，在没有任何外部修正信息的情况下，其航向偏差将达到10°/小时；而气压高度表的精度则相对较高，且其误差随飞行高度呈近似线性变化，一般气压高度表的误差对尺度方面的影响仅仅在正负1.1倍左右。仿真中，我们假设飞行器每飞行15分钟后，拍摄一幅实测图。表1显示了不同旋转和尺度变化对景象匹配影响的对比结果。图1显示了SAR图像的匹配结果。由表1可见，当旋转和尺度变化增大时，景象匹配方法的定位精度开始下降，但本发明提出的4-邻域的对数极坐标变换方法的定位精度比PHD方法的抗旋转尺度变化能力要强的多。当同时存在尺度变化，而旋转角度达到5度时，PHD方法就已经不能匹配了，而本发明的方法在此情况下不受影响。
(2)本发明的方法的步骤1.4.2采用了基于分支点的加权Hausdorff距离精匹配方法，对该方法优点的说明如下为满足景象匹配导航系统实时性的要求，本发明基于细化后提取出的分支特征点，应用加权Hausdorff距离方法进行景象匹配，能同时克服噪声对景象匹配的影响。仿真分析表明，该方法能同时满足景象匹配辅助导航系统实时性和准确性的要求。
为了验证本发明方法的有效性，利用SAR图像为例进行匹配仿真实验，分别测试了景象匹配的准确性和实时性。景象匹配方法采用Visual C++6.0语言编写，在主频为Pentium 2.80GHz、内存512M的PC机上进行测试。在景象匹配过程中，选取匹配参数为参考图的f＝0.85，实测图的f＝0.8。
在噪声对方法精确度影响仿真实验中，采用部分Hausdorff距离(PHD)方法作为对比分析。由于取SAR图像为例，验证对SAR图像影响最大的斑点噪声对景象匹配的影响，选用的SAR图像实测图、参考图像和匹配结果图见图2。具体对比表格见表2。由实验结果可见，在斑点噪声较小的阶段，二种方法的效果差不多，但随着斑点噪声的增大，加权Hausdorff距离方法的精度好于部分Hausdorff距离方法。
表2基于斑点噪声影响的景象匹配结果比较表
在景象匹配前，先通过细化特征点来减少特征文件的冗余度，可以加快匹配速度。我们将没有进行细化预处理的图像进行PHD方法匹配，经过细化预处理的图像进行加权Hausdorff距离匹配，具体匹配时间表见表2。由实验结果可见，细化处理能有效提高系统的实时性。
表3景象匹配实时性结果表
(3)在基于分支点的加权Hausdorff距离精匹配方法中采用了小波多级分层搜索方法，对该方法优点的说明如下本发明针对景象匹配方法不能满足较大尺寸景象匹配的实时性的问题，提出了一种多级实时景象匹配方法。该方法结合小波变换的多分辨率思想来提取多尺度图像特征，从而实现了多级分层实时匹配。
景象匹配方法采用Visual C++6.0语言编写，在主频为赛扬633、内存为256M的PC机上进行测试。仿真实例1中，采用了Lena光学图像，尺寸大小为256×256像素，其中图3(a)为参考图像，图3(b)为在参考图上任意截取的实测图，尺寸大小为84×89像素，图3(c)和图3(d)分别为参考图和实测图的第一级特征图像；图3(e)、图3(f)和图3(g)分别为第三级、第二级和第一级的匹配结果图，且图中的方框位置即代表了实测图在参考图中的位置。
仿真实例2中，采用了光学卫星(IKONOS)图像，尺寸大小为317×292像素，其中图4(a)为参考图像，图4(b)为在参考图上任意截取的实测图，尺寸大小为142×141像素，图4(c)和图4(d)分别为参考图和实测图的第一级特征图像；图4(e)、图4(f)和图4(g)分别为第三级、第二级和第一级的匹配结果图，且图中的方框位置即代表了实测图在参考图中的位置。
仿真实例3中，采用了合成孔径雷达(SAR)图像，尺寸大小为362×283像素，其中图5(a)为参考图像，图5(b)为在参考图上任意截取的实测图，尺寸大小为122×75像素，图5(c)和图5(d)分别为参考图和实测图的第一级特征图像；图5(e)、图5(f)和图5(g)分别为第三级、第二级和第一级的匹配结果图，且图中的方框位置即代表了实测图在参考图中的位置。
当匹配级数为3时，从三种情况下的景象匹配结果图看来，三种情况下的景象匹配结果均定位准确。为了进一步说明本发明提出的多级实时景象匹配方法的性能，本发明还进行了匹配级数为1时的景象匹配仿真研究，下面将用表格的形式对匹配结果加以对比分析说明。表4给出了用于景象匹配的图像尺寸大小，表5给出了匹配级数为1时的景象匹配结果，表6给出了匹配级数为3时的景象匹配结果。
从表5和表6的匹配结果可以清楚看出，相比于仅采用一级景象匹配而言，采用三级景象匹配不仅能保证匹配结果的准确性，而且可大大缩短匹配计算时间。
表4用于景象匹配的图像大小表
表5景象匹配结果(匹配级数＝1)
表5中的匹配位置和正确匹配位置的单位为像素；匹配参数的单位为加权Hausdorff距离。
表6景象匹配结果(匹配级数＝3)
表6中的③、②、①分别表示了第三级匹配、第二级匹配和第一级匹配。
表6中的匹配位置、最终匹配位置和正确匹配位置的单位为像素；匹配参数的单位为加权Hausdorff距离。
实际景象匹配仿真结果表明，光学图像，光学卫星(IKONOS)图像和SAR图像三种情况下的景象匹配结果均定位准确，相比于仅采用一级景象匹配而言，采用三级景象匹配不仅能保证匹配结果的准确性，而且可以大大缩短匹配计算时间。三种情况下的景象匹配均能够在5s以内完成。
因此，通过合理选择图像的大小和相应的分辨率以及景象匹配的级数就完全可以保证SAR景象匹配辅助导航系统对景象匹配的实时性和准确性的性能要求。
(4)本发明采用了组合导航系统，其优点如下本发明特别针对SAR/INS组合导航系统，提出了高度辅助的SAR/INS组合导航系统实现方案，从而解决了景象匹配不能提供高度量测信息的问题。高度辅助的SAR/INS组合导航系统具有很高的定位精度和较强的自主性，而且通过提高SAR景象匹配定位的精度后，该组合导航系统的性能还会有大的提高。此组合导航系统方法也适用于其它具有相同特性的图像传感器和INS惯导系统组合。
本发明考虑到SAR/INS组合导航系统在实际工作时，由于景象匹配定位输出需要耗用不等的匹配计算时间，从而造成了量测的不等间隔频率输出和量测信息滞后，采用常规的卡尔曼滤波方法难以获得高的滤波精度。为此，本发明提出了采用非等间隔并解决滞后的滤波方法以解决上述问题。仿真结果表明，本发明提出的方法具有较高的滤波精度。并且，此滤波方法也同样可以推广到其它具有相同特性的组合导航系统中，因此具有较强的现实应用意义。
考虑到本发明所采用的景象匹配计算时间均能在5s以内完成，同时考虑到SAR图像数据生成的不等间隔和滞后，为此，组合导航仿真时设定景象匹配计算时间为1～5秒的随机数，为了方便问题分析，取匹配计算时间为随机的整数值，并选取状态方程的离散化周期为1s。假设飞机做机动飞行，其飞行轨迹中含有爬升、变速、平飞和转弯等各种飞行状态，其飞行轨迹如图6所示。飞机的初始位置为北纬32°、东经118°、飞行高度为10000m；飞机的初始速度为300(m/s)，航向正北。设惯导系统的等效陀螺漂移为0.1°/h，等效加速度零偏为10-4g；陀螺一阶马尔可夫过程相关时间为3600s，加速度零偏一阶马尔可夫过程相关时间为1800s。设置SAR景象匹配定位的精度为位置误差为30m，航向角度误差为1°；气压高度表输出高度误差为50m。设置滤波器工作初始值如表7所示。
表7卡尔曼滤波器工作初始值
组合导航系统仿真共进行了3600s，采用协方差分析方法评估卡尔曼滤波器对组合导航系统状态误差的估计精度，为了说明问题，采用了常规卡尔曼滤波器，非等间隔卡尔曼滤波器和解决滞后效应的滤波方法三种情况进行仿真分析，其结果如图6～图15所示。由上述的组合导航系统仿真误差曲线可以看出，采用非等间隔卡尔曼滤波器的精度要高于常规的卡尔曼滤波器精度；同时采用解决滞后效应的滤波方法的滤波精度也要高于采用非等间隔卡尔曼滤波器的精度，从而表明了本发明研究的采用非等间隔并解决滞后的滤波方法可以有效提高组合导航系统的滤波精度。采用三种不同的滤波方法所获取的组合导航系统的稳态误差值见表7。
表8组合导航系统的稳态误差值
表8中的1、2、3分别表示常规卡尔曼滤波器，非等间隔卡尔曼滤波器和解决滞后效应的滤波方法三种情况。
综上所述，本发明的方法具有提高导航系统精度、减少景象匹配搜索时间、提高系统的实时性、增强匹配结果抗干扰性的优点。
本发明的仿真系统具备有以下优点可以根据用户要需要加载任意格式的图片，对飞行航迹进行规划，可采用时间和航路点两种方法配准航迹文件，方便用户，根据航迹文件能可靠的仿真航迹数据和IMU数据，通过景象匹配方法对实测图和参考图进行匹配，通过可视化二维显示界面，可以直观的看到匹配结果和修正惯性导航的结果。系统还具有保存和显示航迹数据、惯导数据和匹配结果数据的功能。方便仿真后进行数据分析。同时，系统具有良好的人机操作界面。
四
图1为SAR图像实测图与参考图像匹配示意图(旋转7.5度，尺度变化1.05倍)，其中(a)为参考图，(b)为实测图，(c)为匹配图；图2为SAR图像实测图与参考图像匹配示意图(斑点噪声n＝0.04)，其中(a)为参考图，(b)为实测图，(c)为匹配图；图3为Lena图像实测与参考图像匹配示意图，其中(a)参考图，(b)实测图，(c)参考图的特征图，(d)实测图的特征图，(e)第三级匹配结果图，(f)第二级匹配结果图，(g)第一级匹配结果图；图4为光学卫星(IKONOS)图像实测与参考图像匹配示意图，其中，(a)参考图，(b)实测图，(c)参考图的特征图，(d)实测图的特征图，(e)第三级匹配结果图，(f)第二级匹配结果图，(g)第一级匹配结果图；图5为合成孔径雷达(SAR)图像实测与参考图像匹配示意图，其中，(a)为参考图，(b)为实测图，(c)参考图的特征图，(d)实测图的特征图，(e)第三级匹配结果图，(f)第二级匹配结果图，(g)第一级匹配结果图；图6飞行轨迹示意图；图7东向水平姿态误差角示意图；图8北向水平姿态误差角示意图；图9方位误差角示意图；图10东向速度误差示意图；图11北向速度误差示意图；图12垂直速度误差示意图；
图13纬度误差示意图；图14经度误差示意图；图15高度误差示意图；图16本发明的景象匹配辅助导航仿真系统总体结构示意图；图17景象匹配辅助导航仿真系统软件设计流程图；图18界面显示设计图；图19航迹规划实现流程图；图20航迹规划对话框界面截图；图21航迹二维可视化模块实现流程图；图22航迹二维可视化显示模块的界面截图；图23飞行控制模块实现流程图；图24图像对数极坐标变换示意图；图25ai的4-邻域图；图263×3模板图；图27非等间隔量测中的时间关系图；图28景象匹配辅助导航双机仿真系统的基本工作原理图；图29地形景象辅助仿真计算机串口通讯实现流程图；图30惯性导航仿真计算机串口通讯实现流程图；图31P2P服务器实现流程图；图32为网络通讯实现流程图。
五具体实施例方式
实施例1本实施为利用景象匹配提高导航系统精度的方法，包括下列步骤1.1组合导航系统中的地形景象匹配模块根据地形数字图片读入地形数据地形数字图片采用BMP格式4个字节的文件头，50个字节的文件信息头，图片的数据自下而上存储。首先读入文件头和文件信息头，其中文件信息头包含图片的宽度和高度值，依据该信息可以正确的读入图片数据。由于景象匹配方法需要的图片数据格式是自上而下的，所以需要把获得的图片数据进行反序之后再进行匹配；1.2根据当前惯性导航系统得到的经纬度，在步骤1.1中的地形图片中找到映射点；1.3以步骤1.2中所述的映射点为中心点，根据匹配时间的要求，从地形图片数据中截取和当前惯导误差范围相当大小的参考图的数据；匹配要求时间长，图片则截取大一些，匹配要求时间短，图片则截取小一些；本实施例中，截取3倍惯导误差范围大小的图片因惯导系统存在一定的误差漂移，导致步骤1.2中的位置信息并不准确。以已含有误差的映射点为中心，上下左右放宽数倍当前惯导误差范围，以此截取出来的参考图中必然包含了正确的位置点。
1.4对实测图和参考图进行匹配，该步骤包括下列步骤1.4.1进行抗变形粗匹配如图24、图25所示，设步骤1.4中的实测图上任意一点为ai(i＝1，2，...，n)，参考图上与之最匹配的点为bj(j＝1，2，...，m)，将实测图以ai为中心，进行对数极坐标变换，根据其他n-1个点在极径和极角映射图中分布的数据N来匹配参考图上的bj；为了增强匹配结果的抗干扰性，本步骤分别以欲匹配的点本身和它的4-邻域点为中心计算其相应的对数极坐标映射图，形成5组数据，共同参与匹配；本步骤通过下列公式实现笛卡儿坐标系下的图像中的某一点(x，y)到极坐标系下图像中的某一点(ρ，θ)的变换关系为ρ=log(x-xc)2+(y-yc)2]]>θ=arctan|y-ycx-xc|]]>其中，ρ为对数极坐标系中的极径，θ为极角。(xc，yc)为旋转发生的中心点。
设ρ∈[1，2，…，k]，令实测图极坐标变换得到的映射数组为BH1，参考图上任一点的映射数组为BH2，为了度量这两个数组之间的相似程度，选用相关系数方法Rc和平均绝对值方法Rm，公式如下Rc=Σi=1kBH1(i)gBH2(i)[Σi=1kBH12(i)]12[Σi=1kBH22(i)]12...(1)]]>Rm=1kΣi=1k|BH1(i)-BH2(i)|...(2)]]>当Rc达到最大值所对应的点和Rm达到最小值所对应的点一致时，此点就是参考图上与实测图ai最匹配的点；两幅图像在粗匹配后，将进行初步的配准，并在数字地图中裁减出相应大小的参考图，用于下一步的精匹配；本步骤中，进行景象匹配时，并不是仅仅按照图像中心点来做变换，而选取了图像4-邻域的点共同参与，克服了由于未知小尺度变化造成的图像旋转中心点的偏移，有利于找到更准确的景象匹配点。
1.4.2利用基于分支点的加权Hausdorff多级匹配方法进行精匹配如图26所示，先进行去噪处理，具体步骤如下计算得到当前像素点P的8邻域像素点值；设标志位flag1＝P1+P3+P5+P7；flag2＝(P1+P2+P3)(P5+P6+P7)+(P3+P4+P5)(P7+P8+P1)；若P＝0且flag1≥3，即P的4邻域中有3个以上的值为1的点，则P＝1；否则，若P＝1且flag2＝0，则P＝0。
1.4.2.1提取分支点本步骤首先采用Canny算子获得实测图和参考图的边缘特征二值图，再对所得的二值图像进行细化，先进行第一次扫描(1)从左到右，从上到下顺序扫描整幅图像，计算当前像素点P的8邻域像素点值；(2)令NC为P1、P2、P3、P4、P5、P6、P7、P8、P1中0值变1值的次数；(3)令NB＝P1+P2+P3+P4+P5+P6+P7+P8；(4)若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P7＝0且P1×P5×P7＝0(检测东南边界)；当P3或P5已标记时，若视P3、P5为0，依然有NC＝1(保证宽度为2的线条不断开)，当上述4个条件全部满足，则将该中心像素作标记；第二次扫描的基本步骤同第一次扫描，若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P5＝0且P3×P5×P7＝0(检测西北边界)；当P1或P7已标记时，若视P1、P7为0，依然有NC＝1，上述4个条件全部满足，则将该中心像素作标记；两次扫描完成后，去掉作标记的像素再重复第一、二次扫描，直到所有点都不可删除为止，即完成了二值图像细化；最后，再提取分支特征点分支点周围像素的值从0变到1或从1变到0的次数T为6次，判断公式如下T=Σi=18|Pi-Pi+1|,]]>其中，Pi＝0或1，P9＝P1。
由于边缘特征二值图中存在分支点，且这些分支点不易受噪声的干扰。在细化后的二值图中，提取这些分支点，并将其作为权值较大的特征点来计算加权Hausdorff距离，可以提高景象匹配的精度。
1.4.2.2运用加权Hausdorff精匹配方法进行匹配为了匹配被斑点噪声污染的图像，获得更加准确的目标匹配结果，结合传统Hausdorff距离定义，本发明采用加权Hausdorff距离WHD(Weighted Hausdorff Distance)的方法。具体公式如下按照公式采用加权Hausdorff距离WHD(Weighted Hausdorff Distance)的方法hWHD(A,B)=1NaΣa∈Aw(a)·d(a,B)]]>Σa∈Aw(a)=Na...(3)]]>其中，A、B是两个点集，Na是点集A中特征点的总数，a是属于A中的一个特征点，d(a，B)是点集A中的特征点a到点集B的距离，w(a)是此距离的权值，本发明为了提高景象匹配的准确性，采用以下的方法在边缘特征二值图像中，特征点分为一般特征点、分支点和欲排除的点三类，分别令其权值为w(c)、w(b)和w(e)。其中，w(e)为0；w(c)为1。将点集A中所有点到点集B的距离将按由小到大的顺序排序，若取其中序号为k的距离为dk(A，B)，则可通过调整k的大小，将点集A的一个部分与点集B进行匹配，就可排除由噪声点和漏检点引起的匹配影响，其中k＝f×Na，0≤f≤1。同时令A中分支点的个数为no，即可得分支点的权值w(b)=Na-kno+1...(4)]]>1.4.2.3小波多级实时景象匹配方法设给定用于匹配的数字地图为A，用于匹配的景物图像为B，设定多级匹配的级数为j，且j＝1，2，...J，其中J表示最大匹配级数，并规定以两倍抽取减小用于匹配的图像尺寸大小。定义Aj为数字地图A的第j级特征图像，同理定义Bj为数字地图B的第j级特征图像。景象匹配首先从第J级开始，即j＝J开始，则多级景象匹配步骤如下(1)利用小波变换提取数字地图A和景物图像B的第j级特征图像Aj和Bj；(2)利用加权Hausdorff距离对第j级特征图像Aj和Bj进行景象匹配，按如下方式进行处理
①如果j＝J，则利用特征图像Aj和Bj逐一计算Aj上每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，则τj就反映了在该匹配位置点上的匹配精度，即匹配位置点相对于精确匹配位置点的距离；②如果j＜J，则仅利用特征图像Aj和Bj计算Aj上以点(2×xj+1，2×yj+1)为中心，以2×τj+1为半径的邻域内的每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，其含义同①。
(3)当j＝1时，此时获取的平移点(xj，yj)即为最终的匹配位置点，且此时的匹配参数τj即为最终的匹配定位参数；如果j≠1，则令j＝j-1，并重复上述(1)～(2)步。
该方法在以加权Hausdorff距离作为景象匹配相似性度量的基础上，结合小波变换的多分辨率思想来提取多尺度图像特征，从而实现了多级分层实时匹配，从而快速地获得精确的匹配位置。
1.5利用上述的匹配结果对惯导进行修正1.5.1建立组合导航系统的数学模型为了进一步提高导航精度，以满足长时间自主导航定位的需要，就必须采用组合导航技术。组合导航技术以惯导系统作为主导航系统，并用景象匹配定位系统作为辅助导航系统(定位误差不随时间积累)，通过一定的组合导航模型，从而大大提高导航精度。
如图27所示，本发明采用非等间隔并解决滞后的卡尔曼滤波器进行组合，则组合导航系统的状态方程为INS的误差方程，导航坐标系选为东北天地理坐标系。惯性导航系统的误差状态方程为X·(t)=F(t)X(t)+G(t)W(t)...(5)]]>其中系统状态变量定义为X＝[φE，φN，φU，δvE，δvN，δvU，δL，δλ，δh，εbx，εby，εbz，εrx，εry，εrz，x，y，z]T其中φE，φN，φU为平台误差角；δvE，δvN，δvU为速度误差；δL，δλ，δh为纬度、经度和高度误差；εbx，εby，εbz，εrx，εry，εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；x，y，z为加速度计零偏。
在组合系统中，量测值有两组。一组为水平方向的量测值，由INS输出的航向角度、纬度和经度与景象匹配给出的航向角度、纬度和经度的差值构成，如式(6)。另一组为高度方向的量测值，由INS输出的高度和气压高度表输出的高度的差值构成，如式(7)。
Z1=ψI-ψs(LI-Ks)RM(λI-λs)RNcosL=φURMδLRNcosLδλ+V1V2V3=M100000M03×2M0000RM0M03×10M00000RNcosLMX+V1V2V3...(6)]]>式中V1为景象匹配输出时的航向角度误差，V2为东向位置误差，V3为北向的位置误差，其大小依赖于所采用的景象匹配定位方法的精度。
Z2＝[h1-he]＝δh+V2＝
X+V4(7)
式中V4为气压高度表输出时的高度误差。
由上述式(6)和(7)可以获得组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(8)Z=Z1ΛZ2=φURMδLRNcosLδλΛδh=M100000M03×2M0000RM0M03×10M00000RMcosLMΛΛΛΛΛΛΛΛΛΛ01×8M1M01×9X+V1V2V3ΛV4...(8)]]>由此，建立了组合导航系统数学模型中的状态方程和量测方程。
1.5.2非等间隔及解决量测滞后的滤波方法得到步骤1.5.1中的数学模型后，还要考虑滤波技术。在惯性导航系统和景象匹配系统两个系统输出的基础上，利用滤波技术去估计系统的各种误差，再用误差的估值去校正系统，即可达到组合两个系统的目的。
由于景象匹配定位输出需要耗用不等的匹配计算时间，因此，景象匹配输出的定位信息具有固有的滞后效应。针对景象像匹配定位具有的不等间隔频率输出和量测信息滞后的特点，本发明提出了下述两种滤波方法。
首先，采用解决非等间隔量测的滤波方法进行滤波定义INS的计算周期为TINS，非等间隔量测周期为TS4R，状态方程的离散化周期为T，并令T＝N×TINS，且TSAR＝M×T，式中M、N均为正整数，这样的关系总可以通过合理的M、N选择而得到保证；下面以一个量测周期内的处理过程为例，介绍本发明设计的非等间隔卡尔曼滤波器。其中，tk为某次景象匹配的开始时刻，tk+1为该次景象匹配的结束时刻，tk(i)表示tk+i×T(i＝0，1，2...，M)时刻。从图27的时间关系图，可以清楚地看出，当没有量测信息的输出时，可以在每一个时刻点tk(i)(i＝0，1，2...，M-1)，利用系统状态转移矩阵的特性，只进行卡尔曼滤波器的时间更新；而在量测信息输出的时刻tk(M)，同时进行卡尔曼滤波器的时间更新和量测更新，从而可以解决非等间隔量测的滤波问题。
非等间隔卡尔曼滤波方法为在tk(i)(i＝0，1，2...，M-1)时刻，相应的卡尔曼滤波时间更新方程为X^k(i)=Φk(i),k(i-1)X^k(i-1)...(9)]]>Pk(i)＝Φk(i)，k(i-1)Pk(i-1)Φk(i)，k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T(10)在tk(M)时刻，相应的卡尔曼滤波时间更新和量测更新方程为X^k(M)|k(M-1)=Φk(M),k(M-1)X^k(M-1)...(11)]]>X^k(M)=X^k(M)|k(M-1)+Kk(M)[Zk(M)-Hk(M)X^k(M)|k(M-1)]...(12)]]>Pk(M)＝Φk(M)，k(M-1)Pk(M-1)Φk(M)，k(M-1)T+Γk(M-1)Qk(M-1)Γk(M-1)T(13)
Kk(M)=Pk(M)|k(M-1)Hk(M)T(Hk(M)Pk(M)|k(M-1)Hk(M)T+Rk(M))-1...(14)]]>Pk(M)=(I-Kk(M)Hk(M))Pk(M)|k(M-1)(I-Kk(M)Hk(M))T+Kk(M)Rk(M)Kk(M)T...(15)]]>再采用解决量测滞后的滤波方法进行滤波利用上面提出的解决非等间隔量测的卡尔曼滤波方法，就可以很好地解决量测非等间隔的问题。不过在上面的滤波方法讨论过程中，并没有考虑景象匹配滞后效应的影响，从而会影响组合导航精度。为此，本部分主要是在上面提出的解决非等间隔卡尔曼滤波方法的基础上，提出一种解决量测滞后的方案。
从上述的非等间隔量测中的时间关系图27，可以看出，tk+1时刻的量测信息输出对应于tk时刻的INS状态，因此，可以利用一个数组A记录下tk(i)(i＝0，1，2...，M)时刻的所有INS状态信息，当在tk+1时刻有量测信息输出时，利用该量测信息和数组A中记录下来的tk时刻的INS状态信息进行卡尔曼滤波处理，获得tk时刻的卡尔曼滤波修正参数和相应的估计误差均方误差阵；然后利用系统状态转移矩阵的特性，与上述非等间隔卡尔曼滤波时间更新方程类似，计算出tk+1时刻的修正参数和相应的估计误差均方误差阵，并利用该修正参数对tk+1时刻的INS状态进行修正。
设在tk时刻，相应的卡尔曼滤波方程为X^k(0)|k(-1)=Φk(0),k(-1)X^k(-1)...(16)]]>X^k(0)=X^k(0)|k(-1)+Kk(0)[Zk(0)-Hk(0)X^k(0)|k(-1)]...(17)]]>Pk(0)＝Φk(0)，k(-1)Pk(-1)Φk(0)，k(-1)T+Γk(-1)Qk(-1)Γk(-1)T(18)Kk(0)=Pk(0)|k(-1)Hk(0)T(Hk(0)Pk(0)|k(-1)Hk(0)T+Rk(0))-1...(19)]]>Pk(0)=(I-Kk(0)Hk(0))Pk(0)|k(-1)(I-Kk(0)Hk(0))T+Kk(0)Rk(0)Kk(0)T...(20)]]>在tk+1时刻的修正参数和相应的估计误差均方误差阵计算方程为X^k(M)=Πi=1MΦk(i),k(i-1)X^k(i-1)...(21)]]>Pk(M)=Πi=1M[Φk(i),k(i-1)Pk(i-1)Φk(i),k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T]...(22)]]>至此，完成了一个航迹点的景象匹配辅助导航系统的全过程，当出现新的航迹点时，再进行上述景象匹配方法，不断地循环下去则完成利用景象匹配提高导航系统导航精度的目的。
实施例2本实施例的组合导航仿真系统包括航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块，其中，本系统接受用户指令后，在航迹规划模块中选择并打开地形数字图片文件并在图像显示模块进行显示，然后，设置地形图片的地理位置，再选择地形区域中的航路点，获得该地形区域的航路点信息，完成航迹规划；航迹规划模块的输出信号输入飞行控制模块，在飞行控制模块中，首先利用公式head＝a tan(RNcos Lδλ/RMδL)其中head为航向角，a为航向角变化率，L为经度，λ为纬度，RN为卯酉圈半径，RM为子午圈半径，计算出当前航迹点和下一个航迹点之间的航向角，与当前导航输出的航向角进行比较，根据航向差值，调整航迹发生器IMU仿真中的航向角速率仿真值，从而实现对航迹的控制；航迹发生器的输出一路输入惯性导航模块，惯性导航模块通过航迹发生器仿真的IMU数据，通过惯性导航算法，获得惯性导航数据；另一路输入地形景象匹配模块，其中的组合导航系统需要使用航迹发生器仿真的IMU数据，获得组合导航数据，同时，实测图和参考图进行匹配，输出匹配结果，利用滤波算法，修正惯性导航结果，并将此过程的数据进行保存；地形景象匹配模块的输出数据输入到图像显示模块，此模块动态显示实测图和参考图，对导航结果进行可视化显示，并三维显示飞行器的飞行状态。
图像显示模块包括二维航迹局部显示模块、二维航迹全局显示模块、三维导航显示模块和可视化数据显示模块。二维航迹显示模块的主要目的是对真实航迹和惯性导航输出航迹进行可视化显示，从两个航迹的变化直观的表现地形景象匹配的结果和对惯性导航的修正。其中的二维航迹局部显示模块使导弹进入地形区域后，用户能够直观的看到航迹按照航迹规划模块设置的航迹飞行，动态的反映了导弹的运动过程。二维航迹全局显示模块使用户能够直观的看到整个导航过程的航迹大体变化过程。三维导航显示模块用于显示飞行器的飞行状态。可视化数据显示模块用于显示导航数据和匹配结果的数据。
本实施例为实现本发明的组合导航仿真系统的双机仿真系统，基本工作原理就是将航迹发生器的IMU仿真数据，通过串口或者网络通讯技术发送给惯性导航仿真计算机，然后惯性导航仿真计算机将惯性导航结果发送给景象匹配计算机，最后完成地形的景象匹配和惯性导航结果的修正。景象匹配辅助导航双机仿真系统的基本工作原理图如图28所示。
1)串口通讯的实现本发明采用多线程和Windows串口通讯API函数实现高效的串口通讯。景象匹配辅助仿真计算机串口通讯实现流程图如图29所示，惯性导航仿真计算机串口通讯实现流程图如图30所示，串口通讯的实现可采用图29、图30所示的流程中的一种。
2)网络通讯的实现首先服务器在一个公网IP上等待景象匹配辅助仿真计算机和惯性导航仿真计算机的连接。一旦这两个主机连接之后，服务器从接收到的IP数据包获得这两台主机的IP地址，从UDP协议中，获得这两台主机的通讯端口号。然后将一方的IP地址和端口号发送给另一方。实现的流程图如图31所示。连接服务器之后，景象匹配辅助仿真计算机和惯性导航仿真计算机都获得了对方的外网IP地址和对应的端口号。但是这个时候还不能直接进行通讯，因为现在的网关是不允许外部的数据进入内网的，除非内网中有主机对该数据有需求。所以，经过服务器的连接之后，这两个主机还需要向各自的网关发送请求，允许对方的数据到达。为此，本实施例设计了一个数据传输请求的过程，实现的流程如图32所示。
经过服务器和数据通讯申请流程之后，景象匹配辅助仿真计算机和惯性导航仿真计算机便可以通过图29和图30的通讯流程来进行数据交换。实现了在不同局域网之间的仿真功能。
经过串口和网络的测试，景象匹配辅助导航双机仿真系统不但能够满足实时性的要求，而且比单机系统的性能得到大大的提高。
本实施例的景象匹配辅助导航仿真系统验证了景象匹配算法，演示了图像匹配辅助导航的全过程，其总体结构图如图16所示。其软件设计流程图如图17所示。从整个系统的功能以及操作对系统进行功能划分，其中主要可以分为以下几个模块航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块。选择了微软公司的Windows XP/2000操作系统，程序设计语言采用C/C++，开发平台采用Visual C++6.0，使用Visual C++6.0自带的MFC4.2标准类库进行编程。
1、图像显示模块实现景象匹配辅助导航仿真系统的界面显示主要包括二维航迹局部显示、二维航迹全局显示、三维导航显示和可视化数据显示，界面设计规划如图18所示。使用分割窗口的方法对四个窗口的位置和显示进行管理。
利用Visual C++6.0开发工具，首先将CMyView和CMatchingStatic分割在主窗口的左边和右边，然后将三维显示窗口CMyOpenGlStatic和可视化数字显示窗口CShowNavValueView分割在CMyView的上部和下部。程序实现代码如下m_wndSplitter.CreateStatic(this，1，2)；m_wndSplitter.SetColumnInfo(0，350，350)；m_wndSplitter.CreateView(0，1，RUNTIME_CLASS(CShowMatching)，CSize(600，0)，pContext)；m_wndSplitter1.CreateStatic(&m_wndSplitter，2，1，WS_CHILD|WS_VISIBLE，m_wndSplitter.IdFromRowCol(0，0))；m_wndSplitter1.CreateView(0，0，RUNTIME_CLASS(CMyView)，CSize(0，125)，pContext)；m_wndSplitter1.CreateView(1，0，RUNTIME_CLASS(CShowNavValueView)，CSize(0，500)，pContext)；SetActiveView((CView*)m_wndSplitter.GetPane(0，1))；2、航迹规划模块实现本实施例采用Visual C++6.0中的MFC对话框类来实现航迹规划模块，利用MFC的CFileDialog类选择地形文件，利用CDib类加载和显示地形图片。实现的流程图如图19所示。航迹规划模块的界面显示图如图20所示。
首先用户通过航迹规划模块的文件选择对话框打开所需要的地形数字图片文件，然后可以选择普通和边缘两种模式对地形图片进行显示。普通模式显示地形的8位灰度原始图；边缘显示主要是利用景象匹配算法中的提取图片边缘图算法实现，通过边缘图，可以直观的了解到地形图片中哪些区域比较适合进行景象匹配。调用地形设置对话框设置地形图片的地理位置之后，用户可以通过鼠标选择地形区域中的航路点，程序通过计算获得该地形区域的航路点信息，然后通过表9格式的航迹配置文件格式将所需要的航迹信息写入文件。实现了对飞行航迹的规划。
3、航迹二维可视化显示模块实现实现的流程图如图21所示。
局部显示实现本发明对航迹进行了局部显示。实现的方法为将分辨率disting设置为一个合适的分辨率，根据实际需要，本文默认设置为0.0001，即能将航迹分辨到10米的动态变化。用户可以根据自己的需要，调用设置分辨率对话框CSetDistinguishDlg进行修改。
全局显示实现实现的方法为将分辨率disting设置为一个合适的分辨率，根据实际需要，本文默认设置为0.01，即能将分辨1000米的动态航迹变化。用户可以根据自己的需要，调用设置分辨率对话框CSetDistinguishDlg进行修改。这样虽然分辨率降低了，但是用户能够直观的看到整个导航过程的航迹大体变化过程。
地形图片显示实现首先读入地形配置文件中的地形图片文件路径和地形经纬度，文件格式如表9所示。然后将当前航迹的经纬度与地形图片的经纬度进行比较，根据公式1判断航迹是否进入地形区域，ΔL1＞0、ΔL2＜0、ΔL3＞0及ΔL4＜0时表示进入了地形区域。如果进入地形区域，计算地形的经纬度在二维航迹局部显示坐标系中的位置，最后采用CDib类对地形图片进行显示。
ΔL1=longtitude_track-longtitude_terrain_littleΔL2=longtitude_track-longtitude_terrain_bigΔL3=latitude_track-latitude_terrain_littleΔL4=latitude_track-latitude_terrain_big]]>航迹二维可视化显示的界面截图如图22所示。
4、飞行控制模块实现首先在航迹控制点，程序从航迹配置文件中读入下一个航路点的航迹信息。然后利用式2计算出当前航迹点和下一个航迹点之间的航向角，与当前导航输出的航向角进行比较。根据航向差值，调整航迹发生器IMU仿真中的航向角速率仿真值，从而实现对航迹的控制。根据仿真的需要，本发明对航向角速率的最大控制值为30度/秒。如果当前航迹点和下一个航迹点之间的航向角与当前导航输出的航向角的差值大于30度，则按30度/秒的航向变化率进行控制；否则按照两者的差值大小作为控制值进行航向转向控制。由于航向控制过程需要经过多个航路点，所以对接下来的飞行航迹点进行循环修正，直到两者差值在控制范围之内为止。本发明根据实际情况出发，默认的航向差值范围为-0.01～0.01度之间。飞行控制模块的实现流程图如图23所示。
head＝a tan(RNcos Lδλ/RMδL)其中head为航向角，a为航向角变化率，L为经度，λ为纬度，RN为卯酉圈半径，RM为子午圈半径。
权利要求
1.一种利用景象匹配提高导航系统导航精度的方法，其特征在于包括下列步骤1.1组合导航系统中的地形景象匹配模块根据地形数字图片读入地形数据1.2根据当前惯性导航系统得到的经纬度，在步骤1.1中的地形图片中找到映射点；1.3以步骤1.2中所述的映射点为中心点，根据匹配时间的要求，从地形图片数据中截取和当前惯导误差范围相当大小的参考图的数据；1.4对实测图和参考图进行匹配，该步骤包括下列步骤1.4.1进行抗变形粗匹配设步骤1.4中的实测图上任意一点为ai(i＝1，2，...，n)，参考图上与之最匹配的点为bj(j＝1，2，...，m)，将实测图以ai为中心，进行对数极坐标变换，根据其他n-1个点在极径和极角映射图中分布的数据N来匹配参考图上的bj；为了增强匹配结果的抗干扰性，本步骤分别以欲匹配的点本身和它的4-邻域点为中心计算其相应的对数极坐标映射图，形成5组数据，共同参与匹配；本步骤通过下列公式实现笛卡儿坐标系下的图像中的某一点(x，y)到极坐标系下图像中的某一点(ρ，θ)的变换关系为ρ=log(x-xc)2+(y-yc)2]]>θ=arctan|y-ycx-xc|]]>其中，ρ为对数极坐标系中的极径，θ为极角，(xc，yc)为旋转发生的中心点；设ρ∈[1，2，...，k]，令实测图极坐标变换得到的映射数组为BH1，参考图上任一点的映射数组为BH2，为了度量这两个数组之间的相似程度，选用相关系数方法Rc和平均绝对值方法Rm，公式如下Rc=Σi=1kBH1(i)gBH2(i)[Σi=1kBH12(i)]12[Σi=1kBH22(i)]12---(1)]]>Rm=1kΣi=1k|BH1(i)-BH2(i)|---(2)]]>当Rc达到最大值所对应的点和Rm达到最小值所对应的点一致时，此点就是参考图上与实测图ai最匹配的点；两幅图像在粗匹配后，将进行初步的配准，并在数字地图中裁减出相应大小的参考图，用于下一步的精匹配；1.4.2利用基于分支点的加权Hausdorff多级匹配方法进行精匹配1.4.2.1提取分支点本步骤首先采用Canny算子获得步骤1.4所得的实测图和参考图的边缘特征二值图，再对所得的二值图像进行细化，先进行第一次扫描(1)从左到右，从上到下顺序扫描整幅图像，计算当前像素点P的8邻域像素点值；(2)令NC为P1、P2、P3、P4、P5、P6、P7、P8、P1中0值变1值的次数；(3)令NB＝P1+P2+P3+P4+P5+P6+P7+P8；(4)若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P7＝0且P1×P5×P7＝0(检测东南边界)；当P3或P5已标记时，若视P3、P5为0，依然有NC＝1(保证宽度为2的线条不断开)，当上述4个条件全部满足，则将该中心像素作标记；第二次扫描的基本步骤同第一次扫描，若当前像素点为1(保证为特征点)；NC＝1(保证连通性)且2≤NB≤6(排除P为端点和内部点)；P1×P3×P5＝0且P3×P5×P7＝0(检测西北边界)；当P1或P7已标记时，若视P1、P7为0，依然有NC＝1，上述4个条件全部满足，则将该中心像素作标记；两次扫描完成后，去掉作标记的像素再重复第一、二次扫描，直到所有点都不可删除为止，即完成了二值图像细化；最后，再提取分支特征点分支点周围像素的值从0变到1或从1变到0的次数T为6次，判断公式如下T=Σi=18|Pi-Pi+1|,]]>其中，Pi＝0或1，P9＝P11.4.2.2运用加权Hausdorff精匹配方法进行匹配按照公式采用加权Hausdorff距离WHD的方法hWHD(A,B)=1NaΣa∈Aw(a)·d(a,B)]]>Σa∈Aw(a)=Na---(3)]]>其中，A、B是两个点集，Na是点集A中特征点的总数，a是属于A中的一个特征点，d(a，B)是点集A中的特征点a到点集B的距离，w(a)是此距离的权值；在边缘特征二值图像中，特征点分为一般特征点、分支点和欲排除的点三类，分别令其权值为w(c)、w(b)和w(e)，其中，w(e)为0，w(c)为1，将点集A中所有点到点集B的距离将按由小到大的顺序排序，若取其中序号为k的距离为dk(A，B)，则可通过调整k的大小，将点集A的一个部分与点集B进行匹配，就可排除由噪声点和漏检点引起的匹配影响，其中k＝f×Na，0≤f≤1，同时令A中分支点的个数为no，即可得分支点的权值w(b)=Na-kno+1---(4)]]>1.4.2.3小波多级实时景象匹配方法设给定用于匹配的数字地图为A，用于匹配的景物图像为B，设定多级匹配的级数为j，且j＝1，2，...J，其中J表示最大匹配级数，并规定以两倍抽取减小用于匹配的图像尺寸大小，定义Aj为数字地图A的第j级特征图像，同理定义Bj为数字地图B的第j级特征图像，景象匹配首先从第J级开始，即j＝J开始，则多级景象匹配步骤如下(1)利用小波变换提取数字地图A和景物图像B的第j级特征图像Aj和Bj；(2)利用加权Hausdorff距离对第j级特征图像Aj和Bj进行景象匹配，按如下方式进行处理①如果j＝J，则利用特征图像Aj和Bj逐一计算Aj上每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，则τj就反映了在该匹配位置点上的匹配精度，即匹配位置点相对于精确匹配位置点的距离；②如果j＜J，则仅利用特征图像Aj和Aj计算Aj上以点(2×xj+1，2×yj+1)为中心，以2×τj+1为半径的邻域内的每一个平移点上的加权Hausdorff距离值，并从中选择由最小值所确定的平移点(xj，yj)作为第j级的匹配位置点，且定义此时的加权Hausdorff距离值为τj，其含义同①；(3)当j＝1时，此时获取的平移点(xj，yj)即为最终的匹配位置点，且此时的匹配参数τj即为最终的匹配定位参数；如果j≠1，则令j＝j-1，并重复上述(1)～(2)步。1.5利用上述的匹配结果对惯导进行修正1.5.1建立组合导航系统的数学模型采用非等间隔并解决滞后的卡尔曼滤波器进行组合，则组合导航系统的状态方程为INS的误差方程，导航坐标系选为东北天地理坐标系，惯性导航系统的误差状态方程为X·(t)=F(t)X(t)+G(t)W(t)---(5)]]>其中系统状态变量定义为X＝[φE，φN，φU，δvE，δvN，δvU，δL，δλ，δh，εbx，εby，εbz，εrx，εry，εrz，x，y，z]T其中φE，φN，φU为平台误差角；δvE，δvN，δvU为速度误差；δL，δλ，δh为纬度、经度和高度误差；εbx，εby，εbz，εrx，εry，εrz分别为陀螺常值漂移误差和一阶马尔可夫漂移误差；x，y，z为加速度计零偏；在组合系统中，量测值有两组，一组为水平方向的量测值，由INS输出的航向角度、纬度和经度与景象匹配给出的航向角度、纬度和经度的差值构成，如式(6)；另一组为高度方向的量测值，由INS输出的高度和气压高度表输出的高度的差值构成，如式(7)Z1=ψI-ψs(LI-Ls)RM(λI-λs)RNcosL=φURMδLRN6cosLδλ+V1V2V3M100000M03×2M0000RM0M03×10M00000RNcosLMX+V1V2V3---]]>式中V1为景象匹配输出时的航向角度误差，V2为东向位置误差，V3为北向的位置误差，其大小依赖于所采用的景象匹配定位方法的精度。Z2＝[hI-he]＝δh+V2＝
X+V4(7)式中V4为气压高度表输出时的高度误差。由上述式(6)和(7)可以获得组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(8)Z=Z1ΛZ2=φURMδLRNcosLδλΛδ8h=M100000M03×2M0000RM0M03×10M00000RNcosLMΛΛΛΛΛΛΛΛΛΛ01×8M1M01×9X+V1V2V3ΛV4---]]>由此，建立了组合导航系统数学模型中的状态方程和量测方程；1.5.2非等间隔及解决量测滞后的滤波方法得到步骤1.5.1中的数学模型后，进行滤波，在惯性导航系统和景象匹配系统两个系统输出的基础上，利用滤波技术去估计系统的各种误差，再用误差的估值去校正系统，即可达到组合两个系统的目的；首先，采用解决非等间隔量测的滤波方法进行滤波定义INS的计算周期为TINS，非等间隔量测周期为TSAR，状态方程的离散化周期为T，并令T＝N×TINS，且TSAR＝M×T，式中M、N均为正整数，这样的关系总可以通过合理的M、N选择而得到保证，采用下列公式进行滤波在tk(i)(i＝0，1，2，...，M-1)时刻，相应的卡尔曼滤波时间更新方程为X^k(i)=Φk(i),k(i-1)X^k(i-1)---(9)]]>Pk(i)＝Φk(i)，k(i-1)Pk(i-1)Φk(i)，k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T(10)在tk(M)时刻，相应的卡尔曼滤波时间更新和量测更新方程为X^k(M)|k(M-1)=Φk(M),k(M-1)X^k(M-1)---(11)]]>X^k(M)=X^k(M)|k(M-1)+Kk(M)[Zk(M)-Hk(M)X^k(M)|k(M-1)]---(12)]]>Pk(M)＝Φk(M)，k(M-1)Pk(M-1)Φk(M)，k(M-1)T+Γk(M-1)Qk(M-1)Γk(M-1)T(13)Kk(M)=Pk(M)|k(M-1)Hk(M)T(Hk(M)Pk(M)|k(M-1)Hk(M)T+Rk(M))-1---(14)]]>Pk(M)=(I-Kk(M)Hk(M))Pk(M)|k(M-1)(I-Kk(M)Hk(M))T+Kk(M)Rk(M)Kk(M)T---(15)]]>其中，tk为某次景象匹配的开始时刻，tk+1为该次景象匹配的结束时刻，tk(i)表示tk+i×T(i＝0，1，2，...，M)时刻；再采用解决量测滞后的滤波方法进行滤波利用一个数组A记录下tk(i)(i＝0，1，2，...，M)时刻的所有INS状态信息，当在tk+1时刻有量测信息输出时，利用该量测信息和数组A中记录下来的tk时刻的INS状态信息进行卡尔曼滤波处理，获得tk时刻的卡尔曼滤波修正参数和相应的估计误差均方误差阵；然后利用系统状态转移矩阵的特性，与上述非等间隔卡尔曼滤波时间更新方程类似，计算出tk+1时刻的修正参数和相应的估计误差均方误差阵，并利用该修正参数对tk+1时刻的INS状态进行修正；设在tk时刻，相应的卡尔曼滤波方程为X^k(0)|k(-1)=Φk(0),k(-1)X^k(-1)---(16)]]>X^k(0)=X^k(0)|k(-1)+Kk(0)[Zk(0)-Hk(0)X^k(0)|k(-1)]---(17)]]>Pk(0)＝Φk(0)，k(-1)Pk(-1)Φk(0)，k(-1)T+Γk(-1)Qk(-1)Γk(-1)T(18)Kk(0)=Pk(0)|k(-1)Hk(0)T(Hk(0)Pk(0)|k(-1)Hk(0)T+Rk(0))-1---(19)]]>Pk(0)=(I-Kk(0)Hk(0))Pk(0)|k(-1)(I-Kk(0)Hk(0))T+Kk(0)Rk(0)Kk(0)T---(20)]]>在tk+1时刻的修正参数和相应的估计误差均方误差阵计算方程为X^k(M)=Πi=1MΦk(i),k(i-1)X^k(i-1)---(21)]]>Pk(M)=Πi=1M[Φk(i),k(i-1)Pk(i-1)Φk(i),k(i-1)T+Γk(i-1)Qk(i-1)Γk(i-1)T]---(22)]]>至此，完成了一个航迹点的景象匹配辅助导航系统的全过程，当出现新的航迹点时，再进行上述景象匹配方法，不断地循环下去则完成利用景象匹配提高导航系统导航精度的目的。
2.如权利要求1所述的利用景象匹配提高导航系统导航精度的方法，其特征在于步骤1.4.2.1中的利用基于分支点的加权Hausdorff多级匹配方法进行精匹配步骤中，在进行分支点的提取前，先进行去噪处理，具体步骤如下计算得到当前像素点P的8邻域像素点值；设标志位flag1＝P1+P3+P5+P7，flag2＝(P1+P2+P3)(P5+P6+P7)+(P3+P4+P5)(P7+P8+P1)；若P＝0且flag1≥3，即P的4邻域中有3个以上的值为1的点，则P＝1；否则，若P＝1且flag2＝0，则P＝0。
3.一种如权利要求1所述的利用景象匹配提高导航系统导航精度的方法的组合导航仿真系统，其特征在于，包括航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块，其中，本系统接受用户指令后，在航迹规划模块中选择并打开地形数字图片文件并在图像显示模块进行显示，然后，设置地形图片的地理位置，再选择地形区域中的航路点，获得该地形区域的航路点信息，完成航迹规划；航迹规划模块的输出信号输入飞行控制模块，在飞行控制模块中，首先利用公式head＝atan(RNcosLδλ/RMδL)其中head为航向角，a为航向角变化率，L为经度，λ为纬度，RN为卯酉圈半径，RM为子午圈半径，计算出当前航迹点和下一个航迹点之间的航向角，与当前导航输出的航向角进行比较，根据航向差值，调整航迹发生器IMU仿真中的航向角速率仿真值，从而实现对航迹的控制；航迹发生器的输出一路输入惯性导航模块，惯性导航模块通过航迹发生器仿真的IMU数据，获得惯性导航数据，另一路输入地形景象匹配模块，其中的组合导航系统需要使用航迹发生器仿真的IMU数据，获得组合导航数据，同时，实测图和参考图进行匹配，输出匹配结果，利用滤波算法，修正惯性导航结果，并将此过程的数据进行保存；地形景象匹配模块的输出数据输入到图像显示模块，此模块动态显示实测图和参考图，对导航结果进行可视化显示，并三维显示飞行器的飞行状态。
4.如权利要求3所述的组合导航仿真系统，其特征在于，图像显示模块包括二维航迹局部显示模块、二维航迹全局显示模块、三维导航显示模块和可视化数据显示模块。
全文摘要
本发明公开了一种利用景象匹配提高导航系统导航精度的方法，包括下列步骤1.1组合导航系统中的地形景象匹配模块根据地形数字图片读入地形数据1.2根据当前惯性导航系统得到的经纬度，在步骤1.1中的地形图片中找到映射点；1.3以步骤1.2中所述的映射点为中心点，从地形图片数据中截取参考图的数据；1.4对实测图和参考图进行匹配；1.5利用上述的匹配结果对惯导进行修正，本方法可以大幅度提高惯导系统的导航精度。本发明还公开了一种利用该方法的组合导航仿真系统，包括航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块，具有很高的定位精度和较强的自主性，具有良好的人机操作界面。
文档编号G01S1/00GK101046387SQ20061004113
公开日2007年10月3日 申请日期2006年8月7日 优先权日2006年8月7日
发明者冷雪飞, 刘建业, 熊智, 赖际舟, 曾庆化, 李荣冰, 孙永荣, 赵伟 申请人:南京航空航天大学