CN114967696A - 一种无人艇全局路径规划方法 - Google Patents

一种无人艇全局路径规划方法 Download PDF

Info

Publication number
CN114967696A
CN114967696A CN202210607310.4A CN202210607310A CN114967696A CN 114967696 A CN114967696 A CN 114967696A CN 202210607310 A CN202210607310 A CN 202210607310A CN 114967696 A CN114967696 A CN 114967696A
Authority
CN
China
Prior art keywords
path
node
cost
angle
expansion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210607310.4A
Other languages
English (en)
Other versions
CN114967696B (zh
Inventor
徐鹏飞
曹清波
杜雨昂
王子鹏
丁延旭
常喆
杨灝辰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hohai University HHU
Original Assignee
Hohai University HHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hohai University HHU filed Critical Hohai University HHU
Priority to CN202210607310.4A priority Critical patent/CN114967696B/zh
Publication of CN114967696A publication Critical patent/CN114967696A/zh
Application granted granted Critical
Publication of CN114967696B publication Critical patent/CN114967696B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及无人艇技术领域，公开了一种无人艇全局路径规划方法，基于APF算法的思想，在Lazy Theta*算法的代价函数中引入局部障碍斥力项和偏流角度、增加路径安全距离检查和偏流角度检查的视线检查以及对路径折角进行平滑优化处理。本发明的有益效果为能够避开障碍较多的危险区域，选择障碍较少的区域进行路径规划，降低了发生触礁等危险事故的可能性，加强避障能力，提高了航行的安全性。

Description

一种无人艇全局路径规划方法
技术领域
本发明涉及无人艇技术领域，涉及一种无人艇全局路径规划方法，具体涉及一种基于环境优化Lazy Theta*算法的无人艇全局路径规划方法。
背景技术
无人艇(unmanned surface vehicle,USV)是指依靠船载传感器，以自主或半自主方式在水面航行的智能化平台，在民用和军事领域都取得了广泛的应用。但由于无人艇属于欠驱动系统，易受到环境中风浪流等因素的影响，难以精确控制，且无人艇面对障碍较多的危险区域时，又大大增加了其跟踪难度，因此无人艇路径规划技术则成为无人艇实现运动、作业功能的根本保证。
目前，应用最广且最具有影响力的算法为A*算法，但由于其领域栅格的方向限制，路径不能达到最优解。随着制导技术的不断完善，能够实现任意角度搜索的Theta*算法应运而生；而后又针对其搜索效率较低这一问题不断改进，进而提出Lazy Theta*算法，大大减少了视线检查数目，提高了搜索效率。Lazy Theta*算法凭借其优异的搜索性能，在全局路径规划领域取得了越来越多的应用。
但对于低速无人艇来说，水流对航行的影响最大。当路径转角过多或偏流角度过大时，往往难以保证对路径的准确跟踪，造成持续路径偏航，影响作业性能。此外，相较于陆域或空域，水域环境更为复杂，水面和水下都存在障碍物，且水下障碍更加隐蔽，故无人艇所处的水域环境更为复杂，除水面上的岛屿、沙洲、建筑等可见障碍物，水下还有浅滩、礁石、急流、漩涡等隐蔽危险物。这些问题都使得无人艇在航行、作业过程中发生碰撞，造成破坏。因此，无人艇在水域环境航行的安全性和环境适应性仍是值得研究的问题。
发明内容
针对现有技术中常见启发式算法如A*、Lazy Theta*算法的代价评估函数只考虑路径的移动距离，而忽视了环境对无人艇路径安全性的影响，本发明提供了一种基于环境优化Lazy Theta*算法的无人艇全局路径规划方法，能够避开障碍较多的危险区域，选择障碍较少的区域进行路径规划，降低了发生触礁等危险事故的可能性，加强避障能力，提高了航行的安全性。
为实现上述目的，本发明提供如下技术方案：
一种无人艇全局路径规划方法，基于代价评估函数，引入局部障碍斥力项和偏流角度得到优化代价函数，计算各个拓展点的代价值，选取代价值最低的拓展点作为路径的下一节点，包括以下步骤：
步骤一：设置了2个数据集合：open list和closed list；初始化open list和closed list，将起点S放入open list中；
步骤二：对当前节点x和其父节点p(x)进行视线检查，从而判断视线是否存在；
步骤三：若所述视线不存在，将当前节点x放入closed list，则选择领域closedlist中g(x′)+c(x,x')最小的点为当前节点x的父节点；x'表示当前节点x的拓展点，g(x′)+c(x,x')表示起始点到拓展点的路径代价加当前节点到拓展点的路径代价；
步骤四：若所述视线存在，遍历所述当前节点x的领域，计算领域拓展节点x'的g(x')，g(x')表示当前节点x的拓展点的g值，即表示起始点到该拓展点的路径代价；
步骤五：重复所述步骤四直到算法结束，根据每个节点间的父子关系形成一条规划路径。
进一步地，所述步骤一中局部障碍斥力的计算公式为：
Figure BDA0003671853940000031
偏流角度的计算公式为：
Figure BDA0003671853940000032
Δθ(x)＝θyaw(x)-θstream(x)
其中，θ(x)表示偏流角度项代价，Δθ(x)表示当前位置路径方向与水流方向夹角，θyaw(x)表示当前位置路径方向角，θstream(x)表示当前位置水流流向角；ρ(x,xobs)表示无人艇所在位置与障碍物之间的距离；ρ0表示障碍物影响距离；xobs障碍物位置；x表示当前节点。
进一步地，所述优化代价函数为：
Figure BDA0003671853940000033
其中，c(x,p(x))表示当前节点到其父节点的欧式距离，
Figure BDA0003671853940000034
为局部障碍斥力项，表示一定范围内障碍物对当前节点的斥力之和，krep为局部障碍斥力项系数，n表示一定范围内障碍物的数量；kstreamθ(x)为偏流角度项，反应路径与水流夹角大小，kstream为偏流角度系数；p(x)表示当前节点的父节点；g(p(x))表示起始点到当前节点的父节点的路径代价。
进一步地，所述步骤二中对当前节点x和其父节点p(x)进行视线检查的方法为采用路径安全距离检查和偏流角度检查；所述路径安全距离检查为检查无人艇距障碍物的距离是否大于路径安全距离，路径安全距离是针对船吃水深度，如果是仿真可以设定固定值。
进一步地，所述步骤二中偏流角度检查为检查无人艇路径与水流流向的夹角是否小于路径允许偏流角度，路径允许偏流角度是根据船舶性能得到，为一个设定的固定值。
进一步地，所述步骤二中判断视线存在的条件为：无人艇距障碍物的距离始终大于路径安全距离且路径与水流流向的夹角始终小于路径允许偏流角度。
进一步地，所述步骤四的计算包括以下情况：
如果open list为空表示无路可走，算法结束；
若拓展节点为终点，从终点追踪父节点到达起点形成路径，算法完成；
若拓展节点为障碍物或是closed list中的节点则忽略该节点；
若拓展节点是open list中的点，判断g(x')是否比旧值更小，如果是，更新该拓展节点f(x')值，f(x')表示预估的总代价，包括转角代价和路径距离代价；
若当前节点不是以上任何情况，判断g(p(x))+c(p(x),x')是否小于g(x')，g(p(x))+c(p(x),x')表示起始点到当前节点的父节点的路径代价加当前节点的父节点到拓展点的路径代价；如果是则将该拓展点加入open list并更新拓展点g、f值，g表示起始点到该拓展点的路径代价；f为预估的总代价，记拓展节点x'的父节点为p(x)。
进一步地，还包括步骤六：使用圆弧过度的方法对规划路径进行折角平滑优化处理。
进一步地，所述步骤八中对规划路径进行折角平滑优化处理的方法为：在路径折角处取相邻三点，根据几何关系求出圆弧圆心，将折线转换为圆弧。
进一步地，折角平滑优化处理的具体方法为：已知线段三点P1、P2和P3，以及圆弧半径R，求优化之后线段新端点P4、P5和圆弧圆心R(xR,yR)；根据几何关系可得出P1、P2、P3与P4、P5及圆弧圆心R(xR,yR)之间的转化关系为：
Figure BDA0003671853940000051
Figure BDA0003671853940000052
Figure BDA0003671853940000053
Figure BDA0003671853940000054
Figure BDA0003671853940000055
Figure BDA0003671853940000056
Figure BDA0003671853940000057
Figure BDA0003671853940000058
其中，β表示未加路径优化的转向角；d21表示P1与P2之间的距离；d23表示P2与P3之间的距离；
Figure BDA0003671853940000059
表示P2到P1的向量；
Figure BDA00036718539400000510
表示P3到P2的向量；atan表示方位角的四象限反正切函数。
与现有技术相比，本发明提供了一种无人艇全局路径规划方法，具备以下有益效果：
(1)本发明的路径规划方法，基于APF算法的思想，在Lazy Theta*算法的代价函数中引入局部斥力项，从而让无人艇与障碍物的距离保持在路径安全距离之外，能够避开障碍较多的危险区域，选择障碍较少的区域进行路径规划，降低了发生触礁等危险事故的可能性，加强避障能力，提高了航行的安全性。
(2)本发明的路径规划方法在代价函数中进一步引入偏流角度项，通过令规划路径偏流角度始终小于允许偏流角度，有效降低了路径平均偏流角度，无人艇能够尽可能地顺应水流流向，避免持续路径偏航和更多的能源消耗，降低水流对航行的影响，从而提高了无人艇航行性能。
(3)本发明的路径规划方法能有效避开障碍较多的区域，降低路径平均偏转角度，具有更少的路径点和更小的路径转角。同时使用圆弧过度的方法对路径进行折角平滑处理，使得各段路径能够平滑过渡，规划出的路径更加平滑，减小了无人艇路径跟踪难度，有利于无人艇在复杂水域环境中航行。
(4)本发明的路径规划方法综合考虑环境障碍、水流、视线检查以及折角对路径规划和航行的影响，引入了局部障碍斥力项和偏流角度的代价函数、增加了路径安全距离检查和偏流角度检查的视线检查以及对路径折角进行平滑优化，不断优化算法，达到能够避开障碍较多的危险区域，且保证路径平滑安全，从而大幅度提高航行的安全性。
附图说明
图1为本发明的无人艇全局路径规划方法的流程图；
图2为现有技术中无路径安全距离检查规划路径结果图；
图3为引入本发明的路径安全距离检查规划路径结果图；
图4为现有技术中无偏流角度检查规划路径结果图；
图5为引入本发明的偏流角度检查规划路径结果图；
图6为本发明中路径折角平滑优化原理图；
图7为现有技术中未经折角平滑优化路径结果图；
图8为经本发明折角平滑优化后路径结果图；
图9为现有技术中A*算法的搜索方式；
图10为本发明中Lazy Theta*算法搜索方式。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的路径规划方法基于的整个系统由无人艇以及所搭载的设备构成，具体需搭载的设备如下：除了保证无人艇正常航行的设备外，本发明要求无人艇搭载有：GPS定位系统，无线通讯天线、惯性导航、摄像头、探测仪模块等。通过惯性导航系统和GPS来获取定位信息，通过探测仪获取流速信。
由于无人艇属于欠驱动系统，难以精确控制，在受到水域风浪流等环境因素影响后，需要消耗更多能量以进行路径跟踪。本发明主要考虑规划路径与流向的夹角，即偏流角度的大小来反映水流对无人艇的影响。当路径处于顺流方向时，无人艇路径跟踪难度最低，能源消耗最少。当路径处于顶流方向时，能源消耗增加，但路径跟踪难度不大。当路径处于侧流方向时，无人艇不但要消耗更多能源，而且水流会使得无人艇偏离原有航向，提高跟踪难度。因此，路径规划遵循顺流航行最佳，顶流其次，侧流最差的原则。
如图1至图10所示，灰色表示边界或者陆地，是不可行驶的地方；图中的箭头表示水流流向；图中S表示初始起点，D表示终点。
本发明的路径规划方法基于环境优化Lazy Theta*算法，其原理如下：
Lazy Theta*算法是一种基于A*算法的启发式算法，它不要求拓展节点的父节点必须是其相邻的节点，因此路径行进方向不限制在领域栅格内，允许以任意角度改变路径的方向。算法通过代价评估函数规划出最优路径，开始从起点开始向周边进行扩展，通过代价评估函数计算各个拓展点的代价值，选取代价值最低的拓展点作为路径的下一节点。下一节点对父节点进行视线法(Line of Sight,LOS)检测，如果存在视线则删除中间节点，重复此过程直至达到目标点，生成最终路径。Lazy Theta*算法的代价评估函数f(x)如下：
f(x)＝h(x)+g(x)
式中，f(x)是在当前位置x的总代价，g(x)表示从初始起点S到当前节点x的路径代价,h(x)表示从当前节点x到终点D的路径代价估计值，当移动节点允许上、下、左、右、左上、右上、左下、右下八个方向移动，可以使用切比雪夫距离公式，上、下、左、右的移动代价为1，而左上、右上、左下、右下移动代价为斜对角距离为
Figure BDA0003671853940000081
Figure BDA0003671853940000082
本发明的路径规划方法基于环境优化Lazy Theta*算法，其步骤如下：
基于代价评估函数，引入局部障碍斥力项和偏流角度得到优化代价函数，计算各个拓展点的代价值，选取代价值最低的拓展点作为路径的下一节点。
局部障碍斥力的计算公式为：
Figure BDA0003671853940000083
偏流角度的计算公式为：
Figure BDA0003671853940000084
Δθ(x)＝θyaw(x)-θstream(x)
其中，θ(x)表示偏流角度项代价，Δθ(x)表示当前位置路径方向与水流方向夹角，θyaw(x)表示当前位置路径方向角，θstream(x)表示当前位置水流流向角；ρ(x,xobs)表示无人艇所在位置与障碍物之间的距离；ρ0表示障碍物影响距离；xobs障碍物位置；x表示当前节点(也即当前位置)。
优化代价函数为：
Figure BDA0003671853940000091
其中，c(x,p(x))表示当前节点到其父节点的欧式距离，
Figure BDA0003671853940000092
为局部障碍斥力项，表示一定范围内(一定范围即ρ0范围内)障碍物对当前节点的斥力之和，krep为局部障碍斥力项系数，n表示一定范围内障碍物的数量；kstreamθ(x)为偏流角度项，反应路径与水流夹角大小，kstream为偏流角度系数；p(x)表示当前节点的父节点；g(p(x))表示起始点到当前节点的父节点的路径代价。
步骤一：设置了2个数据集合：open list和closed list，open list用于放置已拓展待考察的节点，；初始化open list和closed list，将起点S放入open list中。
步骤二：引入路径安全距离检查和偏流角度检查，对当前节点x和其父节点p(x)进行视线检查，从而判断视线是否存在；
在本实施例的一种具体实施方式中，步骤二中路径安全距离检查为检查无人艇距障碍物的距离是否大于路径安全距离。
路径安全距离检查的基本思路是扩大视线检查范围，将Lazy Theta*算法的视线检查范围由连接当前节点和父节点的直线扩大为以该直线为中心线，以路径安全距离为半径的检查带。当路径周围安全距离范围内存在障碍物时则判断两点不存在视线，使得障碍物与路径的距离始终大于路径安全距离，确保无人艇航行的安全性。
设置10×20的栅格地图，地图内布置交错障碍物，设路径安全距离为0.5栅格，引入路径安全距离检查后的规划路径结果对比如图2和图3所示，由图2和图3可见，无路径安全距离检查的规划路径从交错障碍中穿过，造成了较多转折。引入路径安全距离检查的规划路径绕开了交错障碍，距障碍物的距离始终大于路径安全距离。
在本实施例的一种具体实施方式中，步骤二中偏流角度检查为检查无人艇路径与水流流向的夹角是否小于路径允许偏流角度。
偏流角度检查的基本思路是计算当前节点和父节点的路径与途径水流流向的夹角，若夹角大于路径允许偏流角度，则判断当前节点不通过检查。使得路径与水流流向的夹角始终小于路径允许偏流角度，让无人艇尽可能顺应水流流向航行，降低路径跟踪难度。
设置10×20的栅格地图，地图内设置随地形变化的水流流向，设路径允许偏流角度为30°，引入偏流角度后的规划路径结果对比如图3和图5所示，由图4和图5可见，无偏流角度检查的规划路径在前期始终存在较大的偏流角度，而引入偏流角度检查的规划路径则始终保持较小的偏流角度，能够顺应水流方向航行。
在本实施例的一种具体实施方式中，由于Lazy Theta*算法的视线检查仅检查当前节点与父节点的之间是否存在障碍，当通过视线检查后将删除中间节点，可能造成优化代价函数中综合考虑局部障碍与水流流向影响的规划结果被忽略的问题，导致最终规划出的路径距离障碍物过近，或路径与水流流向夹角过大。针对这一问题，本文在通过引入路径安全距离检查和偏流角度检查进行优化。只有当前节点同时满足路径安全距离检查和偏流角度检查，才认为当前节点和父节点具有视线，故，步骤四中判断视线存在的条件为：无人艇距障碍物的距离始终大于路径安全距离且路径与水流流向的夹角始终小于路径允许偏流角度。
步骤三：若所述视线不存在，将当前节点x放入closed list，则选择领域closedlist中g(x′)+c(x,x')最小的点为当前节点x的父节点；x'表示当前节点x的拓展点，g(x′)+c(x,x')表示起始点到拓展点的路径代价加当前节点到拓展点的路径代价。
步骤四：若所述视线存在，遍历所述当前节点x的领域，计算领域拓展节点x'的g(x')，g(x')表示当前节点x的拓展点的g值，即表示起始点到该拓展点的路径代价；
步骤四的计算包括以下情况：
如果open list为空表示无路可走，算法结束；
若拓展节点为终点D，从终点追踪父节点到达起点形成路径，算法完成；
若拓展节点为障碍物或是closed list中的节点则忽略该节点；
若拓展节点是open list中的点，判断g(x')是否比旧值更小，如果是，更新该拓展节点f(x')值，f(x')表示预估的总代价，包括转角代价和路径距离代价；
若当前节点不是以上任何情况，判断g(p(x))+c(p(x),x')是否小于g(x')，g(p(x))+c(p(x),x')表示起始点到当前节点的父节点的路径代价加当前节点的父节点到拓展点的路径代价；如果是则将该拓展点加入open list并更新拓展点g、f值，g表示起始点到该拓展点的路径代价；f为预估的总代价，记拓展节点x'的父节点为p(x)。
步骤五：重复步骤四直到算法结束，根据每个节点间的父子关系形成一条规划路径。
步骤六：使用圆弧过度的方法对规划路径进行折角平滑优化处理。
经过代价函数优化和视线检查优化后的路径能够充分考虑环境中障碍物和水流的影响，但是路径始终存在折角，不利于无人艇进行路径跟踪。因此，本发明使用圆弧过度的方法对路径进行折角平滑处理，使得各段路径能够平滑过渡，减小路径跟踪的难度，圆弧半径根据无人艇运动特性设定。
在本实施例的一种具体实施方式中，对规划路径进行折角平滑优化处理的方法为：在路径折角处取相邻三点，根据几何关系求出圆弧圆心，将折线转换为圆弧。
在本实施例的一种具体实施方式中，折角平滑优化处理的具体方法为：如图6所示，已知线段三点P1、P2和P3，以及圆弧半径R，求优化之后线段新端点P4、P5和圆弧圆心R(xR,yR)；根据几何关系可得出P1、P2、P3与P4、P5及圆弧圆心R(xR,yR)之间的转化关系为：
Figure BDA0003671853940000121
Figure BDA0003671853940000122
Figure BDA0003671853940000123
Figure BDA0003671853940000124
Figure BDA0003671853940000125
Figure BDA0003671853940000126
Figure BDA0003671853940000127
Figure BDA0003671853940000128
其中，β表示原来未加路径优化的转向角，通过β和R能够得到图6中P4与P5的坐标；d21表示P1与P2之间的距离；d23表示P2与P3之间的距离；
Figure BDA0003671853940000129
表示P2到P1的向量；
Figure BDA00036718539400001210
表示P3到P2的向量；atan表示方位角的四象限反正切函数。
设置15×20的栅格地图，地图内设置横向交错障碍，设圆弧半径R＝1，经过折角平滑优化后的规划路径对比如图7和图8所示，折角平滑优化后的路径在折角处都进行了圆弧过渡处理，且圆弧都能完美衔接原有路径，使得路径更加平滑。
相比于A*算法，Lazy Theta*算法突破了领域栅格的方向限制，通过视线法制导，使得路径转角和路径点个数都大大减少，路径更为平滑，更适合无人艇使用，从图9和图10相对比，可知图10更适合无人艇使用。
基于APF算法的思想，本发明在Lazy Theta*算法的代价评估函数中引入局部障碍斥力项，使得一定范围内的障碍物可以对无人艇产生斥力影响，从而让无人艇尽可能地避开障碍物。在栅格地图中，无人艇所受局部障碍斥力不需要进行矢量合成，也不需要额外的参数设置，因此将斥力势场函数进一步简化，只保留障碍物与无人艇的距离项。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (10)

1.一种无人艇全局路径规划方法，其特征在于：基于代价评估函数，引入局部障碍斥力项和偏流角度得到优化代价函数，计算各个拓展点的代价值，选取代价值最低的拓展点作为路径的下一节点，包括以下步骤：
步骤一：设置了2个数据集合：open list和closed list；初始化open list和closedlist，将起点S放入open list中；
步骤二：对当前节点x和其父节点p(x)进行视线检查，从而判断视线是否存在；
步骤三：若所述视线不存在，将当前节点x放入closed list，则选择领域closed list中g(x′)+c(x,x')最小的点为当前节点x的父节点；x'表示当前节点x的拓展点，g(x′)+c(x,x')表示起始点到拓展点的路径代价加当前节点到拓展点的路径代价；
步骤四：若所述视线存在，遍历所述当前节点x的领域，计算领域拓展节点x'的g(x')，g(x')表示当前节点x的拓展点的g值，即表示起始点到该拓展点的路径代价；
步骤五：重复所述步骤四直到算法结束，根据每个节点间的父子关系形成一条规划路径。
2.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：所述局部障碍斥力的计算公式为：
Figure FDA0003671853930000011
偏流角度的计算公式为：
Figure FDA0003671853930000012
Δθ(x)＝θyaw(x)-θstream(x)
其中，θ(x)表示偏流角度项代价，Δθ(x)表示当前位置路径方向与水流方向夹角，θyaw(x)表示当前位置路径方向角，θstream(x)表示当前位置水流流向角；ρ(x,xobs)表示无人艇所在位置与障碍物之间的距离；ρ0表示障碍物影响距离；xobs障碍物位置；x表示当前节点。
3.根据权利要求2所述的一种无人艇全局路径规划方法，其特征在于：
所述优化代价函数为：
Figure FDA0003671853930000021
其中，c(x,p(x))表示当前节点到其父节点的欧式距离，
Figure FDA0003671853930000022
为局部障碍斥力项，表示一定范围内障碍物对当前节点的斥力之和，krep为局部障碍斥力项系数，n表示一定范围内障碍物的数量；kstreamθ(x)为偏流角度项，反应路径与水流夹角大小，kstream为偏流角度系数；p(x)表示当前节点的父节点；g(p(x))表示起始点到当前节点的父节点的路径代价。
4.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：所述步骤二中对当前节点x和其父节点p(x)进行视线检查的方法为采用路径安全距离检查和偏流角度检查；所述路径安全距离检查为检查无人艇距障碍物的距离是否大于路径安全距离。
5.根据权利要求4所述的一种无人艇全局路径规划方法，其特征在于：所述步骤二中偏流角度检查为检查无人艇路径与水流流向的夹角是否小于路径允许偏流角度。
6.根据权利要求5所述的一种无人艇全局路径规划方法，其特征在于：所述步骤二中判断视线存在的条件为：无人艇距障碍物的距离始终大于路径安全距离且路径与水流流向的夹角始终小于路径允许偏流角度。
7.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：所述步骤四的计算包括以下情况：
如果open list为空表示无路可走，算法结束；
若拓展节点为终点，从终点追踪父节点到达起点形成路径，算法完成；
若拓展节点为障碍物或是closed list中的节点则忽略该节点；
若拓展节点是open list中的点，判断g(x')是否比旧值更小，如果是，更新该拓展节点f(x')值，f(x')表示预估的总代价，包括转角代价和路径距离代价；
若当前节点不是以上任何情况，判断g(p(x))+c(p(x),x')是否小于g(x')，g(p(x))+c(p(x),x')表示起始点到当前节点的父节点的路径代价加当前节点的父节点到拓展点的路径代价；如果是则将该拓展点加入open list并更新拓展点g、f值，g表示起始点到该拓展点的路径代价；f为预估的总代价，记拓展节点x'的父节点为p(x)。
8.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：还包括步骤六：使用圆弧过度的方法对规划路径进行折角平滑优化处理。
9.根据权利要求1所述的一种无人艇全局路径规划方法，其特征在于：所述步骤八中对规划路径进行折角平滑优化处理的方法为：在路径折角处取相邻三点，根据几何关系求出圆弧圆心，将折线转换为圆弧。
10.根据权利要求9所述的一种无人艇全局路径规划方法，其特征在于：折角平滑优化处理的具体方法为：已知线段三点P1、P2和P3，以及圆弧半径R，求优化之后线段新端点P4、P5和圆弧圆心R(xR,yR)；根据几何关系可得出P1、P2、P3与P4、P5及圆弧圆心R(xR,yR)之间的转化关系为：
Figure FDA0003671853930000041
Figure FDA0003671853930000042
Figure FDA0003671853930000043
Figure FDA0003671853930000044
Figure FDA0003671853930000045
Figure FDA0003671853930000046
Figure FDA0003671853930000047
Figure FDA0003671853930000048
其中，β表示未加路径优化的转向角；d21表示P1与P2之间的距离；d23表示P2与P3之间的距离；
Figure FDA0003671853930000049
表示P2到P1的向量；
Figure FDA00036718539300000410
表示P3到P2的向量；atan表示方位角的四象限反正切函数。
CN202210607310.4A 2022-05-31 2022-05-31 一种无人艇全局路径规划方法 Active CN114967696B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210607310.4A CN114967696B (zh) 2022-05-31 2022-05-31 一种无人艇全局路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210607310.4A CN114967696B (zh) 2022-05-31 2022-05-31 一种无人艇全局路径规划方法

Publications (2)

Publication Number Publication Date
CN114967696A true CN114967696A (zh) 2022-08-30
CN114967696B CN114967696B (zh) 2024-08-20

Family

ID=82957183

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210607310.4A Active CN114967696B (zh) 2022-05-31 2022-05-31 一种无人艇全局路径规划方法

Country Status (1)

Country Link
CN (1) CN114967696B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117148842A (zh) * 2023-09-21 2023-12-01 哈尔滨工程大学 基于环境预报的风能驱动机器人省时全局路径规划方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018176594A1 (zh) * 2017-03-31 2018-10-04 深圳市靖洲科技有限公司 一种面向无人自行车的人工势场路径规划法
CN114545931A (zh) * 2022-01-27 2022-05-27 大连海事大学 一种基于改进人工势场法引导的Bi-RRT算法的水面无人艇路径规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018176594A1 (zh) * 2017-03-31 2018-10-04 深圳市靖洲科技有限公司 一种面向无人自行车的人工势场路径规划法
CN114545931A (zh) * 2022-01-27 2022-05-27 大连海事大学 一种基于改进人工势场法引导的Bi-RRT算法的水面无人艇路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LEI ZHAO: "Intelligent Path Planning System for Shallow Submerged Unmanned Ship Based on MS-FA Method", 2021 IEEE INTERNATIONAL CONFERENCE ON RECENT ADVANCES IN SYSTEMS SCIENCE AND ENGINEERING (RASSE), 14 December 2021 (2021-12-14) *
徐鹏飞: "基于环境优化的无人艇全局路径规划研究", 中国造船, 20 October 2022 (2022-10-20) *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117148842A (zh) * 2023-09-21 2023-12-01 哈尔滨工程大学 基于环境预报的风能驱动机器人省时全局路径规划方法

Also Published As

Publication number Publication date
CN114967696B (zh) 2024-08-20

Similar Documents

Publication Publication Date Title
CN107748561B (zh) 一种基于多传感参数的无人船局部避障系统及方法
CN109828566B (zh) 一种水面无人艇自主航行方法
CN109933067B (zh) 一种基于遗传算法和粒子群算法的无人艇避碰方法
CN113759939B (zh) 一种受限水域智能航行方法及装置
Xue et al. Automatic simulation of ship navigation
CN109597417B (zh) 一种基于避碰准则的多usv群体协同避碰规划方法
CN111665846B (zh) 一种基于快速扫描法的水面无人艇路径规划方法
CN114326702B (zh) 一种基于模型预测控制的内河无人船避碰方法
CN114967696A (zh) 一种无人艇全局路径规划方法
CN112880678A (zh) 一种复杂水域环境中无人艇航行规划方法
CN111984012A (zh) 一种无人船自主避障及航线规划的方法
CN114610046A (zh) 一种考虑动态水深的无人艇动态安全轨迹规划方法
CN111412918B (zh) 无人艇全局安全路径规划方法
Jing et al. Path planning and navigation of oceanic autonomous sailboats and vessels: A survey
CN109916400B (zh) 一种基于梯度下降算法与vo法相结合的无人艇避障方法
CN110618685A (zh) 一种无人水面艇障碍物探测误差修正及安全避碰方法
CN115220459A (zh) 一种基于改进速度障碍法的usv动态精确避障方法
Zhao et al. Decision-making for the autonomous navigation of USVs based on deep reinforcement learning under IALA maritime buoyage system
Lee et al. Multi-resolution path planning for marine surface vehicle considering environmental effects
Liu et al. PE-A* algorithm for ship route planning based on field theory
Blaich et al. Extended grid based collision avoidance considering COLREGs for vessels
CN116414123A (zh) 基于改进快速行进法的水面无人艇路径规划方法
CN117369441A (zh) 一种考虑船舶运动学和CORLEGs的自适应智能船舶路径规划方法
CN116974282A (zh) 基于无人艇的海上搜寻区域覆盖路径规划方法
CN111324128A (zh) 一种有助于跨河架空线路避障船只航行方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant