CN106767836A - 一种auv地形匹配导航滤波方法 - Google Patents

Abstract

本发明提供的是一种AUV海底地形匹配导航的滤波方法。包括对地形匹配定位的误差估计，地形匹配定位的似然函数修正，地形匹配定位的置信区间估计，参考导航与地形匹配结果的融合滤波。本发明的主要部分是地形匹配定位的误差估计方法、地形匹配定位数据和参考导航数据的融合方法，地形匹配导航计算机接收参考导航数据和实时地形测量数据，通过匹配定位，融合滤波两个步骤得到定位位置估计，然后将位置信息送入参考导航计算机进行导航修正，实现连续的递推导航。

Description

一种AUV地形匹配导航滤波方法

技术领域

本发明涉及的是一种水下地形匹配导航方法，具体地说是一种AUV的地形匹配导航滤波方法。

背景技术

水下地形匹配导航具有无累计导航偏差的优点可作为推算导航的偏差修正，修正过程实际上是地形匹配定位结果与推算导航结果的融合滤波过程，由于地形匹配定位通常不具有连续性以及地形变化的具有很强的随机性容易导致滤波结果的发散。目前地形匹配导航方法主要有TERCOM方法，SIATN方法。TERCOM法属于批处理算法，通过获得航线下方的部分地形与载体携带的先验地形配准获得定位位置，TERCOM算法不涉及滤波和递推导航的问题，由于没有充分利用推算导航的信息，导致算法本身不具备较好的可靠性和连续导航能力；SITAN方法属于递推形式的导航方法，但递推过程需要进行地形的线性化，由于地形具有很强的非线性特征和较大的测量误差，这容易导致滤波发散。

发明内容

本发明的目的在于提供一种能实现连续的地形匹配导航和非连续的地形匹配导航，提高导航方式的灵活性的AUV地形匹配导航滤波方法。

本发明的目的是这样实现的:

AUV在水下航行l_t|t距离之后到达地形匹配导航的修正点，推算导航给出了推算导航位置推算导航的误差并开启地形匹配导航程序，此时按照如下的步骤完成地形匹配导航的滤波；

步骤一、向地形匹配导航系统输入初始数据，所述初始数据包括：推算导航位置推算导航误差AUV航程当前时刻测量得到的地形Z以及先验地形图h；

步骤二、计算地形匹配定位的搜索区间估计，所述搜索区间由下式确定：

其中：λ_x，λ_y表示导航偏差占载体的运行里程的百分比，由参导航系统的统计结果事先给出；α≥1表示放大倍数，取(1,1.5]；

以为中心，和为x方向和y方向的长度构成的矩形区域就是搜索区间X_S；

步骤三、地形匹配定位，

根据步骤二中计算得到的搜索范围X_S开始进行地形搜索匹配，假设潮差为t_ij，搜索点ij的先验地形插值序列与测量地形的高度偏差序列：

假设地形高度偏差序列满足白噪声分布而潮差是常量，得到搜索点ij的潮差和地形测量误差估计：

得到了潮差的估计值之后，计算测量地形和先验地形的真实相似程度：

其中：h(x_ij,y_ij)表示测量序列点(x_ij,y_ij)在先验地形图h中的插值结果；z_ij∈Z，Z表示测量地形，z_ij表示测量地形内的点；

查找似然函数L_ij值最大的位置得到地形匹配定位的位置得到定位位置后将定位位置的潮差估计和测量地形的误差估计作为当前定位位置的最优潮差估计和测量误差估计记为：

步骤四、地形匹配地位结果的有效性检验

根据地形匹配定位位置检验该定位点的有效性，建立如下的有效性检验模型：

其中C_t|t为有效区间，由下面的方法得到：

(1)以步骤三中得到的似然函数L_ij为输入，计算定位点似然函数的下界L_low；

其中:σ_p表示地形匹配地位点的测量误差估计；mn表示地形匹配地位点的个数；Z_α/2表示以σ_p为正态分布的上分位点对应的值；

(2)以L_low为等高平面截取步骤三中的似然函数L_ij，得到的等高线其中i表示等高线索引号，假设共有n条等高线；

(3)查找包围地形匹配定位点的等高线 在等高线所包围区域的内部，就是地形匹配定位的置信区间C_t|t；

步骤五、地形匹配定位结果的定位误差估计，其估计方法如下，先计算地形匹配定位点的信息量，I₊，I_-表示正向和负向的信息量，先计算I₊：

式中：m,n表示测量地形的节点数目，k,l表示地形节点的索引号，σ_p表示定位点的测量误差估计，表示测量地形中的(k,l)号节点在先验地形中的插值结果；

由于定位误差等于信息矩阵的逆P₊＝(I₊)^-1进一步简化：

其中：

由此得到了定位点在x₊和y₊方向的定位误差将I₊计算式中的+d换成-d同理计算P_-：

由此得到了定位点在x_-和y_-方向的定位误差以及x₊和y₊方向的定位误差

将定位误差标记为：

步骤六、地形匹配定位与推算导航的融合滤波

利用推算导航输出误差P_t|t和推算导航的位置地形匹配定位位置地形匹配定位误差和求得两个估计值的滤波结果：

再计算滤波误差滤波误差为：

由于地形的强非线性和随机性特征导致地形匹配导航的滤波难以采用常规的方法(如Kalman滤波)容易滤波发散和收敛太慢，通常滤波过程需要计算观测预测值，这就需要进行地形的线性化操作，不仅实现困难还会引入更大的误差。本发明就是为了简化滤波过程，实现地形匹配导航的连续或非连续递推而设计的。本发明提出了一种新的地形匹配导航的滤波方法。该方法可以实现连续的地形匹配导航和非连续的地形匹配导航，并且导航过程中不需要对地形做额外的处理，避免地形的线性化等其他复杂的操作，导航过程可以对地形匹配定位点进行有效性判定和推算导航与地形匹配定位结果的递推融合滤波。

本发明滤波过程简单不需要进行额外的地形线性化处理，由于滤波过程包含了地形匹配定位结果的有效性评估具有较好的鲁棒性，不存在滤波结果发散的问题。利用本方法不仅可以实现连续的地形匹配定位与推算导航的融合滤波，而且还可以进行间断性的融合滤波，也就是说两次滤波之间可以有一段时间只使用推算导航，当获得有效的地形匹配定位位置后再进行滤波融合，提高了导航方式的灵活性。

附图说明

图1是地形匹配导航的滤波过程；

图2是地形匹配定位过程流程图；

图3是地形匹配定位点有效性检测流程图；

图4是地形匹配定位与推算导航定位结果的相对位置关系。

具体实施方式

下面举例对本发明作更详细的描述。

AUV在水下航行l_t|t距离之后到达地形匹配导航的修正点，推算导航给出了推算导航位置推算导航的误差并开启地形匹配导航程序，此时按照如下的步骤完成地形匹配导航的滤波。

步骤100：初始数据输入，向地形匹配导航系统输入初始数据：

推算导航位置

推算导航误差

AUV航程

当前时刻测量得到的地形Z；

先验地形图h。

1.计算地形匹配定位的搜索区间估计(结合图中101)

该搜索区间即使AUV可能的位置区间，搜索区间由下式确定：

式中：

λ_x，λ_y表示导航偏差占载体的运行里程的百分比，由参导航系统的统计结果事先给出；

α≥1表示放大倍数，一般取(1,1.5]；

以为中心，和为x方向和y方向的长度构成的矩形区域就是搜索区间X_S。

2.地形匹配定位(结合图中102)

根据发明步骤1中计算得到的搜索范围X_S(结合图中101)开始进行地形搜索匹配，假设潮差为t_ij，搜索点ij的先验地形插值序列(结合图中201)与测量地形(结合图中103)的高度偏差序列：

假设地形高度偏差序列满足白噪声分布而潮差是常量，得到搜索点ij的潮差和地形测量误差估计(结合图中202)：

得到了潮差的估计值之后，计算测量地形和先验地形的真实相似程度(结合图中203)：

式中：

h(x_ij,y_ij)表示测量序列点(x_ij,y_ij)在先验地形图h中的插值结果；

z_ij∈Z，Z表示测量地形，z_ij表示测量地形内的点。

查找似然函数L_ij值最大的位置(结合图中206)可得到地形匹配定位的位置得到定位为之后将定位位置的潮差估计和测量地形的误差估计作为当前定位位置的最优潮差估计和测量误差估计记为：(结合图中207)。

3.地形匹配地位结果的有效性检验(结合图中107)

根据步骤2中获得的地形匹配定位位置下面即使检验该定位点的有效性，建立如下的有效性检验模型：

(结合图中305，结合图中306)

式中C_t|t为有效区间，由下面的方法得到：

(1)以步骤3中得到的似然函数L_ij为输入(结合图中203)，计算定位点似然函数的下界L_low(结合图中301)；

式中:σ_p表示地形匹配地位点的测量误差估计；

mn表示地形匹配地位点的个数；

Z_α/2表示以σ_p为正态分布的上分位点对应的值。

(2)以L_low为等高平面截取步骤2中的似然函数L_ij，得到的等高线(结合图中302)，其中i表示等高线索引号，假设共有n条等高线；

(3)查找包围地形匹配定位点(结合图中206)的等高线(在等高线所包围区域的内部)(结合图中303)，就是地形匹配定位的置信区间C_t|t(结合图中304)。

4.地形匹配定位结果的定位误差估计(结合图中105)

其估计方法如下，先计算地形匹配定位点的信息量，I₊，I_-表示正向和负向的信息量，先计算I₊：

式中：m,n表示测量地形的节点数目；k,l表示地形节点的索引号，σ_p表示定位点的测量误差估计。表示测量地形中的(k,l)号节点在先验地形中的插值结果。由于定位误差等于信息矩阵的逆P₊＝(I₊)^-1进一步简化：

式中：

由此得到了定位点在x₊和y₊方向的定位误差将I₊计算式中的+d换成-d同理可以计算P_-：

由此得到了定位点在x_-和y_-方向的定位误差以及x₊和y₊方向的定位误差

将定位误差标记为：

5.地形匹配定位与推算导航的融合滤波(108)

利用推算导航输出误差P_t|t和推算导航的位置地形匹配定位位置地形匹配定位误差和直接求得两个估计值的滤波结果：

式中的取值从取得，取值的规则如下：

A.如果地形匹配定位的位置与推算导航的位置点(结合图中402)和点(结合图中401)则有：

B.如果地形匹配定位的位置与推算导航的位置点(结合图中403)和点(结合图中401)则有：

C.如果地形匹配定位的位置与推算导航的位置点(结合图中404)和点(结合图中401)则有：

D.如果地形匹配定位的位置与推算导航的位置点(结合图中405)和点(结合图中401)则有：

E.如果地形匹配定位的位置与推算导航的位置点(结合图中406)和点(结合图中401)则有：

F.如果地形匹配定位的位置与推算导航的位置点(结合图中407)和点(结合图中401)则有：

G.如果地形匹配定位的位置与推算导航的位置点(结合图中408)和点(结合图中401)则有：

H.如果地形匹配定位的位置与推算导航的位置点(结合图中409)和点(结合图中410)则有：

I.如果地形匹配定位的位置与推算导航的位置重合都位于点(结合图中401)则有：

下面就可以计算滤波误差滤波误差为：

通过以上的步骤就可以实现地形匹配导航的递推估计。

Claims (7)

1.一种AUV地形匹配导航滤波方法，其特征是：AUV在水下航行l_t|t距离之后到达地形匹配导航的修正点，推算导航给出了推算导航位置推算导航的误差并开启地形匹配导航程序，此时按照如下的步骤完成地形匹配导航的滤波；

步骤一、向地形匹配导航系统输入初始数据；

步骤二、计算地形匹配定位的搜索区间估计；

步骤三、地形匹配定位；

步骤四、地形匹配地位结果的有效性检验；

步骤五、地形匹配定位结果的定位误差估计；

步骤六、地形匹配定位与推算导航的融合滤波。

2.根据权利要求1所述的AUV地形匹配导航滤波方法，其特征是：所述初始数据包括：推算导航位置推算导航误差AUV航程当前时刻测量得到的地形Z以及先验地形图h。

3.根据权利要求3所述的AUV地形匹配导航滤波方法，其特征是：所述搜索区间由下式确定：

$\left\{\begin{array}{c}{\hat{x}}_{t|t}-\mathrm{\&alpha;}({\mathrm{\&lambda;}}_x{l}_{t|t}^x+3\sqrt{P_{t|t}^x})<x_s<{\hat{x}}_{t|t}+\mathrm{\&alpha;}({\mathrm{\&lambda;}}_x{l}_{t|t}^x+3\sqrt{P_{t|t}^x})\\ {\hat{y}}_{t|t}-\mathrm{\&alpha;}({\mathrm{\&lambda;}}_y{l}_{t|t}^y+3\sqrt{P_{t|t}^y})<y_s<{\hat{y}}_{t|t}+\mathrm{\&alpha;}({\mathrm{\&lambda;}}_y{l}_{t|t}^y+3\sqrt{P_{t|t}^y})\end{array}\right.$

其中：λ_x，λ_y表示导航偏差占载体的运行里程的百分比，由参导航系统的统计结果事先给出；α≥1表示放大倍数，取(1,1.5]；

以为中心，和为x方向和y方向的长度构成的矩形区域就是搜索区间X_S。

4.根据权利要求3所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配定位具体包括：

根据步骤二中计算得到的搜索范围X_S开始进行地形搜索匹配，假设潮差为t_ij，搜索点ij的先验地形插值序列与测量地形的高度偏差序列：

${\mathrm{\&Delta;h}}_{ij}=Z-{\hat{h}}_{ij}$

假设地形高度偏差序列满足白噪声分布而潮差是常量，得到搜索点ij的潮差和地形测量误差估计：

$\left\{\begin{array}{c}{t}_{ij}=mean\left(\mathrm{\&Delta;}{h}_{ij}\right)\\ {\mathrm{\&sigma;}}_{ij}=std\left(\mathrm{\&Delta;}{h}_{ij}\right)\end{array}\right.$

得到了潮差的估计值之后，计算测量地形和先验地形的真实相似程度：

$L_{ij}=\frac{1}{\sqrt{2{\mathrm{\&pi;\&sigma;}}_{ij}^2}}\exp (-\frac{1}{{\mathrm{mn\&sigma;}}_{ij}^2}\underset{i=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{j=1}{\overset{n}{\mathrm{\&Sigma;}}}{\left(z_{ij}+t_{ij}-h\left(x_{ij},y_{ij}\right)\right)}^2)$

其中：h(x_ij,y_ij)表示测量序列点(x_ij,y_ij)在先验地形图h中的插值结果；z_ij∈Z，Z表示测量地形，z_ij表示测量地形内的点；

查找似然函数L_ij值最大的位置得到地形匹配定位的位置得到定位位置后将定位位置的潮差估计和测量地形的误差估计作为当前定位位置的最优潮差估计和测量误差估计记为：

5.根据权利要求4所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配地位结果的有效性检验具体包括：

根据地形匹配定位位置检验该定位点的有效性，建立如下的有效性检验模型：

其中C_t|t为有效区间，由下面的方法得到：

(1)以步骤三中得到的似然函数L_ij为输入，计算定位点似然函数的下界L_low；

$L_{low}=\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_p}\exp (-\frac{{\left(Z_{\mathrm{\&alpha;}/2}\right)}^2}{2{\mathrm{mn\&sigma;}}_p^2})$

其中:σ_p表示地形匹配地位点的测量误差估计；mn表示地形匹配地位点的个数；Z_α/2表示以σ_p为正态分布的上分位点对应的值；

(2)以L_low为等高平面截取步骤三中的似然函数L_ij，得到的等高线其中i表示等高线索引号，假设共有n条等高线；

(3)查找包围地形匹配定位点的等高线 在等高线所包围区域的内部，就是地形匹配定位的置信区间C_t|t。

6.根据权利要求5所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配定位结果的定位误差估计的估计方法为：

先计算地形匹配定位点的信息量，I₊，I_-表示正向和负向的信息量，先计算I₊：

式中：m,n表示测量地形的节点数目，k,l表示地形节点的索引号，σ_p表示定位点的测量误差估计，表示测量地形中的(k,l)号节点在先验地形中的插值结果；

由于定位误差等于信息矩阵的逆P₊＝(I₊)^-1进一步简化：

$\left\{\begin{array}{c}{P}_{+}=\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{a_{11}{a}_{22}-a_{12}{a}_{21}}\left[\begin{array}{cc}{a}_{22}& -a_{21}\\ -a_{12}& a_{11}\end{array}\right]\\ P_{+}^x=a_{22}\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{a_{11}{a}_{22}-a_{12}{a}_{21}}\\ P_{+}^y=a_{11}\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{a_{11}{a}_{22}-a_{12}{a}_{21}}\end{array}\right.$

其中：

由此得到了定位点在x₊和y₊方向的定位误差将I₊计算式中的+d换成-d同理计算P_-：

$\left\{\begin{array}{c}{P}_{-}=\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{b_{11}{b}_{22}-b_{12}{b}_{21}}\left[\begin{array}{cc}{b}_{22}& -b_{21}\\ -b_{12}& b_{11}\end{array}\right]\\ P_{-}^x=b_{22}\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{b_{11}{b}_{22}-b_{12}{b}_{21}}\\ P_{-}^y=b_{11}\frac{{\left({\mathrm{\&sigma;}}_p\right)}^2}{b_{11}{b}_{22}-b_{12}{b}_{21}}\end{array}\right.$

$\left\{\begin{array}{c}{b}_{11}=\underset{k=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{l=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl}-d,y_{kl}\left)\right)}^2}{d^2}\\ b_{12}=b_{21}=\underset{k=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{l=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl}-d,y_{kl}-d\left)\right)}^2}{2d^2}\\ b_{22}=\underset{k=1}{\overset{m}{\mathrm{\&Sigma;}}}\underset{l=1}{\overset{n}{\mathrm{\&Sigma;}}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl},y_{kl}-d\left)\right)}^2}{d^2}\end{array}\right.$

由此得到了定位点在x_-和y_-方向的定位误差以及x₊和y₊方向的定位误差

将定位误差标记为：

$\left\{\begin{array}{c}{P}_{t|t}^{x-}=P_{-}^x\\ P_{t|t}^{y-}=P_{-}^y\end{array}\right.,\left\{\begin{array}{c}{P}_{t|t}^{x+}=P_{+}^x\\ P_{t|t}^{y+}=P_{+}^y\end{array}\right.;$

7.根据权利要求6所述的AUV地形匹配导航滤波方法，其特征是所述地形匹配定位与推算导航的融合滤波具体包括：

利用推算导航输出误差P_t|t和推算导航的位置地形匹配定位位置地形匹配定位误差和求得两个估计值的滤波结果：

${\hat{\hat{X}}}_{t|t}={(P_{t|t}^P+P_{t|t})}^{-1}(X_{t|t}^P{P}_{t|t}^P+{\hat{X}}_{t|t}{P}_{t|t})$

再计算滤波误差滤波误差为：

${\hat{P}}_{t|t}={({\left(P_{t|t}^P\right)}^{-1}+{\left(P_{t|t}\right)}^{-1})}^{-1}.$