CN111399506B - 一种基于动力学约束的全局-局部混合无人船路径规划方法 - Google Patents
一种基于动力学约束的全局-局部混合无人船路径规划方法 Download PDFInfo
- Publication number
- CN111399506B CN111399506B CN202010177283.2A CN202010177283A CN111399506B CN 111399506 B CN111399506 B CN 111399506B CN 202010177283 A CN202010177283 A CN 202010177283A CN 111399506 B CN111399506 B CN 111399506B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- path
- global
- obstacle
- local
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 84
- 230000001133 acceleration Effects 0.000 claims abstract description 8
- 230000002265 prevention Effects 0.000 claims abstract description 6
- 238000004458 analytical method Methods 0.000 claims abstract description 4
- 238000011156 evaluation Methods 0.000 claims description 12
- 230000008569 process Effects 0.000 claims description 9
- 239000002131 composite material Substances 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 5
- 230000004888 barrier function Effects 0.000 claims description 4
- 238000010586 diagram Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000002950 deficient Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000000007 visual 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
-
- 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/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of 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/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
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种基于动力学约束的全局‑局部混合无人船路径规划方法。包括:S1、构建无人船运动学模型,并进行动力学分析;S2、考虑无人船安全范围,采用存储节点的方式,实现无人船全局路径的规划;以优化生成的路径点,约束无人船安全行驶。S3、针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,实现无人船局部路径的规划;精确地将可驱动的前向、航向角速度和加速度考虑到局部路径的规划中,产生可跟踪且能够实现实时局部避碰的路径。S4、融合上述无人船全局路径规划和无人船局部路径规划方案,形成全局局部混合避障策略。通过在全局最优路径上***虚拟路径点,使全局和局部路径规划完好结合,完成整个路径跟踪任务。
Description
技术领域
本发明涉及无人船技术领域,具体而言,尤其涉及一种基于动力学约束的全局-局部混合无人船路径规划方法。
背景技术
随着无人船技术在民用和军事领域的广泛应用,为了使无人船能够在人类无法到达的严峻环境下自主执行任务,设计一种符合无人船运动特性并能保证产生安全路径的路径规划方法显得尤为重要。在实际的路径规划算法中,要将全局规划和实时动态避碰结合,才能很好地解决路径规划问题。将地图处理为栅格地图的方法被许多游戏和机器人的路径规划所采用,这样的方法有A*方法、A*PS方法等等,但是这种方法产生的路径过长,且产生不现实的路径不能满足实际需求。有人基于A*方法提出一种可视图方法,但相比于A*方法其计算量更大,效率更低。一些智能搜索方法,例如蚁群方法、遗传方法、粒子群方法,这些方法通过不断进化迭代,最终产生路径,但这样的路径也不能满足实际行驶的需要。人工势场方法,具有计算简便且计算量小的特点,但可能导致无人船陷入局部极小值,使路径陷入死循环,导致事故的发生。
发明内容
根据上述提出已有的路径规划方法不能快速地产生一条符合无人船机动特性且能够进行实时避碰的最短路径的技术问题,而提供一种基于动力学约束的全局-局部混合无人船路径规划方法。本发明将无人船安全范围考虑到规划方法当中,并提出一种新型存储节点的方式,开发了全局路径规划方法,以优化生成的路径点,约束无人船安全行驶。针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,通过控制前向和航向角速度,从而实现大范围和近距离的避碰。在FDM模糊决策模块与无人船动力学相关约束的作用下,精确地将可驱动的前向、航向角速度和加速度考虑到局部路径的规划中,从而产生可跟踪且能够实现实时局部避碰的路径。通过在全局最优路径上***虚拟路径点,使全局和局部路径规划能完好结合,从而完成了整个路径跟踪任务。
本发明采用的技术手段如下:
一种基于动力学约束的全局-局部混合无人船路径规划方法,包括如下步骤:
S1、构建无人船运动学模型,并对构建的无人船运动学模型进行动力学分析;
S2、考虑无人船安全范围,采用存储节点的方式,实现无人船全局路径的规划;
S3、针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,实现无人船局部路径的规划;
S4、融合上述无人船全局路径规划方案和无人船局部路径规划方案,形成全局局部混合避障策略。
进一步地,所述步骤S1具体为:
S11、构建无人船运动学模型,所述无人船运动学模型具体如下:
其中,(x,y,ψ)表示无人船在地球坐标系的位置和航向,(u,v,r)分别表示在船体坐标系下无人船的前向速度、横向速度和航向角速度;
S12、针对欠驱动无人船模型,由于前向和偏航动力的限制,横向速度v是有界的,因此在动力学约束的情况下,需约束前向速度和航向角速度,且这两个速度受限于:
S13、设定安全范围,以无人船中心为圆心设定一个在整个避障过程中不能与障碍物接触的安全范围DsA,保证无人船的安全行驶,安全范围DsA被定义为:
其中,ps=[xs,ys]T,ps表示无人船当前位置坐标,xs,ys分别表示无人船当前位置横坐标和纵坐标,p=[x,y]T,ds为安全范围半径,p(t)表示无人船安全范围内点的坐标,x,y分别表示无人船安全范围内点的横坐标和纵坐标。
进一步地,所述步骤S2具体是为了得到从起点到目标点的Nw个最佳航路点,并且保证相邻航路点间的连线没有与障碍物发生干涉,使产生的路径Lp安全且最短,Lp的表达式为:
其中,Nw表示产生全局航路点的总数,P0和Pf分别为起始点和目标点。
进一步地,所述步骤S2还包括如下步骤:
S21、采用评价函数,评估每个节点的权值,确定全局最佳航路点:
S22、设置Sopen,Sclose,Stree三个点集,分别存储没被扩展的节点、被扩展过的节点、航路点;Sopen和Stree采用三层存储结构比较得出最优航路点并将其存放在Stree中,同时也将其存储在Sclose中,并从Sopen中删除点
进一步地,所述步骤S22具体为:
S221、将起点加入到开放列表Sopen中,起点的父节点定为起点本身;
S223、进一步扩展首先检查没被扩展的子节点是否与的父节点存在视线制导,若的父节点和子节点的连线没有穿过障碍物及边界,则存在视线制导,将的父节点改为同时更新和并且更新Stree;若的父节点和子节点的连线与障碍物相交,则不更新。
进一步地,所述步骤S3具体为:
S31、构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,具体如下:
其中,pobs=[x0,y0]T表示障碍物的位置,dso=η·u是由障碍物的速度决定的安全扩展域长度,η>0表示安全扩展系数,在这里传感器的测量范围是0<LFDW<LFDM<∞;LFDW表示FDW模块的作用范围。
S32、所述FDM模糊决策模块为了躲避距离较远的障碍物,通过考虑路径点和障碍物的相对方向,建立模糊规则表;
S33、在所述FDW精准动态窗口模块中:
通过可达速度集Vr的约束形成速度量动态窗口,且该动态窗口里的速度可以在下一个时间间隔内精准到达,基于当前无人船的速度(uc,rc),可达速度集Vr定义如下:
其中,T>0是时间间隔,uc和rc分别表示无人船当前的前向速度和航向角速度;
基于无人船动力学固有特性影响,可执行性集Vp定义如下:
结合Va、Vr和Vp,最终得出复合约束集Vc:
Vc=Va∩Vr∩Vp
S34、在复合约束集Vc的约束条件下,采用如下评价函数得出最优速度:
Gc(u,r)=α1heading(u,r)+βdist(u,r)+γvelocity(u,r)
其中,heading(u,r)表示使无人船朝向目标点航行的评价因素,其中表示目标点的方向,表示无人船的航向;dist(u,r)表示在相应预测轨迹上无人船与最近障碍物的距离,如果距离太大或者在轨迹附近没有障碍物,则此评价因素被赋值为定量常数;velocity(u,r)表示无人船沿着相应轨迹的航行速度;α>0、β>0、γ>0是定义的权重系数。
进一步地,所述步骤S4具体为:
S41、规划无人船全局路径,通过输入的离线地图和起始点规划出一条最优的全局路径;
S42、采用视线制导策略,对全局路径进行优化;
S43、当跟踪全局路径的过程中遇到未知或者不可预知的局部障碍物时,采用FDM模糊决策模块进行远距离的局部避障;
S44、当离障碍物较近时,采用FDW精准动态窗口模块进行近距离的障碍物躲避;
S45、局部避障任务完成时,无人船重新返回到全局路径,完成任务路径跟踪。
进一步地,所述步骤S4还包括在距离障碍物前LFDW处设置了一个虚拟航路点的步骤,该虚拟航路点也是再次进行全局路径跟踪的起始点。
较现有技术相比,本发明具有以下优点:
1、本发明提供的于动力学约束的全局-局部混合无人船路径规划方法,将无人船安全范围考虑到规划方法当中,并提出一种新型存储节点的方式,开发了全局路径规划方法,以优化生成的路径点,约束无人船安全行驶,使得全局路径规划效率更高,且更具安全性。
2、本发明提供的于动力学约束的全局-局部混合无人船路径规划方法,针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,通过控制前向和航向角速度,从而实现大范围和近距离的避碰。使得规划的局部路径能更早的对未知环境障碍物避碰,产生的路径更符合无人船运动特性。同时,采用模块化设计方式,可在实际使用中根据实际情况只使用某部分,大大提高了灵活性。
3、本发明提供的于动力学约束的全局-局部混合无人船路径规划方法,能更好地应对复杂多变的障碍物环境,提高了无人船实时规避风险的能力。
基于上述理由本发明可在无人船等领域广泛推广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明方法流程图。
图2为本发明无人船安全范围示意图。
图3(a)为本发明可视时的视线制导策略图。
图3(b)为本发明不可视时的视线制导策略图。
图4为本发明全局路径规划与局部避碰的连接图。
图5为本发明全局规划与其他方法在临海港生成路径对比图。
图6为本发明全局规划与其他方法在大窑湾生成路径对比图。
图7为本发明全局规划与其他方法在临海港生成路径与障碍物最近距离对比图。
图8为本发明全局规划与其他方法在大窑湾生成路径与障碍物最近距离对比图。
图9为本发明方法与FMM方法局部避碰生成路径对比图。
图10为本发明方法在单静态-单动态障碍物环境下生成路径图。
图11为Theta*-FDW在单静态-单动态障碍物环境下生成路径图。
图12为本发明方法在多静态-单动态障碍物环境下生成路径图。
图13为Theta*-FDW在多静态-单动态障碍物环境下生成路径图。
图14为本发明方法在多静态-多动态障碍物环境下生成路径图。
图15为Theta*-FDW在多静态-多动态障碍物环境下生成路径图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
实施例
如图1所示,本发明提供了一种基于动力学约束的全局-局部混合无人船路径规划方法,包括如下步骤:
S1、构建无人船运动学模型,并对构建的无人船运动学模型进行动力学分析;
进一步地作为本发明优选的实施方式,所述步骤S1具体为:
S11、构建无人船运动学模型,所述无人船运动学模型具体如下:
其中,(x,y,ψ)表示无人船在地球坐标系的位置和航向,(u,v,r)分别表示在船体坐标系下无人船的前向速度、横向速度和航向角速度;
S12、针对欠驱动无人船模型,由于前向和偏航动力的限制,横向速度v是有界的,因此在动力学约束的情况下,需约束前向速度和航向角速度,且这两个速度受限于:
S13、设定安全范围,如图2所示,以无人船中心为圆心设定一个在整个避障过程中不能与障碍物接触的安全范围DsA,保证无人船的安全行驶,安全范围DsA被定义为:
其中,ps=[xs,ys]T,ps表示无人船当前位置坐标,xs,ys分别表示无人船当前位置横坐标和纵坐标,p=[x,y]T,ds为安全范围半径,p(t)表示无人船安全范围内点的坐标,x,y分别表示无人船安全范围内点的横坐标和纵坐标。
S2、考虑无人船安全范围,采用存储节点的方式,实现无人船全局路径的规划;
进一步地作为本发明优选的实施方式,所述步骤S2具体是为了得到从起点到目标点的Nw个最佳航路点,并且保证相邻航路点间的连线没有与障碍物发生干涉,使产生的路径Lp安全且最短,Lp的表达式为:
其中,Nw表示产生全局航路点的总数,P0和Pf分别为起始点和目标点。
S21、采用评价函数,评估每个节点的权值,确定全局最佳航路点:
S22、设置Sopen,Sclose,Stree三个点集,分别存储没被扩展的节点、被扩展过的节点、航路点;Sopen和Stree采用三层存储结构比较得出最优航路点并将其存放在Stree中,同时也将其存储在Sclose中,并从Sopen中删除点
进一步地作为本发明优选的实施方式,所述步骤S22具体为:
S221、将起点加入到开放列表Sopen中,起点的父节点定为起点本身;
S223、进一步扩展首先检查没被扩展的子节点是否与的父节点存在视线制导,如图3(a)所示,若的父节点和子节点的连线没有穿过障碍物及边界,则存在视线制导,将的父节点改为同时更新和并且更新Stree;如图3(b)所示,若的父节点和子节点的连线与障碍物相交,则不更新。
S3、针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,实现无人船局部路径的规划;
进一步地作为本发明优选的实施方式,所述步骤S3具体为:
S31、构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,具体如下:
其中,pobs=[x0,y0]T表示障碍物的位置,dso=η·u是由障碍物的速度决定的安全扩展域长度,η>0表示安全扩展系数,在这里传感器的测量范围是0<LFDW<LFDM<∞;LFDW表示FDW模块的作用范围。
S32、所述FDM模糊决策模块为了躲避距离较远的障碍物,通过考虑路径点和障碍物的相对方向,建立模糊规则表;如下述表1所示:
表1
表1中,LF、F和RF分别代表目标点在无人船的左前,正前和右前;类比于目标点的隶属度函数,OBLF,OBF和OBRF代表无人船的左前,正前和右前方有障碍物,其中“N”“Z”“P”代表无人船的输出期望角;
本实施例中,该模块通过精确计算无人船的运动特性与安全距离的约束实现近距离避碰。对于欠驱动无人船,主要考虑前向速度u和航向角速度r这两个动态性能。所以,在有限的时间间隔内,由于受到无人船控制力和扭矩的限制,速度量(u,r)的选取必须符合相应性能要求。在本发明中,假定在一个很短的时间间隔内速度量是一个定量常数,因此在该时间间隔内局部避碰路径视为一段定曲率圆弧,基于以上论述,精准动态窗口模块考虑了可行性、可达性以及可执行性三种限制因素。
S33、在所述FDW精准动态窗口模块中:
通过可达速度集Vr的约束形成速度量动态窗口,且该动态窗口里的速度可以在下一个时间间隔内精准到达,基于当前无人船的速度(uc,rc),可达速度集Vr定义如下:
其中,T>0是时间间隔,uc和rc分别表示无人船当前的前向速度和航向角速度;
基于无人船动力学固有特性影响,可执行性集Vp定义如下:
结合Va、Vr和Vp,最终得出复合约束集Vc:
Vc=Va∩Vr∩Vp
S34、在复合约束集Vc的约束条件下,采用如下评价函数得出最优速度:
Gc(u,r)=α1heading(u,r)+βdist(u,r)+γvelocity(u,r)
其中,heading(u,r)表示使无人船朝向目标点航行的评价因素,其中表示目标点的方向,表示无人船的航向;dist(u,r)表示在相应预测轨迹上无人船与最近障碍物的距离,如果距离太大或者在轨迹附近没有障碍物,则此评价因素被赋值为定量常数;velocity(u,r)表示无人船沿着相应轨迹的航行速度;α>0、β>0、γ>0是定义的权重系数,不同的权值分配会影响避障效果的侧重点。
S4、融合上述无人船全局路径规划方案和无人船局部路径规划方案,形成全局局部混合避障策略。
进一步地作为本发明优选的实施方式,所述步骤S4具体为:
S41、规划无人船全局路径,通过输入的离线地图和起始点规划出一条最优的全局路径;
S42、采用视线制导策略,对全局路径进行优化;
S43、当跟踪全局路径的过程中遇到未知或者不可预知的局部障碍物时,采用FDM模糊决策模块进行远距离的局部避障;
S44、当离障碍物较近时,采用FDW精准动态窗口模块进行近距离的障碍物躲避;
S45、局部避障任务完成时,无人船重新返回到全局路径,完成任务路径跟踪。
进一步地作为本发明优选的实施方式,所述步骤S4还包括在距离障碍物前LFDW处设置了一个虚拟航路点的步骤,如图4所示,该虚拟航路点也是再次进行全局路径跟踪的起始点。
为了检验本发明所提出的基于动力学约束的全局-局部混合无人船路径规划方法的有效性,在本实施例中,我们利用Cybership I船舶在中国辽宁省大连市临海港和大窑湾进行了实景模拟无人船自主路径规划,并且用该发明提出的路径规划方法与其他传统方法进行对比,进一步说明了本发明方法的效果,该船舶模型参数为:
m11=19kg、m22=35.2kg、m33=4.2kg、d11=4kg/s、d22=1kg/s、d33=10kg/s
动力学约束条件如下表2所示:
表2
图5和图6分别显示本发明所提出的方法在临海港和大窑湾与A*方法、A*PS方法和Theta*方法产生的路径。通过下述表3可以看出所提出算法和其他传统算法的在航路点数目、路径长度、方法时长的比较。
表3
图7和图8分别对比了本发明所提出的方法和其他方法产生路径与障碍物的距离,可以看出,利用A*PS和Theta*方法产生的路径,非常接近障碍物。由图6和图8可看出,对于狭窄航道,本发明所提出的方法可以有效避免通过危险路径,然而A*、A*PS和Theta*方法对安全性考虑欠缺。显然本发明所提出的方法更安全。
如下述表4所示,给出了各个方法的综合比对表:
表4
从表4中可以看出,本发明所提出的方法比Theta*方法的速度提高了2%-8%,但比A*方法耗时多了近一倍。但对于初步全局路径规划,20秒的时间是完全可以接受的。
如图9所示,快速行进方法(FMM)产生的路径会在障碍物前出现急转弯,但本发明所提出的方法会生成平滑的路径,充分考虑无人船机动特性,保证了路径的安全。如图10所示,本发明所提出的方法使得无人船能合理地绕过移动船只的尾部。将Theta*与FDW结合与所提出方法作比较,如图11所示,Theta*-FDW产生的路径迫使无人船从移动船只船首强行穿越,由此对比可看出本发明所提出的方法的安全性。如图12和13所示,对于多静态障碍物,本发明所提出的方法能快速采取局部避碰动作。如图14和15所示,本发明所提出的方法可以同时处理多个动、静态障碍物。但Theta*-FDW过于靠近静止障碍物。在遇到多艘船时本发明所提出的方法能有效安全避碰。由表4可得出结论,DGL和Theta*-FDW方法生成的总路径长度几乎相同,且本发明所提出的方法产生的路径更安全。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (6)
1.一种基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,包括如下步骤:
S1、构建无人船运动学模型,并对构建的无人船运动学模型进行动力学分析;所述步骤S1具体为:
S11、构建无人船运动学模型,所述无人船运动学模型具体如下:
其中,(x,y,ψ)表示无人船在地球坐标系的位置和航向,(u,v,r)分别表示在船体坐标系下无人船的前向速度、横向速度和航向角速度;
S12、针对欠驱动无人船模型,由于前向和偏航动力的限制,横向速度v是有界的,因此在动力学约束的情况下,需约束前向速度和航向角速度,且这两个速度受限于:
其中,★∈{u,r}是受限边界;表示前向加速度,表示航向角加速度;
S13、设定安全范围,以无人船中心为圆心设定一个在整个避障过程中不能与障碍物接触的安全范围DsA,保证无人船的安全行驶,安全范围DsA被定义为:
其中,ps=[xs,ys]T,ps表示无人船当前位置坐标,xs,ys分别表示无人船当前位置横坐标和纵坐标,p=[x1,y1]T,ds为安全范围半径,p(t)表示无人船安全范围内点的坐标,x1,y1分别表示无人船安全范围内点的横坐标和纵坐标;
S2、考虑无人船安全范围,采用存储节点的方式,实现无人船全局路径的规划;
S3、针对动态的不可预见的环境,构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,实现无人船局部路径的规划;所述步骤S3具体为:
S31、构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构,具体如下:
其中,pobs=[x0,y0]T表示障碍物的位置,dso=η·u1是由障碍物的速度决定的安全扩展域长度,η>0表示安全扩展系数,在这里传感器的测量范围是0<LFDW<LFDM<∞;LFDW表示FDW模块的作用范围;
S32、所述FDM模糊决策模块为了躲避距离较远的障碍物,通过考虑路径点和障碍物的相对方向,建立模糊规则表;
S33、在所述FDW精准动态窗口模块中:
可行速度限制必须能确保无人船紧急停车实现紧急避碰,因此可行性速度集合定义如下:
其中,l(p,po)是在规划的弧形路径上无人船与最近障碍物的距离;
通过可达速度集Vr的约束形成速度量动态窗口,且该动态窗口里的速度可以在下一个时间间隔内精准到达,基于当前无人船的速度(uc,rc),可达速度集Vr定义如下:
其中,T>0是时间间隔,uc和rc分别表示无人船当前的前向速度和航向角速度;
基于无人船动力学固有特性影响,可执行性集Vp定义如下:
结合Va、Vr和Vp,最终得出复合约束集Vc:
Vc=Va∩Vr∩Vp
S34、在复合约束集Vc的约束条件下,采用如下评价函数得出最优速度:
Gc(u,r)=αheading(u,r)+βdist(u,r)+γvelocity(u,r)
其中,heading(u,r)表示使无人船朝向目标点航行的评价因素,其中表示目标点的方向,表示无人船的航向;dist(u,r)表示在相应预测轨迹上无人船与最近障碍物的距离,如果距离太大或者在轨迹附近没有障碍物,则此评价因素被赋值为定量常数;velocity(u,r)表示无人船沿着相应轨迹的航行速度;α>0、β>0、γ>0是定义的权重系数;
S4、融合上述无人船全局路径规划方案和无人船局部路径规划方案,形成全局局部混合避障策略。
2.根据权利要求1所述的基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,基于动力学约束的全局-局部混合无人船路径规划方法,所述步骤S2具体是为了得到从起点到目标点的Nw个最佳航路点,并且保证相邻航路点间的连线没有与障碍物发生干涉,使产生的路径Lp安全且最短,Lp的表达式为:
其中,Nw表示产生全局航路点的总数,P0和Pf分别为起始点和目标点。
3.根据权利要求1所述的基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,所述步骤S2还包括如下步骤:
S21、采用评价函数,评估每个节点的权值,确定全局最佳航路点:
其中,是当前节点的候选路径点,表示的父节点,表示与起始点间规划的路径长度最小值,其更新迭代过程如下所示:
其中,表示当前点到目标点Pf的欧氏距离;根据当前点的估价值的大小,决定当前点是否被选择到全局路径;表示两相邻航路点和的欧氏距离;
S22、设置Sopen,Sclose,Stree三个点集,分别存储没被扩展的节点、被扩展过的节点、航路点;Sopen和Stree采用三层存储结构比较得出最优航路点并将其存放在Stree中,同时也将其存储在Sclose中,并从Sopen中删除点
4.根据权利要求3所述的基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,所述步骤S22具体为:
S221、将起点加入到开放列表Sopen中,起点的父节点定为起点本身;
S222、更新当前点,当前点为Sopen中估价值最小的点
S223、进一步扩展首先检查没被扩展的子节点是否与的父节点存在视线制导,若的父节点和子节点的连线没有穿过障碍物及边界,则存在视线制导,将的父节点改为同时更新和并且更新Stree;若的父节点和子节点的连线与障碍物相交,则不更新;
S224、最终搜索到目标点,从Stree中获取全局路径完成全局路径规划,其中,p0和pf分别为起点和目标点。
5.根据权利要求1所述的基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,所述步骤S4具体为:
S41、规划无人船全局路径,通过输入的离线地图和起始点规划出一条最优的全局路径;
S42、采用视线制导策略,对全局路径进行优化;
S43、当跟踪全局路径的过程中遇到未知或者不可预知的局部障碍物时,采用FDM模糊决策模块进行远距离的局部避障;
S44、当离障碍物较近时,采用FDW精准动态窗口模块进行近距离的障碍物躲避;
S45、局部避障任务完成时,无人船重新返回到全局路径,完成任务路径跟踪。
6.根据权利要求5所述的基于动力学约束的全局-局部混合无人船路径规划方法,其特征在于,所述步骤S4还包括在距离障碍物前LFDW处设置了一个虚拟航路点的步骤,该虚拟航路点也是再次进行全局路径跟踪的起始点。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010177283.2A CN111399506B (zh) | 2020-03-13 | 2020-03-13 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010177283.2A CN111399506B (zh) | 2020-03-13 | 2020-03-13 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111399506A CN111399506A (zh) | 2020-07-10 |
CN111399506B true CN111399506B (zh) | 2023-04-25 |
Family
ID=71434781
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010177283.2A Active CN111399506B (zh) | 2020-03-13 | 2020-03-13 | 一种基于动力学约束的全局-局部混合无人船路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111399506B (zh) |
Families Citing this family (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113960995A (zh) * | 2020-07-20 | 2022-01-21 | 炬星科技(深圳)有限公司 | 一种避障规划方法、***及设备 |
CN111881580A (zh) * | 2020-07-28 | 2020-11-03 | 交通运输部水运科学研究所 | 一种无人船规避障碍物的运动规划方法 |
CN112207826A (zh) * | 2020-09-28 | 2021-01-12 | 深圳市优必选科技股份有限公司 | 机器人及其路径规划方法和装置 |
CN112578790B (zh) * | 2020-10-30 | 2022-12-23 | 芜湖哈特机器人产业技术研究院有限公司 | 局部路径规划方法及agv小车 |
CN112596513B (zh) * | 2020-10-30 | 2022-12-23 | 芜湖哈特机器人产业技术研究院有限公司 | Agv导航***及agv小车 |
CN112327850B (zh) * | 2020-11-06 | 2023-10-13 | 大连海事大学 | 一种水面无人艇路径规划方法 |
CN112462766B (zh) * | 2020-11-23 | 2022-09-09 | 江苏科技大学 | 一种用于无人船自主避障的装置及方法 |
CN112363512B (zh) * | 2020-11-25 | 2023-08-01 | 中国船舶科学研究中心 | 一种基于艏向及航速的智能船避碰路径规划方法 |
CN112327885B (zh) * | 2020-12-01 | 2024-04-09 | 大连海事大学 | 一种无人船自适应全局-局部混合路径规划的方法 |
CN112556714B (zh) * | 2020-12-08 | 2023-07-07 | 武汉理工光科股份有限公司 | 一种消防灭火救援智能路径规划方法及*** |
CN112947447B (zh) * | 2021-02-10 | 2023-11-24 | 大连理工大学 | 一种基于同步规划-控制策略的水面无人舰艇自主导航方法 |
CN113296519A (zh) * | 2021-05-26 | 2021-08-24 | 上海大学 | 一种基于麦克纳姆轮的移动机器人运动规划方法及*** |
CN113296521B (zh) * | 2021-05-26 | 2021-11-05 | 紫清智行科技(北京)有限公司 | 一种基于动态窗口的障碍物局部避障方法 |
CN113341970A (zh) * | 2021-06-01 | 2021-09-03 | 苏州天准科技股份有限公司 | 智能巡检导航避障***、方法、存储介质和巡检车 |
CN113253761B (zh) * | 2021-06-08 | 2021-11-09 | 北京远度互联科技有限公司 | 实时路径规划方法、装置、可移动载具及存储介质 |
CN113671968B (zh) * | 2021-08-25 | 2023-12-12 | 大连海事大学 | 一种无人水面艇实时避碰方法 |
CN113741477B (zh) * | 2021-09-14 | 2022-10-21 | 中国船舶重工集团公司第七0七研究所九江分部 | 一种欠驱动船舶靠泊路径规划方法 |
CN114237256B (zh) * | 2021-12-20 | 2023-07-04 | 东北大学 | 一种适用于欠驱动机器人的三维路径规划与导航方法 |
CN114281083B (zh) * | 2021-12-28 | 2024-05-14 | 江苏大学 | 一种基于混合路径规划自主导航的无人船水质监测物联网控制***及方法 |
CN115817535B (zh) * | 2022-12-23 | 2023-09-29 | 远峰科技股份有限公司 | 基于模糊决策的自动泊车路径规划方法和装置 |
CN117234206B (zh) * | 2023-09-05 | 2024-05-14 | 酷哇科技有限公司 | 基于复杂障碍物场景下的避障路径规划方法 |
CN117308964B (zh) * | 2023-11-24 | 2024-02-13 | 陕西欧卡电子智能科技有限公司 | 智能游船的路径规划方法、装置、无人船及介质 |
CN117950398B (zh) * | 2023-11-27 | 2024-07-09 | 江苏海洋大学 | 基于速度障碍与模糊参数的无人艇规划方法 |
CN118131783B (zh) * | 2024-05-10 | 2024-07-05 | 山东科技大学 | 一种基于改进融合算法的多机器人路径规划方法 |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101738195A (zh) * | 2009-12-24 | 2010-06-16 | 厦门大学 | 基于环境建模与自适应窗口的移动机器人路径规划方法 |
CN103558856A (zh) * | 2013-11-21 | 2014-02-05 | 东南大学 | 动态环境下服务动机器人导航方法 |
CN104965518A (zh) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | 基于三维数字地图的电力巡检飞行机器人航线规划方法 |
CN108444490A (zh) * | 2018-03-16 | 2018-08-24 | 江苏开放大学(江苏城市职业学院) | 基于可视图和a*算法深度融合的机器人路径规划方法 |
CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
CN108733062A (zh) * | 2018-06-25 | 2018-11-02 | 山东大学 | 家庭陪护机器人自主充电***及方法 |
CN108958282A (zh) * | 2018-06-21 | 2018-12-07 | 浙江大学 | 基于动态球形窗口的三维空间路径规划方法 |
CA3076498A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corp. | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
CN110032184A (zh) * | 2019-03-12 | 2019-07-19 | 哈尔滨工程大学 | 一种基于遗传与滑动窗口法的水上无人艇避碰方法 |
-
2020
- 2020-03-13 CN CN202010177283.2A patent/CN111399506B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101738195A (zh) * | 2009-12-24 | 2010-06-16 | 厦门大学 | 基于环境建模与自适应窗口的移动机器人路径规划方法 |
CN103558856A (zh) * | 2013-11-21 | 2014-02-05 | 东南大学 | 动态环境下服务动机器人导航方法 |
CN104965518A (zh) * | 2015-05-21 | 2015-10-07 | 华北电力大学 | 基于三维数字地图的电力巡检飞行机器人航线规划方法 |
CA3076498A1 (en) * | 2017-09-22 | 2019-03-28 | Locus Robotics Corp. | Dynamic window approach using optimal reciprocal collision avoidance cost-critic |
CN108444490A (zh) * | 2018-03-16 | 2018-08-24 | 江苏开放大学(江苏城市职业学院) | 基于可视图和a*算法深度融合的机器人路径规划方法 |
CN108594834A (zh) * | 2018-03-23 | 2018-09-28 | 哈尔滨工程大学 | 一种面向未知环境下多auv自适应目标搜索和避障方法 |
CN108958282A (zh) * | 2018-06-21 | 2018-12-07 | 浙江大学 | 基于动态球形窗口的三维空间路径规划方法 |
CN108733062A (zh) * | 2018-06-25 | 2018-11-02 | 山东大学 | 家庭陪护机器人自主充电***及方法 |
CN110032184A (zh) * | 2019-03-12 | 2019-07-19 | 哈尔滨工程大学 | 一种基于遗传与滑动窗口法的水上无人艇避碰方法 |
Non-Patent Citations (5)
Title |
---|
吴博 ; 文元桥 ; 吴贝 ; 周思杨 ; 肖长诗 ; .水面无人艇避碰方法回顾与展望.武汉理工大学学报(交通科学与工程版).2016,第40卷(第03期),第456-461页. * |
张亚博 ; 姜明 ; 肖强 ; 王军 ; 陈霖 ; 张海燕 ; .一种欠驱动无人船避障及目标跟踪鲁棒控制方法.无人***技术.2019,第2卷(第06期),第40-45页. * |
汪四新 ; 谭功全 ; 罗玉丰 ; 樊石 ; .基于改进人工势场-模糊算法的路径规划算法研究.四川理工学院学报(自然科学版).2019,第32卷(第05期),第42-48页. * |
苏义鑫 ; 石兵华 ; 张华军 ; .水面无人艇航迹规划算法设计与仿真.武汉理工大学学报.2016,第38卷(第06期),第84-88页. * |
范云生 ; 孙晓界 ; 王国峰 ; 郭晨 ; .一种无人水面艇自主动态避碰跟踪控制方法.***仿真学报.2018,第30卷(第10期),第3781-3788页. * |
Also Published As
Publication number | Publication date |
---|---|
CN111399506A (zh) | 2020-07-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111399506B (zh) | 一种基于动力学约束的全局-局部混合无人船路径规划方法 | |
CN111780777B (zh) | 一种基于改进a*算法和深度强化学习的无人车路径规划方法 | |
Wu | Coordinated path planning for an unmanned aerial-aquatic vehicle (UAAV) and an autonomous underwater vehicle (AUV) in an underwater target strike mission | |
CN109933067B (zh) | 一种基于遗传算法和粒子群算法的无人艇避碰方法 | |
CN108398960B (zh) | 一种改进APF与分段Bezier相结合的多无人机协同目标追踪方法 | |
CN112034887A (zh) | 无人机躲避柱状障碍物到达目标点的最优路径训练方法 | |
Nguyen et al. | Energy-constrained motion planning for information gathering with autonomous aerial soaring | |
CN111830978A (zh) | 一种欠驱动无人艇避障路径规划及控制方法及*** | |
Li et al. | Three-dimensional path planning for AUVs in ocean currents environment based on an improved compression factor particle swarm optimization algorithm | |
Yang et al. | Intelligent path planning of underwater robot based on reinforcement learning | |
CN115143970A (zh) | 一种基于威胁度评估的水下航行器避障方法和*** | |
Zeng et al. | Path planning for rendezvous of multiple AUVs operating in a variable ocean | |
Zhang et al. | Research on Path Planning Method of Unmanned Boat Based on Improved Artificial Potential Field Method | |
CN115657683B (zh) | 一种可用于巡检作业任务的无人无缆潜水器实时避障方法 | |
Zhi-Wen et al. | Path planning for UUV in dynamic environment | |
CN115877837A (zh) | 一种改进动态窗口算法的差速轮式机器人路径规划方法 | |
Zhao et al. | Local path planning for unmanned surface vehicles based on hybrid A* and B-spline | |
CN115617076A (zh) | 近地搜索无人机的轨迹规划及动态避障方法 | |
Wang et al. | Stepwise cooperative trajectory planning for multiple BUVs based on temporal–spatial Bezier curves | |
Zhang et al. | Local path planning of unmanned underwater vehicle based on improved APF and rolling window method | |
Leng et al. | UAV path planning in 3D complex environments using genetic algorithms | |
CN114756017A (zh) | 一种无人机与无人艇结合的导航避障方法 | |
CN112799414A (zh) | 一种auv松弛轨迹规划方法 | |
Wei et al. | A hybrid algorithm of UAV path planning for rescue in bushfire environments | |
Novikov et al. | Navigation Method in Non-Stationary Environment for Embedded Control System of Mobile Robots |
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 |