-
由于水下通讯的限制,AUV(autonomous underwater vehicle)在水下长时间潜伏航行后需要上浮接受卫星信号进行位置修正,对于大潜深和长航程任务AUV来说增加了时间和能源的消耗[1-3]。一些声学定位的修正方法也被用于AUV位置修正,但由于水声定位的距离限制和成本等因素的考虑也不适合长航程的AUV使用[4]。地形匹配导航主要是利用地形特征为AUV提供位置参考,从而获得AUV相对于先验地形图的绝对位置。文献[2]研究了利用多波束进行地形匹配定位,得到似然函数具有渐进高斯分布的特点;文献[3]提出了一种基于残差检验方法的地形匹配定位点的置信区间估计方法;文献[4]研究了低成本AUV的水下地形匹配滤波的相关问题;文献[5]提出在初始导航误差较大时利用地形匹配定位的置信区间进行导航滤波初始化,以缩小初始时刻的位置不确定性,从而加快滤波的收敛速度;文献[6]提出了一种基于航迹线追踪的定位算法;文献[7]提出了基于M估计的匹配算法以降低噪声影响。更多关于地形匹配定位与导航的相关研究进展在文献[8-10]有详细的描述。水下地形匹配导航技术主要是借鉴陆地地形匹配技术而发展起来的,但由于水下环境与陆地环境的区别也导致水下地形匹配与陆地地形匹配导航有很大的差别。水下很难得到大范围的高精度地形图;测深设备的数据更新速度低;海洋环境噪声干扰以及测量误差相互耦合,这些因素的影响加上地形的强非线性和测量误差非高斯性使得地形匹配定位发生跳变。想要分析地形匹配定位点的置信区间,必须建立TAP(terrain aided position)误差和局部地形特征、测量误差之间的数学关系式。关于TAP的置信区间估计方法的研究很少,文献[3]假设地形测点的统计服从高斯分布,并根据这一假设建立了定位点的残差统计量,这个统计量服从χ2分布:
$$ S(X)=\sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n} \frac{\left(h_{i j}\left(X^{p}\right)-z_{i j}\right)^{2}}{\left(\sigma^{p}\right)^{2}} \sim \chi^{2}(m n-1) $$ (1) 式中,hij(Xp)表示地形测量序列在先验地图中的插值高程序列;zij表示测量序列;i、j表示序列索引号;χ2(mn-1)表示自由度为(mn-1)的卡方分布。
因此,地形匹配定位残差的1-α置信区间的残差方差和函数的上确界为:
$$ S^{\prime}(X)_{1-a}=\frac{S\left(X^{p}\right) \chi_{1-\alpha}^{2}(m n-1)}{m n-1} $$ (2) 式中,S(Xp)表示测量误差的估计值;m、n分别表示测量地形的节点行数和列数;χ1-α2(mn-1)表示自由度为(mn-1)的卡方分布在(1-α)置信度的取值。但该方法只考虑了测量地形的误差而没有考虑测量数据与定位点之间的非线性关系,所以其估计结果并不高。
-
如图 1所示,地形匹配定位的观测数据为多波束测量脚点的三维坐标序列,任意第i Ping下的测点序列为(xija,yija,zija), 其中j表示测量脚点编号。载体坐标系下多波束脚点与AUV重心(xa,ya)的相对位置不变。如果通过搜索匹配的方式得到(xija,yija,zija)在先验地形图中的绝对位置,则AUV相对于先验地形图的绝对位置可求。
为了描述测量误差与地形匹配定位误差之间的关系, 建立了新的地形匹配观测模型如图 2所示。
该模型将面观测模型的地形空间划分成独立的区间,各个观测点也相互独立,每一个观测点被分割在各自的区间内。两个模型的区别在于对观测值和测量误差的定义,第一个模型将每一个观测数据视为独立的AUV位置相关参数的观测,而第二个模型则将所有的观测数据视为AUV位置相关参数的观测序列。地形测量误差的形成非常复杂,主要由设备安装误差、声速误差、位姿传感器量测误差等相互耦合形成,而且传感器的输出误差还与作业环境有关。因此,几乎不可能得到精确且可靠的地形测量误差表达式,一个较实际的解决方案就是利用测量数据以及匹配残差进行统计估计。为了便于进一步的分析,根据建立的模型和文献[2]的假设与结论,做出如下的前提假设:①多波束测量脚点的高度误差服从独立同分布N(0, (σp)2);②多波束测量数据足够大,可以保证其误差估计结果的有效性。
根据以上的假设,可以通过如下步骤得到地形匹配定位点Xp的估计:
1) 计算搜索区间内索引号为(k, l)的搜索点Xkl的初匹配残差序列:
$$ \Delta h_{i j}\left(X_{k l}\right)=h_{i j} X_{k l}-z_{i j} $$ (3) 式中,(i, j)表示测量地形的节点序列索引。
2) 估计搜索点Xkl的潮差tkl和测量误差方差σkl2:
$$ \left\{\begin{aligned} t_{k l} &=\frac{1}{m n} \sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n} \Delta h_{i j}\left(X_{k l}\right) \\ \sigma_{k l}^{2} &=\frac{1}{m n-1} \sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n}\left(\Delta h_{i j}\left(X_{k l}\right)-t_{k l}\right)^{2} \end{aligned}\right. $$ (4) 3) 获得匹配定位点Xp即σkl2取得最小时的搜索点,以及定位点的潮差tp和测量误差σp的估计值。设此搜索点对应的索引号为(kp, lp),则:
$$ X^{p}=\operatorname{argmin}\left(\sigma_{k l}^{2}\right) $$ (5) $$ \left\{\begin{array}{l}{t_{p}=t_{k_{p} l_{p}}} \\ {\sigma_{p}=\sigma_{k_{p} l_{p}}}\end{array}\right. $$ (6) -
首先建立地形匹配定位点的跳变模型。如图 3(a)所示,把地形匹配定位点(红色点)视为一个跳跃点,似然函数在定位点位置取得极大值,似然函数的极大值位置表明该点是测量地形与先验地形图最相似的点,理论上地形匹配定位点Xp可以向搜索区间Xs内任意一点跳变;如图 3(b)所示,随着似然函数的取值下降,定位点向该点跳变的概率越低,定义一个非常小的置信度α,直到似然函数下降到某一个取值L1-α时,定位点以非常小的概率α向该点跳变,这个点就是地形匹配定位的点的1-α置信区间的边界点。
地形匹配定位点Xp的似然函数如式(7)所示。该式表示在定位点位置的先验地形与测量地形的相似程度量化。
$$ \left\{\begin{array}{l}{L^{p}=\frac{1}{\sqrt{2 \pi \sigma_{p}}} \exp (-A)} \\ {A=\frac{1}{\sqrt{2 m n \sigma_{p}^{2}}}\left(\sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n}\left(h_{i j}\left(X^{p}\right)-z_{i j}-t_{p}\right)^{2}\right)}\end{array}\right. $$ (7) 式中,σp表示地形测量误差;Xp表示地形匹配定位点位置;tp表示潮差。考察式(7)的对数形式:
$$ \begin{array}{c}{l\left(X^{p}\right)=\ln \left(\frac{1}{\sqrt{2 \pi \sigma_{p}}}\right)-} \\ {\frac{1}{\sqrt{2 m n \sigma_{p}^{2}}}\left(\sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n}\left(h_{i j}\left(X^{p}\right)-z_{i j}-t_{p}\right)^{2}\right)}\end{array} $$ (8) 假设在地形匹配定位点Xp的置信区间内, l(Xp)可以近似为二次曲面,则置信区间内的任意一点X的似然函数对数取值l(X)可以表示为:
$$ \begin{array}{c}{l(X)=l\left(X^{p}\right)+} \\ {\frac{1}{2} \frac{\partial^{2} l\left(X^{p}\right)}{\partial e^{2}}\left|\Delta x_{e}\right|^{2}+0\left(\left|\Delta x_{e}\right|^{n}\right)}\end{array} $$ (9) 式中,Δxe表示地形匹配定位偏差,且Δxe=Xp-X; 0(|Δxe|n)表示Δxe的高阶无穷小;e表示以Xp为原点指向X的单位方向向量。
定义式(10)为定位点残差的平方和等式:
$$ \left\{\begin{array}{l}{S\left(X^{p}\right)=\sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n} S_{i j}\left(X^{p}\right)} \\ {S_{i j}\left(X^{p}\right)=\left(h_{i j}\left(X^{p}\right)-z_{i j}-t_{p}\right)^{2}}\end{array}\right. $$ (10) 同样的,假设在Xp的置信区间内任意一点X的平方和函数S(X)可以表示为关于Xp的二次曲面:
$$ \begin{array}{l}{S(X)=S\left(X^{p}\right)+\frac{1}{\sigma_{p}^{2}} \frac{\partial S\left(X^{\rho}\right)}{\partial e} \Delta x_{e}+} \\ {\frac{1}{2 \sigma_{p}^{2}} \frac{\partial^{2} S\left(X^{p}\right)}{\partial e^{2}}\left|\Delta x_{e}\right|^{2}+0\left(\left|\Delta x_{e}\right|^{n}\right)}\end{array} $$ (11) 式中,σp2表示定位点的地形测量误差;e表示以TAP定位点为原点的任意单位方向向量; 其他含义同前。由于似然函数在定位点Xp处的一阶倒数等于0,所以式(11)的第二项等于0,式(11)可以简写成:
$$ S(X) \approx S\left(X^{p}\right)+\frac{1}{2}\left(\frac{\partial^{2} S\left(X^{p}\right)}{\partial e^{2}}\right)\left|\Delta x_{e}\right|^{2} $$ (12) 很显然, 如果对式(12)括号内的表达式求期望值,并考虑式(10)的结果,这个期望值就是定位点的信息量:
$$ I=\frac{1}{\sigma_{p}^{2}} \boldsymbol{E}\left(\sum\limits_{{\rm i}=1}^{\mathrm{m}} \sum\limits_{\mathrm{j}=1}^{\mathrm{n}} \frac{\partial^{2} \mathrm{S}_{\mathrm{ij}}\left(\mathrm{X^{p}}\right)}{\partial \mathrm{e}^{2}}\right) $$ (13) 根据前面的假设,S(Xp)在定位点Xp近似二次型。根据统计学中的参数估计理论[11],估计值Xp的方差估计V(|Δxe|)可以通过信息矩阵求得:
$$ V\left(\left|\Delta x_{e}\right|\right)=I^{-1} $$ (14) 根据前面的假设,每一个测点的误差服从同一分布,根据文献[12],对于给定的观测误差σp,l(X)在Xp的置信区间内近似为X二次型,则一个近似的1-α置信区间为:
$$ \sum\limits_{i=1}^{m} \sum\limits_{j=1}^{n}\left(\frac{\partial^{2} S_{i j}\left(X^{\rho}\right)}{\partial e^{2}}\right)\left|\Delta x_{e}\right|^{2}<2 \sigma_{p}^{2} \chi_{1-\alpha}^{2}(m n-1) $$ (15) 考虑关系式(10),并将式(15)代入似然函数(12),就可以得到似然函数在置信度为1-α下的等值线计算公式:
$$ S(X)_{1-\alpha} \approx S\left(X^{p}\right)+\sigma_{p}^{2} \chi_{1-\alpha}^{2}(m n-1) $$ (16) 根据式(4)、式(6)和式(10)可以得到关系式:
$$ \sigma_{p}^{2}=\frac{S\left(X^{p}\right)}{m n-1} $$ (17) 将式(17)代入到式(16)得:
$$ S(X)_{1-\alpha} \approx S\left(X^{p}\right)\left[1+\frac{\chi_{1-\alpha}^{2}(m n-1)}{m n-1}\right] $$ (18) 式中,χ1-α2(mn-1)的定义与式(2)相同。式(18)表示定位点的平方和函数在1-α置信区间上确界。因平方和函数越大,则表示似然函数值越小,所以取其1-α置信区间上确界是合理的。将式(17)代入到定位点Xp的似然函数表达式(7)中,得到定位点Xp的似然函数的1-α置信度下确界:
$$ L_{1-\alpha}^{p}=\frac{1}{\sqrt{2 \pi \sigma_{p}}} \exp \left(\frac{S(X)_{1-a}}{\sigma_{p}^{2}}\right) $$ (19) 当λ=1时,可以得到不等式:
$$ S(X)_{1-a}>S^{\prime}(X)_{1-a} $$ (20) 该不等式表明本文得到的TAP定位点位置的残差平方和上确界要高于原有的方法。这也表明,文中给出的TAP置信区间估计方法获得的置信区间将比原有的估计结果大。
-
利用船载采集数据对两种估计方法进行对比验证,实验数据采集位置在山东省胶州湾中沙礁海域,仿真中每一个MTM地形图中包含10 Ping的测量数据。图 4表示实时测量地图的地形粗糙度,从中可以看到6~16号地形匹配定位点的测量地形的粗糙度较低,而1~5、17、18号定位点的MTM粗糙度较高。
图 5(a)表示5~9号地形匹配定位点的匹配似然函数、TAP定位点、GPS定位点的局部放大图,图 5(b)表示采用文献[3]方法得到的5~9号地形匹配定位点的置信区间,可以很明显地看到,本文方法得到的置信区间较文献[3]方法得到的要大。
图 6表示地形匹配定位的偏差曲线,图 6(a)和6(b)分别表示实验航线上地形匹配定位点的定位偏差以及置信区间内是否包含真实点(GPS点),黑色点表示当前位置的定位结果在置信区间内,红色点表示定位结果不在置信区间内。可以看到现有的估计方法没有考虑地形非线性的影响,得到的地形匹配定位点的置信区间过于保守,在考虑地形的非线性影响后得到的地形匹配定位点的置信区间估计结果范围扩大。
从图 6可以看到,仍然有部分定位点的置信区间估计出现较大的偏差,如图 7所示的17、19和21号匹配点。17和21号定位点的置信区间偏小,而19号置信区间偏大。图 8为每一点的测量误差标准差以及潮差的估计结果,可以看到17号定位点的测量误差估计值为0.084 3,21号为0.121 2,此两点的误差估计明显小于其他匹配点的估计值。如图 9(a)所示为21号定位点的残差信号和直方图统计结果,虽然直方图统计结果和正态分布拟合结果表明残差的统计值接近正态分布,但是从其残差序列可知残差信号具有相关性(图 9(b)中黑色矩形),所以在后续的研究中需要考虑残差序列的相关性。而19号匹配点的测量误差标准差估计为0.268 5,明显高于其他点的估计结果,同样绘出其直方图统计结果(见图 9(c)),可以看到,19号匹配点的残差明显偏离了正态分布的假设,导致其标准差的估计大于真实值以及置信区间估计偏大。
图 7 测量波束为10 Ping时17~21号匹配点的似然函数、GPS定位点、TAP定位点和置信区间
Figure 7. Likelihood Function, GPS Positioning Point, TAP Positioning Point and Confidence Interval of Matching Point 17-21 at 10 Ping
图 8 测量波束为10 Ping时地形匹配定位点的测量误差和潮差估计
Figure 8. Measurement Error and Tidal Range Estimation of TAP Points at 10 Ping
图 9 测量波束为10 Ping时匹配点的残差序列和直方图统计结果
Figure 9. Residual Sequence and Histogram Statistics at TAP Position at 10 Ping
根据文献[2]的描述,随着测量波束的增加,地形匹配定位的似然函数将渐进高斯分布,此时的残差也会逐渐逼近高斯分布。也就是说,随着测量波束的增加,非高斯分布的不利影响将被削弱,将测量波束增加到20 Ping得到如图 10所示的测量误差和潮差估计。
图 10 测量波束为20 Ping时地形匹配定位点的测量误差和潮差估计
Figure 10. Measurement Error and Tidal Range Estimation of TAP Points at 20 Ping
测量波束数分别为10 Ping、20 Ping、30 Ping时的潮差和测量误差标准差统计结果分别列在表 1和表 2中。测量波束为10 Ping时获得的测量误差标准差的估计均值为0.181 5,估计值的标准差为0.089 8;测量波束为20 Ping时获得的测量误差标准差的估计均值为0.220 3,估计值的标准差为0.090 6,测量误差的均值提高了21.38%,估计值的标准差增加了0.89%;而测量波束为30 Ping时获得的测量误差标准差的估计均值为0.233 8,估计值的标准差为0.087 6,较测量波束为10 Ping时测量误差的均值提高了28.8%,估计值的标准差降低了2.45%。说明波束较小时,测量误差的估计结果偏小,波束增加后,测量误差的估计结果稳定增加。而此时的潮差估计值的均值分别为2.553 8、2.539 0和2.528 0,较测量波束为20 Ping时潮差的估计值分别降低了0.58%,30 Ping时降低了1.01%,潮差估计的标准差为0.163 7、0.113 5和0.121 6,较测量波束为10 Ping的情况20 Ping时潮差的估计值分别降低了30.6%,30 Ping时降低了25.72%,说明测量波束增加后潮差的估计值降低而且估计值更加稳定。综上所述,波束增加后, 测量误差的方差估计值增加, 而潮差的估计结果降低,总体来讲, 估计值的方差在降低,估计结果趋于稳定。
表 1 测量波束增加时测量误差对比
Table 1. Comparison of Measurement Errors when Measuring Beam Increase
测量波束/Ping 估计序列均值 估计序列标准差 与10 Ping结果比较/% 均值 标准差 10 0.181 5 0.089 8 - - 20 0.220 3 0.090 6 ↑21.38 ↑0.89 30 0.233 8 0.087 6 ↑28.80 ↓2.45 表 2 测量波束增加时潮差估计对比
Table 2. Estimation of Tidal when Measured Beam Increase
测量波束/Ping 估计序列均值 估计序列标准差 与10 Ping结果比较/% 均值 标准差 10 2.553 8 0.163 7 - - 20 2.539 0 0.113 5 ↓0.58 ↓30.60 30 2.528 0 0.121 6 ↓1.01 ↓25.72 图 11所示为21号匹配点的残差统计直方图,与测量波束为10 Ping时的情况(图 9(a))进行比较,测量波束为30 Ping时残差序列的直方图统计已经接近高斯分布,较10 Ping时有了很大的改善。图 12中绘出了30 Ping测量波束下的匹配似然函数云图、GPS定位点、TAP定位点及置信区间,可以看到测量波束增加到30 Ping后,置信区间的估计得到很大的改善。
图 11 测量波束为30 Ping时21号匹配点的残差直方图统计
Figure 11. Residual Histogram Statistics of TAP Point 21 at 30 Ping
图 12 测量波束为30 Ping时17~21号匹配点的似然函数、GPS定位点、TAP定位点和置信区间
Figure 12. Likelihood Function, GPS Positioning Point, TAP Positioning Point and Confidence Interval of Matching Points 17-21 at 30 Ping
从实验结果可以看到,原有的地形匹配定位置信区间估计方法仅从地形匹配残差的假设检验入手,通过残差的高斯分布假设以及统计估计方法得到关于残差的χ2分布,进而得到某一置信区间下的残差平方和函数的取值区间,由于计算中未考虑地形的非线性影响,导致估计结果偏小。本文中考虑了地形的非线性影响,将似然函数在置信区间内假设为二次曲面并将其在置信区间内线性化,从而将定位点的置信区间估计问题转化为二次曲面参数的置信区间估计问题,而且比较式(2)和式(17)可知, 文中提出的估计方法得到的置信区间要大于原有的方法。在测量波束较少的情况下,残差的统计结果很难满足高斯分布的假设,这种情况会随着测量波束的增加而得到改善。
-
在对地形匹配的置信区间进行估计时需要同时考虑测量误差和地形曲面的非线性,假设地形匹配定位残差的平方和函数在定位点附近是近似二次型曲面,将似然函数在定位点的置信区间内线性化,在地形误差近似服从高斯分布时,利用本文中推导的估计方法可以得到更加合理、实用的估计结果。在测量波束较少时,由于地形测量的局部畸变误差,在某些位置匹配的残差不再符合高斯分布的假设,这些点的置信区间估计结果仍然会出现异常,但这一现象可以通过增加测量波束得到改善。在今后的研究中,考虑测量误差的非高斯特点并加入修正项也是一个值得研究的问题。此外,在地形匹配导航的初始阶段有较大的累积误差时,置信区间估计方法可以将初始位置约束在较小的范围内,基于置信区间估计的地形匹配导航滤波初始化方法也是值得研究的方向。
-
摘要: 地形匹配定位(terrain aided position,TAP)的似然函数反映了AUV(autonomous underwater vehicle)的位置在空间中的分布概率,由于地形的强非线性、随机性以及测量误差的非高斯分布使得似然函数也表现出非高斯分布的特点。TAP的误差与局部地形特征和地形测量误差密切相关,由于现有的方法未考虑局部地形特征,仅考虑了测量误差的统计置信区间,使得TAP置信区间的估计结果明显偏小。为解决TAP置信区间的估计问题,建立了TAP定位点的跳变模型。设TAP定位点Xp可以向搜索区间内任一点跳变,且向某一点的跳变概率与该点的似然函数值正相关,Xp向某一点跳变的置信度小于α时,认为xα不会向该点跳变,该点设为置信区间的边界点。另外,设地形匹配定位点的置信区间内匹配残差平方和函数为二次曲面,而Xp视为该曲面的待估计参数,则可以通过曲面参数的置信区间估计方法获得1-α置信度下的置信区间。新方法得到的置信区间范围大于现有的估计方法,试验结果表明,测量波束较少时,置信区间估计会出现异常,增加测量波束可以提高潮差和测量误差的估计精度,从而提高置信区间的估计精度,但测量误差非高斯分布条件下的补偿方法仍然需要进一步研究。Abstract: The TAP(terrain aided position) likelihood function reflects the probability of the position of AUV (autonomous underwater vehicle) in space. Due to the strong nonlinearity and randomness of the terrain and the non-Gauss distribution of the measurement error, the likelihood function also shows the characteristics of non-Gauss. The error of TAP is closely related to the local topographic and measurement error. Because the existing method does not consider the local topographic features, the statistical confidence interval of the measurement error is only established, so the estimation results of the TAP are obviously smaller. In this paper, a jump model of the TAP position Xp is established. It can jump to any point in the searching interval, and the probability jumping to any point is positively correlated with the likelihood function of the point. When the confidence of the jump to a certain point is less than α, Xp will not jump to this point and this point is called the boundary point of the confidence interval. Assumed the sum of squares of the matched residuals in the confidence interval of the TAP is quadric surface, Xp is regarded as the parameter of the quadric surface, and the confidence interval of TAP with confidence 1-α can be obtained by the confidence interval estimation method of the surface parameters. The confidence interval obtained by new estimate method is larger than the existing method. The experimental results show that the confidence interval estimation will be abnormal when the measuring beam is less. The increase of the measurement beam can improve the estimation accuracy of the tidal and measurement errors, thus can promote the estimation accuracy of the confidence interval, but the compensation method under the condition of non-Gauss distribution of measurement error is still needed in further work.
-
表 1 测量波束增加时测量误差对比
Table 1. Comparison of Measurement Errors when Measuring Beam Increase
测量波束/Ping 估计序列均值 估计序列标准差 与10 Ping结果比较/% 均值 标准差 10 0.181 5 0.089 8 - - 20 0.220 3 0.090 6 ↑21.38 ↑0.89 30 0.233 8 0.087 6 ↑28.80 ↓2.45 表 2 测量波束增加时潮差估计对比
Table 2. Estimation of Tidal when Measured Beam Increase
测量波束/Ping 估计序列均值 估计序列标准差 与10 Ping结果比较/% 均值 标准差 10 2.553 8 0.163 7 - - 20 2.539 0 0.113 5 ↓0.58 ↓30.60 30 2.528 0 0.121 6 ↓1.01 ↓25.72 -
[1] 陈小龙. AUV水下地形匹配辅助导航技术研究[D].哈尔滨工程大学, 2013 http://cdmd.cnki.com.cn/Article/CDMD-10217-1014134060.htm Chen Xiaolong. AUV Underwater Terrain Mat-ching Aided Navigation Technology[D]. Harbin: Harbin Engineering University, 2013 http://cdmd.cnki.com.cn/Article/CDMD-10217-1014134060.htm [2] Nygren I. Terrain Navigation for Underwater Vehicles[D]. Sweden: Signal Processing Stockholm, 2005 https://www.researchgate.net/publication/265632151_Terrain_Navigation_for_Underwater_Vehicles [3] Xie Yingrong. Terrain Aided Navigation[D]. Sweden: Stockholm, 2005 [4] Deborah K M. Terrain Relative Navigation for Sensor-Limited Systems with Application to Underwater Vehicle[D]. Palo Alto: Stanford University, 2011 http://web.stanford.edu/group/arl/projects/terrain-relative-navigation [5] 王汝鹏, 李晔, 马腾, 等.AUV地形匹配导航快速收敛滤波[J].华中科技大学学报(自然科学版), 2018, 46(7):94-97 http://d.old.wanfangdata.com.cn/Periodical/hzlgdxxb201807018 Wang Rupeng, Li Ye, Ma Teng, etal. AUV Topographic Matching Navigation Fast Convergence Filter[J]. Journal of Huazhong University of Scien-ce and Technology (Natural Science Edition), 2018, 46(7):94-97 http://d.old.wanfangdata.com.cn/Periodical/hzlgdxxb201807018 [6] 刘现鹏, 张立华, 王涛, 等.基于航迹线追踪的TERPM定位算法[J].武汉大学学报·信息科学版, 2018, 43(2):219-226 http://ch.whu.edu.cn/CN/abstract/abstract5972.shtml Liu Xianpeng, Zhang Lihua, Wang Tao, et al. TERPM Localization Algorithm Based on Track Tracking[J]. Geomatics and Information Science of Wuhan University, 2018, 43(2):219-226 http://ch.whu.edu.cn/CN/abstract/abstract5972.shtml [7] 张凯, 赵建虎, 张红梅.一种基于M估计的水下地形抗差匹配算法[J].武汉大学学报·信息科学版, 2015, 40(4):558-562 http://ch.whu.edu.cn/CN/abstract/abstract3244.shtml Zhang Kai, Zhao Jianhu, Zhang Hongmei.An Underwater Terrain Robust Matching Algorithm Based on M Estimation[J]. Geomatics and Information Science of Wuhan University, 2015, 40(4):558-562 http://ch.whu.edu.cn/CN/abstract/abstract3244.shtml [8] Paull L, Saeedi S, Seto M, et al. AUV Navigation and Localization:A Review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1):131-149 doi: 10.1109/JOE.2013.2278891 [9] Zeng Z, Lian L, Sammut K, et al. A Survey on Path Planning for Persistent Autonomy of Autonomous Underwater Vehicles[J]. Ocean Engineering, 2015, 110:303-313 doi: 10.1016/j.oceaneng.2015.10.007 [10] Chen L, Wang S, Mcdonald-Maier K, et al. Towards Autonomous Localization and Mapping of AUVs:A Survey[J]. International Journal of Intelligent Unmanned Systems, 2013, 1(2):97-120 doi: 10.1108/20496421311330047 [11] Trees H L V. Detection, Estimation, and Modulation Theory, Part IV, Optimum Array Processing[J]. A Papoulis Probability Random Variables & Stochastic Processes, 2001, 8(10):293-303 http://cn.bing.com/academic/profile?id=ce1ca94f4a41c5ef18f08d8950757273&encoded=0&v=paper_preview&mkt=zh-cn [12] Box G E P, Jenkins G M, Reinsel G C. Time Series Analysis, Forecasting and Control[M]. 4th ed. New Jersey:John Wiley & Sons, 2008 -