CN110220510A - 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 - Google Patents

Abstract

本发明公开一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，属于水下机器人领域。本发明包括：建立环境模型，结合海底地形特征和地图数据准确程度两部分信息，依据Sigmoid函数将标准差信息非线性映射至[0，1]空间，形成归一化的海底地形特征图；实施H‑RRT*算法进行路径求解，求解过程为迭代‑采样‑寻优的过程。路径规划方法能够充分利用已有信息，如海底地形特征和地图测量数据来源，作为影响水下机器人地形匹配导航性能的因素考虑在内；路径规划方法搜索性能强，可以实现超大地图下的路径搜索和寻优。

Description

一种考虑地图准确性的水下机器人海底地形匹配导航路径规 划方法

技术领域

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

背景技术

水下机器人地形匹配导航是一种依据地理信息进行的自主导航方式，能够消除长时间推算导航或惯性的导航累积偏差而使导航有界，并且在长航时特别是北极冰下数千公里航程的航行应用中，可以使水下机器人脱离成本高昂的声学基线定位以及冰区无法上浮进行卫星定位的局限。

依据地形匹配导航相关的贝叶斯公式，先验数字地图中的地形信息量是决定水下机器人地形匹配导航性能。地势平坦、地形特征不丰富的区域可能会导致地形匹配导航结果发散；地图不准确、成图偏差大的区域同样可能导致地形匹配导航失败。在水下机器人的超长航程航行之前，应依据已有的数字地图建立全局路径，并且应保证跟踪此路径的过程中，自主水下机器人能够依据自身传感器获取足够丰富的地理信息，确保地形匹配导航收敛和稳定，这就需要提出适用于这种背景的路径规划方法。

发明内容

本发明的目的是提供一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，通过结合数字地图的地形特征和地图数据准确程度，形成表征地形信息量的环境模型，并在此基础上运行启发式快速随机树算法(Heuristic Rapidly-exploringRandom Tree star，H-RRT*)进行路径搜索和优化，最后给出一条连接始末点、综合路径长度和地形信息量接近最优的平面路径。方法实用性强，能将海底数字地形图的地形高程信息和数据源信息考虑在内，解决单一的数字地图高程信息不能完全反映真实海底地形情况而可能带来的水下机器人地形匹配导航发散的问题；且方法搜索性能强，可以适用于超大地图的路径搜索。

本发明的目的是这样实现的：

一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，包含以下步骤：

步骤1：建立环境模型，结合两部分信息：

环境模型的一部分信息是海底地形特征，遍历数字地图中每一个栅格点，求取栅格点各自的地形特征，每个栅格点的地形特征由此点水平面一定范围内的所有栅格点的地形高程标准差表示，依据Sigmoid函数将标准差信息非线性映射至[0,1]空间，形成归一化的海底地形特征图；

环境模型的另一部分信息是地图数据准确程度，遍历地形高程数据源图的每一个栅格点，依据不同的数据源：多波束声纳、但波束声纳和Olex，确定每个栅格点地形高程数据的测量误差，依据Sigmoid函数将误差信息非线性映射至[0,1]空间，形成归一化的地图数据准确程度图；

海底地形特征图和地图数据准确程度图的结合依据贝叶斯公式：

p(x_t|u_t，x_t-1，m)＝ηp(x_t|m)p(m)p(x_t|u_t，x_t-1)

其中，p(x_t|m)表示数字地图的地形特征对地形匹配导航精度的作用，p(m)表示数字地图的准确程度，p(x_t|u_t，x_t-1)表示其他导航方法。

步骤2：实施H-RRT*算法进行路径求解，求解过程为迭代-采样-寻优的过程。

每一次迭代包含如下步骤：

步骤2-1：采样，生成引导树生长的太阳点x_p；太阳点的选区采用启发式策略，只取水深大于某参数值的位置，只取地形信息量大于某参数值的位置，只取用户指定的其它约束区域之外的位置，按照一定的百分比将太阳点确定为路径终点；

步骤2-2：选择生长节点x_g；在参数设定的欧氏距离范围内，选择已有的树节点集合中离太阳点最近的节点作为生长节点x_g；

步骤2-3：启发式生长，生成新增树节点x_new；新增树节点的计算采用启发式策略，依据生长节点和太阳节点，确定中心生长方向，并结合用户给定的生长步长，确定生长邻域；新增树节点即位于其中；

步骤2-4：为新增树节点选择父节点，形成从节点x_new到起始点的部分通路；从已有的树节点中选择一个树节点，使联通新增树节点-所选节点-所选节点的父节点…起始点这一路径的消耗值最小，路径消耗依据下式计算：

其中，β_i表示协调地形信息量和路径长度因素的权值参数；

步骤2-5：尝试为新增树节点x_new选择子节点，如果存在，则将节点x_new添加至已有的部分通路中，如果不存在，节点x_new则作为树的末端节点存在；可能存在的子节点，位于节点x_new一定的欧氏距离范围内，并且该子节点已在部分通路的路径消耗大于该子节点经x_new形成部分通路的消耗；

步骤2-6：判断是否达到迭代次数和是否建立了连接始点的路径通路，如果是则结束迭代，本次搜索成功得到近似最优路径，从终点开始按照索引父节点逆向求取的方式确定所有的路径点，并返回求得的路径点序列；如果只达到迭代次数，表示搜索失败；未达到迭代次数，那么继续迭代。

本发明的优点在于：

1.路径规划方法能够充分利用已有信息，如海底地形特征和地图测量数据来源，作为影响水下机器人地形匹配导航性能的因素考虑在内；

2.路径规划方法搜索性能强，可以实现超大地图下的路径搜索和寻优；

3.建模方法贴近实际，可使自主水下机器人有效的利用当前稀疏测量、无重复测量甚至是局部无法测量得到的海底地图。

附图说明

图1为启发式的路径规划算法流程；

图2为路径规划算法中启发式的太阳点随机搜索；

图3为路径规划算法中启发式的新增树节点局部最优搜索示意；

图4为基于本发明方法和北极数字地图给出的全局路径；

图5是本发明水下机器人海底地形匹配导航路径规划方法流程图。

具体实施方式

一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，包含考虑地图准确程度的地形信息量环境模型建立方法和超大地图下的启发式快速随机树(H-RRT*)路径寻优方法两部分；环境模型的建立需结合地形特征和数据准确程度两部分；建立环境模型时，地形特征和数据准确程度的结合方式依据自主水下机器人海底地形匹配导航性能的贝叶斯公式：p(x_t|u_t，x_t-1，m)＝ηp(x_t|m)p(m)p(x_t|u_t，x_t-1)，p(x_t|m)表示数字地图的地形特征对地形匹配导航精度的作用，p(m)表示数字地图的数据准确程度，p(x_t|u_t，x_t-1)表示其他导航方法(如航位推算导航、惯性导航)的性能；地形特征采用水平面一定范围内的地形高程标准差参数表示；地图数据的准确程度依据地图成图时不同地理位置的测量数据来源求取；依据表示所有地形高程数据测量方法的数据来源图，结合各测量方式的高程测量误差，形成测量误差图，通过Sigmoid函数映射成归一化的数据准确程度图；采用启发式的方法提升超大地图下的路径搜索和优化效率；H-RRT*算法的太阳点采样阶段，只取水深大于某参数值的位置，只取地形信息量大于某参数值的位置，按照一定的概率将路径终点确定为太阳点；在树的趋向生长阶段，通过求取局部邻域内最优值的方法确定树的新节点；通过搜索地形信息量较大值的区域点作为全局路径经过点，避免进入地形特征匮乏的平坦海底区域以及测量数据准确程度低成图误差大的区域，以足够的地形信息量保证水下机器人地形匹配导航收敛；搜索地形信息量较大值的区域点作为全局路径点，是通过如下的消耗函数，对区域点进行评价的：

x1和x2为路径任意直线段l_x的两个端点，β_i表示协调地形信息量和路径长度的权值参数。

本发明涉及一种路径规划方法，特别涉及一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法。自主水下机器人跟踪此方法得到的全局路径可提升海底地形匹配导航性能。

下面结合附图和具体实施方式对本发明作进一步详细的说明。

本实例实施于水下机器人北极冰下穿越的不间断超长时间航行应用中，在北极海底数字地形图和测量数据来源图的基础上计算考虑地形匹配导航性能的全局路径。但并不因此将本发明限制在所述的实例范围之中。

本发明包含考虑地图准确程度的地形信息量环境模型建立方法和超大地图下的启发式快速随机树(H-RRT*)路径寻优方法两部分，采用启发式的方法提升超大地图下的路径搜索和优化效率，通过搜索地形信息量较大值的区域点作为全局路径经过点，避免进入地形特征匮乏的平坦海底区域以及测量数据准确程度低成图误差大的区域，以足够的地形信息量保证水下机器人地形匹配导航收敛。

建立环境模型。环境模型的建立需综合两部分信息。

环境模型的一部分信息是海底地形特征。地形特征采用水平面一定范围内的地形高程标准差参数表示；遍历数字地图中每一个栅格点，求取栅格点各自的地形特征，每个栅格点的地形特征的由此点水平面一定范围内的所有栅格点的地形高程标准差表示，依据Sigmoid函数将标准差信息非线性映射至[0，1]空间，形成归一化的海底地形特征图。

环境模型的另一部分信息是地图数据准确程度。地图数据的准确程度依据地图成图时不同地理位置的测量数据来源求取；依据表示所有地形高程数据测量方法的数据来源图，结合各测量方式的高程测量误差，形成测量误差图，通过Sigmoid函数映射成归一化的数据准确程度图。遍历地形高程数据源图的每一个栅格点，依据不同的数据源，如多波束声纳、但波束声纳、Olex等，确定每个栅格点地形高程数据的测量误差，依据Sigmoid函数将误差信息非线性映射至[0，1]空间，形成归一化的地图数据准确程度图；

首先初始化地理信息矩阵和用户参数。

用四个大小相同的矩阵表达地理信息：LAT_2400×43200，LON_2400×43200，BATH_2400×43200，SRC_2400×43200，其中前三个组成数字高程模型(Digital Elevation Model，DEM)，第1、2、4个组成地图成图测量数据来源模型。4个矩阵对应行和列的位置分别表示海底某点的纬度、经度、海水深度，测深数据来源，矩阵LAT_2400×43200的列之间相等，行元素呈增量为-360/43200的递减等差数列且LAT(1，：)＝90，矩阵LON_2400×43200的行之间相等，列元素呈增量为360/43200的递增等差数列且LON(：，1)＝-180。

用户参数包含：建模参数和搜索参数。建模参数包含：求取地形特征时的窗口半径，地形特征归一化的Sigmoid函数参数和，数据误差数组，测量误差归一化的Sigmoid函数参数。搜索参数包含：地理北极点隔离半径，地磁北极点隔离半径，树生长步长，启发式的目标点选择概率，迭代次数，线段消耗最小值栅格临界数，新增节点父节点搜索半径，新增节点子节点搜索半径。

然后建立地形信息量环境模型，由矩阵LAT_2400×43200，LON_2400×43200，Info_2400×43200表示，有Info＝Std_Dev_I.×sigma_I，计算依据时水下机器人海底地形匹配导航的贝叶斯公式：

p(x_t|u_t，x_t-1，m)＝ηp(x_t|m)p(m)p(x_t|u_t，x_t-1)

p(x_t|m)表示数字地图的地形特征对地形匹配导航精度的作用，p(m)表示数字地图的数据准确程度，p(x_t|u_t，x_t-1)表示其他导航方法(如推算导航或惯性的导航)的性能。解释为，决定地形匹配导航性能的地形信息量，由地形标准差归一化值和地图准确性归一化值的乘积决定。

矩阵中每一栅格的地形标准差的求取依据公式：

是栅格G(r，c，)的临近栅格集合，其大小由R₀(para_M_R)决定，A_G(r，c)表示栅格G(r，c)的面积。遍历每一个栅格栅格即可得到地形标准差矩阵Std_Dev。地形标准差的归一化依据公式：Std_Dev_I＝1./1+exp)para_M_zoom·(Std_Dev-para_M_move))

北极地图的测量数据源矩阵SRC包含6类不同大小的值，0-陆地，1-多波束声纳，2-单波束，3-Olex，4-来自其它数字地图的等高线图，5-来自其他图形处理方法。海底部分，依据数据来源对应的测量误差数组para_M_src_σ_1×5，可以得到测量误差矩阵sigma_2400×43200，地图准确度矩阵sigma_I可由矩阵sigma按照公式求取：

sigma_I＝1./1+exp(para_M_zoom·(sigma-para_M_move))。

接下来实施H-RRT*算法，在上述地形信息量环境模型的基础上进行路径搜索和优化。

初始化树节点集合τ，每一个树节点x_τi作为一个结构体对象包含4个属性[RowCol，LonLat，parent，cost]_τi，分别是节点在地图矩阵中的行列索引值，实际经纬度，父节点的行列索引值，从树节点x_τi沿父节点到起始点x_start的部分通路的消耗。

路径规划的过程为迭代-采样-寻优的过程，每一次迭代包含如下步骤：

S-1，采样，生成引导树生长的太阳点x_p，如图2。太阳点的选择采用启发式策略，只取水深大于某参数值的位置，只取地形信息量大于某参数值的位置，只取用户指定的其它约束区域之外的位置，只采样地理北极点和地磁北极点一定范围之外的位置，按照一定的百分比将太阳点确定为路径终点，在树的趋向生长阶段，通过求取局部邻域内最优值的方法确定树的新节点，即：

表示地理信息量环境模型x₀的子集，x_Gp，x_Mp，x_Bl，x_Il表示水下机器人不能进入的栅格集合；x_Gp和x_Mp表示以地理北极点g_Gp和地磁北极点g_Mp为中心的栅格集合，因为考虑罗经装置的测量原理所以选择避开；χ_Bl表示水深值小于某参数的栅格集合；χ_Il表示地形信息量数值小于某参数的栅格集合。具体表达如下：

x_Gp＝{g_lon，lat∈x₀|lat＞para_LAT or ||g-g_Gp||≤para_R_Gp}，

x_Mp＝{g_lon，lat∈x₀|||g-g_Mp||≤para_R_Mp}，

x_Bl＝{g_lon，lat∈x₀|DEM(lon，lat)＞para_d_min}，

x_Il＝{g_lon，lat∈x₀|TTF(lon，lat)＜para_f_min}，

χ_Gp和χ_Mp用经纬度表示，para_LAT为纬度临界值参数，和为距离临界值参数，para_d_min和para_f_min为水深和地形信息量临界参数。

S-2，选择生长节点x_g。在参数设定的欧氏距离范围内，选择已有的树节点集合中离太阳点最近的节点作为生长节点x_g，满足x_g∈τ，有

S-3，启发式生长，生成新增树节点x_new，如图3。新增树节点的计算按启发式策略，搜索生长邻域中的局部最优值，依据生长节点和太阳节点，确定中心生长方向，并结合用户给定的生长步长，确定生长邻域，新增树节点即位于其中。生长邻域：

启发式策略：

a)在生长邻域中选择使路径的平均信息量最大的点作为临时新增树节点x′_new：平均信息量的判断依据：

x1和x2为路径直线段l_x的两个端点，I(l)为直线段l_x经过的栅格点的信息量；

b)依据几何关系将连接x′_new和x_g的线段l_x离散为栅格集合x_lx，k_lx表示集合中信息量低于某参数的栅格数量，如果k_lx超过某参数，则舍弃x′_new并依据a条件继续搜索，反之将x′_new赋值给x_new。

S-4，为新增树节点选择父节点，形成从节点x_new到起始点的部分通路。从已有的树节点中选择一个节点，使联通新增树节点-所选节点-所选节点的父节点…起始点这一路径的消耗值最小，路径消耗依据：

β_i表示协调地形信息量和路径长度影响的权值参数。

S-5，尝试为新增树节点x_new选择子节点，如果存在，则将节点x_new添加至已有的部分通路中，如果不存在，节点x_new则作为树的末端节点存在。可能存在的子节点，位于节点x_new一定的欧氏距离范围内，并且该子节点已在部分通路的路径消耗大于该子节点经x_new形成部分通路的消耗。

S-6，判断是否达到迭代次数和是否建立了连接始末端的通路，如果是则结束迭代，本次搜索成功得到近似最优路径，从终点开始按照索引父节点逆向求取的方式确定所有的路径点，并返回求得的路径点序列，如图4；如果只达到迭代次数，表示搜索失败；未达到迭代次数，那么继续迭代。

综上所述：本发明公开一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，该方法分两步。第一步建立地形信息量环境模型，环境模型的建立结合地形特征和数据准确程度，地形特征由地形高程标准差表示，数据准确程度依据不同数据测量方式导致的测量误差确定，地形特征和地图数据准确程度依据贝叶斯公式结合形成地形信息量环境模型。第二步在环境模型的基础上运行启发式的快速随机树(H-RRT*)算法，在算法的随机点生成阶段和树的引导生长阶段采用启发式的策略提升搜索效率，择优评价函数综合考虑路径长度和地形信息量，算法得到的路径是一条接近最优的全局路径。本发明的优点在于：实用性强，将地图不同位置、不同测量手段带来的成图数据准确程度问题考虑在内，使自主水下机器人可以有效的在当前稀疏测量、无重复测量甚至是局部无法测量得到的海底地图情况下保证地形匹配导航性能；搜索性能强，可以实现超大地图下的路径搜索。

Claims (3)

1.一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，其特征在于，包含以下步骤：

步骤1：建立环境模型，结合两部分信息：

环境模型的一部分信息是海底地形特征，遍历数字地图中每一个栅格点，求取栅格点各自的地形特征，每个栅格点的地形特征由此点水平面一定范围内的所有栅格点的地形高程标准差表示，依据Sigmoid函数将标准差信息非线性映射至[0，1]空间，形成归一化的海底地形特征图；

环境模型的另一部分信息是地图数据准确程度，遍历地形高程数据源图的每一个栅格点，依据不同的数据源：多波束声纳、但波束声纳和Olex，确定每个栅格点地形高程数据的测量误差，依据Sigmoid函数将误差信息非线性映射至[0，1]空间，形成归一化的地图数据准确程度图；

步骤2：实施H-RRT*算法进行路径求解，求解过程为迭代-采样-寻优的过程。

2.根据权利要求1所述的一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，其特征在于，海底地形特征图和地图数据准确程度图的结合依据贝叶斯公式：

p(x_t|u_t，x_t-1，m)＝ηp(x_t|m)p(m)p(x_t|u_t，x_t-1)

其中，p(x_t|m)表示数字地图的地形特征对地形匹配导航精度的作用，p(m)表示数字地图的准确程度，p(x_t|u_t，x_t-1)表示其他导航方法的性能。

3.根据权利要求1所述的一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法，其特征在于，每一次迭代包含如下步骤：

步骤2-1：采样，生成引导树生长的太阳点x_p；太阳点的选区采用启发式策略，只取水深大于某参数值的位置，只取地形信息量大于某参数值的位置，只取用户指定的其它约束区域之外的位置，按照一定的百分比将太阳点确定为路径终点；

步骤2-2：选择生长节点x_g；在参数设定的欧氏距离范围内，选择已有的树节点集合中离太阳点最近的节点作为生长节点x_g；

步骤2-3：启发式生长，生成新增树节点x_new；新增树节点的计算采用启发式策略，依据生长节点和太阳节点，确定中心生长方向，并结合用户给定的生长步长，确定生长邻域；新增树节点即位于其中；

步骤2-4：为新增树节点选择父节点，形成从节点x_new到起始点的部分通路；从已有的树节点中选择一个树节点，使联通新增树节点-所选节点-所选节点的父节点…起始点这一路径的消耗值最小，路径消耗依据下式计算：

其中，β_i表示协调地形信息量和路径长度因素的权值参数；

步骤2-5：尝试为新增树节点x_new选择子节点，如果存在，则将节点x_new添加至已有的部分通路中，如果不存在，节点x_new则作为树的末端节点存在；可能存在的子节点，位于节点x_new一定的欧氏距离范围内，并且该子节点已在部分通路的路径消耗大于该子节点经x_new形成部分通路的消耗；

步骤2-6：判断是否达到迭代次数和是否建立了连接始点的路径通路，如果是则结束迭代，本次搜索成功得到近似最优路径，从终点开始按照索引父节点逆向求取的方式确定所有的路径点，并返回求得的路径点序列；如果只达到迭代次数，表示搜索失败；未达到迭代次数，那么继续迭代。