CN106885576A - 一种基于多点地形匹配定位的auv航迹偏差估计方法 - Google Patents

Abstract

本发明提供的是一种基于多点地形匹配定位的AUV航迹偏差估计方法。(1)地形匹配定位的搜索区间估计；(2)未知潮差和测量误差情况下的地形匹配定位；(3)测量误差估计和定位置信区间估计；(4)定位误差估计；(5)多点地形匹配定位初航迹偏差估计；(6)航迹关联与误定位点剔除；(7)航迹二次拟合。本发明的优点是结合了推算导航在时域和空域都缓慢变化而地形匹配定位结果在时域和空域不具有扩散性的特点，在航迹线上获得多个密集的地形匹配定位点，多点的定位信息拟合推算导航的航迹线从而使得定位的精度和可靠性大幅增加。

Description

一种基于多点地形匹配定位的AUV航迹偏差估计方法

技术领域

本发明涉及的是一种地形匹配定位方法，具体地说是一种AUV航迹偏差估计方法。

背景技术

地形匹配定位的有效性和可靠性是地形匹配定位和地形匹配导航技术的关键内容，如在地形匹配导航的初始阶段通常推算导航系统的导航偏差非常大，从而使得地形匹配定位的搜索区间非常大这种情况很容易产生误定位和伪点。而且，较大的推算导航使得地形匹配导航结果收敛较慢，而且容易导致导航结果的发散。目前较常用的地形匹配导航方法如TERCOM方法采用批处理的方法实现定位，但该方法并未提及如何处理定位伪点的问题，SITAN方法采用了递推滤波技术，但需要对地形进行线性化而且新型化带来的滤波误差很容易导致滤波发散，此外哈尔滨工程大学陈小龙博士在其博士论文AUV《水下地形匹配辅助导航技术研究》中，将粒子滤波技术应用于地形匹配导航定位使得滤波过不需要线性化地形，但仍然无法解决初始匹配点的推算导航误差大带来的滤波收敛慢，滤波发散等问题。因此，地形匹配导航系统的性能与初始位置的定位精度有很大的关系，我们希望在地形匹配导航的初始地形匹配定位点能以较高的精度和可靠性接近AUV真实的位置，也就是说在初始的地形匹配定位点AUV的推算导航偏差估计一定要精确可靠。

发明内容

本发明的目的在于提供一种定位的精度和可靠性高的基于多点地形匹配定位的AUV航迹偏差估计方法。

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

(1)地形匹配定位的搜索区间估计

对于第n个地形匹配点，根据推算导航的输出位置Xⁿ＝(xⁿ,yⁿ)推算导航的估计误差Wⁿ＝(W^xn,W^yn)，推算导航给出的AUV的航行里程估计AUV推算导航系统给出的当前匹配点的推算导航偏差区间，即当前点的地形匹配定位的搜索区间

其中：xⁿ和yⁿ表示推算导航的位置，令Xⁿ＝(xⁿ,yⁿ)；

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

和表示载体在x方向和y方向的运行里程，由推算导航给出；

P^xn和P^yn表示推算导航的误差，由推算导航给出。

(2)未知潮差和测量误差情况下的地形匹配定位

对于搜索区间内的每一个搜索点ij估计该点的先验地形与测量地形的高度偏差并估计高度偏差序列的均值和标准差设为潮差估计、为两次叠加的测量误差估计:

带入似然函数：

获得搜索点的似然函数值，则似然函数取值最大的位置就是地形匹配定位的位置：

(3)测量误差估计和定位置信区间估计

根据步骤(2)得到的定位信息X^pn，得到定位点的地形面插值高度序列和地形的测量高度差序列Δh^pn：

Δh^pn＝Zⁿ-h(X^pn)

以及高度偏差的标准差：

s^pn＝std(Δh^pn)；

在获得误差估计和定位位置之后，对地形匹配定位的结果进行有效性检验，设地形测量误差服从高斯分布，则定位点位置的高度偏差Δh^pn服从统计分布：

得到χ²(N-1)的置信度为α的分位点

由每一个搜索点的似然函数和χ²(N-1)的置信度为α的分位点值，得到最小容许的似然函数值L_-(x^pn)：

式中：

L_-(x^pn)表示定点的似然函数值在置信度为α时能到达的下界，这个下界就是一个截平面，每一个定位点的似然函数L(x^pn)与这个截平面的交线构成了一个等值线，等值线包围的区域就是定位的置信区间C^pn。

(4)定位误差估计

地形匹配定位的误差估计，首先计算定位点测量地形的Fisher信息量Iⁿ，由于地形的信息量沿x和y的正向和负方向不同，分别用和表示正向和负向，

采用双线性插值得到测量节点在先验地形图中的插值

其中：(x₁,y₁)(x₂,y₂)(x₃,y₃)(x₄,y₄)分别表示插值点所在的地形网格内，左下角为1号点顺时针排列的四个地形节点；

由于定位误差等于信息矩阵的逆进一步简化：

式中：

由此得到了定位点在x₊和y₊方向的定位误差

同理计算

由此得到了定位点在x_-和y_-方向的定位误差

以x_-和x₊的大值作为x的定位误差，以y_-和y₊的大值作为y的定位误差，得到最终的定位误差为：

(5)多点地形匹配定位初航迹偏差估计

根据地形匹配定位的位置X^pn＝(x^pn,y^pn)和参考导航位置(xⁿ,yⁿ)，利用加权最小二乘估计得到(Δx,Δy)的估计结果

式中，λ^xn，λ^yn表示对第i定位结果在x和y方向的加权，且满足加权系数λ^xn，λ^yn按如下方法进行求取，

从而得到当前时刻的定位位置：

(6)航迹关联与误定位点剔除

根据步骤(3)得到的地形匹配定位的有效区间构建的地形匹配定位的有效检验条件如下：

式中：Xⁿ表示经过地形匹配定位修正后地形匹配定位点在推算导航航线上的位置。

(7)航迹二次拟合

如果在步骤(6)中有无效的定位点，就将该定位点剔除；如果剔除了定位点，则重新计算各个定位点的权重，对航迹进行再一次的拟合计算，

且为有效

最后就得到最终的航线偏移量

本发明提出了一种基于多点地形匹配定位的推算导航航迹偏差修正方法，该方法的优点是结合了推算导航在时域和空域都缓慢变化而地形匹配定位结果在时域和空域不具有扩散性的特点，在航迹线上获得多个密集的地形匹配定位点，多点的定位信息拟合推算导航的航迹线从而使得定位的精度和可靠性大幅增加。

与常规的地形匹配算法不同的是，本发明利用了AUV推算导航在时间和地形空间上的推算导航偏差缓慢变化和AUV地形匹配定位结果在时间和地形空间上不扩散的特点。地形匹配定位的时间不扩散性和推算导航的时域扩散性形成互补，利用地形匹配定位可以修正推算导航长时间的累积偏差；推算导航偏差在空间的缓慢变化的特性与地形匹配定位的空间跳变特性形成互补，利用这种缓变特性可以对地形匹配定位结果进行航迹关联和有效监测，提高地形匹配定位的有效性。本发明提出的方法可以解决由于初始匹配时推算导航偏差过大导致的收敛慢和地形精度低的问题，同时还可以对定位点进行有效性检测，和定位伪点，误定位点的筛选，提高地形匹配导航的精度和可靠性。

附图说明

图1是匹配定位点与参考导航点位置关系；

图2a-图2b是偏差初估计与定位点有效性检验及二次偏差估计模型；

图3是基于航线拟合的水下地形匹配定位方法流程图；

图4是双线性插值节点位置示意图；

图5是本发明的流程图。

具体实施方式

下面举例对基于多点地形匹配定位的AUV航迹偏差估计方法的主要步骤做进一步描述：

步骤一：多点地形匹配定位

假设规划了N个地形匹配定位点来修正航迹偏差，当前时刻到达第n个地形匹配定位点，当前地形匹配点的地形测量序列Zⁿ，推算导航系统给出的推算位置Xⁿ＝(xⁿ,yⁿ)，推算导航的误差Wⁿ＝(W^xn,W^yn)，推算导航给出的航程(图3中第300步)。

Zⁿ表示第n个地形匹配定位点的测量地形；

i表示测量地形中的节点索引号；

k表示测量地形中的节点总数；

表示第n个地形面中的第i个地形节点；

首先计算地形匹配导航点的搜索区间(图3中第317步)：

其中：xⁿ和yⁿ表示推算导航的位置；

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

和表示载体在x方向和y方向的运行里程，由推算导航给出；

W^xn和W^yn表示推算导航的误差，由推算导航给出。

以(xⁿ,yⁿ)为中心，和为x方向和y方向的长度构成的矩形区域就是搜索区间X^sn(图3中第317步)。

遍历搜索区间内的每一个搜索点计算搜索点处的地形面插值序列与测量地形的插值

序列的高度差(图3中第301步)。

计算的均值和标准差(图3中第305步)：

通过极大似然估计函数(图3中第304步)：

式中：

得到每一个匹配点的匹配似然函数Lⁿ(图3中第304步)，其中X^sn表示对第n参考导航点Xⁿ进行匹配定位时匹配区域节点序列，h(X^sn)表示搜索点地形序列X^sn对应的地形插值高度序列。

似然函数Lⁿ最大值位置对应的就是地形匹配的定位位置X^pn＝(x^pn,y^pn)(图3中第302步)，黑色点(图1、图2a和图2b中的100)表示地形匹配导航点在推算导航航迹线上的位置，圆圈(图1、图2a和图2b中的101)表示地形匹配导航点的对地形匹配位置。

由此；我们可以通过这一步获得第n个地形匹配导航点的定位坐标X^pn，似然函数Lⁿ(X^sn)。步骤二：测量误差估计和定位置信区间估计

通过步骤一得到定位信息X^pn(图3中第302步)就可以得到定位点的地形面插值高度序列和地形的测量高度差序列Δh^pn(图3中第318步)：

Δh^pn＝Zⁿ-h(X^pn)

以及高度偏差的标准差：

s^pn＝std(Δh^pn)(图3中第318步)

在获得误差估计和定位位置之后就可以对地形匹配定位的结果进行有效性检验(图3中第309步)。若假设地形测量误差服从高斯分布，那么定位点位置的高度偏差Δh^pn服从统计分布：

得到χ²(N-1)的置信度为α(α通常取值在(0.95,1))的分位点

由每一个搜索点的似然函数和χ²(N-1)的置信度为α的分位点值可以得到最小容许的似然函数值L_-(x^pn)：

式中：

L_-(x^pn)表示定点的似然函数值在置信度为α时能到达的下界，这个下界就是一个截平面。每一个定位点的似然函数L(x^pn)与这个截平面的交线构成了一个等值线，等值线包围的区域就是定位的置信区间C^pn(图3中第309步)。

步骤三：定位误差估计

步骤二我们得到了定位点的测量误差估计s^pn(图3中第318步)，下面我们就可以对地形匹配的定位精度进行估计(图3中第306,307步)，根据估计能达到的下界来近似的获得定位的精度，下面就对定位点的定位误差Pⁿ进行估计，首先计算定位点地形的Fisher信息量Iⁿ(图3中第306步)，由于地形的信息量沿x和y的正向和负方向不同，分别用和表示正向和负向。

采用双线性插值得到测量节点在先验地形图中的插值(图3中第303步)。

其中：(x₁,y₁)(图4中的401)(x₂,y₂)(图4中的402)(x₃,y₃)(图4中的403)(x₄,y₄)(图4中的404)分别表示插值点所在的地形网格内，左下角为1号点顺时针排列的四个地形节点，待插值点(图4中的400)。

由于定位误差等于信息矩阵的逆进一步简化：

式中：

由此得到了定位点在x₊和y₊方向的定位误差

同理可以计算

由此得到了定位点在x_-和y_-方向的定位误差

以x_-和x₊的大值作为x的定位误差，以y_-和y₊的大值作为y的定位误差，得到最终的定位误差为：

(图3中第307,308步)

完成当前工作之后判断当前点数是否等于N(图3中第309步)，若是则进入步骤四若不是则进入步骤一。

步骤四：多定位点初航迹拟合

步骤三我们得到了地形匹配定位的定位误差估计，这一步对推算导航的航线进行初步的拟合，假设推算导航航线具有固有偏移Δx(图1中103)和Δy(图1中104)，参考导航所提供的航线在每个地形匹配导航点的导航误差认为是偏差相同，这一假设是基于短时间内参考导航误差小的原因。步骤一计算得到的地形匹配定位的位置X^pn＝(x^pn,y^pn)和参考导航位置(xⁿ,yⁿ)，利用加权最小二乘估计可以得到(Δx,Δy)的估计结果(图1中103^,104和图3中312步)。

式中，λ^xn，λ^yn表示对第i定位结果在x和y方向的加权，且满足

下面就对加权系数λ^xn，λ^yn进行求取(图3中第311步)。

(图3中第311步)

从而得到当前时刻的定位位置：

(图1中103，104和图3中第312步)。

步骤五：航迹关联与误定位点剔除

由步骤四得到了拟合航线下的推算导航航线上的地形匹配导航点的位置(图2a中202)，下面就对每一个地形匹配定位位置进行有效性检验。根据步骤二得到的地位点的置信区间估计C^pn(图3中第313步)，虚线椭圆(图2a中200)表示每一个地形匹配定位点的置信区间，构建的有效检验条件(图3中第314步)如下：

式中：Xⁿ表示经过地形匹配定位修正后地形匹配定位点在推算导航航线上的位置。

步骤六：航迹二次拟合

如果在步骤五中有无效的定位点，那么就将该定位点(图2a中203)剔除。如果剔除了定位点(图3中第315步)那么就要重新计算各个定位点的权重(图3中第316步)，对航迹进行再一次的拟合计算(图3中第317步)，拟合计算与步骤四相同。

(图3中第316步)

且为有效(图3中第317步)

最后就得到了最终的航线偏移量(图2b中205,206)。

最后：输出结果(图3中第318步)。

Claims (8)

1.一种基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是包括如下步骤：

(1)地形匹配定位的搜索区间估计；

(2)未知潮差和测量误差情况下的地形匹配定位；

(3)测量误差估计和定位置信区间估计；

(4)定位误差估计；

(5)多点地形匹配定位初航迹偏差估计；

(6)航迹关联与误定位点剔除；

(7)航迹二次拟合。

2.根据权利要求1所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(1)具体包括：

对于第n个地形匹配点，根据推算导航的输出位置Xⁿ＝(xⁿ,yⁿ)推算导航的估计误差Wⁿ＝(W^xn,W^yn)，推算导航给出的AUV的航行里程估计AUV推算导航系统给出的当前匹配点的推算导航偏差区间，即当前点的地形匹配定位的搜索区间

$\left\{\begin{array}{c}{x}^n-({\mathrm{\&lambda;}}_x{l}_x^n+3\sqrt{W^{xn}})<X_x^{sn}<X^n+({\mathrm{\&lambda;}}_x{l}_x^n+3\sqrt{W^{xn}})\\ y^n-({\mathrm{\&lambda;}}_y{l}_y^n+3\sqrt{W^{yn}})<X_y^{sn}<y^n+({\mathrm{\&lambda;}}_y{l}_y^n+3\sqrt{W^{yn}})\end{array}\right.$

其中：xⁿ和yⁿ表示推算导航的位置，令Xⁿ＝(xⁿ,yⁿ)；

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

和表示载体在x方向和y方向的运行里程，由推算导航给出；

P^xn和P^yn表示推算导航的误差，由推算导航给出。

3.根据权利要求2所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(2)具体包括：

对于搜索区间内的每一个搜索点ij估计该点的先验地形与测量地形的高度偏差并估计高度偏差序列的均值和标准差设为潮差估计、为两次叠加的测量误差估计:

$\left\{\begin{array}{c}{\mathrm{\&Delta;h}}_{ij}^{sn}=Z^n-h\left(X_{ij}^{sn}\right)\\ s_{ij}^{sn}=std\left({\mathrm{\&Delta;h}}_{ij}^{sn}\right)\\ a_{ij}^{sn}=mean\left({\mathrm{\&Delta;h}}_{ij}^{sn}\right)\end{array}\right.$

带入似然函数：

$L^n\left(X^{sn}\right)=\frac{1}{C}\exp \&lsqb;-\frac{1}{2mn{\left(s_{ij}^{sn}\right)}^2}{\&lsqb;Z^n-(a_{ij}^{sn}+h(X_{ij}^{sn}\left)\right)\&rsqb;}^T\&lsqb;Z^n-(a_{ij}^{sn}+h(X_{ij}^{sn}\left)\right)\&rsqb;\&rsqb;$

获得搜索点的似然函数值，则似然函数取值最大的位置就是地形匹配定位的位置：

$X^{pn}=\underset{X^{pn}\&Element;X^{sn}}{\mathrm{argmax}}\left(L\right(X^{sn}\left)\right).$

4.根据权利要求3所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(3)具体包括：

根据步骤(2)得到的定位信息X^pn，得到定位点的地形面插值高度序列和地形的测量高度差序列Δh^pn：

Δh^pn＝Zⁿ-h(X^pn)

以及高度偏差的标准差：

s^pn＝std(Δh^pn)；

在获得误差估计和定位位置之后，对地形匹配定位的结果进行有效性检验，设地形测量误差服从高斯分布，则定位点位置的高度偏差Δh^pn服从统计分布：

$\frac{(N-1)S^2\left({\mathrm{\&Delta;h}}^{pn}\right)}{{\left(s^{pn}\right)}^2}~{\mathrm{\&chi;}}^2(N-1)$

得到χ²(N-1)的置信度为α的分位点

$P\{{\mathrm{\&chi;}}^2(N-1)>{\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(N-1)\}=\mathrm{\&alpha;}$

由每一个搜索点的似然函数和χ²(N-1)的置信度为α的分位点值，得到最小容许的似然函数值L_-(x^pn)：

$L_{-}\left(x^{pn}\right)=\frac{1}{c}\exp \&lsqb;-\frac{1}{2{(N-1)}^2}{\mathrm{\&chi;}}_{\mathrm{\&alpha;}}^2(N-1)\&rsqb;$

式中：

L_-(x^pn)表示定点的似然函数值在置信度为α时能到达的下界，这个下界就是一个截平面，每一个定位点的似然函数L(x^pn)与这个截平面的交线构成了一个等值线，等值线包围的区域就是定位的置信区间C^pn。

5.根据权利要求4所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(4)具体包括：

地形匹配定位的误差估计，首先计算定位点测量地形的Fisher信息量Iⁿ，由于地形的信息量沿x和y的正向和负方向不同，分别用和表示正向和负向，

$I_{+}^n=(-\frac{1}{2{\left(s^{pn}\right)}^2})\left[\begin{array}{cc}\underset{k=1}{\overset{m}{\&Sigma;}}\underset{l=1}{\overset{n}{\&Sigma;}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl}+d,y_{kl}\left)\right)}^2}{d^2}& \underset{k=1}{\overset{m}{\&Sigma;}}\underset{l=1}{\overset{n}{\&Sigma;}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl}+d,y_{kl}+d\left)\right)}^2}{2d^2}\\ \underset{k=1}{\overset{m}{\&Sigma;}}\underset{k=1}{\overset{n}{\&Sigma;}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl}+d,y_{kl}+d\left)\right)}^2}{2d^2}& \underset{k=1}{\overset{m}{\&Sigma;}}\underset{k=1}{\overset{n}{\&Sigma;}}\frac{{(z_{kl}-{\hat{h}}_{kl}(x_{kl},y_{kl}+d\left)\right)}^2}{d^2}\end{array}\right]$

采用双线性插值得到测量节点在先验地形图中的插值

$\hat{h}(x_{kl},y_{kl})=\left[\begin{array}{cc}{x}_2-x_{kl}& x_{kl}-x_1\end{array}\right]\left[\begin{array}{cc}\frac{z_{11}}{(x_2-x_1)(y_2-y_1)}& \frac{z_{12}}{(x_2-x_1)(y_2-y_1)}\\ \frac{z_{21}}{(x_2-x_1)(y_2-y_1)}& \frac{z_{22}}{(x_2-x_1)(y_2-y_1)}\end{array}\right]\left[\begin{array}{c}{y}_2-y_{kl}\\ y_{kl}-y_1\end{array}\right]$

其中：(x₁,y₁)(x₂,y₂)(x₃,y₃)(x₄,y₄)分别表示插值点所在的地形网格内，左下角为1号点顺时针排列的四个地形节点；

由于定位误差等于信息矩阵的逆进一步简化：

$\left\{\begin{array}{c}{P}_{+}^n=\frac{{\left(s^{pn}\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_{+}^{xn}=a_{22}\frac{{\left(s^{pn}\right)}^2}{a_{11}{a}_{22}-a_{12}{a}_{21}}\\ P_{+}^{yn}=a_{11}\frac{{\left(s^{pn}\right)}^2}{a_{11}{a}_{22}-a_{12}{a}_{21}}\end{array}\right.$

式中：

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

由此得到了定位点在x₊和y₊方向的定位误差

同理计算

$\left\{\begin{array}{c}{P}_{-}^n=\frac{{\left(s^{pn}\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_{-}^{xn}=b_{22}\frac{{\left(s^{pn}\right)}^2}{b_{11}{b}_{22}-b_{12}{b}_{21}}\\ P_{-}^{yn}=b_{11}\frac{{\left(s^{pn}\right)}^2}{b_{11}{b}_{22}-b_{12}{b}_{21}}\end{array}\right.$

$\left\{\begin{array}{c}{b}_{11}=\underset{k=1}{\overset{m}{\&Sigma;}}\underset{l=1}{\overset{n}{\&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}{\&Sigma;}}\underset{l=1}{\overset{n}{\&Sigma;}}\frac{{(z_k-{\hat{h}}_{kl}(x_{kl}-d,y_{kl}-d\left)\right)}^2}{2d^2}\\ b_{22}=\underset{k=1}{\overset{m}{\&Sigma;}}\underset{k=1}{\overset{n}{\&Sigma;}}\frac{{(z_k-{\hat{h}}_{kl}(x_{kl},y_{kl}-d\left)\right)}^2}{d^2}\end{array}\right.$

由此得到了定位点在x_-和y_-方向的定位误差

以x_-和x₊的大值作为x的定位误差，以y_-和y₊的大值作为y的定位误差，得到最终的定位误差为：

$\left\{\begin{array}{c}{P}^{xn}=\max (P_{+}^{xn},P_{-}^{xn})\\ P^{yn}=\max (P_{+}^{yn},P_{-}^{yn})\end{array}\right..$

6.根据权利要求5所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(5)具体包括：

根据地形匹配定位的位置X^pn＝(x^pn,y^pn)和参考导航位置(xⁿ,yⁿ)，利用加权最小二乘估计得到(Δx,Δy)的估计结果

$\left\{\begin{array}{c}\mathrm{\&Delta;}\hat{x}=\underset{\mathrm{\&Delta;}x}{\arg \min }\underset{i=1}{\overset{N}{\&Sigma;}}{\mathrm{\&lambda;}}^{xn}{(x^n-(x^{pn}+\mathrm{\&Delta;}x\left)\right)}^2\\ \mathrm{\&Delta;}\hat{y}=\underset{\mathrm{\&Delta;}y}{\arg \min }\underset{i=1}{\overset{N}{\&Sigma;}}{\mathrm{\&lambda;}}^{yn}{(y^n-(y^{pn}+\mathrm{\&Delta;}y\left)\right)}^2\end{array}\right.$

式中，λ^xn，λ^yn表示对第i定位结果在x和y方向的加权，且满足加权系数λ^xn，λ^yn按如下方法进行求取，

$\left\{\begin{array}{c}{\mathrm{\&lambda;}}^{xn}=\frac{P^{xn}}{\underset{i=1}{\overset{N}{\&Sigma;}}{P}^{ix}}\\ {\mathrm{\&lambda;}}^{yn}=\frac{P^{yn}}{\underset{i=1}{\overset{N}{\&Sigma;}}{P}^{iy}}\end{array}\right.$

从而得到当前时刻的定位位置：

$\left\{\begin{array}{c}{\hat{x}}^{pn}=x^{pn}+\mathrm{\&Delta;}\hat{x}\\ {\hat{y}}^{pn}=y^{pn}+\mathrm{\&Delta;}\hat{y}\end{array}\right..$

7.根据权利要求6所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(6)具体包括：

根据步骤(3)得到的地形匹配定位的有效区间构建的地形匹配定位的有效检验条件如下：

式中：Xⁿ表示经过地形匹配定位修正后地形匹配定位点在推算导航航线上的位置。

8.根据权利要求7所述的基于多点地形匹配定位的AUV航迹偏差估计方法，其特征是所述步骤(7)具体包括：

如果在步骤(6)中有无效的定位点，就将该定位点剔除；如果剔除了定位点，则重新计算各个定位点的权重，对航迹进行再一次的拟合计算，

$\left\{\begin{array}{c}{\mathrm{\&lambda;}}_{avi}^{xn}=\frac{P_{avi}^{xn}}{\underset{i=1}{\overset{K}{\&Sigma;}}{P}_{avi}^{ix}}\\ {\mathrm{\&lambda;}}_{avi}^{yn}=\frac{P_{avi}^{yn}}{\underset{i=1}{\overset{K}{\&Sigma;}}{P}_{avi}^{iy}}\end{array}\right.$

且为有效

最后就得到最终的航线偏移量