CN103162687B - 基于信息可信度的图像/惯导组合导航方法 - Google Patents

Abstract

一种基于信息可信度的图像/惯导组合导航方法，其步骤为：(1)位置误差归一化：以图像匹配辅助导航系统和惯性导航系统输出的位置之差作为观测量，根据观测量的误差范围，将其数值归一化为0～1区间；(2)卡尔曼滤波观测噪声协方差矩阵的矩阵校正：以上述步骤(1)得到的图像匹配和导航系统输出的位置为基准，计算新的观测噪声协方差矩阵R_new，进行卡尔曼滤波。本发明具有原理简单、操作方便、能够显著提高导航精度等优点。

Description

基于信息可信度的图像/惯导组合导航方法

技术领域

本发明主要涉及到组合导航领域，特指一种基于信息可信度的图像/惯导组合导航方法。

背景技术

传统的图像/惯导组合导航方法是把图像配准得到的位置信息认为是“可信的”，以此来作为卡尔曼滤波器输入进行组合导航。然而，在某些特征不明显的图像区域，往往会出现误匹配。一旦出现误匹配，若仍然采用传统方法进行组合导航，其结果将会有很大的偏差，甚至完全失去意义，因此就亟需设计一种考虑信息可信度的组合导航方法。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种原理简单、操作方便、能够显著提高导航精度的基于信息可信度的图像/惯导组合导航方法。

为解决上述技术问题，本发明采用以下技术方案：

一种基于信息可信度的图像/惯导组合导航方法，其步骤为：

(1)位置误差归一化：以图像匹配辅助导航系统和惯性导航系统输出的位置之差作为观测量，根据观测量的误差范围，将其数值归一化为0～1区间；

(2)卡尔曼滤波观测噪声协方差矩阵的矩阵校正：以上述步骤(1)得到的图像匹配和导航系统输出的位置为基准，计算新的观测噪声协方差矩阵Rnew，进行卡尔曼滤波。

作为本发明的进一步改进，所述步骤(1)的具体步骤为：

定义图像辅助导航系统装载的基准图对应北向和东向大小为x_range×y_range，图像匹配位置和惯性导航位置对应在图像上的坐标分别为(x_img，y_img)，(x_ins，y_ins)；根据下式(1)对观测量进行归一化处理，令z₁和z₂分别为组合导航滤波北向和东向的观测误差，有：

$z_{\mathrm{normalized}}=\left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{c}{s}_1(x_{\mathrm{img}}-x_{\mathrm{ins}}+x_{\mathrm{range}})\\ s_2(y_{\mathrm{img}}-y_{\mathrm{ins}}+y_{\mathrm{range}})\end{array}\right]---\left(1\right)$

其中，尺度因子为：

$\left\{\begin{array}{c}{s}_1=\frac{1}{{2x}_{\mathrm{range}}}\\ s_2=\frac{1}{{2y}_{\mathrm{range}}}\end{array}\right.---\left(2\right).$

作为本发明的进一步改进，所述步骤(2)的具体步骤为：

根据下式(3)计算新的观测噪声协方差矩阵R_new：

$R_{\mathrm{new}}=\left[\begin{array}{cc}{\mathrm{\σ}}_{\mathrm{img}1}^2+A_1& 0\\ 0& {\mathrm{\σ}}_{\mathrm{img}2}^2+A_2\end{array}\right]---\left(3\right)$

其中，

$\left\{\begin{array}{c}{A}_1={s_1}^{2}Q[({{-z}_1}^2+z_1)Q^2+({{-z}_1}^2+z_1-\frac{1}{4})Q+({z_1}^2-z_1+\frac{1}{3})]\\ A_2={s_2}^{2}Q[({{-z}_2}^2+z_2)Q^2+({{-z}_2}^2+z_2-\frac{1}{4})Q+({z_2}^2-z_2+\frac{1}{3})]\end{array}\right.$

其中，σ_img1和σ_img2分别为图像匹配北向和东向误差的均方差；Q＝1-p，p为飞行器当前位置点对应的匹配可信度，p通过下式(4)得到：

$p=\frac{N_{\mathrm{correct}}}{N_{\mathrm{total}}}---\left(4\right).$

与现有技术相比，本发明的优点在于：本发明的基于信息可信度的图像/惯导组合导航方法，通过匹配可信度对观测噪声协方差矩阵校正来进行卡尔曼滤波，相对于不考虑可信度的传统卡尔曼滤波方法，其导航误差显著减小，能够大大提高导航精度，进而对图像/惯导组合导航理论研究和工程实际应用有重要的意义。

附图说明

图1是本发明的流程示意图。

图2是本发明在具体应用实例中的飞行轨迹示意图；其中图2a为飞行器在真实世界坐标中的航线的示意图，图2b为飞行器对应在图像中的航线的示意图。

图3是本发明在具体应用实例中飞行器对应时刻位置点的匹配可信度示意图。

图4是本发明在具体应用实例中组合导航结果与传统组合导航方法对比示意图。其中图4a为组合导航维度误差对比结果的示意图，图4b为组合导航经度误差对比结果的示意图，图4c为组合导航横滚角误差对比结果的示意图，图4d为组合导航俯仰角误差对比结果的示意图，图4e为组合导航偏航角误差对比结果的示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

如图1所示，本发明的基于信息可信度的图像/惯导组合导航方法，其具体流程为：

1、位置误差归一化：以图像匹配辅助导航系统和惯性导航系统输出的位置之差为观测量，根据观测量的误差范围，将其数值归一化为0～1区间，该数值即为z_normalized。

定义图像辅助导航系统装载的基准图对应北向和东向大小为x_range×y_range，图像匹配位置和惯性导航位置对应在图像上的坐标分别为(x_img，y_img)，(x_ins，y_ins)。根据下式(1)对观测量进行归一化处理，令z₁和z₂分别为组合导航滤波北向和东向的观测误差，有：

$z_{\mathrm{normalized}}=\left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{c}{s}_1(x_{\mathrm{img}}-x_{\mathrm{ins}}+x_{\mathrm{range}})\\ s_2(y_{\mathrm{img}}-y_{\mathrm{ins}}+y_{\mathrm{range}})\end{array}\right]---\left(1\right)$

其中，尺度因子为：

$\left\{\begin{array}{c}{s}_1=\frac{1}{{2x}_{\mathrm{range}}}\\ s_2=\frac{1}{{2y}_{\mathrm{range}}}\end{array}\right.---\left(2\right)$

2、卡尔曼滤波观测噪声协方差矩阵校正，以上述步骤1得到的图像匹配和导航系统输出的位置为基准，计算新的观测噪声协方差矩阵R_new，然后采用经典的卡尔曼滤波进行图像匹配组合导航。根据下式(3)计算新的观测噪声协方差矩阵R_new：

$R_{\mathrm{new}}=\left[\begin{array}{cc}{\mathrm{\σ}}_{\mathrm{img}1}^2+A_1& 0\\ 0& {\mathrm{\σ}}_{\mathrm{img}2}^2+A_2\end{array}\right]---\left(3\right)$

其中，

$\left\{\begin{array}{c}{A}_1={s_1}^{2}Q[({{-z}_1}^2+z_1)Q^2+({{-z}_1}^2+z_1-\frac{1}{4})Q+({z_1}^2-z_1+\frac{1}{3})]\\ A_2={s_2}^{2}Q[({{-z}_2}^2+z_2)Q^2+({{-z}_2}^2+z_2-\frac{1}{4})Q+({z_2}^2-z_2+\frac{1}{3})]\end{array}\right.$

其中，σ_img1和σ_img2分别为图像匹配北向和东向误差的均方差。Q＝1-p，p为飞行器当前位置点对应的匹配可信度，类似于图像匹配概率的计算公式，为下式(4)：

$p=\frac{N_{\mathrm{correct}}}{N_{\mathrm{total}}}---\left(4\right)$

其中，N_correct为正确匹配的次数，N_total为总匹配次数。可以证明式(3)为考虑可信度情况下方差最小的融合公式。

如图2所示，为一个具体的应用实施例，该实例中图像辅助导航系统装载的基准图为600×1650，4米分辨率的SAR图像，图中直线L为飞行轨迹，时间为55秒。通过上式(4)可以计算得到飞行轨迹位置点对应的匹配可信度如图3所示。

采用经典的7状态位置匹配组合导航模型：

$\mathrm{\δ}\stackrel{\·}{x}=\mathrm{F\δx}+\mathrm{Gu}$

δx＝[δαδβδγδv_Nδv_EδLδl]^T

u＝[δω_xδω_yδω_zδf_xδω_y]^T

$F=\left[\begin{array}{ccccccc}0& -\left(\begin{array}{c}\mathrm{\Ω}\sin L+\\ \frac{v_E}{R}\tan L\end{array}\right)& \frac{v_N}{R}& 0& \frac{1}{R}& -\mathrm{\Ω}\sin L& 0\\ \left(\begin{array}{c}\mathrm{\Ω}\sin L+\\ \frac{v_E}{R}\tan L\end{array}\right)& 0& \mathrm{\Ω}\cos L+\frac{v_E}{R}& -\frac{1}{R}& 0& 0& \frac{v_N}{R^2}\\ -\frac{v_N}{R}& -\mathrm{\Ω}\cos L-\frac{v_E}{R}& 0& 0& -\frac{\tan L}{R}& -\mathrm{\Ω}\cos L-\frac{v_E}{{R\cos }^{2}L}& 0\\ 0& {-f}_D& f_E& \frac{v_D}{R}& -2\left(\begin{array}{c}\mathrm{\Ω}\sin L+\\ \frac{v_E}{R}\tan L\end{array}\right)& {-v}_E(2\mathrm{\Ω}\cos L+\frac{v_E}{R{\cos }^{2}L})& 0\\ f_D& 0& {-f}_N& \left(\begin{array}{c}2\mathrm{\Ω}\sin L+\\ \frac{v_E}{R}\tan L\end{array}\right)& \frac{1}{R}\left(\begin{array}{c}{v}_N\tan L\\ +v_D\end{array}\right)& \left(\begin{array}{c}2\mathrm{\Ω}(v_N\cos L-v_D\sin L)\\ +\frac{v_N{v}_E}{{R\cos }^{2}L}\end{array}\right)& 0\\ 0& 0& 0& \frac{1}{R}& 0& 0& 0\\ 0& 0& 0& 0& \frac{1}{R\cos L}& \frac{v_E\tan L}{R\cos L}& 0\end{array}\right]$

$G=\left[\begin{array}{ccccc}{-c}_{11}& {-c}_{12}& {-c}_{13}& 0& 0\\ {-c}_{21}& {-c}_{22}& {-c}_{23}& 0& 0\\ {-c}_{31}& {-c}_{32}& {-c}_{33}& 0& 0\\ 0& 0& 0& c_{11}& c_{12}\\ 0& 0& 0& c_{21}& c_{22}\end{array}\right]$

相关参数为：

	惯导	图像	单位
				初始经度、纬度	120，3	120，3	deg
初始高度	10000	10000	m
				初始速度(北东地)	0，100，0	0，100，0	m/s
初始姿态(横滚、俯仰偏航)	0，0，0	0，0，0	deg

惯性器件性能：

陀螺常值漂移	1(deg/h)
		加速度计零偏	10-3(g)

如图4所示，为不考虑匹配可信度和考虑匹配可信度的组合导航卡尔曼滤波欧拉角及位置误差，其中CKF(conventiona1Kalmanfilter)为传统卡尔曼滤波组合导航结果，RKF(reliabilitybasedKalmanfilter)为考虑为本方法的滤波组合导航结果。图4a～4e分别表示维度、经度、横滚角、俯仰角、偏航角组合导航对比误差。通过这组结果可以表明考虑匹配可信度后，组合导航误差大为降低。综上所述，本发明能够简单快捷的在考虑可信度的情况下进行图像/惯导组合导航，对图像/惯导组合导航具有重要的理论和实用价值。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (3)

1.一种基于信息可信度的图像/惯导组合导航方法，其特征在于，步骤为：

(1)位置误差归一化：以图像匹配辅助导航系统和惯性导航系统输出的位置之差作为观测量，根据观测量的误差范围，将其数值归一化为0～1区间；

(2)卡尔曼滤波观测噪声协方差矩阵的矩阵校正：以上述步骤(1)得到的图像匹配和导航系统输出的位置为基准，计算新的观测噪声协方差矩阵R_new，进行卡尔曼滤波。

2.根据权利要求1所述的基于信息可信度的图像/惯导组合导航方法，其特征在于，所述步骤(1)的具体步骤为：

定义图像辅助导航系统装载的基准图对应北向和东向大小为x_range×y_range，图像匹配位置和惯性导航位置对应在图像上的坐标分别为(x_img,y_img)，(x_ins,y_ins)；根据下式(1)对观测量进行归一化处理，令z₁和z₂分别为组合导航滤波北向和东向的观测误差，有：

$z_{\mathrm{normalized}}=\left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{c}{s}_1(x_{\mathrm{img}}-x_{\mathrm{ins}}+x_{\mathrm{range}})\\ s_2(y_{\mathrm{img}}-y_{\mathrm{ins}}+y_{\mathrm{range}})\end{array}\right]---\left(1\right)$

其中，尺度因子为：

$\left\{\begin{array}{c}{s}_1=\frac{1}{2x_{\mathrm{range}}}\\ s_2=\frac{1}{2y_{\mathrm{range}}}\end{array}\right.---\left(2\right).$

3.根据权利要求2所述的基于信息可信度的图像/惯导组合导航方法，其特征在于，所述步骤(2)的具体步骤为：

根据下式(3)计算新的观测噪声协方差矩阵R_new：

$R_{\mathrm{new}}=\left[\begin{array}{cc}{\mathrm{\σ}}_{\mathrm{img}1}^2+A_1& 0\\ 0& {\mathrm{\σ}}_{\mathrm{img}2}^2+A_2\end{array}\right]---\left(3\right)$

其中，

$\left\{\begin{array}{c}{A}_1={s_1}^{2}Q[(-{z_1}^2+z_1)Q^2+(-{z_1}^2+z_1-\frac{1}{4})Q+({z_1}^2-z_1+\frac{1}{3})]\\ A_2={s_2}^{2}Q[(-{z_2}^2+z_2)Q^2+(-{z_2}^2+z_2-\frac{1}{4})Q+({z_2}^2-z_2+\frac{1}{3})]\end{array}\right.$

其中，σ_img1和σ_img2分别为图像匹配北向和东向误差的均方差；Q＝1-p，p为飞行器

当前位置点对应的匹配可信度，p通过下式(4)得到：

$p=\frac{N_{\mathrm{correct}}}{N_{\mathrm{total}}}---\left(4\right)$

其中，N_correct为正确匹配的次数，N_total为总匹配次数。