CN113419535A - 双艇路径规划方法、装置、设备及计算机可读存储介质 - Google Patents
双艇路径规划方法、装置、设备及计算机可读存储介质 Download PDFInfo
- Publication number
- CN113419535A CN113419535A CN202110755255.9A CN202110755255A CN113419535A CN 113419535 A CN113419535 A CN 113419535A CN 202110755255 A CN202110755255 A CN 202110755255A CN 113419535 A CN113419535 A CN 113419535A
- Authority
- CN
- China
- Prior art keywords
- node
- path
- boat
- planning
- cost function
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 230000004888 barrier function Effects 0.000 claims abstract description 14
- 230000006870 function Effects 0.000 claims description 111
- 238000004891 communication Methods 0.000 description 8
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 6
- 238000011156 evaluation Methods 0.000 description 5
- 230000008569 process Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
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/0206—Control of position or course in two dimensions specially adapted to water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Traffic Control Systems (AREA)
Abstract
本发明公开了一种双艇路径规划方法、装置、设备及计算机可读存储介质,该方法包括:获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;获取所述主艇的第二参数;根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。在第一路径的基础上规划第二路径,计算路径规划所需节点的估算代价时,加入路径代价函数这个约束条件,使主艇和辅艇协同作业的场景下,避障路径的规划规划更加快速。
Description
技术领域
本发明涉及路径规划领域,尤其涉及双艇路径规划方法、装置、设备及计算机可读存储介质。
背景技术
随着国家蓝海计划的推进,海洋作业的需求越来越多,水面勘测、信息中继通信等任务需求量不断提高,对于水面无人艇的作业能力提出了更高的要求。以水面通信中继无人艇为例,在水面作业时,无人艇能够在接到任务指令的时候,基于传统的水面无人艇避障路径规划算法,完成从起点到终点的避障路径规划,但涉及到双艇协同作业时,增加了更多的约束条件,使得双艇的避障路径规划难以实现。
发明内容
本发明的主要目的在于提供一种双艇路径规划方法、装置、设备及计算机可读存储介质,旨在解决现有的水面无人艇避障路径规划算法无法实现双艇协同的避障路径规划的技术问题。
此外,为实现上述目的,本发明还提供一种双艇路径规划方法,所述双艇路径规划方法包括以下步骤:
获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;
根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;
获取所述主艇的第二参数;
根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
可选地,所述加载区域栅格地图的步骤包括:
通过预设扫描设备获取预设区域对应的目标栅格地图,其中,所述目标栅格地图中的每个栅格对应一个节点;
获取所述目标栅格地图中被障碍物占据的目标栅格,将所述目标栅格对应的节点作为目标节点,所述目标栅格在所述目标栅格地图中的位置作为障碍物信息;
获取所述目标栅格地图对应的所有节点中除去所述目标节点的节点集合,将所述节点集合中的节点在所述目标栅格地图中的位置作为节点信息。
可选地,所述根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径的步骤包括:
确定所述节点信息中的第一起始节点和第一终止节点,根据所述第一起始节点、所述第一终止节点、所述第一参数中的辅艇尺寸和辅艇机动参数和所述障碍物信息,规划得到所述辅艇的第一路径。
可选地,所述根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径的步骤包括:
获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点,其中,所述规划节点与所述第一路径的距离在所述间距区间内,所述规划节点包括第二起始节点和第二终止节点;
定义父节点和子节点,其中,所述父节点和所述子节点属于所述规划节点,所述父节点和所述子节点互为相邻节点;
定义所述第二起始节点的代价函数值为目标固定值,当所述父节点为所述第二起始节点时,根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值;
将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点,将所述新的父节点的代价函数值作为新的目标固定值,并返回所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤;
直至所述新的父节点为所述第二终止节点,将所有父节点连接得到第二路径。
可选地,所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤之后,包括:
根据所述规划节点和所述障碍物信息,判断是否更新所述路径代价函数对应的间距区间;
若判定更新所述间距区间,则返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
可选地,所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤包括:
根据所述第二参数中的主艇尺寸和主艇机动参数,以及所述障碍物信息,计算所述父节点相邻的子节点的将付代价;
根据所述父节点相邻的子节点与所述第一路径的距离,确定所述父节点相邻的子节点的路径代价;
对所述目标固定值、所述将付代价和所述路径代价进行计算,得到所述父节点相邻的子节点的代价函数值。
可选地,所述将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点的步骤之后,包括:
若所述新的父节点的代价函数值大于预设阈值,则更新所述间距区间,并返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
此外,为实现上述目的,本发明还提供一种双艇路径规划装置,所述双艇路径规划装置包括:
栅格地图加载模块,用于获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;
第一路径规划模块,用于根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;
第二参数获取模块,用于获取所述主艇的第二参数;
第二路径规划模块,用于根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
此外,为实现上述目的,本发明还提供一种双艇路径规划设备,所述双艇路径规划设备包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的双艇路径规划程序,所述双艇路径规划程序被所述处理器执行时实现如上述的双艇路径规划方法的步骤。
此外,为实现上述目的,本发明还提供一种计算机可读存储介质,所述计算机可读存储介质上存储有双艇路径规划程序,所述双艇路径规划程序被处理器执行时实现如上述的双艇路径规划方法的步骤。
本发明实施例提出的双艇路径规划方法可应用于主艇和辅艇协同作业的场景,首先,获取辅艇的第一参数,加载主艇和辅艇协同作业区域的区域栅格地图,区域栅格地图中包含节点信息和障碍物信息,然后,根据节点信息,障碍物信息和第一参数,规划得到辅艇的第一路径,在先规划得到第一路径的基础上,获取主艇的第二参数,最终根据节点信息、障碍物信息、第二参数、第一路径和预定义的路径代价函数,规划得到主艇的第二路径,在第一路径的基础上规划第二路径,计算路径规划所需节点的估算代价时,加入路径代价函数这个约束条件,使主艇和辅艇协同作业的场景下,避障路径的规划规划更加快速。
附图说明
图1为本发明实施例提供的双艇路径规划设备一种实施方式的硬件结构示意图;
图2为本发明双艇路径规划方法第一实施例的流程示意图;
图3为本发明双艇路径规划方法第一实施例中区域栅格地图的示意图;
图4为本发明双艇路径规划方法第二实施例的流程示意图;
图5为本发明双艇路径规划装置一实施例的功能模块示意图。
本发明目的的实现、功能特点及优点将结合实施例,参照附图做进一步说明。
具体实施方式
应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。
在后续的描述中,使用用于表示元件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明,其本身没有特定的意义。因此,“模块”、“部件”或“单元”可以混合地使用。
本发明实施例双艇路径规划终端(又叫终端、设备或者终端设备)可以是水面无人艇,以及其他需要共同作业的移动设备,还可以是个人电脑等具有程序编译功能的设备。
如图1所示,该终端可以包括:处理器1001,例如CPU,网络接口1004,用户接口1003,存储器1005,通信总线1002。其中,通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard),可选用户接口1003还可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口(如WI-FI接口)。存储器1005可以是高速RAM存储器,也可以是稳定的存储器(non-volatile memory),例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解,图1中示出的终端结构并不构成对终端的限定,可以包括比图示更多或更少的部件,或者组合某些部件,或者不同的部件布置。
如图1所示,作为一种计算机可读存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及双艇路径规划程序。
在图1所示的终端中,网络接口1004主要用于连接后台服务器,与后台服务器进行数据通信;用户接口1003主要用于连接客户端(用户端),与客户端进行数据通信;而处理器1001可以用于调用存储器1005中存储的双艇路径规划程序,所述双艇路径规划程序被处理器执行时实现下述实施例提供的双艇路径规划方法中的操作。
基于上述设备硬件结构,提出了本发明双艇路径规划方法的实施例。
参照图2,在本发明双艇路径规划方法的第一实施例中,所述双艇路径规划方法包括:
步骤S10,获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息。
本实施例中的双艇路径规划方法应用于需要共同作业的移动设备(例如,水面无人艇)的路径规划,以共同在水面作业的双艇(主艇和辅艇)为例,主艇和辅艇相互关联,首先获取辅艇的第一参数,其中,第一参数包括辅艇的尺寸,辅艇的朝向,以及辅艇的转弯半径范围等参数,经过视觉和激光雷达等元器件扫描主艇和辅艇的作业区域,得到区域栅格地图,区域栅格地图的分辨率可以根据激光雷达的扫描分辨率进行调整,扫描分辨率越大,区域栅格地图的分辨率也越大,区域栅格地图的栅格尺寸越小,可知地,本实施例中的区域栅格地图可以根据作业区域的大小,艇的大小,以及作业区域内的障碍物大小灵活调整,区域栅格地图中的每个栅格对应一个节点,这个节点可以在栅格的任意位置,需要说明的是,节点在每个栅格地图中的位置是统一的,节点在区域栅格地图中的位置构成了节点信息,确定区域栅格地图后,还可以根据占据栅格地图的概念,确定区域栅格地图中被障碍物占据的栅格,以及障碍物在区域栅格地图中的位置,这些构成了障碍物信息。
步骤S20,根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径。
可知地,在本实施例中的双艇路径规划方法的技术方案中,辅艇的避障路径是先规划出来的,具体地,可以基于混合A星算法规划辅艇的避障路径(即本实施例中的第一路径),混合A星算法的核心是启发式搜索,定义一个启发式估价函数f(x)=g(x)+h(x),其中,x是区域栅格地图中除被障碍物占据的节点之外的节点,f(x)表示搜索到x节点时对应的启发函数,g(x)表示起始节点到x节点的实际代价,h(x)表示x节点到终点节点的估算代价,在已确定辅艇的起始节点和终止节点的情况下,规划辅艇的避障路径时,节点到障碍物之间的距离,节点到上一个确定节点的距离,节点位于上一个确定节点的方位,以及辅艇的第一参数都可以作为衡量一个节点的估算代价的标准,在选取某个确定节点的下一个节点时,通过实际已付代价和估算代价计算与这个确定节点相邻的所有待选取节点的启发式估价函数的值,并将启发式估价函数的值最小的待选取节点作为这个确定节点的下一个确定节点,并依次选取确定的节点,最后将所有确定的节点连接起来便得到了辅艇的避障路径,即本实施例中的第一路径。
步骤S30,获取所述主艇的第二参数。
步骤S40,根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
可知地,本实施例应用在协同作业的双艇的路径规划场景中,两艇之间的距离也是衡量规划主艇避障路径的节点的代价的约束条件,在不同的业务需求中,协同作业的双艇之间的距离是不同的,本实施例中预定义的路径代价函数实质上是描述第一路径与主艇避障路径之间的距离关系的函数,预定义的路径代价函数对应一个距离区间,这个距离区间是主艇与辅艇之间保持的距离范围,当第一路径确定时,距离区间也是主艇避障路径与第一路径之间的距离,本实施例中的第二参数包括主艇的尺寸和主艇的机动参数等,这些决定了主艇的转弯半径等参数大小。
本实施例在启发式估价函数的基础上,加入路径代价函数对两艇之间的距离进行约束,具体地,在第一路径已经规划好的前提下,获取预定义的路径代价函数对应的距离区间,确定主艇在区域栅格地图中的起始节点和终止节点,然后确定与第一路径的距离在距离区间内的节点所处的范围,由于主艇要与辅艇保持在这个距离区间内,所以这个范围内包含主艇的起始节点和终止节点,对主艇的避障路径规划实质上就是确定一条从主艇的起始节点到主艇的终止节点之间的路径,而决定主艇路径对应的节点的约束条件包括,与第一路径之间的距离,主艇的尺寸,主艇的机动参数,节点在这个范围内的位置,以及障碍物在这个范围内的位置,根据上述约束条件,规划得到主艇的第二路径,规划路径的具体过程将在下述实施例中详述。
具体地,步骤S10细化的步骤包括:
步骤a1,通过预设扫描设备获取预设区域对应的目标栅格地图,其中,所述目标栅格地图中的每个栅格对应一个节点。
步骤a2,获取所述目标栅格地图中被障碍物占据的目标栅格,将所述目标栅格对应的节点作为目标节点,所述目标栅格在所述目标栅格地图中的位置作为障碍物信息。
步骤a3,获取所述目标栅格地图对应的所有节点中除去所述目标节点的节点集合,将所述节点集合中的节点在所述目标栅格地图中的位置作为节点信息。
可知地,本实施例可以通过激光雷达等元器件扫描主艇和辅艇的作业区域,得到区域栅格地图,然后根据占据栅格地图的相关算法确定障碍物占据的栅格,又由于本实施例中区域栅格地图中每个栅格都对应一个节点,将上述实施例中与第一路径的距离在距离区间内的节点所处的范围对应的所有节点去除被障碍物占据的节点,作为规划第二路径的可用节点,本实施例中的目标栅格地图是指通过激光雷达等元器件(即本实施例中的预设扫描设备)扫描主艇和辅艇的作业区域(即预设区域),得到的初步栅格地图,根据占据栅格地图的相关算法确定目标栅格地图中被障碍物占据的栅格(即本实施例中的目标栅格),将目标栅格占据的节点作为目标节点,将目标栅格在所述目标栅格地图中的位置作为障碍物信息,如图3所示,图3为区域栅格地图,图3中黑色方块的位置即为障碍物信息,将目标栅格地图对应的所有节点中除去目标节点的其他节点作为节点集合,将节点集合中的节点在目标栅格地图中的位置作为节点信息,即节点信息包含的节点理论上都是主艇和辅艇能到达的节点。
具体地,步骤S20之后的步骤包括:
步骤b1,确定所述节点信息中的第一起始节点和第一终止节点,根据所述第一起始节点、所述第一终止节点、所述第一参数中的辅艇尺寸和辅艇机动参数和所述障碍物信息,规划得到所述辅艇的第一路径。
可知地,如图3所示,图3中A点为辅艇在区域栅格地图中对应的起始节点(即本实施例中的第一起始节点),C点为辅艇在区域栅格地图中对应的的终点(即本实施例中的第一终止节点),辅艇的起点和终点是预先确定的,但第一起始节点和第一终止节点是在区域栅格地图构建完成后才确定的,规划第一路径时可以从第一起始节点开始朝第一终止节点规划,也可以从第一终止节点开始朝第一起始节点规划,本实施例以第一起始节点开始朝第一终止节点规划为例,当开始规划第一路径时,辅艇位于A点对应的节点,A点对应的节点为父节点,与A点对应的节点在第一起始节点朝向第一终止节点的方向上相邻的所有节点为子节点,根据第一参数中的辅艇尺寸和辅艇机动参数,以及障碍物信息,计算上述所有子节点的代价值,即从A点对应的节点到每个子节点辅艇需要付出的代价,将上述子节点中代价值最低的子节点作为新的父节点,连接第一起始节点与上述新的父节点,便得到了一小段规划好的路径,然后从第一起始节点向第一终止节点,依次确定新的父节点,当最后确定的父节点是第一终止节点时,第一路径规划完成,如图3所示,A点到C点之间的路径即是第一路径的示意图。
在本实施例中双艇路径规划方法可应用于主艇和辅艇协同作业的场景,首先,获取辅艇的第一参数,加载主艇和辅艇协同作业区域的区域栅格地图,区域栅格地图中包含节点信息和障碍物信息,然后,根据节点信息,障碍物信息和第一参数,规划得到辅艇的第一路径,在先规划得到第一路径的基础上,获取主艇的第二参数,最终根据节点信息、障碍物信息、第二参数、第一路径和预定义的路径代价函数,规划得到主艇的第二路径,在第一路径的基础上规划第二路径,计算路径规划所需节点的估算代价时,加入路径代价函数这个约束条件,使主艇和辅艇协同作业的场景下,避障路径的规划规划更加快速。
进一步地,参照图4,在本发明上述实施例的基础上,提出了本发明双艇路径规划方法的第二实施例。
本实施例是第一实施例中步骤S40细化的步骤,本实施例与本发明上述实施例的区别在于:
步骤S41,获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点,其中,所述规划节点与所述第一路径的距离在所述间距区间内,所述规划节点包括第二起始节点和第二终止节点。
步骤S42,定义父节点和子节点,其中,所述父节点和所述子节点属于所述规划节点,所述父节点和所述子节点互为相邻节点。
步骤S43,定义所述第二起始节点的代价函数值为目标固定值,当所述父节点为所述第二起始节点时,根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值。
步骤S44,将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点,将所述新的父节点的代价函数值作为新的目标固定值,并返回所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤。
步骤S45,直至所述新的父节点为所述第二终止节点,将所有父节点连接得到第二路径。
可知地,规划得到第一路径后,在规划主艇的第二路径时,需要先获取预定义的路径代价函数对应的间距区间,然后确定节点信息中的规划节点,其中,规划节点与第一路径的距离在间距区间内,规划节点包括主艇的起点(即本实施例中的第二起始节点)和终点(即本实施例中的第二终止节点),定义父节点和子节点,其中,父节点和子节点属于规划节点,父节点和子节点互为相邻节点,本实施例中对于相邻的定义是,两个节点之间不相隔其他节点,当父节点为第二起始点时,定义第二起始节点的代价函数值为目标固定值,因为第二起始节点的代价函数值是指,第二起始节点的前一个节点到第二起始节点所付出的代价,由于本实施例中第二起始节点的前一个节点不存在,所以本实施例定义第二起始节点的代价函数值为一个无限大的值,即本实施例中的目标固定值,当父节点为第二起始节点时,子节点为与第二起始点相邻的节点,根据第二参数中的主艇尺寸和主艇机动参数、障碍物信息、目标固定值和路径代价函数,计算第二起始节点相邻的所有子节点的代价函数值,将第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点,然后将新的父节点的代价函数值作为新的目标固定值,继而返回根据第二参数中的主艇尺寸和主艇机动参数、障碍物信息、目标固定值和路径代价函数,计算第二起始节点相邻的所有子节点的代价函数值的步骤,如此循环,直至新的父节点是第二终止节点为止,最终将所有被作为过父节点的节点连接,便得到了主艇的第二路径,得到的第二路径如图3中B点到D点之间的路径所示。
具体地,步骤S41之后的步骤包括:
步骤c1,根据所述规划节点和所述障碍物信息,判断是否更新所述路径代价函数对应的间距区间。
步骤c2,若判定更新所述间距区间,则返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
可知地,若获取到路径代价函数对应的间距区间,并因此确定了主艇可以规划路径的区域后,若区域内不存在第二起始节点指向第二终止节点的路径(即主艇可以规划路径的区域内第二起始节点指向第二终止节点的所有主艇可行走路径上均存在障碍物)时,判定需要更新路径代价函数对应的间距区间,这种更新一般是扩大路径代价函数对应的间距区间,目的是为了确定规划主艇路径的区域内可以规划第二起始节点指向第二终止节点的路径,在路径代价函数对应的间距区间更新后,返回获取所述路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤,并将规划节点定义为规划节点与第一路径的距离在新的间距区间内。
具体地,步骤S43细化的步骤包括:
步骤d1,根据所述第二参数中的主艇尺寸和主艇机动参数,以及所述障碍物信息,计算所述父节点相邻的子节点的将付代价。
步骤d2,根据所述父节点相邻的子节点与所述第一路径的距离,确定所述父节点相邻的子节点的路径代价。
步骤d3,对所述目标固定值、所述将付代价和所述路径代价进行计算,得到所述父节点相邻的子节点的代价函数值。
可知地,父节点到每个子节点的代价是本实施例中的将付代价,也是每个子节点对应的将付代价,将付代价与第二参数中的主艇尺寸和主艇机动参数,以及障碍物信息有关,例如,子节点到障碍物的距离越小,子节点对应的将付代价就越大,子节点与父节点之间的距离越大,子节点对应的将付代价就越大,根据第二参数中的主艇尺寸和主艇机动参数,以及障碍物信息,计算父节点相邻的所有子节点的将付代价,然后根据父节点相邻的所有子节点与第一路径之间的距离,确定父节点相邻的子节点的路径代价,例如,子节点到第一路径的距离越大,子节点对应的将付代价就越大,最后,将目标固定值、将付代价和路径代价进行计算,得到父节点相邻的所有子节点的代价函数值,进而可以根据每个子节点的代价函数值,确定父节点相邻的所有子节点中可以作为新的父节点的节点。
具体地,步骤S44细化的步骤包括:
步骤e1,若所述新的父节点的代价函数值大于预设阈值,则更新所述间距区间,并返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
可知地,若确定父节点相邻的所有子节点中代价函数值最小的子节点为子节点a,且子节点a的代价函数值也大于预设阈值,即,从一个父节点到与其相邻的所有子节点,所付出的代价都很大,这种情况下,为了确保规划的路径有价值,需要更新间距区间,这种更新一般是扩大,目的是为了确定规划主艇路径的区域内可以规划出第二起始节点指向第二终止节点的有价值路径,在路径代价函数对应的间距区间更新后,返回获取所述路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤,并将规划节点定义为规划节点与第一路径的距离在新的间距区间内。
在本实施例中通过计算与预定义的路径代价函数相对应的规划节点的代价函数值,并以此对规划节点进行筛选,得到主艇的避障路径,实现了双艇避障路径的快速规划。
此外,参照图5,本发明实施例还提出一种双艇路径规划装置,所述双艇路径规划装置包括:
栅格地图加载模块10,用于获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;
第一路径规划模块20,用于根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;
第二参数获取模块30,用于获取所述主艇的第二参数;
第二路径规划模块40,用于根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
可选地,所述栅格地图加载模块10,包括:
目标栅格地图获取单元,用于通过预设扫描设备获取预设区域对应的目标栅格地图,其中,所述目标栅格地图中的每个栅格对应一个节点;
目标栅格获取单元,用于获取所述目标栅格地图中被障碍物占据的目标栅格,将所述目标栅格对应的节点作为目标节点,所述目标栅格在所述目标栅格地图中的位置作为障碍物信息;
节点集合获取单元,用于获取所述目标栅格地图对应的所有节点中除去所述目标节点的节点集合,将所述节点集合中的节点在所述目标栅格地图中的位置作为节点信息。
可选地,所述第一路径规划模块20,包括:
第一路径规划单元,用于确定所述节点信息中的第一起始节点和第一终止节点,根据所述第一起始节点、所述第一终止节点、所述第一参数中的辅艇尺寸和辅艇机动参数和所述障碍物信息,规划得到所述辅艇的第一路径。
可选地,所述路径代价函数获取模块30,包括:
规划节点确定单元,用于获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点,其中,所述规划节点与所述第一路径的距离在所述间距区间内,所述规划节点包括第二起始节点和第二终止节点;
节点定义单元,用于定义父节点和子节点,其中,所述父节点和所述子节点属于所述规划节点,所述父节点和所述子节点互为相邻节点;
代价函数值计算单元,用于定义所述第二起始节点的代价函数值为目标固定值,当所述父节点为所述第二起始节点时,根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值;
代价函数值循环计算单元,用于将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点,将所述新的父节点的代价函数值作为新的目标固定值,并返回所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤;
父节点连接单元,用于直至所述新的父节点为所述第二终止节点,将所有父节点连接得到第二路径。
可选地,所述双艇路径规划装置,包括:
判断模块,用于根据所述规划节点和所述障碍物信息,判断是否更新所述路径代价函数对应的间距区间;
判定返回模块,用于若判定更新所述间距区间,则返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
可选地,所述代价函数值计算单元,包括:
将付代价计算单元,用于根据所述第二参数中的主艇尺寸和主艇机动参数,以及所述障碍物信息,计算所述父节点相邻的子节点的将付代价;
路径代价确定单元,用于根据所述父节点相邻的子节点与所述第一路径的距离,确定所述父节点相邻的子节点的路径代价;
计算单元,用于对所述目标固定值、所述将付代价和所述路径代价进行计算,得到所述父节点相邻的子节点的代价函数值。
可选地,所述双艇路径规划装置,包括:
间距区间更新模块,用于若所述新的父节点的代价函数值大于预设阈值,则更新所述间距区间,并返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
此外,本发明实施例还提出一种计算机可读存储介质,所述计算机可读存储介质上存储有双艇路径规划程序,所述双艇路径规划程序被处理器执行时实现上述实施例提供的双艇路径规划方法中的操作。
上述各程序模块所执行的双艇路径规划方法可参照本发明双艇路径规划方法各个实施例,此处不再赘述。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体/操作/对象与另一个实体/操作/对象区分开来,而不一定要求或者暗示这些实体/操作/对象之间存在任何这种实际的关系或者顺序;术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者系统不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者系统所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括该要素的过程、方法、物品或者系统中还存在另外的相同要素。
对于装置实施例而言,由于其基本相似于方法实施例,所以描述得比较简单,相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的,其中作为分离部件说明的单元可以是或者也可以不是物理上分开的。可以根据实际的需要选择中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下,即可以理解并实施。
上述本发明实施例序号仅仅为了描述,不代表实施例的优劣。
通过以上的实施方式的描述,本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现,当然也可以通过硬件,但很多情况下前者是更佳的实施方式。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来,该计算机软件产品存储在如上所述的一个存储介质(如ROM/RAM、磁碟、光盘)中,包括若干指令用以使得一台终端设备(可以是手机,计算机,服务器,空调器,或者网络设备等)执行本发明各个实施例所述的双艇路径规划方法。
以上仅为本发明的优选实施例,并非因此限制本发明的专利范围,凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本发明的专利保护范围内。
Claims (10)
1.一种双艇路径规划方法,其特征在于,所述双艇路径规划方法所述双艇路径规划方法应用于主艇,所述双艇路径规划方法包括以下步骤:
获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;
根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;
获取所述主艇的第二参数;
根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
2.如权利要求1所述的双艇路径规划方法,其特征在于,所述加载区域栅格地图的步骤包括:
通过预设扫描设备获取预设区域对应的目标栅格地图,其中,所述目标栅格地图中的每个栅格对应一个节点;
获取所述目标栅格地图中被障碍物占据的目标栅格,将所述目标栅格对应的节点作为目标节点,所述目标栅格在所述目标栅格地图中的位置作为障碍物信息;
获取所述目标栅格地图对应的所有节点中除去所述目标节点的节点集合,将所述节点集合中的节点在所述目标栅格地图中的位置作为节点信息。
3.如权利要求1所述的双艇路径规划方法,其特征在于,所述根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径的步骤包括:
确定所述节点信息中的第一起始节点和第一终止节点,根据所述第一起始节点、所述第一终止节点、所述第一参数中的辅艇尺寸和辅艇机动参数和所述障碍物信息,规划得到所述辅艇的第一路径。
4.如权利要求1所述的双艇路径规划方法,其特征在于,所述根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径的步骤包括:
获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点,其中,所述规划节点与所述第一路径的距离在所述间距区间内,所述规划节点包括第二起始节点和第二终止节点;
定义父节点和子节点,其中,所述父节点和所述子节点属于所述规划节点,所述父节点和所述子节点互为相邻节点;
定义所述第二起始节点的代价函数值为目标固定值,当所述父节点为所述第二起始节点时,根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值;
将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点,将所述新的父节点的代价函数值作为新的目标固定值,并返回所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤;
直至所述新的父节点为所述第二终止节点,将所有父节点连接得到第二路径。
5.如权利要求4所述的双艇路径规划方法,其特征在于,所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤之后,包括:
根据所述规划节点和所述障碍物信息,判断是否更新所述路径代价函数对应的间距区间;
若判定更新所述间距区间,则返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
6.如权利要求4所述的双艇路径规划方法,其特征在于,所述根据所述第二参数中的主艇尺寸和主艇机动参数、所述障碍物信息、所述目标固定值和所述路径代价函数,计算所述父节点相邻的子节点的代价函数值的步骤包括:
根据所述第二参数中的主艇尺寸和主艇机动参数,以及所述障碍物信息,计算所述父节点相邻的子节点的将付代价;
根据所述父节点相邻的子节点与所述第一路径的距离,确定所述父节点相邻的子节点的路径代价;
对所述目标固定值、所述将付代价和所述路径代价进行计算,得到所述父节点相邻的子节点的代价函数值。
7.如权利要求4所述的双艇路径规划方法,其特征在于,所述将所述第二起始节点相邻的子节点中代价函数值最小的子节点作为新的父节点的步骤之后,包括:
若所述新的父节点的代价函数值大于预设阈值,则更新所述间距区间,并返回所述获取预定义的路径代价函数对应的间距区间,确定所述节点信息中的规划节点的步骤。
8.一种双艇路径规划装置,其特征在于,所述双艇路径规划装置包括:
栅格地图加载模块,用于获取与所述主艇关联的辅艇的第一参数,并加载区域栅格地图,其中,所述区域栅格地图包含节点信息和障碍物信息;
第一路径规划模块,用于根据所述节点信息、所述障碍物信息和所述第一参数,规划得到所述辅艇的第一路径;
第二参数获取模块,用于获取所述主艇的第二参数;
第二路径规划模块,用于根据所述节点信息、所述障碍物信息、所述第二参数、所述第一路径和预定义的路径代价函数,规划得到所述主艇的第二路径。
9.一种双艇路径规划设备,其特征在于,所述双艇路径规划设备包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的双艇路径规划程序,所述双艇路径规划程序被所述处理器执行时实现如权利要求1至7中任一项所述的双艇路径规划方法的步骤。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质上存储有双艇路径规划程序,所述双艇路径规划程序被处理器执行时实现如权利要求1至7中任一项所述的双艇路径规划方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110755255.9A CN113419535B (zh) | 2021-07-05 | 2021-07-05 | 双艇路径规划方法、装置、设备及计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110755255.9A CN113419535B (zh) | 2021-07-05 | 2021-07-05 | 双艇路径规划方法、装置、设备及计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113419535A true CN113419535A (zh) | 2021-09-21 |
CN113419535B CN113419535B (zh) | 2024-02-27 |
Family
ID=77720188
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110755255.9A Active CN113419535B (zh) | 2021-07-05 | 2021-07-05 | 双艇路径规划方法、装置、设备及计算机可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113419535B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109540136A (zh) * | 2018-10-25 | 2019-03-29 | 广东华中科技大学工业技术研究院 | 一种多无人艇协同路径规划方法 |
CN110244720A (zh) * | 2019-06-04 | 2019-09-17 | 浙江海洋大学 | 用于海上无人艇的路径规划方法及系统 |
CN110398250A (zh) * | 2019-08-13 | 2019-11-01 | 哈尔滨工程大学 | 一种无人艇全局路径规划方法 |
CN110609556A (zh) * | 2019-10-09 | 2019-12-24 | 广东华中科技大学工业技术研究院 | 一种基于los导航法的多无人艇协同控制方法 |
CN111665848A (zh) * | 2020-06-28 | 2020-09-15 | 北京航空航天大学 | 拓扑切换下无人机与无人车异构集群编队跟踪控制方法 |
-
2021
- 2021-07-05 CN CN202110755255.9A patent/CN113419535B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109540136A (zh) * | 2018-10-25 | 2019-03-29 | 广东华中科技大学工业技术研究院 | 一种多无人艇协同路径规划方法 |
CN110244720A (zh) * | 2019-06-04 | 2019-09-17 | 浙江海洋大学 | 用于海上无人艇的路径规划方法及系统 |
CN110398250A (zh) * | 2019-08-13 | 2019-11-01 | 哈尔滨工程大学 | 一种无人艇全局路径规划方法 |
CN110609556A (zh) * | 2019-10-09 | 2019-12-24 | 广东华中科技大学工业技术研究院 | 一种基于los导航法的多无人艇协同控制方法 |
CN111665848A (zh) * | 2020-06-28 | 2020-09-15 | 北京航空航天大学 | 拓扑切换下无人机与无人车异构集群编队跟踪控制方法 |
Also Published As
Publication number | Publication date |
---|---|
CN113419535B (zh) | 2024-02-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6632173B1 (ja) | ロボットの地図構築及び位置推定 | |
KR102407664B1 (ko) | 물체 검출을 위한 방법, 장치 및 기기 | |
CN110749901B (zh) | 自主移动机器人及其地图拼接方法、装置和可读存储介质 | |
CN111780775A (zh) | 路径规划的方法、装置、机器人及存储介质 | |
CN108021625B (zh) | 车辆异常聚集地监控方法及系统、计算机可读存储介质 | |
CN111580524A (zh) | 基于路径规划的车辆换道方法、装置、设备及存储介质 | |
CN111292377B (zh) | 目标检测方法、装置、计算机设备和存储介质 | |
CN113189989B (zh) | 车辆意图预测方法、装置、设备及存储介质 | |
CN112889100B (zh) | 移动体行为预测装置 | |
CN114162130B (zh) | 驾驶辅助模式切换方法、装置、设备及存储介质 | |
CN114777792A (zh) | 路径规划方法、装置、计算机可读介质及电子设备 | |
CN111752275A (zh) | 一种用于机器人的自动巡航方法、装置和存储介质 | |
CN112433528A (zh) | 机器人乘用电梯的方法、装置、机器人及存储介质 | |
CN115145289A (zh) | 多智能体协同围捕方法、系统、设备及存储介质 | |
CN110789520B (zh) | 行驶控制方法、装置及电子设备 | |
CN115575976A (zh) | 沿边路径规划方法、装置、计算机可读介质及电子设备 | |
CN113419535A (zh) | 双艇路径规划方法、装置、设备及计算机可读存储介质 | |
CN113869652A (zh) | 共享单车还车规划方法、装置、设备及存储介质 | |
CN113298715A (zh) | 机器人栅格地图构建方法、装置、机器人及存储介质 | |
CN113473357A (zh) | 辅助定位方法、装置、设备和存储介质 | |
CN117765512A (zh) | 基于激光雷达的静态障碍物检测方法、装置、设备及介质 | |
CN115205823A (zh) | 一种确定可行驶区域的方法、系统及自动驾驶车辆 | |
CN114821543B (zh) | 障碍物检测方法、装置、车辆和存储介质 | |
CN112049929B (zh) | 车辆驻车方法、装置、设备及存储介质 | |
CN114407916B (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 |