一种改进的基于快速随机搜索树的船舶路径规划方法

本发明属于路径规划，具体涉及一种船舶路径规划方法。

背景技术：

1、海上运输在国际贸易中起着不可或缺的作用。它具有运输量大、成本低的优点。然而，由于船员判断失误等原因所导致的海上事故造成的损失往往是巨大的，如生命、经济损失和环境污染等。船舶路径规划的研究被认为是解决这一问题的有效途径之一，有助于减少人为因素造成的海上交通事故数量。

2、对于路径规划，以快速随机搜索树(rrt)为代表的规划方法在实际规划中取得了大量的成果。rrt算法所规划路径是基于树状生长和随机采样，可快速的探索地图空间，适用于求解动态路径规划问题。其次，基于随机抽样的rrt算法不需要搜索空间中的所有点，也不需要精确地构建障碍物空间。并且rrt路径树的生长方式为随机采样生长，虽然对于同一任务其运行结果不具备可重复性，但是其规划结果的随机性对船舶路径规划的所需要寻找的可行路径影响不大。因此，基于rrt的算法已广泛应用于机器人手臂路径规划、无人机(uav)运动规划等机器人运动规划问题。

3、然而，由于rrt算法生长节点的随机性，在路径规划过程中会产生无效节点，影响路径规划的效率。并且rrt算法节点之间的直线连接使得最终所得路径一般是曲折不平滑的，与实际船舶航行路径差距较大。

4、综上所述，针对rrt算法中存在的缺点，研究适合复杂海域环境的船舶路径规划方法具有重要的研究意义，即增强该方法的目的趋向性，进而减少方法规划过程中的生成的无效节点，同时该方法最终生成的规划路径可以满足船舶在复杂海域环境下的需求。

技术实现思路

1、为了克服现有技术的不足，本发明提供了一种改进的基于快速随机搜索树的船舶路径规划方法，首先引入以目的地中心的引力势场概念，通过随机扩展节点的方向与引力势场方向叠加，确定新的节点扩展方向，以对rrt算法中随机树的扩展方向加以约束，保证树的扩展方向始终向目的地靠拢，进而减少树生长过程中的无效分支；并且在路径树的扩展过程中结合船舶周围障碍物情况，从而可以更快的到达目标点。此外，本方法采用三角形内切圆方法对路径拐点进行平滑处理，使得最终生成的路径更适合实际应用。最后采用相同情况下的模拟数据进行验证了该方法的有效性和一定的先进性，非常适合于动态场景下的路径实时规划，尤其适合航海领域的船舶路径规划。

2、本发明解决其技术问题所采用的技术方案包括如下步骤：

3、步骤1：选取海域及船舶数据，进行区域障碍物环境建模；

4、步骤1-1：获取海域图像数据，并进行二值化处理；

5、步骤1-2：二值化处理后的海域图像表示为f＝{fij}，fij为像素点(i，j)的灰度值；使用光栅扫描法扫描图像f，当扫描到像素点(i，j)的灰度值fij≠0时对fij的连通域进行轮廓提取，最终输出海域障碍物图像轮廓点集；

6、步骤1-3：使用最小二乘法圆拟合进行海域障碍物填充，建立最小二乘法拟合圆曲线r2＝(x-a)2+(y-b)2，其中r为拟合圆半径，(a，b)为拟合圆圆心坐标，x、y分别为海域障碍物所对应的最小外接圆上各点坐标；

7、步骤1-4：针对步骤1-2中所获取到的轮廓点集(xi，yi)，i∈{1，2，3，......，n}，各障碍物轮廓对应最小内切圆圆心坐标为(ai，bi)，圆半径为ri，xi，yi到圆心的距离为di，n表示该区域障碍物数量；

8、令当最小时求解ai、bi、ci，则拟合圆参数为

9、步骤2：完成节点、单次扩展步长和路径树初始化；

10、将船舶起始点作为初始点qinit，单次扩展步长的初始化值与船舶速度相等，路径树初始化为

11、步骤3：进行船舶-障碍物碰撞检测并依据引力势场更新扩展方向；

12、步骤3-1：将扩展点对目标点的趋向性定义为引力势场，在路径树扩展方向上，采用引力势场和随机方向叠加后的生长方向；引力势场的方向定义为由生长点指向目标点，引力势场的大小其中为引力增益常量，d2(q，qgoal)为扩展点q和目标点qgoal的距离的平方，s为距离生长点最近的障碍物面积，d*为障碍物影响范围；

13、步骤3-2：在以单次扩展步长为半径的范围内，进行障碍物存在判断，若不存在则选择目标点作为随机树生长方向，对于新节点扩展方向的计算由公式给出；若存在障碍物，则选择结合引力势场的随机生长方式，对于新节点扩展方向的计算由公式给出，t为单次扩展步长；

14、步骤3-3：依据步骤3-2的扩展方向选取随机节点qrand；

15、步骤4：进行随机点选取和路径树填充；

16、步骤4-1：遍历随机树中的每一个节点，计算每一个节点与该循环生成的随机点之间的距离，找出距离此随机点最近的节点，记为qnear；

17、步骤4-2：依据步骤2中的单次扩展步长，当找到qnear时，qnear向qrand方向扩展该步长，扩展后产生新的节点qnew；对qnew和障碍物进行碰撞检测，若出现碰撞，则重新产生新的随机点；若不发生碰撞则将(qnear，qnew)加入路径树；

18、步骤5：进行到达检测，判断规划方法是否继续；

19、计算当前点和目标点之间的欧氏距离，若该距离小于单次扩展步长则结束路径规划，输出规划路径集合p；若该距离大于单次扩展步长，则重复步骤3和步骤4；

20、步骤6：获取规划后路径，进行路径平滑化；

21、步骤6-1：对规划路径集合pi∈p，i∈{1，2，3，......，n}，并将路径集合进行划分pk＝{pk，pk+1，pk+2}；

22、步骤6-2：计算pk，pk+1和pk+1，pk+2所构成线段的斜率，若斜率相等，则不进行轨迹平滑；若斜率不相等，则转到步骤6-3；

23、步骤6-3：计算pk对应三点所组成的三角形内切圆圆心l1，l2，l3为三点组成三角形的边长；将对应路径段的轨迹替换为内切圆圆弧，完成路径平滑。

24、优选地，所述二值化处理具体为：

25、根据函数对图像进行二值化处理，图像中灰度值src(x，y)大于阈值thresh的点，将其灰度值设定为255，灰度值小于或等于阈值的点，将其灰度值设定为0。

26、本发明的有益效果如下：

27、本发明不仅适用于海洋障碍物复杂的环境，并且通过引导节点在生长过程中尽可能的朝向目标点，解决了传统快速随机搜索树算法存在的无效节点数量多，存储成本高的问题，在此之上减少了路径规划所需的时间。本发明实现了对规划后路径的平滑化处理，解决了传统快速随机搜索树算法所规划路径曲折不平滑的缺点。本发明非常适合于动态场景下的路径实时规划，尤其适合航海领域的船舶路径规划。

技术特征：

1.一种改进的基于快速随机搜索树的船舶路径规划方法，其特征在于，包括如下步骤：

2.根据权利要求1所述的一种改进的基于快速随机搜索树的船舶路径规划方法，其特征在于，所述二值化处理具体为：

技术总结
本发明公开了一种改进的基于快速随机搜索树的船舶路径规划方法，首先引入以目的地中心的引力势场概念，通过随机扩展节点的方向与引力势场方向叠加，确定新的节点扩展方向，以对RRT算法中随机树的扩展方向加以约束，保证树的扩展方向始终向目的地靠拢，进而减少树生长过程中的无效分支；并且在路径树的扩展过程中结合船舶周围障碍物情况，从而可以更快的到达目标点。此外，本方法采用三角形内切圆方法对路径拐点进行平滑处理，使得最终生成的路径更适合实际应用。最后采用相同情况下的模拟数据进行验证了该方法的有效性和一定的先进性，非常适合于动态场景下的路径实时规划，尤其适合航海领域的船舶路径规划。

技术研发人员：朱怡安,苏将,张黎翔,李佳钰,李联
受保护的技术使用者：西北工业大学
技术研发日：
技术公布日：2024/1/13