CN110763234A - 一种水下机器人海底地形匹配导航路径规划方法 - Google Patents

Abstract

本发明属于水下机器人的水下导航领域，具体涉及一种水下机器人海底地形匹配导航路径规划方法。本发明将地形起伏层度考虑为水下机器人状态转移概率的影响因子，可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题，保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度。本发明通过粒子滤波器实时对水下机器人状态进行跟踪，并根据状态跟踪结果选择最优动作，实现了对水下机器人运动不确定性的考虑。本发明可以使水下机器人应用地形匹配导航的可靠性大大提高。

Description

一种水下机器人海底地形匹配导航路径规划方法

技术领域

本发明属于水下机器人的水下导航领域，具体涉及一种水下机器人海底地形匹配导航路径规划方法。

背景技术

随着多波束测深技术的发展，高精度海底地形测绘成为可能，因而以多波束声纳测绘得到的海底地形为信息源的海底地形匹配导航技术已经得到了世界各国的广泛重视。然而由于地形匹配精度受海底地形起伏程度影响严重，因而需要对装备海底地形匹配导航系统的水下机器人进行路径规划，使其实现对地形过于平坦区域的避让。同时，由于状态转移噪声等导致的水下机器人运动不确定性，水下机器人在执行任务过程中需要实时对自身位置进行估计并在线规划航线。

目前现有的海底地形匹配导航的路径规划方法多将地形起伏程度作为奖励函数或代价函数考虑。虽然地形起伏程度越高匹配效果越好，但两者之间的关系并非线性的，因而大多数算法所规划的最大化总地形起伏程度的路径并不能获得最好的地形匹配效果，反而会因为前往特定地形起伏极端剧烈区域导致水下机器人不得不经过某些地形平坦区域，并导致地形匹配算法失效。如公开日为2018年11月23日、公开号为CN108871351A、发明名称为“一种AUV海底地形匹配的动态路径规划方法”的专利申请，该方法将路径规划划分为了全局路径规划和局部规划两个阶段，实现了动态避障等过程。但该发明没有考虑水下机器人运动的不确定性，无法实现对水下机器人运动状态的跟踪估计，一旦水下机器人偏离航线很难及时进行补救。

发明内容

本发明的目的在于提供解决海底地形匹配导航的最优路径规划问题的一种水下机器人海底地形匹配导航路径规划方法。

本发明的目的通过如下技术方案来实现：

具体包括以下步骤：

步骤1：将水下机器人作业区域先验海底地形图进行栅格划分；

步骤2：计算每个栅格的值函数；

步骤3：粒子初始化；

步骤4：针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新；

步骤5：为粒子选择最优动作；

步骤6：根据多波束测深数据进行观测更新；

步骤7：计算每个粒子的权重；

步骤8：根据所有粒子对应的权重和最优动作计算水下机器人的控制输入；

步骤9：重采样更新粒子；

步骤10：判断水下机器人是否到达终点；若水下机器人到达终点，则完成路径规划；若水下机器人未到达终点。则执行控制输入后返回步骤4。

本发明还可以包括：

所述的步骤2中计算每个栅格值函数的具体方法为：

步骤2.1：若当前栅格为终点，令其值函数V(x₀)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式为：

其中，

为多波束声纳单个波束测量噪声的标准差；h_i(x)为海底地形平面拟合方程，a和b分别代表东向和北向；M表示地形测点个数；

步骤2.2：针对当前栅格周围的八个栅格，分别计算其参考导航协方差Σ，计算公式如下：

其中，σ_east为参考导航系统的东向标准差；σ_north为参考导航系统的北向标准差；σ_east,north为参考导航系统的东向-北向标准差；

步骤2.3：计算给定控制输入u后水下机器人由当前栅格x₀转移到周边其他八个栅格x_i(i＝1,2,...,8)的状态转移概率p(x_i|x₀,u)i＝1,2,...,8，计算公式为：

步骤2.5：计算当前栅格的值函数V(x₀)，并记录值函数所对应的水下机器人动作u；所述的当前栅格的值函数V(x₀)的计算公式为：

所述的步骤4中针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新具体包括：针对粒子k，其中k＝1,2,...,K，利用参考导航系统给出的控制输入u对其位置进行更新，得到x_k′＝x_k+u+Q，其中Q为水下机器人状态转移噪声的协方差。

所述的步骤5中为粒子选择最优动作具体为：针对粒子k在状态转移之后的位置x_k′，根据马尔科夫决策过程，以使当前栅格的值函数V(x₀)最大的运动输入u_k作为其对应最优控制输入。

所述的步骤6中根据多波束测深数据进行观测更新具体为：针对每个粒子，根据步骤4中的状态更新值，提取其实测地图在先验地图中对应的预测结果，并计算当前预测地形的地形粗糙度rough，对于大小为M×N的实测地图，粗糙度rough的计算公式为：

其中r_x和r_y分别为地形粗糙度在x和y方向的分量，计算公式如下：

其中h(i,j)为先验地图中(i,j)处的地形高程。

所述的步骤7中计算每个粒子的权重的方法具体为：针对每个粒子，根据其预测地形以及多波束声纳获得的测深数据z′(i,j)，其中i∈[1,M],j∈[1,N]，并计算新息量Δz：

则第k个粒子的权重w_k为：

w_k＝Δz_k·rough_k

其中Δz_k和rough_k为该粒子对应的新息量和预测地形粗糙度。

所述的步骤8中根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u的具体计算公式为：

其中K为粒子个数。

本发明的有益效果在于：

本发明将地形起伏层度考虑为水下机器人状态转移概率的影响因子，可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题，保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度。本发明通过粒子滤波器实时对水下机器人状态进行跟踪，并根据状态跟踪结果选择最优动作，实现了对水下机器人运动不确定性的考虑。本发明可以使水下机器人应用地形匹配导航的可靠性大大提高。

附图说明

图1是本发明的总流程图。

图2是本发明中惩罚函数选择示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明的目的在于提供能够一种水下机器人海底地形匹配导航路径规划方法，该方法通过计算水下机器人的状态转移概率函数对海底地形起伏层度进行评估，利用马尔可夫决策过程选取水下机器人在各个栅格的最优动作，并通过粒子滤波实现对水下机器人状态的跟踪估计和控制输入计算，从而解决海底地形匹配导航的最优路径规划问题。

本发明将影响水下机器人地形匹配导航效果的地形起伏程度作为水下机器人状态转移概率函数的影响因子，利用马尔科夫决策过程进行最优规划；通过粒子滤波实现对水下机器人运动状态的跟踪估计，根据估计结果对路线进行在线规划。本发明将地形起伏程度作为水下机器人状态转移概率函数的影响因子，事实上是搜索了所有节点地形起伏均较为剧烈的路径，从而保证了水下机器人在整条路径上的匹配导航精度。

结合图一，本发明的具体流程如下：

步骤一、将水下机器人作业区域先验海底地形图划分为等大小的栅格。

步骤二、根据海底地形起伏程度和参考导航系统参数，计算各个栅格的值函数并执行步骤三。

步骤2.1、若当前栅格为终点，令其值函数V(x₀)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式如下

其中，

为多波束声纳单个波束测量噪声的标准差，h_i(x)为海底地形平面拟合方程，a和b代表东向和北向，M表示地形测点个数。

步骤2.2、针对当前栅格周围的八个栅格，分别计算其参考导航协方差Σ，计算公式如下：

其中参考导航系统的东向标准差σ_east，北向标准差σ_north，东向-北向标准差σ_east,north由参考导航系统参数给出。

步骤2.3、在通过步骤2.1和步骤2.2计算得到栅格x_i(i＝1,2,...,8)对应的地形匹配过程协方差Ψ_i和参考导航协方差Σ_i后，计算给定控制输入u后水下机器人由当前栅格x₀转移到周边其他八个栅格x_i(i＝1,2,...,8)的状态转移概率p(x_i|x₀,u)(i＝1,2,...,8)，计算公式为：

步骤2.4、考虑水下机器人经过地形不明显区域的惩罚，结合图2可以看到，当匹配过程协方差Ψ_i和参考导航协方差Σ_i之和越大，对应概率分布越扁平，概率小于0.05的区域越大，因而令

步骤2.5、计算当前栅格的值函数，方程为：

并记录值函数所对应的水下机器人动作u。

步骤三、进行粒子初始化，执行步骤四。

步骤四、针对每个粒子，根据水下机器人的参考导航数据进行状态更新，并利用马尔可夫决策过程求解当前粒子的最优动作，执行步骤五。

步骤4.1、针对粒子k(k＝1,2,...,K)，利用参考导航系统给出的控制输入u对其位置进行更新，得到x_k′＝x_k+u+Q，其中Q为水下机器人状态转移噪声的协方差。

步骤4.2、针对粒子k在状态转移之后的位置x_k′，根据马尔科夫决策过程，以在步骤2.5中给出的其值函数最大的运动输入u_k作为其对应最优控制输入。

步骤五、利用多波束声纳观测值进行观测更新，根据每个粒子当前状态的先验地形和多波束声纳观测值计算各个粒子权重，执行步骤六。

步骤5.1、针对每个粒子，根据步骤4.2给出的状态更新值x′，提取其实测地图在先验地图中对应的预测结果，并计算当前预测地形的地形粗糙度rough，对于大小为M×N的实测地图，其计算公式为：

其中r_x和r_y分别为地形粗糙度在x和y方向的分量，计算公式如下：

其中h(i,j)为先验地图中(i,j)处的地形高程。

步骤5.2、针对每个粒子，根据其预测地形以及多波束声纳获得的测深数据z′(i,j)(i∈[1,M],j∈[1,N])，并计算新息量Δz：

步骤5.3、针对第k个粒子(k＝1,2,...,K)，计算其权重w_k：

w_k＝Δz_k·rough_k

其中Δz_k和rough_k为该粒子对应的新息量和预测地形粗糙度。

步骤六、根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u：

其中K为粒子个数。

步骤七、重采样更新粒子。

步骤八、判断水下机器人是否到达终点，是则算法结束；否则水下机器人执行控制输入后算法执行步骤四。

本发明的有益效果在于提出的海底地形匹配导航路径规划方法将地形起伏层度考虑为水下机器人状态转移概率的影响因子，可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题，保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度；同时，本发明通过粒子滤波器实时对水下机器人状态进行跟踪，并根据状态跟踪结果选择最优动作，实现了对水下机器人运动不确定性的考虑。以上两点有益效果可使水下机器人应用地形匹配导航的可靠性大大提高。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种水下机器人海底地形匹配导航路径规划方法，其特征在于，具体包括以下步骤：

步骤1：将水下机器人作业区域先验海底地形图进行栅格划分；

步骤2：计算每个栅格的值函数；

步骤3：粒子初始化；

步骤4：针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新；

步骤5：为粒子选择最优动作；

步骤6：根据多波束测深数据进行观测更新；

步骤7：计算每个粒子的权重；

步骤8：根据所有粒子对应的权重和最优动作计算水下机器人的控制输入；

步骤9：重采样更新粒子；

步骤10：判断水下机器人是否到达终点；若水下机器人到达终点，则完成路径规划；若水下机器人未到达终点。则执行控制输入后返回步骤4。

2.根据权利要求1所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤2中计算每个栅格值函数的具体方法为：

步骤2.1：若当前栅格为终点，令其值函数V(x₀)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式为：

其中，

为多波束声纳单个波束测量噪声的标准差；h_i(x)为海底地形平面拟合方程，a和b分别代表东向和北向；M表示地形测点个数；

步骤2.2：针对当前栅格周围的八个栅格，分别计算其参考导航协方差Σ，计算公式如下：

其中，σ_east为参考导航系统的东向标准差；σ_north为参考导航系统的北向标准差；σ_east,north为参考导航系统的东向-北向标准差；

步骤2.3：计算给定控制输入u后水下机器人由当前栅格x₀转移到周边其他八个栅格x_i(i＝1,2,...,8)的状态转移概率p(x_i|x₀,u)i＝1,2,...,8，计算公式为：

步骤2.5：计算当前栅格的值函数V(x₀)，并记录值函数所对应的水下机器人动作u；所述的当前栅格的值函数V(x₀)的计算公式为：

3.根据权利要求2所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤4中针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新具体包括：针对粒子k，其中k＝1,2,...,K，利用参考导航系统给出的控制输入u对其位置进行更新，得到x_k′＝x_k+u+Q，其中Q为水下机器人状态转移噪声的协方差。

4.根据权利要求3所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤5中为粒子选择最优动作具体为：针对粒子k在状态转移之后的位置x_k′，根据马尔科夫决策过程，以使当前栅格的值函数V(x₀)最大的运动输入u_k作为其对应最优控制输入。

5.根据权利要求1或2或3或4所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤6中根据多波束测深数据进行观测更新具体为：针对每个粒子，根据步骤4中的状态更新值，提取其实测地图在先验地图中对应的预测结果，并计算当前预测地形的地形粗糙度rough，对于大小为M×N的实测地图，粗糙度rough的计算公式为：

其中r_x和r_y分别为地形粗糙度在x和y方向的分量，计算公式如下：

其中h(i,j)为先验地图中(i,j)处的地形高程。

6.根据权利要求5所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤7中计算每个粒子的权重的方法具体为：针对每个粒子，根据其预测地形以及多波束声纳获得的测深数据z′(i,j)，其中i∈[1,M],j∈[1,N]，并计算新息量Δz：

则第k个粒子的权重w_k为：

w_k＝Δz_k·rough_k

其中Δz_k和rough_k为该粒子对应的新息量和预测地形粗糙度。

7.根据权利要求6所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤8中根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u的具体计算公式为：

其中K为粒子个数。

Images