[发明专利]利用景象匹配提高导航系统精度的方法及组合导航仿真系统

摘要

本发明公开了一种利用景象匹配提高导航系统导航精度的方法，包括下列步骤：1.1组合导航系统中的地形景象匹配模块根据地形数字图片读入地形数据：1.2根据当前惯性导航系统得到的经纬度，在步骤1.1中的地形图片中找到映射点；1.3以步骤1.2中所述的映射点为中心点，从地形图片数据中截取参考图的数据；1.4对实测图和参考图进行匹配；1.5利用上述的匹配结果对惯导进行修正，本方法可以大幅度提高惯导系统的导航精度。本发明还公开了一种利用该方法的组合导航仿真系统，包括航迹规划模块、飞行控制模块、航迹发生器、惯性导航模块、地形景象匹配模块和图像显示模块，具有很高的定位精度和较强的自主性，具有良好的人机操作界面。

主权项

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)到极坐标系下图像中的某一点(ρ，θ)的变换关系为：$\rho =\log \sqrt{{(x-x_c)}^2+{(y-y_c)}^2}$ $\theta =\arctan \left|\frac{y-y_c}{x-x_c}\right|$ 其中，ρ为对数极坐标系中的极径，θ为极角，(xc，yc)为旋转发生的中心点；设ρ∈[1，2，...，k]，令实测图极坐标变换得到的映射数组为BH1，参考图上任一点的映射数组为BH2，为了度量这两个数组之间的相似程度，选用相关系数方法Rc和平均绝对值方法Rm，公式如下：$R_c=\frac{\underset{i=1}{\overset{k}{\Sigma }}\mathrm{BH}1\left(i\right)\mathrm{gBH}2\left(i\right)}{{\left[\underset{i=1}{\overset{k}{\Sigma }}{\mathrm{BH}1}^2\left(i\right)\right]}^{\frac{1}{2}}{\left[\underset{i=1}{\overset{k}{\Sigma }}{\mathrm{BH}2}^2\left(i\right)\right]}^{\frac{1}{2}}}---\left(1\right)$ $R_m=\frac{1}{k}\underset{i=1}{\overset{k}{\Sigma }}|\mathrm{BH}1\left(i\right)-\mathrm{BH}2\left(i\right)|---\left(2\right)$ 当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=\underset{i=1}{\overset{8}{\Sigma }}|P_i-P_{i+1}|,$ 其中，Pi＝0或1，P9＝P1 1.4.2.2运用加权Hausdorff精匹配方法进行匹配：按照公式采用加权Hausdorff距离WHD的方法：$h_{\mathrm{WHD}}(A,B)=\frac{1}{N_a}\underset{a\in A}{\Sigma }w\left(a\right)·d(a,B)$ $\underset{a\in A}{\Sigma }w\left(a\right)=N_a---\left(3\right)$ 其中，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\left(b\right)=\frac{N_a-k}{\mathrm{no}}+1---\left(4\right)$ 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的误差方程，导航坐标系选为东北天地理坐标系，惯性导航系统的误差状态方程为$\stackrel{·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(5\right)$ 其中系统状态变量定义为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)：$Z_1=\left[\begin{array}{c}{\psi }_I-{\psi }_s\\ (L_I-L_s)R_M\\ ({\lambda }_I-{\lambda }_s)R_N\cos L\end{array}\right]=\left[\begin{array}{c}{\phi }_U\\ R_M\mathrm{\delta L}\\ R_{N6}\cos \mathrm{L\delta \lambda }\end{array}\right]+\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\end{array}\right]\left[\begin{array}{cccccccc}& M1& 0& 0& 0& 0& 0& M\\ 0_{3\times 2}& M0& 0& 0& 0& R_M& 0& {M0}_{3\times 10}\\ & M0& 0& 0& 0& 0& R_N\cos L& M\end{array}\right]X+\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\end{array}\right]---$ 式中V1为景象匹配输出时的航向角度误差，V2为东向位置误差，V3为北向的位置误差，其大小依赖于所采用的景象匹配定位方法的精度。Z2＝[hI-he]＝δh+V2＝[01×8 M1 M01×9]X+V4 (7)式中V4为气压高度表输出时的高度误差。由上述式(6)和(7)可以获得组合导航系统的量测方程Z(t)＝H(t)X(t)+V(t)，其具体形式如式(8)：$Z=\left[\begin{array}{c}{Z}_1\\ \Lambda \\ Z_2\end{array}\right]=\left[\begin{array}{c}{\phi }_U\\ R_M\mathrm{\delta L}\\ R_N\cos \mathrm{L\delta \lambda }\\ \Lambda \\ \delta 8h\end{array}\right]=\left[\begin{array}{cccccccccc}& M& 1& 0& 0& 0& 0& 0& M& \\ 0_{3\times 2}& M& 0& 0& 0& 0& R_M& 0& M& 0_{3\times 10}\\ & M& 0& 0& 0& 0& 0& R_N\cos L& M& \\ \Lambda & \Lambda & \Lambda & \Lambda & \Lambda & \Lambda & \Lambda & \Lambda & \Lambda & \Lambda \\ 0_{1\times 8}& & M& & 1& & & M& & 0_{1\times 9}\end{array}\right]X+\left[\begin{array}{c}{V}_1\\ V_2\\ V_3\\ \Lambda \\ V_4\end{array}\right]---$ 由此，建立了组合导航系统数学模型中的状态方程和量测方程；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)时刻，相应的卡尔曼滤波时间更新方程为：${\hat{X}}_{k\left(i\right)}={\Phi }_{k\left(i\right),k(i-1)}{\hat{X}}_{k(i-1)}---\left(9\right)$ 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)时刻，相应的卡尔曼滤波时间更新和量测更新方程为：${\hat{X}}_{k\left(M\right)|k(M-1)}={\Phi }_{k\left(M\right),k(M-1)}{\hat{X}}_{k(M-1)}---\left(11\right)$ ${\hat{X}}_{k\left(M\right)}={\hat{X}}_{k\left(M\right)|k(M-1)}+K_{k\left(M\right)}[Z_{k\left(M\right)}-H_{k\left(M\right)}{\hat{X}}_{k\left(M\right)|k(M-1)}]---\left(12\right)$ 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)$K_{k\left(M\right)}=P_{k\left(M\right)|k(M-1)}{H}_{k\left(M\right)}^T{(H_{k\left(M\right)}{P}_{k\left(M\right)|k(M-1)}{H}_{k\left(M\right)}^T+R_{k\left(M\right)})}^{-1}---\left(14\right)$ $P_{k\left(M\right)}=(I-K_{k\left(M\right)}{H}_{k\left(M\right)})P_{k\left(M\right)|k(M-1)}{(I-K_{k\left(M\right)}{H}_{k\left(M\right)})}^T+K_{k\left(M\right)}{R}_{k\left(M\right)}{K}_{k\left(M\right)}^T---\left(15\right)$ 其中，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时刻，相应的卡尔曼滤波方程为：${\hat{X}}_{k\left(0\right)|k(-1)}={\Phi }_{k\left(0\right),k(-1)}{\hat{X}}_{k(-1)}---\left(16\right)$ ${\hat{X}}_{k\left(0\right)}={\hat{X}}_{k\left(0\right)|k(-1)}+K_{k\left(0\right)}[Z_{k\left(0\right)}-H_{k\left(0\right)}{\hat{X}}_{k\left(0\right)|k(-1)}]---\left(17\right)$ Pk(0)＝Φk(0)，k(-1)Pk(-1)Φk(0)，k(-1)T+Γk(-1)Qk(-1)Γk(-1)T (18)$K_{k\left(0\right)}=P_{k\left(0\right)|k(-1)}{H}_{k\left(0\right)}^T{(H_{k\left(0\right)}{P}_{k\left(0\right)|k(-1)}{H}_{k\left(0\right)}^T+R_{k\left(0\right)})}^{-1}---\left(19\right)$ $P_{k\left(0\right)}=(I-K_{k\left(0\right)}{H}_{k\left(0\right)})P_{k\left(0\right)|k(-1)}{(I-K_{k\left(0\right)}{H}_{k\left(0\right)})}^T+K_{k\left(0\right)}{R}_{k\left(0\right)}{K}_{k\left(0\right)}^T---\left(20\right)$ 在tk+1时刻的修正参数和相应的估计误差均方误差阵计算方程为：${\hat{X}}_{k\left(M\right)}=\underset{i=1}{\overset{M}{\Pi }}{\Phi }_{k\left(i\right),k(i-1)}{\hat{X}}_{k(i-1)}---\left(21\right)$ $P_{k\left(M\right)}=\underset{i=1}{\overset{M}{\Pi }}[{\Phi }_{k\left(i\right),k(i-1)}{P}_{k(i-1)}{{\Phi }_{k\left(i\right),k(i-1)}}^T+{\Gamma }_{k(i-1)}{Q}_{k(i-1)}{{\Gamma }_{k(i-1)}}^T]---\left(22\right)$ 至此，完成了一个航迹点的景象匹配辅助导航系统的全过程，当出现新的航迹点时，再进行上述景象匹配方法，不断地循环下去则完成利用景象匹配提高导航系统导航精度的目的。