由于水下通讯的限制，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]假设地形测点的统计服从高斯分布，并根据这一假设建立了定位点的残差统计量，这个统计量服从χ²分布：

$ 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)

式中，h_ij(X^p)表示地形测量序列在先验地图中的插值高程序列；z_ij表示测量序列；i、j表示序列索引号；χ²(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(X^p)表示测量误差的估计值；m、n分别表示测量地形的节点行数和列数；χ_1-α²(mn-1)表示自由度为(mn-1)的卡方分布在(1-α)置信度的取值。但该方法只考虑了测量地形的误差而没有考虑测量数据与定位点之间的非线性关系，所以其估计结果并不高。

1 TAP观测模型以及观测误差估计

如图 1所示，地形匹配定位的观测数据为多波束测量脚点的三维坐标序列，任意第i Ping下的测点序列为(x_ij^a，y_ij^a，z_ij^a), 其中j表示测量脚点编号。载体坐标系下多波束脚点与AUV重心(x_a，y_a)的相对位置不变。如果通过搜索匹配的方式得到(x_ij^a，y_ij^a，z_ij^a)在先验地形图中的绝对位置，则AUV相对于先验地形图的绝对位置可求。

为了描述测量误差与地形匹配定位误差之间的关系, 建立了新的地形匹配观测模型如图 2所示。

该模型将面观测模型的地形空间划分成独立的区间，各个观测点也相互独立，每一个观测点被分割在各自的区间内。两个模型的区别在于对观测值和测量误差的定义，第一个模型将每一个观测数据视为独立的AUV位置相关参数的观测，而第二个模型则将所有的观测数据视为AUV位置相关参数的观测序列。地形测量误差的形成非常复杂，主要由设备安装误差、声速误差、位姿传感器量测误差等相互耦合形成，而且传感器的输出误差还与作业环境有关。因此，几乎不可能得到精确且可靠的地形测量误差表达式，一个较实际的解决方案就是利用测量数据以及匹配残差进行统计估计。为了便于进一步的分析，根据建立的模型和文献[2]的假设与结论，做出如下的前提假设：①多波束测量脚点的高度误差服从独立同分布N(0, (σ^p)²)；②多波束测量数据足够大，可以保证其误差估计结果的有效性。

根据以上的假设，可以通过如下步骤得到地形匹配定位点X^p的估计：

1) 计算搜索区间内索引号为(k, l)的搜索点X_kl的初匹配残差序列：

$ \Delta h_{i j}\left(X_{k l}\right)=h_{i j} X_{k l}-z_{i j} $

(3)

式中，(i, j)表示测量地形的节点序列索引。

2) 估计搜索点X_kl的潮差t_kl和测量误差方差σ_kl²:

$ \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) 获得匹配定位点X^p即σ_kl²取得最小时的搜索点，以及定位点的潮差t_p和测量误差σ_p的估计值。设此搜索点对应的索引号为(k_p, l_p)，则：

$ 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)

2 地形匹配定位的置信区间估计

首先建立地形匹配定位点的跳变模型。如图 3(a)所示，把地形匹配定位点(红色点)视为一个跳跃点，似然函数在定位点位置取得极大值，似然函数的极大值位置表明该点是测量地形与先验地形图最相似的点，理论上地形匹配定位点X^p可以向搜索区间X^s内任意一点跳变；如图 3(b)所示，随着似然函数的取值下降，定位点向该点跳变的概率越低，定义一个非常小的置信度α，直到似然函数下降到某一个取值L_1-α时，定位点以非常小的概率α向该点跳变，这个点就是地形匹配定位的点的1-α置信区间的边界点。

地形匹配定位点X^p的似然函数如式(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表示地形测量误差；X^p表示地形匹配定位点位置；t_p表示潮差。考察式(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)

假设在地形匹配定位点X^p的置信区间内, l(X^p)可以近似为二次曲面，则置信区间内的任意一点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)

式中，Δx_e表示地形匹配定位偏差，且Δx_e=X^p-X; 0(|Δx_e|ⁿ)表示Δx_e的高阶无穷小；e表示以X^p为原点指向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)

同样的，假设在X^p的置信区间内任意一点X的平方和函数S(X)可以表示为关于X^p的二次曲面:

$ \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)

式中，σ_p²表示定位点的地形测量误差；e表示以TAP定位点为原点的任意单位方向向量; 其他含义同前。由于似然函数在定位点X^p处的一阶倒数等于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(X^p)在定位点X^p近似二次型。根据统计学中的参数估计理论^[11]，估计值X^p的方差估计V(|Δx_e|)可以通过信息矩阵求得:

$ V\left(\left|\Delta x_{e}\right|\right)=I^{-1} $

(14)

根据前面的假设，每一个测点的误差服从同一分布，根据文献[12]，对于给定的观测误差σ_p，l(X)在X^p的置信区间内近似为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-α²(mn-1)的定义与式(2)相同。式(18)表示定位点的平方和函数在1-α置信区间上确界。因平方和函数越大，则表示似然函数值越小，所以取其1-α置信区间上确界是合理的。将式(17)代入到定位点X^p的似然函数表达式(7)中，得到定位点X^p的似然函数的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置信区间估计方法获得的置信区间将比原有的估计结果大。

3 实验验证

利用船载采集数据对两种估计方法进行对比验证，实验数据采集位置在山东省胶州湾中沙礁海域，仿真中每一个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号匹配点的残差明显偏离了正态分布的假设，导致其标准差的估计大于真实值以及置信区间估计偏大。

根据文献[2]的描述，随着测量波束的增加，地形匹配定位的似然函数将渐进高斯分布，此时的残差也会逐渐逼近高斯分布。也就是说，随着测量波束的增加，非高斯分布的不利影响将被削弱，将测量波束增加到20 Ping得到如图 10所示的测量误差和潮差估计。

测量波束数分别为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%，说明测量波束增加后潮差的估计值降低而且估计值更加稳定。综上所述，波束增加后, 测量误差的方差估计值增加, 而潮差的估计结果降低，总体来讲, 估计值的方差在降低，估计结果趋于稳定。

图 11所示为21号匹配点的残差统计直方图，与测量波束为10 Ping时的情况(图 9(a))进行比较，测量波束为30 Ping时残差序列的直方图统计已经接近高斯分布，较10 Ping时有了很大的改善。图 12中绘出了30 Ping测量波束下的匹配似然函数云图、GPS定位点、TAP定位点及置信区间，可以看到测量波束增加到30 Ping后，置信区间的估计得到很大的改善。

从实验结果可以看到，原有的地形匹配定位置信区间估计方法仅从地形匹配残差的假设检验入手，通过残差的高斯分布假设以及统计估计方法得到关于残差的χ²分布，进而得到某一置信区间下的残差平方和函数的取值区间，由于计算中未考虑地形的非线性影响，导致估计结果偏小。本文中考虑了地形的非线性影响，将似然函数在置信区间内假设为二次曲面并将其在置信区间内线性化，从而将定位点的置信区间估计问题转化为二次曲面参数的置信区间估计问题，而且比较式(2)和式(17)可知, 文中提出的估计方法得到的置信区间要大于原有的方法。在测量波束较少的情况下，残差的统计结果很难满足高斯分布的假设，这种情况会随着测量波束的增加而得到改善。

4 结语

在对地形匹配的置信区间进行估计时需要同时考虑测量误差和地形曲面的非线性，假设地形匹配定位残差的平方和函数在定位点附近是近似二次型曲面，将似然函数在定位点的置信区间内线性化，在地形误差近似服从高斯分布时，利用本文中推导的估计方法可以得到更加合理、实用的估计结果。在测量波束较少时，由于地形测量的局部畸变误差，在某些位置匹配的残差不再符合高斯分布的假设，这些点的置信区间估计结果仍然会出现异常，但这一现象可以通过增加测量波束得到改善。在今后的研究中，考虑测量误差的非高斯特点并加入修正项也是一个值得研究的问题。此外，在地形匹配导航的初始阶段有较大的累积误差时，置信区间估计方法可以将初始位置约束在较小的范围内，基于置信区间估计的地形匹配导航滤波初始化方法也是值得研究的方向。