CN105352496A - 基于声呐辅助自主导航的auv导航方法 - Google Patents

Abstract

本发明涉及基于声呐辅助自主导航的AUV导航方法，属于海底探测技术领域；主要解决现有测绘系统理想探测轨迹与实际值存在差异的问题。本发明利用iSAM算法运行常数项步数的策略，将目标区域进行分块，使处于局部测绘区域的AUV对选择继续探索未知区域还是回访之前的海底环境做出智能化的决策，兼顾海底导航性能和目标区域覆盖率两种竞争诉求，为解决海底运动观测时定位误差累计这一关键技术问题提供了可行性方案。

Description

基于声呐辅助自主导航的AUV导航方法

技术领域

本发明涉及基于声呐辅助自主导航的AUV导航方法，属于海底探测技术领域。

背景技术

目前AUV执行测绘任务时，理想探测轨迹与实际探测轨迹往往存在差异。理论上按照预先设定轨迹可以提供对目标海域的100％有效覆盖，但其中隐含的假设是航行中任何一点都满足有界误差性能，在实际的调查任务中，由于对目标区域中真实海底环境的特征分布是一无所知的，倘若移动观测设备没有精准的定位，就没有高质量高准确度的海底地形地貌图。因此如何提高移动观测设备的定位准确性成为移动探测领域所面临的技术焦点和瓶颈问题。

发明内容

本发明的目的在于克服现有AUV导航模块存在的上述缺陷，提出了一种基于声呐辅助自主导航的AUV导航方法，弥补了现有测绘系统理想探测轨迹与实际值存在差异的问题，本发明利用iSAM算法(incrementalSmoothingandMapping，递增平滑与地图构建算法)运行常数项步数的策略，将目标区域进行分块，使处于局部测绘区域的AUV对选择继续探索未知区域还是回访之前的海底环境做出智能化的决策，兼顾海底导航性能和目标区域覆盖率两种竞争诉求，为解决海底运动观测时定位误差累计这一关键技术问题提供了可行性方案。

本发明所述的基于声呐辅助自主导航的AUV导航方法，包括如下步骤：

步骤一、AUV潜入海底，侧扫声呐工作，对海底图像特征进行采集；

步骤二、将高维海底声呐探测图像数据进行降维处理；

采用词袋BoW(BagofWords)局部特征表达模型来描绘海底探测图像，以满足对海量观测信息的快速的反应能力；

步骤三、分别计算两侧声呐图像的特征显著率：

采用归一化图像熵来表示声呐图像的特征显著率，

$S=\frac{{\mathrm{\Σ}}_{i}p\left({\mathrm{\ω}}_i\right){\log }_{2}p\left({\mathrm{\ω}}_i\right)}{{\log }_2\left|\mathrm{\ω}\right|}$

其中|ω|表示声呐观测图像中的局部特征总数，log₂|ω|为特征均匀分布时获得的最大熵，p(w_i)为局部特征ω_i的先验概率密度函数，图像熵表示声呐探测图像中特征的多样性；将上述特征显著率的计算结果与自定义阈值进行比较，产生具有显著特性的声呐观测节点；

步骤四、当航行器航行100步时，计算其当前航行的不确定度P，并与不确定阈值P₀进行比较：

$P={\left|{\mathrm{\Σ}}_a^{-}\right|}^{\frac{1}{6}}={\left|{\mathrm{\Σ}}_{\exp }\right|}^{\frac{1}{6}},P_0={\left|{\mathrm{\Σ}}_{allow}\right|}^{\frac{1}{6}}$

其中|Σ_exp|为自航式测绘系统探索时位姿的一步预测协方差的模，|Σ_allow|表示其位姿允许协方差的模；如果不确定度大于等于不确定阈值，则导航决策AUV重访某显著性位姿点；如果不确定度小于此不确定阈值，则导航决策AUV继续探索。

步骤五、AUV根据步骤四的决策进行探索或回访：

若AUV导航决策判决为回访，则根据最终得到回访点和之前得到的回访路径进行回访。然后原路返回。将之前探索的区域标记为已探索过的区域，然后再继续进行未知区域的探索；若AUV导航决策判决为继续探索，则AUV继续连续探索100步后，同时也将之前探索过的所有区域，标记为已探索过的区域，然后转向步骤四。

进一步地，步骤二中，采用词袋BoW局部特征表达模型来描绘海底探测图像的过程分为三部分：

第一部分，利用SURF(SpeededUpRobustFeatures,快速鲁棒性特征)算法从探测声呐图像中提取海底词汇向量，这些向量代表的是声呐图像中局部不变的特征点；

第二部分，将提取到的特征映射到字典树；

第三部分，利用海底声呐观测图像提取的局部特征生成典型样本，将典型样本对应不同词汇；通过此种方法将高维海量海底探测数据映射到低维。

进一步地，步骤四中显著性位姿点的选取将由100步运行的不确定度的总和和100步内区域覆盖率决定；AUV航行100步，再进行与阈值之间的比对，而不选择每一步都进行比较，是因为对于大量的图像信息的处理不仅需要耗费时间，而且随着时间的推移，计算总的位姿不确定度的复杂度越来越高，因此选择只对当前100步进行一次导航决策，使计算复杂度可控。

此回访显著性位姿点的选取步骤如下：

a，对步骤三得到的显著性声呐观测节点进行聚类，得到候选回访点。

b，对候选回访点进行路径规划。

c，计算到每个候选回访点的总不确定度

候选回访点的总的不确定度由三部分组成，包括里程计信息矩阵、声呐信息矩阵以及iSAM信息矩阵。

d，计算子区域覆盖率。

子区域覆盖率即水下航行器在100步之内对已探索的区域和探索目标区域的比值。其中子探索目标区域A_{sub_target}为当前100步探索的区域和未探索区域的总和，也就等于总的目标区域减去已探索过的区域，则子区域覆盖率的表达式为：

$A_{100redundant}^k=\left\{\begin{array}{c}=0\\ =l\left(P^k\right)\·D>0\end{array}\right.$

当AUV进行探索时，的值为0；当水下航行器进行回访时，的值为l(P^k)与D的乘积。其中，l(P^k)是由回访第k^th个节点所增加的期望路径的长度，D是声纳的视图宽度值，是通过回访行为所产生的预计的冗余覆盖区域。

e，确定回访点k。

惩罚因子的由两部分组成，一部分是水下航行器运动的不确定度参数另一部分则是子区域覆盖率惩罚项的表达式为：

其中α是权重。

惩罚项取相反数即可得到综合回馈值。即：

R^k＝-C^k

最后，重访问的第k^*个节点被定义为最小的综合回馈值，即：

k^*＝argmaxR^k＝argminC^k

其中k∈{0,1,2...N_wp},当k＝0时对应回访行为。

另外，进行下一次探索之前，将当前位置设置为局部起始点，忽略已探索过的区域。

本发明的有益效果是：本发明提出的基于声呐辅助自主导航的导航方法，其采用iSAM算法预期测绘位置精度较同配置卡尔曼滤波方法可提高30％以上。本发明克服了传统导航模块单一的期望通过提高导航性能来获得高准确度的海底地形地貌测绘信息的缺陷，提高导航性能，具有定位的高度一致性，并兼顾了目标覆盖率，使100％最大航程范围内累计误差可控。本发明可为进行高质量高准确度的地形地貌探测奠定坚固的技术基础。

附图说明

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

图2是探索/回访行为选择原理示意图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1-2所示，本发明所述的基于声呐辅助自主导航的AUV导航方法，包括如下步骤：

步骤一、AUV潜入海底，侧扫声呐工作，对海底图像特征进行采集；

步骤二、将高维海底声呐探测图像数据进行降维处理：

采用词袋BoW局部特征表达模型来描绘海底探测图像，以满足对海量观测信息的快速的反应能力；

步骤三、分别计算两侧声呐图像的特征显著率：

采用归一化图像熵来表示声呐图像的特征显著率，

$S=\frac{{\mathrm{\Σ}}_{i}p\left({\mathrm{\ω}}_i\right){\log }_{2}p\left({\mathrm{\ω}}_i\right)}{1{\mathrm{og}}_2\left|\mathrm{\ω}\right|}---\left(1\right)$

其中|ω|表示声呐观测图像中的局部特征总数，log₂|ω|为特征均匀分布时获得的最大熵，p(w_i)为局部特征ω_i的先验概率密度函数，图像熵表示声呐探测图像中特征的多样性；将上述特征显著率的计算结果与自定义阈值进行比较，产生具有显著特性的声呐观测节点；

步骤四、当航行器航行100步时，计算其当前航行的不确定度P，并与不确定阈值P₀进行比较：

$P={\left|{\mathrm{\Σ}}_a^{-}\right|}^{\frac{1}{6}}={\left|{\mathrm{\Σ}}_{\exp }\right|}^{\frac{1}{6}},P_0={\left|{\mathrm{\Σ}}_{allow}\right|}^{\frac{1}{6}}---\left(2\right)$

其中|Σ_exp|为自航式测绘系统探索时位姿的一步预测协方差的模，|Σ_allow|表示其位姿允许协方差的模；如果不确定度大于等于不确定阈值，则导航决策AUV重访某显著性位姿点；如果不确定度小于此不确定阈值，则导航决策AUV继续探索。

a，对步骤三得到的显著性声呐观测节点进行聚类，得到候选回访点；

b，对候选回访点进行路径规划；

c，计算到每个候选回访点的总不确定度

候选回访点的总的不确定度由三部分组成，包括里程计信息矩阵、声呐信息矩阵以及iSAM信息矩阵：

①里程计信息矩阵

其中Λ_odo为里程计信息矩阵，为回访路径上的两个虚拟节点n_i与n_i+1之间的雅克比矩阵，p为回访虚拟节点个数，R为里程计的测量噪声协方差。

②声呐信息矩阵

其中Λ_sonar为声呐信息矩阵，为虚拟节点i与其候选匹配节点m之间的雅克比矩阵，p为回访虚拟节点个数，ζ_i是i的声呐测量的候选节点的索引集合，P_L为成功链接的概率，R为声呐的测量噪声协方差。

③ISAM信息矩阵

Λ_isam＝(A^TA)^-1＝(R^TR)^-1(5)

其中A为测量雅克比矩阵，R为对A进行QR分解得到的上三角矩阵。其中A与R的计算方法如下：

I.SLAM的概率模型

根据信度网模型来表述SLAM问题，则所有变量和测量的联合概率密度模型如下：

$P(X,L,U,Z)\∝P\left(x_0\right)\underset{i=1}{\overset{M}{\Π}}P\left(x_i\right|x_{i-1},u_i)\underset{k=1}{\overset{K}{\Π}}P\left(z_k\right|x_{i_k},l_{j_k})---\left(6\right)$

其中机器人位姿X＝{x_i}，路标L＝{l_j}，控制输入U＝{u_i}，特征测量Z＝{z_k}，P(x₀)表示初始状态，P(x_i|x_i-1,u_i)表示运动概率模型，表示测量概率模型。

运动方程为：x_i＝f_i(x_i-1,u_i)+w_i(7)

其中w_i是服从均值为0，协方差为Λ_i的正态分布过程噪声。其运动概率模型为：

$P\left(x_i\right|x_{i-1},u_i)\∝\exp -\frac{1}{2}\left|\right|f_i(x_{i-1},u_i)-x_i|{|}_{{\mathrm{\Λ}}_i}^2---\left(8\right)$

测量方程为： $z_k=h_k(x_{i_k},l_{j_k})+v_k---\left(9\right)$

其中v_k是服从均值为0，协方差为Γ_k的正态分布观测噪声。其测量概率模型为：

$P\left(z_k\right|x_{i_k},l_{j_k})\∝\exp -\frac{1}{2}\left|\right|h_k(x_{i_k},l_{j_k})-z_k|{|}_{{\mathrm{\Γ}}_k}^2---\left(10\right)$

II.把SLAM问题变为最小二乘法问题

根据有效的测量，为了得到未知环境的最优估计，将SLAM问题转换为等价的基于最大后验(MAP)估计的最小二乘问题。全程轨迹X和特征地图L的MAP估计表示如下：

$X^{*},L^{*}=\arg \underset{X,L}{max}P(X,L,U,Z)=\arg \underset{X,L}{\min }-\log P(X,L,U,Z)---\left(11\right)$

结合过程方程和测量方程可得到如下非线性最小二乘法问题：

$X^{*},L^{*}=\arg \underset{X,L}{\min }\left\{\underset{i=1}{\overset{M}{\Σ}}\right||f_i\left(x_{i-1}{u}_i\right)-x_i|{|}_{\mathrm{\Λ}}^2+\underset{k=1}{\overset{M}{\Σ}}\left|\right|h_k(x_{i_k},l_{j_k})-z_k\left|{|}_{{\mathrm{\Γ}}_k}^2\right\}---\left(12\right)$

这里为了计算简单省略了初始值P(x₀)。

III.线性化

运用泰勒展开式来线性化上述方程中的测量方程和运动方程，于是可得过程方程的一阶线性化式：

$\begin{array}{c}{f}_i(x_{i-1},u_i)-x_i\≈\{f_i(x_{i-1}^0,u_i)+F_i^{i-1}{\mathrm{\δx}}_{i-1}\}-\{x_i^0+{\mathrm{\δx}}_i\}\\ =\{F_i^{i-1}{\mathrm{\δx}}_{i-1}-{\mathrm{\δx}}_i\}-{\mathrm{\α}}_i\end{array}---\left(13\right)$

其中是状态预测误差。是过程模型f_i(·)在线性化点的雅克比矩阵：

$F_i^{i-1}:=\frac{\∂f(x_{i-1},u_i)}{\∂x_{i-1}}{|}_{x_{i-1}^0}---\left(14\right)$

同理可得测量方程的一阶线性化式：

$\begin{array}{c}{h}_k(x_{i_k},l_{j_k})-z_k\≈\{h_k(x_{i_k}^0,l_{j_k}^0)+H_k^{i_k}{\mathrm{\δx}}_{i_k}+J_k^{i_k}{\mathrm{\δl}}_{j_k}\}-z_k\\ =\{H_k^{i_k}{\mathrm{\δx}}_{i_k}+J_k^{j_k}{\mathrm{\δl}}_{j_k}\}-c_k\end{array}---\left(15\right)$

其中是测量预测误差，和分别是h_k(·)在的雅克比矩阵。分别把线性化的式子带入(12)式可得

${\mathrm{\δ\θ}}^{*}=\arg \underset{\mathrm{\δ}\mathrm{\θ}}{\min }\left\{\underset{i=1}{\overset{M}{\Σ}}\right||F_i^{i-1}{\mathrm{\δx}}_{i-1}+G_i^i{\mathrm{\δx}}_{i-1}-{\mathrm{\α}}_i|{|}_{{\mathrm{\Λ}}_i}^2+\Σ\left|\right|H_K^{i_k}{\mathrm{\δx}}_{i_k}+J_K^{j_k}{\mathrm{\δl}}_{j_k}-c_k\left|{|}_{{\mathrm{\Γ}}_k}^2\right\}---\left(16\right)$

此时，将位姿变量和特征变量整合成一个单一的变量n＝Md_x+Nd_l，其中d_x、d_l分别是位姿和路标变量的尺寸大小，

为了简化上述公式，改变马氏距离的形式：

$\left|\right|e|{|}_{\mathrm{\Σ}}^2:=e^T{\mathrm{\Σ}}^{-1}e={\left({\mathrm{\Σ}}^{-T/2}e\right)}^T\left({\mathrm{\Σ}}^{-T/2}e\right)=|\left|{\mathrm{\Σ}}^{-T/2}e\right|{|}^2,$ 消除公式中的Λ_i和Γ_k。

把所有的雅可比矩阵整合成稀疏的具有m行测量值的测量雅可比矩阵把向量α_i、c_k整合成右向量于是得到标准的最小二乘法问题：

${\mathrm{\θ}}^{*}=\arg \underset{\mathrm{\θ}}{\min }\left|\right|A\mathrm{\θ}-b|{|}^2---\left(17\right)$

这里省略掉了符号δ。此最小二乘法式通过设置||Aθ-b||²为0可以转换为普通的线性方程，继而可以得到标准方程A^TAθ＝A^Tb，用柯列斯基(cholesky)分解矩阵A^TA，但是必须计算A^TA，计算量非常大。为了减小计算量，直接对测量雅可比矩阵进行QR分解。

IV运用QR分解

测量雅可比矩阵A的QR分解定义为：

$A:=Q\[\begin{array}{c}R\\ 0\end{array}\]---\left(18\right)$

这里是上三角平方根信息矩阵(由R^TR＝A^TA给定)，是正交矩阵。

至此，求解出A和R则能得到ISAM的信息矩阵Λ_isam。

水下航行器总的信息阵为：Λ_pdm＝Λ_odo+Λ_sonar+Λ_isam，根据Λ_pdnΣ＝I_n×n，Λ_pdmΣ_*n＝e_n，即可获得第k^th个节点的最终的方差

水下航行器不确定度是下一步最优行动的局部不确定性，即PDM的总体的协方差Σ_nn与用户定义不确定阈值P₀的比值。

d，计算子区域覆盖率；

子区域覆盖率即水下航行器在100步之内对已探索的区域和探索目标区域的比值。其中子探索目标区域A_{sub_target}为当前100步探索的区域和未探索区域的总和，也就等于总的目标区域减去已探索过的区域，则子区域覆盖率的表达式为：

$A_{100redundant}^k=\left\{\begin{array}{c}=0\\ =l\left(P^k\right)\·D>0\end{array}\right.---\left(21\right)$

当AUV进行探索时，的值为0；当水下航行器进行回访时，的值为l(P^k)与D的乘积。其中，l(P^k)是由回访第k^th个节点所增加的期望路径的长度，D是声纳的视图宽度值，是通过回访行为所产生的预计的冗余覆盖区域。

e，确定回访点k；

惩罚因子的由两部分组成，一部分是水下航行器运动的不确定度参数另一部分则是子区域覆盖率惩罚项的表达式为：

其中α是权重。

惩罚项取相反数即可得到综合回馈值。即：

R^k＝-C^k(23)

最后，重访问的第k^*个节点被定义为最小的综合回馈值，即：

k^*＝argmaxR^k＝argminC^k(24)

其中k∈{0,1,2...N_wp},当k＝0时对应回访行为。

步骤五、AUV根据步骤四的决策进行探索或回访：

若AUV导航决策判决为回访，则根据最终得到回访点和之前得到的回访路径进行回访。然后原路返回。将之前探索的区域标记为已探索过的区域，然后再继续进行未知区域的探索；若AUV导航决策判决为继续探索，则AUV继续连续探索100步后，同时也将之前探索过的所有区域，标记为已探索过的区域，然后转向步骤四。

上述内容仅为本发明的较佳实施例，不能被认为用于限定对本发明的实施例范围。本发明也并不仅限于上述举例，本技术领域的普通技术人员在本发明的实质范围内所做出的均等变化与改进等，均应归属于本发明的专利涵盖范围内。

Claims (3)

1.一种基于声呐辅助自主导航的AUV导航方法，其特征在于，包括如下步骤：

步骤一、AUV潜入海底，侧扫声呐工作，对海底图像特征进行采集；

步骤二、将高维海底声呐探测图像数据进行降维处理：

采用词袋BoW局部特征表达模型来描绘海底探测图像，以满足对海量观测信息的快速的反应能力；

步骤三、分别计算两侧声呐图像的特征显著率：

采用归一化图像熵来表示声呐图像的特征显著率，

$S=\frac{{\mathrm{\Σ}}_{i}p\left({\mathrm{\ω}}_i\right){\log }_{2}p\left({\mathrm{\ω}}_i\right)}{{\log }_2\left|\mathrm{\ω}\right|}$

其中|ω|表示声呐观测图像中的局部特征总数，log₂|ω|为特征均匀分布时获得的最大熵，p(w_i)为局部特征ω_i的先验概率密度函数，图像熵表示声呐探测图像中特征的多样性；将上述特征显著率的计算结果与自定义阈值进行比较，产生具有显著特性的声呐观测节点；

步骤四、当航行器航行100步时，计算其当前航行的不确定度P，并与不确定阈值P₀进行比较：

$P=|{\mathrm{\Σ}}_a^{-}{|}^{\frac{1}{6}}=|{\mathrm{\Σ}}_{\exp }{|}^{\frac{1}{6}},P_0=|{\mathrm{\Σ}}_{allow}{|}^{\frac{1}{6}}$

其中|Σ_exp|为自航式测绘系统探索时位姿的一步预测协方差的模，|Σ_allow|表示其位姿允许协方差的模；如果不确定度大于等于不确定阈值，则导航决策AUV重访某回访显著性位姿点；如果不确定度小于此不确定阈值，则导航决策AUV继续探索；

步骤五、AUV根据步骤四的决策进行探索或回访：

若AUV导航决策判决为回访，则根据最终得到回访点和之前得到的回访路径进行回访；然后原路返回；将之前探索的区域标记为已探索过的区域，然后再继续进行未知区域的探索；若AUV导航决策判决为继续探索，则AUV继续连续探索100步后，同时也将之前探索过的所有区域，标记为已探索过的区域，然后转向步骤四。

2.根据权利要求1所述的基于声呐辅助自主导航的AUV导航方法，其特征在于，步骤二中，采用词袋BoW局部特征表达模型来描绘海底探测图像的过程分为三部分：

第一部分，利用SURF算法从探测声呐图像中提取海底词汇向量，这些向量代表的是声呐图像中局部不变的特征点；

第二部分，将提取到的特征映射到字典树；

第三部分，利用海底声呐观测图像提取的局部特征生成典型样本，将典型样本对应不同词汇；通过此种方法将高维海量海底探测数据映射到低维。

3.根据权利要求1所述的基于声呐辅助自主导航的AUV导航方法，其特征在于，步骤四中回访显著性位姿点的选取将由100步运行的不确定度的总和和100步内区域覆盖率决定；

此回访显著性位姿点的选取步骤如下：

a，对步骤三得到的显著性声呐观测节点进行聚类，得到候选回访点；

b，对候选回访点进行路径规划；

c，计算到每个候选回访点的总不确定度

候选回访点的总的不确定度由三部分组成，包括里程计信息矩阵、声呐信息矩阵以及iSAM信息矩阵：

①里程计信息矩阵

其中Λ_odo为里程计信息矩阵，为回访路径上的两个虚拟节点n_i与n_i+1之间的雅克比矩阵，p为回访虚拟节点个数，R为里程计的测量噪声协方差；

②声呐信息矩阵

其中Λ_sonar为声呐信息矩阵，为虚拟节点i与其候选匹配节点m之间的雅克比矩阵，p为回访虚拟节点个数，ζ_i是i的声呐测量的候选节点的索引集合，P_L为成功链接的概率，R为声呐的测量噪声协方差；

③ISAM信息矩阵

Λ_isam＝(A^TA)^-1＝(R^TR)^-1

其中A为测量雅克比矩阵，R为对A进行QR分解得到的上三角矩阵；其中A与R的计算方法如下：

I.SLAM的概率模型

根据信度网模型来表述SLAM问题，则所有变量和测量的联合概率密度模型如下：

$P(X,L,Y,Z)\∝P\left(x_0\right)\underset{i=1}{\overset{M}{\Π}}P\left(x_i\right|x_{i-1},u_i)\underset{k=1}{\overset{K}{\Π}}P\left(z_k\right|x_{i_k},l_{j_k})$

II.把SLAM问题变为最小二乘法问题

根据有效的测量，为了得到未知环境的最优估计，将SLAM问题转换为等价的基于最大后验估计的最小二乘问题；全程轨迹X和特征地图L的MAP估计表示如下：

$X^{*},L^{*}=\arg \underset{X,L}{max}P(X,L,U,Z)=\arg \underset{X,L}{\min }-\log P(X,L,U,Z)$

结合过程方程和测量方程可得到如下非线性最小二乘法问题：

$X^{*},L^{*}=\arg \underset{X,L}{\min }\left\{\underset{i=1}{\overset{M}{\Σ}}\right||f_i\left(x_{i-1,}{u}_i\right)-x_i|{|}_{\mathrm{\Λ}}^2+\underset{k=1}{\overset{M}{\Σ}}\left|\right|h_k\left(x_{i_k},l_{j_k}\right)-z_k\left|{|}_{{\mathrm{\Γ}}_k}^2\right\}$

这里为了计算简单省略了初始值P(x₀)；

III线性化

运用泰勒展开式来线性化上述方程中的测量方程和运动方程，可得过程方程的一阶线性化式：

$\begin{array}{c}{f}_i(x_{i-1},u_i)-x_i\≈\{f_i(x_{i-1}^0,u_i)+F_i^{i-1}{\mathrm{\δx}}_{i-1}\}-\{x_i^0+{\mathrm{\δx}}_i\}\\ =\left\{F_i^{i-1}{\mathrm{\δx}}_{i-1}-{\mathrm{\δx}}_i\right\}-{\mathrm{\α}}_i\end{array}$

同理可得测量方程的一阶线性化式：

$\begin{array}{c}{h}_k(x_{i_k},l_{j_k})-z_k\≈\{h_k(x_{i_k}^0,l_{j_k}^0)+H_k^{i_k}{\mathrm{\δx}}_{i_k}+J_k^{j_k}{\mathrm{\δl}}_{j_k}\}-z_k\\ =\left\{H_k^{i_k}{\mathrm{\δx}}_{i_k}+J_k^{j_k}{\mathrm{\δl}}_{j_k}\right\}-c_k\end{array}$

整合得到标准的最小二乘法问题：

${\mathrm{\θ}}^{*}=\arg \underset{\mathrm{\θ}}{\min }\left|\right|A\mathrm{\θ}-b|{|}^2$

IV运用QR分解

测量雅可比矩阵A的QR分解定义为：

$A:=Q\left[\begin{array}{c}R\\ 0\end{array}\right]$

这里是上三角平方根信息矩阵，是正交矩阵；

至此，求解出A和R则能得到ISAM的信息矩阵Λ_isam；

水下航行器总的信息阵为：Λ_pdm＝Λ_odo+Λ_sonar+Λ_isam，根据Λ_pdnΣ＝I_n×n，Λ_pdmΣ_*n＝e_n，即可获得第k^th个节点的最终的方差

水下航行器不确定度是下一步最优行动的局部不确定性，即PDM的总体的协方差Σ_nn与用户定义不确定阈值P₀的比值；

d，计算子区域覆盖率；

子区域覆盖率即水下航行器在100步之内对已探索的区域和探索目标区域的比值；其中子探索目标区域A_{sub_target}为当前100步探索的区域和未探索区域的总和，也就等于总的目标区域减去已探索过的区域，则子区域覆盖率的表达式为：

$A_{100redundant}^k=\left\{\begin{array}{c}=0\\ =l\left(P^k\right)\·D>0\end{array}\right.$

当AUV进行探索时，的值为0；当水下航行器进行回访时，的值为l(P^k)与D的乘积；其中，l(P^k)是由回访第k^th个节点所增加的期望路径的长度，D是声纳的视图宽度值，是通过回访行为所产生的预计的冗余覆盖区域；

e，确定回访点k；

惩罚因子的由两部分组成，一部分是水下航行器运动的不确定度参数另一部分则是子区域覆盖率惩罚项的表达式为：

其中α是权重；

惩罚项取相反数即可得到综合回馈值；即：

R^k＝-C^k

最后，重访问的第k^*个节点被定义为最小的综合回馈值，即：

k^*＝argmaxR^k＝argminC^k

其中k∈{0,1,2...N_wp},当k＝0时对应回访行为。