CN109737970A - 一种基于改进rrt算法的水面无人艇路径规划方法 - Google Patents

Abstract

本发明涉及一种基于改进RRT算法的水面无人艇路径规划方法，提供一无人艇及设置在无人艇上的无人艇核心处理器和一GPS定位系统，具体包括以下步骤：步骤S1：获取实时地图数据，摘取地图数据实时地图信息，并输入无人艇核心处理器中；步骤S2：无人艇核心处理器对地图中执行路径规划的区域进行处理；步骤S3：根据无人艇任务或作业要求，在步骤S2中处理后的地图数据上设置作业点；并采用快速拓展随机树改进算法对路径进行实时规划；步骤S4：根据GPS定位系统和电子罗盘采集的信息，控制艇体动力系统按照预定路线航行；若路线不变则按照预定路线航行；否则返回步骤S3,重新规划路线。本发明能够减少路线长度，降低能源消耗、提升作业效率。

Description

一种基于改进RRT算法的水面无人艇路径规划方法

技术领域

本发明涉及智能无人船控制领域，特别是一种基于改进RRT算法的水面无人艇路径规划方法。

背景技术

伴随着人类社会活动日益频繁，全球能源的急剧消耗以及环境的急剧恶化，人类开始加大对江河湖海等水体资源的开发和利用，因此有关水面和海洋工程的应用与发展迅速增长。由于水体工作环境相对较为复杂，绝大多数的水上任务将借助船艇来完成。同时，大量的无人设备在人工智能、群体智能、控制科学等相关技术理论的突破下被广泛应用。而由于在浅滩、内湖内河等水体的作业环境相对较为复杂，小型无人艇应运而生。无人船艇是无人设备的一大新兴研究领域，是一种现代无人技术与多领域技术相结合的产物，具有一定的自主路径规划、自主循迹航行、自主控制调节功能等。在无人船艇的相关技术领域中，路径规划方法研究是其重要技术之一，与其智能程度息息相关。现有路径规划方法大多是应用于陆地的移动机器人和应用于空中飞行的无人机，而针对水面小型无人艇作业应用场景的路径规划方法数量很少。由于小型无人艇工作环境的特殊性和控制系系统的复杂性，小型无人艇需要在工作过程中根据卫星地图以及实际工作环境中所感知的信息实时地规划路线。

在路径规划领域中，现有的常用技术有A*算法、人工势场法、栅格法、快速拓展随机树法等。应用在小型无人艇上的路径规划方法在数目上较无人车和无人机较少，因此方法不成熟。由于应用在小型无人艇上的现有方法的自身缺陷性，在小型无人艇作业时进行路径规划时，往往表现出计算时间长、路线不是最优、易陷入局部最优点等不足，从而导致小型无人艇在执行任务时表现出续航能力降低、作业路线欠优、工作效率低下等缺点，无法达到船舶智能化的目标。

发明内容

有鉴于此，本发明的目的是提出一种基于改进RRT算法的水面无人艇路径规划方法，使无人艇在执行任务时能够显著提升运算速度，减少路线长度，降低能源消耗和提升作业效率。

本发明采用以下方案实现：一种基于改进RRT算法的水面无人艇路径规划方法，提供一无人艇及设置在无人艇上的无人艇核心处理器和一GPS定位系统，包括以下步骤：

步骤S1：获取实时地图数据，摘取所述地图数据的基本重要信息即实时地图信息，并输入所述无人艇核心处理器中；所述地图数据的基本重要信息包括地图范围及实物与地图比例尺；

步骤S2：所述无人艇核心处理器接收所述实时地图信息，并对地图中执行路径规划的区域截取出来，再把截取到的图像进行锐化和去噪处理，处理成所述无人艇核心处理器能够读取的数据类型；

步骤S3：根据无人艇任务或作业要求，在步骤S2中处理后的地图数据上设置作业点；所述无人艇核心处理器采用快速拓展随机树改进算法对路径进行实时规划；

步骤S4：由所述无人艇核心处理器规划生成路线后，根据所述GPS定位系统和电子罗盘采集的信息，控制艇体动力系统按照预定路线航行；若路线不变则按照预定路线航行；否则返回步骤S3,重新规划路线。

进一步地，所述步骤S2的具体内容为：所述无人艇核心处理器对接收到的实时地图信息根据地图中执行路径规划区域进行截取，并对截取的图像进行锐化和去噪；生成一个仅包含0/1的n维数组；其中，障碍物为1，自由区域为0；根据经纬度与障碍物和自由区域生成n维数组数据，并根据所述无人艇航行路线和地图比例尺，建立坐标系。

进一步地，所述步骤S3具体包括以下步骤：

步骤S31：获取无人艇任务或作业要求并根据无人艇任务或作业要求，依据所述建立的坐标系，设置任务需要途径的作业点坐标，并确定每一路段的起点和终点；

步骤S32：初始化改进算法，设置起始点x_start，目标点x_goal，以及步长p；令最近节点为x_near；新节点为x_new；随机采样点x_rand；初始化时将起始点x_start作为新节点x_near；

步骤S33：判断新节点x_new和目标点x_goal之间的距离是否小于设定值r，若是，则算法结束，生成路径；若否，则将新节点加入随机树tree，然后执行步骤S34；

步骤S34：在所述自由区域内生成随机采样点x_rand，遍历搜索随机树tree上距离随机采样点x_rand最近的节点x_near；

步骤S35：判断最近的节点x_near与障碍物X_obs的距离是否大于设定的距离d；若否，则执行步骤S36；若是，则将目标引力函数加入到新节点的生成中；

步骤S36：根据最近节点x_near距离障碍物X_obs的距离结果，调整步长p；

其中，p₁＝0.5*p，p₂＝(1.0～1.2)*p；

步骤S37：再次判断生成新节点x_new与目标点x_goal的距离是否小于可视范围，即预设值r；若是，则路径规划结束，生成路径；若否，则返回重复执行步骤S33。

进一步地，所述在新节点生成时，加入目标引力函数的方法如下：

θ₁＝acrtan(x_rand-X_near)

θ₂＝acrtan(x_goal-X_near)

θ＝k₁θ₁+k₂θ₂

x_new＝X_near+p*(sinθ,cosθ)

其中，θ₁为节点X_near和节点x_rand连线与坐标轴的夹角；θ₂为节点X_near和节点x_goal连线与坐标轴的夹角；k₁、k₂分别为θ₁、θ₂的权重；且k₁+k₂＝1；通过改变θ₁和θ₂所占权值，即k₁、k₂的大小，来调整新节点生成时向目标点偏移的程度。

与现有技术相比，本发明有以下有益效果：

本发明在快速拓展随机树法的基础上，通过判断新节点距离障碍物的距离，在狭窄区域减小单歩步长、减弱目标引力，在开阔地带增大单歩步长、加强目标引力，有效提升了快速拓展随机树法在狭窄路段的通过性与开阔区域的拓展速率，使无人艇在执行任务时能够显著提升运算速度，减少路线长度，降低能源消耗和提升作业效率。

附图说明

图1为本发明实施例的采用快速拓展随机树改进算法对路径进行实时规划的具体流程图。

图2为本发明实施例的步骤S2处理后所得地图信息图像。

图3为本发明实施例的模拟任务中的作业点分布图。

图4为本发明实施例的改进后方法模拟规划路线效果图。

图5为本发明实施例的快速拓展随机树法与改进后方法规划所得路线效果对比图。

图6为本发明实施例的改进前后方法路径规划所得路线长度对比柱状图。

图7为本发明实施例的总体流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

如图7所示，本实施例提供了一种基于改进RRT算法的水面无人艇路径规划方法，提供一无人艇及设置在无人艇上的无人艇核心处理器和一GPS定位系统，包括以下步骤：

步骤S1：获取实时地图数据，摘取所述地图数据的基本重要信息即实时地图信息，并输入所述无人艇核心处理器中；所述地图数据的基本重要信息包括地图范围及实物与地图比例尺；

步骤S2：所述无人艇核心处理器接收所述实时地图信息，并对地图中将要执行路径规划的区域根据地图中执行路径规划区域进行截取，并对截取的图像进行锐化和去噪；处理成所述无人艇核心处理器能够读取的数据类型；

步骤S3：根据无人艇任务或作业要求，在步骤S2中处理后的地图数据上设置作业点；所述无人艇核心处理器采用快速拓展随机树改进算法对路径进行实时规划；

步骤S4：由所述无人艇核心处理器规划生成路线后，根据所述GPS定位系统和电子罗盘采集的信息，控制艇体动力系统按照预定路线航行；如遇突发状况，再采取紧急措施，若路线不变则按照预定路线航行；否则返回步骤S3,重新规划路线。

在本实施例中，所述步骤S1具体包括以下步骤：

步骤S11：由网络获取符合要求的实时地图，对地图进行数字化解析，生成电子地图信息数据。

步骤S12：将电子地图的部分数据导入无人艇核心处理器，包含经纬度坐标及比例尺。

在本实施例中，所述步骤S2的具体内容为：所述无人艇核心处理器对接收到的实时地图信息(电子地图数据)根据地图中执行路径规划区域进行截取，并对截取的图像进行锐化和去噪；生成一个仅包含0/1的n维数组；其中，障碍物为1，自由区域为0；根据经纬度与障碍物和自由区域生成n维数组数据，并根据所述无人艇航行路线明确执行任务的区域和地图比例尺，建立合适的坐标系。

如图1所示，在本实施例中，所述步骤S3具体包括以下步骤：

步骤S31：获取无人艇任务或作业要求并根据无人艇任务或作业要求，依据所述建立的坐标系，设置执行具体任务需要途径的作业点坐标，并确定每一路段的起点和终点；

步骤S32：初始化改进算法程序，设置起始点x_start，目标点x_goal，以及步长p；令最近节点为x_near；新节点为x_new；随机采样点x_rand；初始化时将起始点x_start作为新节点x_near；

步骤S33：判断新节点x_new和目标点x_goa ^l之间的距离是否小于设定值r。r的大小视具体任务要求而定，通常情况下其值为无人艇长度。若是，则算法结束，生成路径；若否，则将新节点加入随机树tree，然后执行步骤S34；

步骤S34：在自由区域内生成随机采样点x_rand，遍历搜索随机树tree上距离随机采样点x_rand最近的节点x_near；

步骤S35：判断最近的节点x_near与障碍物X_obs的距离是否大于设定的距离d；若否，则执行步骤S36；若是，则将目标引力函数加入到新节点的生成中；

步骤S36：根据最近节点x_near距离障碍物X_obs的距离结果，动态地调整步长p；

根据多次测试，其中，p₁＝0.5*p，p₂＝(1.0～1.2)*p；是较为实用的数值比例。

步骤S37：再次判断生成新节点x_new与目标点x_goal的距离是否小于规定可视范围，即预设值r；若是，则路径规划结束，生成路径；若否，则返回重复执行步骤S33。

在本实施例中，所述在新节点生成时，加入目标引力函数的方法如下：

θ₁＝acrtan(x_rand-X_near)

θ₂＝acrtan(x_goal-X_near)

θ＝k₁θ₁+k₂θ₂

x_new＝X_near+p*(sinθ,cosθ)

其中，θ₁为节点X_near和节点x_rand连线与坐标轴的夹角；θ₂为节点X_near和节点x_goal连线与坐标轴的夹角；k₁、k₂分别为θ₁、θ₂的权重；且通常情况下，k₁+k₂＝1；通过改变θ₁和θ₂所占权值，即k₁、k₂的大小，来调整新节点生成时向目标点偏移的程度。

较佳的，在本实施例中所述步骤S4具体实现为：按照步骤S3所得路线，参考无人艇现在所在位置的经纬度坐标与电子罗盘反馈的无人艇姿态信息，控制动力系统的推进器转速与舵机角度，使无人艇能够按照所给路线循迹航行。如遇突发状况，则根据自身定位点和目标点，重复执行步骤S3，重新规划路线。

特别的，本实施例选取某一海湾水域作为实施区域，具体内容如下：

步骤S1，从网络获取实时地图，选取地图基本重要信息，输入无人艇核心处理器中。

步骤S2，无人艇核心处理器接收地图信息，按照作业要求，截取地图中合适的范围，生成一组包含0和1的数组，并确定坐标系，代表地图数据，便于处理器进行后续处理。将自由区域设置为0，障碍物区域设置为1，如图2所示。无人艇核心处理器将在自由区域内部规划出全部所需路径。

步骤S3，按照步骤S2确定的坐标系及地图数据结合作业要求，由作业点经纬度推出作业点坐标，如图3，共设立6个模拟作业点。模拟作业点设置完毕后，开始进行路线规划。

步骤S301，根据无人艇任务或作业要求，结合步骤S2建立的坐标系，设置执行仿真任务需要经过的作业点，转化为图中坐标，并确定每一段路径的起点和终点以及其坐标。

步骤S302，初始化改进算法程序，设置作业点1作为起始点x_start，作业点2作为目标点x_goal，以及步长p。定义变量最近节点x_near，新节点x_new，随机采样点x_rand。初始状态将起始点x_start作为新节点x_new。待作业点1到作业点2的路径规划完成后，再按照作业点顺序依次进行下一路段的规划。

步骤S303,判断新节点x_new和目标点x_goal之间的距离是否小于设定值r。此处r的值为初始步长p。若是，则算法结束，生成路径。若否，则将新节点加入随机树tree，然后执行下一步骤。

步骤S304,在自由区域内生成随机采样点x_rand，搜索随机数tree上距离随机采样点x_rand最近的节点x_near。

步骤S305，判断节点x_near距离障碍物的距离是否大于规定距离d。若否，执行下一步骤；若是，则将目标引力函数加入到新节点的生成中。在新节点生成时，加入目标引力函数的方法如下：

θ₁＝acrtan(x_rand-X_near)

θ₂＝acrtan(x_goal-X_near)

θ＝k₁θ₁+k₂θ₂

x_new＝X_near+p*(sinθ,cosθ)

其中，θ₁为节点X_near和节点x_rand连线与坐标轴的夹角；θ₂为节点X_near和节点x_goal连线与坐标轴的夹角；k₁、k₂分别为θ₁、θ₂的权重；且k₁+k₂＝1；通过改变θ₁和θ₂所占权值，即k₁、k₂的大小，来调整新节点生成时向目标点偏移的程度。

步骤S306，调整动态步长p。根据判断节点x_rand距离障碍物的距离结果，动态地调整步长p。

根据多次测试，p₁＝0.5*p，p₂＝(1.0～1.2)*p是较为实用的数值比例。

步骤S307，生成新节点x_new，并且再次判断新节点x_new与障碍物的距离是否小于预设值r。若是，则路径规划结束，生成路径；若否，则返回执行步骤S303。

步骤S3执行结束后，得到路径如图4，再执行步骤S4。将生成路线数据反馈给处理器，处理器结合GPS定位系统和电子罗盘采集的信息，控制船体动力系统按照预定路线航行。如遇突发状况，再采取紧急措施，返回步骤S3,重新规划路线。

经改进后的快速拓展随机树方法与未经改进的快速拓展随机树法进行的路径规划最终规划路线比较结果如图5。

表1

表2

分析数据如下，每段路径长度比较如图6和表1，每段路径计算时间如表2。由对比试验结果可以看出，改进的方法规划出的路线航程更短、路线更优，且运算时间更短。

以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (4)

1.一种基于改进RRT算法的水面无人艇路径规划方法，提供一无人艇及设置在无人艇上的无人艇核心处理器和一GPS定位系统，其特征在于：包括以下步骤：

步骤S1：获取实时地图数据，摘取所述地图数据的基本重要信息即实时地图信息，并输入所述无人艇核心处理器中；所述地图数据的基本重要信息包括地图范围及实物与地图比例尺；

步骤S2：所述无人艇核心处理器接收所述实时地图信息，并对地图中执行路径规划的区域截取出来，再把截取到的图像进行锐化和去噪处理，处理成所述无人艇核心处理器能够读取的数据类型；

步骤S3：根据无人艇任务或作业要求，在步骤S2中处理后的地图数据上设置作业点；所述无人艇核心处理器采用快速拓展随机树改进算法对路径进行实时规划；

步骤S4：由所述无人艇核心处理器规划生成路线后，根据所述GPS定位系统和电子罗盘采集的信息，控制艇体动力系统按照预定路线航行；若路线不变则按照预定路线航行；否则返回步骤S3,重新规划路线。

2.根据权利要求1所述的一种基于改进RRT算法的水面无人艇路径规划方法，其特征在于：所述步骤S2的具体内容为：所述无人艇核心处理器对接收到的实时地图信息根据地图中执行路径规划区域进行截取，并对截取的图像进行锐化和去噪；生成一个仅包含0/1的n维数组；其中，障碍物为1，自由区域为0；根据经纬度与障碍物和自由区域生成n维数组数据，并根据所述无人艇航行路线和地图比例尺，建立坐标系。

3.根据权利要求2所述的一种基于改进RRT算法的水面无人艇路径规划方法，其特征在于：所述步骤S3具体包括以下步骤：

步骤S31：获取无人艇任务或作业要求并根据无人艇任务或作业要求，依据所述建立的坐标系，设置任务需要途径的作业点坐标，并确定每一路段的起点和终点；

步骤S32：初始化改进算法，设置起始点x_start，目标点x_goal，以及步长p；令最近节点为x_near；新节点为x_new；随机采样点x_rand；初始化时将起始点x_start作为新节点x_near；

步骤S33：判断新节点x_new和目标点x_goal之间的距离是否小于设定值r。若是，则认为到达目标点算法结束，生成路径；若否，则将新节点x_new加入随机树tree，然后执行步骤S34；

步骤S34：在所述自由区域内生成随机采样点x_rand，遍历搜索随机树tree上距离随机采样点x_rand最近的节点x_near；

步骤S35：判断最近的节点x_near与障碍物X_obs的距离是否大于设定的距离d；若否，则执行步骤S36；若是，则将目标引力函数加入到新节点的生成中；

步骤S36：根据最近节点x_near距离障碍物X_obs的距离结果，调整步长p；

其中，p₁＝0.5*p，p₂＝(1.0～1.2)*p；

步骤S37：再次判断生成新节点x_new与目标点x_goal的距离是否小于可视范围，即预设值r；若是，则路径规划结束，生成路径；若否，则返回重复执行步骤S33。

4.根据权利要求4所述的一种基于改进RRT算法的水面无人艇路径规划方法，其特征在于：所述在新节点生成时，加入目标引力函数的方法如下：

θ₁＝acrtan(x_rand-X_near)

θ₂＝acrtan(x_goal-X_near)

θ＝k₁θ₁+k₂θ₂

x_new＝X_near+p*(sinθ,cosθ)

其中，θ₁为节点X_near和节点x_rand连线与坐标轴的夹角；θ₂为节点X_near和节点x_goal连线与坐标轴的夹角；k₁、k₂分别为θ₁、θ₂的权重；且k₁+k₂＝1；通过改变θ₁和θ₂所占权值，即k₁、k₂的大小，来调整新节点生成时向目标点偏移的程度。