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

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

A navigation path planning method for underwater vehicle based on terrain matching

【技术实现步骤摘要】
一种水下机器人海底地形匹配导航路径规划方法
本专利技术属于水下机器人的水下导航领域，具体涉及一种水下机器人海底地形匹配导航路径规划方法。
技术介绍
随着多波束测深技术的发展，高精度海底地形测绘成为可能，因而以多波束声纳测绘得到的海底地形为信息源的海底地形匹配导航技术已经得到了世界各国的广泛重视。然而由于地形匹配精度受海底地形起伏程度影响严重，因而需要对装备海底地形匹配导航系统的水下机器人进行路径规划，使其实现对地形过于平坦区域的避让。同时，由于状态转移噪声等导致的水下机器人运动不确定性，水下机器人在执行任务过程中需要实时对自身位置进行估计并在线规划航线。目前现有的海底地形匹配导航的路径规划方法多将地形起伏程度作为奖励函数或代价函数考虑。虽然地形起伏程度越高匹配效果越好，但两者之间的关系并非线性的，因而大多数算法所规划的最大化总地形起伏程度的路径并不能获得最好的地形匹配效果，反而会因为前往特定地形起伏极端剧烈区域导致水下机器人不得不经过某些地形平坦区域，并导致地形匹配算法失效。如公开日为2018年11月23日、公开号为CN108871351A、专利技术名称为“一种AUV海底地形匹配的动态路径规划方法”的专利申请，该方法将路径规划划分为了全局路径规划和局部规划两个阶段，实现了动态避障等过程。但该专利技术没有考虑水下机器人运动的不确定性，无法实现对水下机器人运动状态的跟踪估计，一旦水下机器人偏离航线很难及时进行补救。
技术实现思路
本专利技术的目的在于提供解决海底地形匹配导航的最优路径规划问题的一种水下机器人海底地形匹配导航路径规划方法。本专利技术的目的通过如下技术方案来实现：具体包括以下步骤：步骤1：将水下机器人作业区域先验海底地形图进行栅格划分；步骤2：计算每个栅格的值函数；步骤3：粒子初始化；步骤4：针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新；步骤5：为粒子选择最优动作；步骤6：根据多波束测深数据进行观测更新；步骤7：计算每个粒子的权重；步骤8：根据所有粒子对应的权重和最优动作计算水下机器人的控制输入；步骤9：重采样更新粒子；步骤10：判断水下机器人是否到达终点；若水下机器人到达终点，则完成路径规划；若水下机器人未到达终点。则执行控制输入后返回步骤4。本专利技术还可以包括：所述的步骤2中计算每个栅格值函数的具体方法为：步骤2.1：若当前栅格为终点，令其值函数V(x0)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式为：其中，为多波束声纳单个波束测量噪声的标准差；hi(x)为海底地形平面拟合方程，a和b分别代表东向和北向；M表示地形测点个数；步骤2.2：针对当前栅格周围的八个栅格，分别计算其参考导航协方差Σ，计算公式如下：其中，σeast为参考导航系统的东向标准差；σnorth为参考导航系统的北向标准差；σeast,north为参考导航系统的东向-北向标准差；步骤2.3：计算给定控制输入u后水下机器人由当前栅格x0转移到周边其他八个栅格xi(i＝1,2,...,8)的状态转移概率p(xi|x0,u)i＝1,2,...,8，计算公式为：步骤2.5：计算当前栅格的值函数V(x0)，并记录值函数所对应的水下机器人动作u；所述的当前栅格的值函数V(x0)的计算公式为：所述的步骤4中针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新具体包括：针对粒子k，其中k＝1,2,...,K，利用参考导航系统给出的控制输入u对其位置进行更新，得到xk′＝xk+u+Q，其中Q为水下机器人状态转移噪声的协方差。所述的步骤5中为粒子选择最优动作具体为：针对粒子k在状态转移之后的位置xk′，根据马尔科夫决策过程，以使当前栅格的值函数V(x0)最大的运动输入uk作为其对应最优控制输入。所述的步骤6中根据多波束测深数据进行观测更新具体为：针对每个粒子，根据步骤4中的状态更新值，提取其实测地图在先验地图中对应的预测结果，并计算当前预测地形的地形粗糙度rough，对于大小为M×N的实测地图，粗糙度rough的计算公式为：其中rx和ry分别为地形粗糙度在x和y方向的分量，计算公式如下：其中h(i,j)为先验地图中(i,j)处的地形高程。所述的步骤7中计算每个粒子的权重的方法具体为：针对每个粒子，根据其预测地形以及多波束声纳获得的测深数据z′(i,j)，其中i∈[1,M],j∈[1,N]，并计算新息量Δz：则第k个粒子的权重wk为：wk＝Δzk·roughk其中Δzk和roughk为该粒子对应的新息量和预测地形粗糙度。所述的步骤8中根据所有粒子对应的权重和最优动作计算水下机器人的控制输入u的具体计算公式为：其中K为粒子个数。本专利技术的有益效果在于：本专利技术将地形起伏层度考虑为水下机器人状态转移概率的影响因子，可以避免传统海底地形匹配导航路径规划算法由于追求路径总地形起伏最大化导致的路径会经过某些平坦区域的问题，保证了地形匹配算法在规划得到的整条上均能得到较高的匹配精度。本专利技术通过粒子滤波器实时对水下机器人状态进行跟踪，并根据状态跟踪结果选择最优动作，实现了对水下机器人运动不确定性的考虑。本专利技术可以使水下机器人应用地形匹配导航的可靠性大大提高。附图说明图1是本专利技术的总流程图。图2是本专利技术中惩罚函数选择示意图。具体实施方式下面结合附图对本专利技术做进一步描述。本专利技术的目的在于提供能够一种水下机器人海底地形匹配导航路径规划方法，该方法通过计算水下机器人的状态转移概率函数对海底地形起伏层度进行评估，利用马尔可夫决策过程选取水下机器人在各个栅格的最优动作，并通过粒子滤波实现对水下机器人状态的跟踪估计和控制输入计算，从而解决海底地形匹配导航的最优路径规划问题。本专利技术将影响水下机器人地形匹配导航效果的地形起伏程度作为水下机器人状态转移概率函数的影响因子，利用马尔科夫决策过程进行最优规划；通过粒子滤波实现对水下机器人运动状态的跟踪估计，根据估计结果对路线进行在线规划。本专利技术将地形起伏程度作为水下机器人状态转移概率函数的影响因子，事实上是搜索了所有节点地形起伏均较为剧烈的路径，从而保证了水下机器人在整条路径上的匹配导航精度。结合图一，本专利技术的具体流程如下：步骤一、将水下机器人作业区域先验海底地形图划分为等大小的栅格。步骤二、根据海底地形起伏程度和参考导航系统参数，计算各个栅格的值函数并执行步骤三。步骤2.1、若当前栅格为终点，令其值函数V(x0)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式如下其中，为多波束声纳单个波束测量噪声的标准本文档来自技高网...

【技术保护点】
1.一种水下机器人海底地形匹配导航路径规划方法，其特征在于，具体包括以下步骤：/n步骤1：将水下机器人作业区域先验海底地形图进行栅格划分；/n步骤2：计算每个栅格的值函数；/n步骤3：粒子初始化；/n步骤4：针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新；/n步骤5：为粒子选择最优动作；/n步骤6：根据多波束测深数据进行观测更新；/n步骤7：计算每个粒子的权重；/n步骤8：根据所有粒子对应的权重和最优动作计算水下机器人的控制输入；/n步骤9：重采样更新粒子；/n步骤10：判断水下机器人是否到达终点；若水下机器人到达终点，则完成路径规划；若水下机器人未到达终点。则执行控制输入后返回步骤4。/n

【技术特征摘要】
1.一种水下机器人海底地形匹配导航路径规划方法，其特征在于，具体包括以下步骤：
步骤1：将水下机器人作业区域先验海底地形图进行栅格划分；
步骤2：计算每个栅格的值函数；
步骤3：粒子初始化；
步骤4：针对每个粒子，根据水下机器人的控制输入数据和先验地图进行状态更新；
步骤5：为粒子选择最优动作；
步骤6：根据多波束测深数据进行观测更新；
步骤7：计算每个粒子的权重；
步骤8：根据所有粒子对应的权重和最优动作计算水下机器人的控制输入；
步骤9：重采样更新粒子；
步骤10：判断水下机器人是否到达终点；若水下机器人到达终点，则完成路径规划；若水下机器人未到达终点。则执行控制输入后返回步骤4。


2.根据权利要求1所述的一种水下机器人海底地形匹配导航路径规划方法，其特征在于：所述的步骤2中计算每个栅格值函数的具体方法为：
步骤2.1：若当前栅格为终点，令其值函数V(x0)为1000，否则针对当前栅格周围的八个栅格，分别计算其地形匹配过程协方差Ψ，计算公式为：



其中，为多波束声纳单个波束测量噪声的标准差；hi(x)为海底地形平面拟合方程，a和b分别代表东向和北向；M表示地形测点个数；
步骤2.2：针对当前栅格周围的八个栅格，分别计算其参考导航协方差Σ，计算公式如下：



其中，σeast为参考导航系统的东向标准差；σnorth为参考导航系统的北向标准差；σeast,north为参考导航系统的东向-北向标准差；
步骤2.3：计算给定控制输入u后水下机器人由当前栅格x0转移到周边其他八个栅格xi(i＝1,2,...,8)的状态转移概率p(xi|x0,u)i＝1,2,...,8，计算公式为：






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





3.根据权利要求2所述的一...

【专利技术属性】
技术研发人员：马腾，李晔，姜言清，张强，丛正，范佳佳，贡雨森，凌宇，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：黑龙;23