CN113219984A - 一种水面无人艇全区域覆盖路径规划方法 - Google Patents
一种水面无人艇全区域覆盖路径规划方法 Download PDFInfo
- Publication number
- CN113219984A CN113219984A CN202110542102.6A CN202110542102A CN113219984A CN 113219984 A CN113219984 A CN 113219984A CN 202110542102 A CN202110542102 A CN 202110542102A CN 113219984 A CN113219984 A CN 113219984A
- Authority
- CN
- China
- Prior art keywords
- grid
- unmanned
- planning
- path
- area
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000003993 interaction Effects 0.000 claims abstract description 5
- 238000012544 monitoring process Methods 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000010845 search algorithm Methods 0.000 claims description 3
- 238000007726 management method Methods 0.000 claims description 2
- 230000008569 process Effects 0.000 description 11
- 238000004364 calculation method Methods 0.000 description 4
- 238000001514 detection method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 241000283690 Bos taurus Species 0.000 description 1
- PPTYJKAXVCCBDU-UHFFFAOYSA-N Rohypnol Chemical compound N=1CC(=O)N(C)C2=CC=C([N+]([O-])=O)C=C2C=1C1=CC=CC=C1F PPTYJKAXVCCBDU-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000004387 environmental modeling Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000009313 farming Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000035484 reaction time Effects 0.000 description 1
- 238000006467 substitution reaction Methods 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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种水面无人艇全区域覆盖路径规划方法,属于无人船技术领域,本发明首先采用栅格法,完成无人艇执行任务区域的环境建模;其次,采用生成树覆盖算法,对工作区域的全区域路径进行规划;其算法简单,计算量小,可以快速生成无重复路径,完全覆盖的路径;最后,采用贝塞尔曲线对生成树覆盖算法生成的转弯路径进行优化,生成平滑且处处可微的轨迹。本发明使用地面站人机交互界面与栅格法结合的技术方案,使工作环境更具有实际意义。采用STC算法进行区域覆盖的技术方案,能够快速的进行覆盖区域的路径规划。
Description
技术领域
本发明涉及无人船技术领域,具体而言,尤其涉及一种水面无人艇全区域覆盖路径规划方法。
背景技术
水面无人艇是海洋开发的重要设备,可以实现勘探、巡航、测量等诸多功能,是海洋资源开发的重要设备。对于水面无人艇的自主航行,路径决策是一项重要的内容,良好的路径规划算法可以制定更好的路径,使路径的损耗更小,更符合无人船的运动特性。路径规划可以分为点对点与完全遍历两种方式,点对点的路径规划已经取得了较大的发展,完全遍历路径规划问题的研究相对较少,这一问题随着应用需求的增加而受到人们关注。完全遍历路径规划常见的方法如:单元分解、栅格法、牛耕法、内螺旋遍历算法、A-Star以及遗传算法等。
发明内容
根据上述提出的技术问题,而提供一种水面无人艇全区域覆盖路径规划方法。本发明首先利用栅格法,完成无人艇执行任务区域的环境建模;其次采用生成树覆盖算法,对工作区域的全区域路径进行规划;其算法简单,计算量小,可以快速生成无重复路径,完全覆盖的路径;最后采用贝塞尔曲线对生成树覆盖算法生成的转弯路径进行优化,生成平滑且处处可微的轨迹。
本发明采用的技术手段如下:
一种水面无人艇全区域覆盖路径规划方法,包括如下步骤:
S1、采用栅格法,完成无人艇执行任务区域的环境建模;
S2、采用生成树覆盖算法,对工作区域的全区域路径进行规划;
S3、采用贝塞尔曲线对生成树覆盖算法生成的转弯路径进行优化,生成平滑且处处可微的轨迹。
进一步地,所述步骤S1的具体实现过程如下:
S11、定义无人艇移动范围、可达区域、不可达区域、无障碍物区域、无人船开始工作后遍历过的栅格、环境地图的最长长度、最长宽度、每个栅格的尺寸、环境地图中的尺寸;
S12、选取栅格的大小;
S13、地面站确定环境边界;在地面站上位机电子地图中选出n个坐标点,将n个坐标点的经纬度作为输入,用直线依次将n个坐标点连接形成一个多边形,多边形的边为环境边界;
S14、将环境地图进行栅格化,完成环境建模,得到栅格地图MAP。
进一步地,所述步骤S2的具体实现过程如下:
S21、输入步骤:几何描述的环境,大小为2x2栅格的图形结构G,设置起点为S;
S22、预处理步骤:从起点S开始,利用深度优先搜寻算法或普里姆算法给图形结构G构建一个生成树,把大小2x2的栅格分解成四个大小为1的子单元格;
S23、执行覆盖步骤:从起点S的子单元开始,沿生成树逆时针方向在相邻的子单元格间覆盖,直到开始的单元格再次覆盖,则停止完成覆盖。
进一步地,所述步骤S3的具体实现过程如下:
S31、假设存在一条含有m个控制点P0,P1,P2,...Pm的贝塞尔曲线,其数学表达式如下所示:
S32、选择4次贝塞尔曲线对拐点进行平滑处理,其中4次贝塞尔曲线的表达式为:
P(t)=P0(1-t)4+4P1(1-t)3t+6P2(1-t)2t2+4P3(1-t)t3+P4t4
式中,t∈[0,1];其中P0~P4为5个控制点,坐标分别用(x0,y0)~(x4,y4)表示,曲线上任意一点的曲率公式为:
得到P0和P4控制点的位置约束条件,通过确定其余三个点即可确定一条四阶贝塞尔曲线;
S33、假设无人艇最小转弯半径为R,得到最大曲率为kmax=1/R,最小的曲率为kmin=0;所以,将曲率的约束条件表示为:kmin≤k(t)≤kmax。
进一步地,所述步骤S11中:
定义无人艇移动范围为S,S={(x,y)|x,y}x,y表示无人艇移动范围内的点;
定义无人船开始工作后遍历过的栅格为ocup(x,y)=2;
定义环境地图的最长长度为Ll米,最长宽度为Lk米,每个栅格的尺寸均为Dsquare米,环境地图中的尺寸为(Ll/Dsquare)×(Lk/Dsquare)。
进一步地,所述步骤S12中选取栅格的大小需要考虑如下条件:
条件一:环境地图的大小与障碍物的复杂程度;
条件二:无人船的船身尺寸,环境地图中栅格的边长不小于无人船的船宽;
条件三:实际应用情况、不同的应用场景和用途;
条件四:计算机的运算和存储能力。
进一步地,所述步骤S13中的地面站是人机交互的智能化控制平台,用户通过地面站实现对无人船的实时管控操作,给无人船发布任务,规划航线;监测无人船的状态,包括无人船运行时间、经纬度、加速度、角速度、角度、GPS航向、GPS地速、超声波数据、工控机和电池能源板的温度;视频监控无人船的周围环境。
进一步地,所述步骤S14具体包括如下步骤:
S141、将输入的坐标数组S0={P1(x1,y1),P2(x2,y2),…,Pn(xn,yn)}的经纬度乘以107,得到新的数组S'0={P'1(x1,y1),P'2(x2,y2),…,P'n(xn,yn)};
S142、在数组S'0中搜索xi、yi最小值为xmin、ymin,将(xmin,ymin)作为基数;
S144、每个栅格的长宽都为Dsquare米,Dsquare米对应的纬度为(sin(0.000009×Dsquare))°,Dsquare米对于的经度值为(cos(0.000009×Dsquare))°;将数组SM的每个元素Pi M(xi,yi)中的xi/(90×Dsquare)、yi/(103×Dsquare),得到数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)},并对元素的经纬度值向下取整,得到新的数组U={U1(x1,y1),U2(x2,y2),…,Un(xn,yn)};
S145、在数组U中搜索xi、yi最大值为xmax、ymax,构建的栅格地图的X轴长为xmax+1,Y轴长为ymax+1;将数组U的元素在栅格地图中标记为黑色,栅格值ocup(x,y)=0;
S146、依次将数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)}的元素连接,形成线段D1D2、D2D3、.Dn-1Dn、DnD1,将所有线段经过的栅格标记为黑色,栅格值ocup(x,y)=0;
S147、利用两个循环,填充边界外围的栅格为黑色,栅格值ocup(x,y)=0;首先取Y轴的中心栅格值为middle=(xmax/2)+1,第一个双循环是从1到ymax,嵌套循环从1到middle,每次加1,当第一次找到该栅格的值时,栅格值ocup(x,y)=0,将从该栅格到第一个栅格的值设置为0;第二个循环从1到ymax,嵌套循环是从ymax到middle,每次减1,当第一次找到该栅格的栅格值ocup(x,y)=0时,将中间的栅格栅格值设置为ocup(x,y)=0;
S148、通过往返前进算法规划遍历的路径,放入数组J={J1(x1,y1),J2(x2,y2),…,Jn(xn,yn)},确定起点坐标(xstart,ystart)、终点坐标(xend,yend),数组J中的元素都是单位坐标系,将数组J的每个元素J(xi,yi)中的xi×(90×Dsquare)+xmin、yi×(103×Dsquare)+ymin,得到经纬度坐标系数组Z={Z1(x1,y1),Z2(x2,y2),…,Zn(xn,yn)},数组Z的元素就是路径规划中无人艇将要行驶坐标点的经纬度;
S149、提取出四个坐标点P1、P2、P3、P4的经纬度数据,在经纬度坐标系中标记;将经纬度坐标转化为栅格坐标系,依次将四个点用直线连;再将直线经过的栅格标记为黑色,栅格值设置为ocup(x,y)=0;最后填充边界外围的栅格为黑色,栅格值为ocup(x,y)=0。
较现有技术相比,本发明具有以下优点:
1、本发明提供的水面无人艇全区域覆盖路径规划方法,使用地面站人机交互界面与栅格法结合,使工作环境更具有实际意义。
2、本发明提供的水面无人艇全区域覆盖路径规划方法,采用STC算法进行区域覆盖,能够快速的进行覆盖区域的路径规划。
基于上述理由本发明可在无人船等领域广泛推广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明方法流程图。
图2为本发明实施例提供的栅格地图。
图3为本发明实施例提供的在图2中的栅格地图下形成生成树。
图4为本发明实施例提供的生成树覆盖。
图5为本发明实施例提供的平滑路径。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
如图1所示,本发明提供了一种水面无人艇全区域覆盖路径规划方法,包括如下步骤:
S1、采用栅格法,完成无人艇执行任务区域的环境建模;
建立栅格地图模型是采用一定的数学模型来表示无人船的工作区域。地图栅格化是将无人艇执行任务的区域划分为一系列大小相同的栅格正方形结构空间,并根据可达区域和不可达区域的情况将该栅格设置不同的值。栅格坐标系的位置与经纬度坐标相对应,能够准确定位,方便于环境建模和算法规划,更具实际应用价值。具体实施时,作为本发明优选的实施方式,所述步骤S1的具体实现过程如下:
S11、定义无人艇移动范围、可达区域、不可达区域、无障碍物区域、无人船开始工作后遍历过的栅格、环境地图的最长长度、最长宽度、每个栅格的尺寸、环境地图中的尺寸;
所述步骤S11中:
定义无人艇移动范围为S,S={(x,y)|x,y}x,y表示无人艇移动范围内的点;
定义无人船开始工作后遍历过的栅格为ocup(x,y)=2;
定义环境地图的最长长度为Ll米,最长宽度为Lk米,每个栅格的尺寸均为Dsquare米,环境地图中的尺寸为(Ll/Dsquare)×(Lk/Dsquare)。
S12、选取栅格的大小;
在栅格环境地图建模过程中,栅格大小的选取是一个很重要的问题,其直接影响环境地图信息的精度,也同样影响着完全遍历路径规划算法的性能和效率。栅格大小的确定要考虑很多因素,比如无人船的大小、计算机的存储能力、水下地形探测时声呐的探测宽度、水面垃圾打捞装置的宽度、摄像头拍摄有效画面的宽度等。
环境地图划分的栅格设置的越大,无人船抗干扰能力越强,环境地图的信息存储量就越小、分辨率降低,计算机对环境信息的描述就越粗糙,但计算机的计算速度会越快,计算时间也会变短,无人船反应时间会越快,路径规划时间也会短。反之,地图划分的栅格设置越小,无人船抗干扰能力越弱,环境地图的信息储存量就会越大、分辨率提高,计算机对环境信息的描述也就越详细,但是计算机的计算速度会变慢,计算时间也会变长,无人船的反应会变慢,路径规划时间也会变长。具体实施时,作为本发明优选的实施方式,所述步骤S12中选取栅格的大小需要考虑如下条件:
条件一:环境地图的大小与障碍物的复杂程度;
条件二:无人船的船身尺寸,环境地图中栅格的边长不小于无人船的船宽;
条件三:实际应用情况、不同的应用场景和用途;
条件四:计算机的运算和存储能力。
S13、地面站确定环境边界;在地面站上位机电子地图中选出n个坐标点,S0={P1(x1,y1),P2(x2,y2),…,Pn(xn,yn)},将n个坐标点的经纬度作为输入,用直线依次将n个坐标点连接形成一个多边形,多边形的边为环境边界;
其中,地面站是人机交互的智能化控制平台,用户通过地面站实现对无人船的实时管控操作,给无人船发布任务,规划航线;监测无人船的状态,包括无人船运行时间、经纬度、加速度、角速度、角度、GPS航向、GPS地速、超声波数据、工控机和电池能源板的温度;视频监控无人船的周围环境。
S14、将环境地图进行栅格化,完成环境建模,得到栅格地图MAP。
无人船作为水面交通工具,可以看成是在二维平面内运动。具体实施时,作为本发明优选的实施方式,所述步骤S14具体包括如下步骤:
S141、将输入的坐标数组S0={P1(x1,y1),P2(x2,y2),…,Pn(xn,yn)}的经纬度乘以107,得到新的数组S'0={P'1(x1,y1),P'2(x2,y2),…,P'n(xn,yn)};
S142、在数组S'0中搜索xi、yi最小值为xmin、ymin,将(xmin,ymin)作为基数;
S144、每个栅格的长宽都为Dsquare米,Dsquare米对应的纬度为(sin(0.000009×Dsquare))°,Dsquare米对于的经度值为(cos(0.000009×Dsquare))°;将数组SM的每个元素Pi M(xi,yi)中的xi/(90×Dsquare)、yi/(103×Dsquare),得到数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)},并对元素的经纬度值向下取整,得到新的数组U={U1(x1,y1),U2(x2,y2),…,Un(xn,yn)};
S145、在数组U中搜索xi、yi最大值为xmax、ymax,构建的栅格地图的X轴长为xmax+1,Y轴长为ymax+1;将数组U的元素在栅格地图中标记为黑色,栅格值ocup(x,y)=0;
S146、依次将数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)}的元素连接,形成线段D1D2、D2D3、Dn-1Dn、DnD1,将所有线段经过的栅格标记为黑色,栅格值ocup(x,y)=0;
S147、利用两个循环,填充边界外围的栅格为黑色,栅格值ocup(x,y)=0;首先取Y轴的中心栅格值为middle=(xmax/2)+1,第一个双循环是从1到ymax,嵌套循环从1到middle,每次加1,当第一次找到该栅格的值时,栅格值ocup(x,y)=0,将从该栅格到第一个栅格的值设置为0;第二个循环从1到ymax,嵌套循环是从ymax到middle,每次减1,当第一次找到该栅格的栅格值ocup(x,y)=0时,将中间的栅格栅格值设置为ocup(x,y)=0;
S148、通过往返前进算法规划遍历的路径,放入数组J={J1(x1,y1),J2(x2,y2),…,Jn(xn,yn)},确定起点坐标(xstart,ystart)、终点坐标(xend,yend),数组J中的元素都是单位坐标系,将数组J的每个元素J(xi,yi)中的xi×(90×Dsquare)+xmin、yi×(103×Dsquare)+ymin,得到经纬度坐标系数组Z={Z1(x1,y1),Z2(x2,y2),…,Zn(xn,yn)},数组Z的元素就是路径规划中无人艇将要行驶坐标点的经纬度;
S149、提取出四个坐标点P1、P2、P3、P4的经纬度数据,在经纬度坐标系中标记;将经纬度坐标转化为栅格坐标系,依次将四个点用直线连;再将直线经过的栅格标记为黑色,栅格值设置为ocup(x,y)=0;最后填充边界外围的栅格为黑色,栅格值为ocup(x,y)=0。
S2、采用生成树覆盖算法,对工作区域的全区域路径进行规划;
无人船作为水域交通的重要个体,其路径规划一直是研究的热点,路径规划是无人船完成任务的前提和保障。随着无人船的应用越来越广泛,各种各样的产品相继出现,用户对无人船的功能要求也不断提高,其中一个基本的需求就是实现完全遍历路径规划。本实施例中将详细介绍无人船完全遍历路径规划。
一、区域覆盖路径规划的核心问题
环境建模和路径搜索是无人船路径规划的核心问题。路径规划就是在某个环境中,通过路径搜索方法找到一条从起点到目标点的最优路径。路径规划中主要考虑以下几个问题:
(1)无人船在行驶过程中,具有能避开障碍物和导航定位的能力;
(2)如何有效地表示任务环境,便于规划路径;
(3)如何选取有效的路径搜索方法,寻找一条最优路径。
(4)路径搜索是在某空间中利用搜索技术进行搜索并判断,然后做出决策找到一条路径。路径搜索是在实际环境中通过自身传感器探测周围环境,在已经建立的环境模型中,通过搜索技术找到一条通路。
二、区域覆盖算法性能评价指标
无人船完全遍历算法的性能主要采用以下五个性能指标来衡量:
(1)遍历覆盖率:是指路径规划完成后,无人艇所行驶过的面积与可达区域的比例,最后表示结果是一个百分比形式,如下所示:
ZH=SH/Sφ
其中,SH代表的是无人船实际行驶的距离,ZH代表的是遍历覆盖率。完全遍历时,尽可能地保证所有可达安全区域都要遍历。遍历覆盖百分比越高,说明该算法的性能越好。
(2)遍历重叠率:是指路径规划完成后,无人艇在行驶过程中出现两次或者多次重复遍历的面积总和与可达安全区域面积的比值,如下所示:
ZC=SC/Sφ
其中,SC代表的是重复的行驶路径之和,ZC代表的是遍历重叠率。
(3)转弯次数:是指无人艇在完成遍历行驶任务中,不可能避免的改变方向的次数。本实施例中,默认该无人艇只能进行90°的转向,即无人艇只能向前、后、左、右四个方向进行行驶。用N代表无人艇的转弯次数。由于无人艇在转弯的时候,消耗的能量往往比同等距离下直行更大,速度更慢,所有无人艇在遍历任务过程中,转弯次数越小,说明路径规划的算法更好。
(4)路径总长度:是指无人船完成遍历路径规划后,得出的无人船移动距离。在栅格地图下,无人船行驶的栅格步数NSTEP和路径总长度LPATH的关系如下所示:
LPATH=NSTEP×Dsquare
在相同的遍历覆盖率下,路径总长度越短,说明遍历算法越好。
(5)工作时间:是指无人船从开始到结束所用的时间总和Ttotal,在相同的遍历覆盖率下,工作总时间越短,说明算法的性能越好。
STC(生成树覆盖)算法,通过巧妙的将四个单元格构造城一个大的单元格,每个大单元格是畅通或者阻碍的(前提是阻碍区域不能小于单元大小的四倍)。然后将每个畅通的大单元格转化为一个节点,为每个相邻的节点加入连接线,然后使用生成树算法构建最小生成树。无人艇沿逆时针(或顺时针)方向环绕生成树,生成一个简单的闭合路径,也就是覆盖问题的最优解。
具体实施时,作为本发明优选的实施方式,所述步骤S2的具体实现过程如下:
S21、输入步骤:几何描述的环境,大小为2x2栅格的图形结构G,如图2所示,设置起点为S;
S22、预处理步骤:从起点S开始,利用深度优先搜寻算法或普里姆算法给图形结构G构建一个生成树,如图3所示,把大小2x2的栅格分解成四个大小为1的子单元格;
S23、执行覆盖步骤:从起点S的子单元开始,沿生成树逆时针方向在相邻的子单元格间覆盖,直到开始的单元格再次覆盖,则停止完成覆盖。如图4所示。
S3、采用贝塞尔曲线对生成树覆盖算法生成的转弯路径进行优化,生成平滑且处处可微的轨迹,如图5所示。
在无人系统的运动规划中,规划的轨迹应满足于以下准则:轨迹连续;轨迹曲率连续;轨迹基本满足系统动力学约束,易被系统跟随,且轨迹容易生成。贝塞尔曲线是由少量特征控制点、起始点已经终止点决定的一条连续平滑曲线。它通过自身的高阶导数连续性,来保证曲线在起点和终点之间平滑变化,有助于解决路径规划中冗余节点问题。具体实施时,作为本发明优选的实施方式,所述步骤S3的具体实现过程如下:
S31、假设存在一条含有m个控制点P0,P1,P2,...Pm的贝塞尔曲线,其数学表达式如下所示:
S32、选择4次贝塞尔曲线对拐点进行平滑处理,其中4次贝塞尔曲线的表达式为:
P(t)=P0(1-t)4+4P1(1-t)3t+6P2(1-t)2t2+4P3(1-t)t3+P4t4
式中,t∈[0,1];其中P0~P4为5个控制点,坐标分别用(x0,y0)~(x4,y4)表示,曲线上任意一点的曲率公式为:
得到P0和P4控制点的位置约束条件,通过确定其余三个点即可确定一条四阶贝塞尔曲线;
S33、假设无人艇最小转弯半径为R,得到最大曲率为kmax=1/R,最小的曲率为kmin=0;所以,将曲率的约束条件表示为:kmin≤k(t)≤kmax。
本实施例中,选择贝塞尔曲线来对STC算法拐点处进行优化,利用4次贝塞尔曲线对转弯进行优化,使无人艇的轨迹更加平滑,对无人艇自身的冲击也将减小。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (8)
1.一种水面无人艇全区域覆盖路径规划方法,其特征在于,包括如下步骤:
S1、采用栅格法,完成无人艇执行任务区域的环境建模;
S2、采用生成树覆盖算法,对工作区域的全区域路径进行规划;
S3、采用贝塞尔曲线对生成树覆盖算法生成的转弯路径进行优化,生成平滑且处处可微的轨迹。
2.根据权利要求1所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S1的具体实现过程如下:
S11、定义无人艇移动范围、可达区域、不可达区域、无障碍物区域、无人船开始工作后遍历过的栅格、环境地图的最长长度、最长宽度、每个栅格的尺寸、环境地图中的尺寸;
S12、选取栅格的大小;
S13、地面站确定环境边界;在地面站上位机电子地图中选出n个坐标点,将n个坐标点的经纬度作为输入,用直线依次将n个坐标点连接形成一个多边形,多边形的边为环境边界;
S14、将环境地图进行栅格化,完成环境建模,得到栅格地图MAP。
3.根据权利要求1所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S2的具体实现过程如下:
S21、输入步骤:几何描述的环境,大小为2x2栅格的图形结构G,设置起点为S;
S22、预处理步骤:从起点S开始,利用深度优先搜寻算法或普里姆算法给图形结构G构建一个生成树,把大小2x2的栅格分解成四个大小为1的子单元格;
S23、执行覆盖步骤:从起点S的子单元开始,沿生成树逆时针方向在相邻的子单元格间覆盖,直到开始的单元格再次覆盖,则停止完成覆盖。
4.根据权利要求1所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S3的具体实现过程如下:
S31、假设存在一条含有m个控制点P0,P1,P2,...Pm的贝塞尔曲线,其数学表达式如下所示:
S32、选择4次贝塞尔曲线对拐点进行平滑处理,其中4次贝塞尔曲线的表达式为:
P(t)=P0(1-t)4+4P1(1-t)3t+6P2(1-t)2t2+4P3(1-t)t3+P4t4
式中,t∈[0,1];其中P0~P4为5个控制点,坐标分别用(x0,y0)~(x4,y4)表示,曲线上任意一点的曲率公式为:
得到P0和P4控制点的位置约束条件,通过确定其余三个点即可确定一条四阶贝塞尔曲线;
S33、假设无人艇最小转弯半径为R,得到最大曲率为kmax=1/R,最小的曲率为kmin=0;所以,将曲率的约束条件表示为:kmin≤k(t)≤kmax。
6.根据权利要求2所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S12中选取栅格的大小需要考虑如下条件:
条件一:环境地图的大小与障碍物的复杂程度;
条件二:无人船的船身尺寸,环境地图中栅格的边长不小于无人船的船宽;
条件三:实际应用情况、不同的应用场景和用途;
条件四:计算机的运算和存储能力。
7.根据权利要求2所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S13中的地面站是人机交互的智能化控制平台,用户通过地面站实现对无人船的实时管控操作,给无人船发布任务,规划航线;监测无人船的状态,包括无人船运行时间、经纬度、加速度、角速度、角度、GPS航向、GPS地速、超声波数据、工控机和电池能源板的温度;视频监控无人船的周围环境。
8.根据权利要求2所述的水面无人艇全区域覆盖路径规划方法,其特征在于,所述步骤S14具体包括如下步骤:
S141、将输入的坐标数组S0={P1(x1,y1),P2(x2,y2),…,Pn(xn,yn)}的经纬度乘以107,得到新的数组S′0={P′1(x1,y1),P′2(x2,y2),…,P′n(xn,yn)};
S142、在数组S′0中搜索xi、yi最小值为xmin、ymin,将(xmin,ymin)作为基数;
S144、每个栅格的长宽都为Dsquare米,Dsquare米对应的纬度为(sin(0.000009×Dsquare))°,Dsquare米对于的经度值为(cos(0.000009×Dsquare))°;将数组SM的每个元素Pi M(xi,yi)中的xi/(90×Dsquare)、yi/(103×Dsquare),得到数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)},并对元素的经纬度值向下取整,得到新的数组U={U1(x1,y1),U2(x2,y2),…,Un(xn,yn)};
S145、在数组U中搜索xi、yi最大值为xmax、ymax,构建的栅格地图的X轴长为xmax+1,Y轴长为ymax+1;将数组U的元素在栅格地图中标记为黑色,栅格值ocup(x,y)=0;
S146、依次将数组D={D1(x1,y1),D2(x2,y2),…,Dn(xn,yn)}的元素连接,形成线段D1D2、D2D3、·Dn-1Dn、DnD1,将所有线段经过的栅格标记为黑色,栅格值ocup(x,y)=0;
S147、利用两个循环,填充边界外围的栅格为黑色,栅格值ocup(x,y)=0;首先取Y轴的中心栅格值为middle=(xmax/2)+1,第一个双循环是从1到ymax,嵌套循环从1到middle,每次加1,当第一次找到该栅格的值时,栅格值ocup(x,y)=0,将从该栅格到第一个栅格的值设置为0;第二个循环从1到ymax,嵌套循环是从ymax到middle,每次减1,当第一次找到该栅格的栅格值ocup(x,y)=0时,将中间的栅格栅格值设置为ocup(x,y)=0;
S148、通过往返前进算法规划遍历的路径,放入数组J={J1(x1,y1),J2(x2,y2),…,Jn(xn,yn)},确定起点坐标(xstart,ystart)、终点坐标(xend,yend),数组J中的元素都是单位坐标系,将数组J的每个元素J(xi,yi)中的xi×(90×Dsquare)+xmin、yi×(103×Dsquare)+ymin,得到经纬度坐标系数组Z={Z1(x1,y1),Z2(x2,y2),…,Zn(xn,yn)},数组Z的元素就是路径规划中无人艇将要行驶坐标点的经纬度;
S149、提取出四个坐标点P1、P2、P3、P4的经纬度数据,在经纬度坐标系中标记;将经纬度坐标转化为栅格坐标系,依次将四个点用直线连;再将直线经过的栅格标记为黑色,栅格值设置为ocup(x,y)=0;最后填充边界外围的栅格为黑色,栅格值为ocup(x,y)=0。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110542102.6A CN113219984A (zh) | 2021-05-18 | 2021-05-18 | 一种水面无人艇全区域覆盖路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110542102.6A CN113219984A (zh) | 2021-05-18 | 2021-05-18 | 一种水面无人艇全区域覆盖路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113219984A true CN113219984A (zh) | 2021-08-06 |
Family
ID=77092775
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110542102.6A Pending CN113219984A (zh) | 2021-05-18 | 2021-05-18 | 一种水面无人艇全区域覆盖路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113219984A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114371692A (zh) * | 2021-11-26 | 2022-04-19 | 中国人民解放军军事科学院国防科技创新研究院 | 能源约束下的巡逻船区域覆盖路径规划方法、系统及装置 |
CN115857516A (zh) * | 2023-03-02 | 2023-03-28 | 之江实验室 | 结合牛耕式运动与遗传算法的全覆盖路径规划方法和装置 |
-
2021
- 2021-05-18 CN CN202110542102.6A patent/CN113219984A/zh active Pending
Non-Patent Citations (3)
Title |
---|
HONGCHUAN LUO 等: "Complete Coverage Path Planning of UUV for Marine Mine Countermeasure Using Grid Division and Spanning Tree", 《IEEE》, pages 5016 - 5021 * |
吕霞付 等: "基于改进A*算法的无人船完全遍历路径规划", 《水下无人系统学报》, vol. 27, no. 6, pages 695 - 698 * |
崔根群 等: "融合遗传贝塞尔曲线的智能汽车路径规划", 《现代电子技术》, vol. 44, no. 1, pages 144 - 148 * |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114371692A (zh) * | 2021-11-26 | 2022-04-19 | 中国人民解放军军事科学院国防科技创新研究院 | 能源约束下的巡逻船区域覆盖路径规划方法、系统及装置 |
CN114371692B (zh) * | 2021-11-26 | 2023-10-13 | 中国人民解放军军事科学院国防科技创新研究院 | 能源约束下的巡逻船区域覆盖路径规划方法、系统及装置 |
CN115857516A (zh) * | 2023-03-02 | 2023-03-28 | 之江实验室 | 结合牛耕式运动与遗传算法的全覆盖路径规划方法和装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Sang et al. | The hybrid path planning algorithm based on improved A* and artificial potential field for unmanned surface vehicle formations | |
Zhou et al. | The review unmanned surface vehicle path planning: Based on multi-modality constraint | |
Pivtoraiko et al. | Differentially constrained mobile robot motion planning in state lattices | |
Ai et al. | Coverage path planning for maritime search and rescue using reinforcement learning | |
Yao et al. | Three-dimensional path planning for AUV based on interfered fluid dynamical system under ocean current (June 2018) | |
MahmoudZadeh et al. | Uninterrupted path planning system for Multi-USV sampling mission in a cluttered ocean environment | |
Han et al. | A dynamically hybrid path planning for unmanned surface vehicles based on non-uniform Theta* and improved dynamic windows approach | |
Macwan et al. | Target-motion prediction for robotic search and rescue in wilderness environments | |
CN113219984A (zh) | 一种水面无人艇全区域覆盖路径规划方法 | |
Xu et al. | Heuristic and random search algorithm in optimization of route planning for Robot’s geomagnetic navigation | |
Yan et al. | A novel reinforcement learning based tuna swarm optimization algorithm for autonomous underwater vehicle path planning | |
Sun et al. | A novel fuzzy control algorithm for three-dimensional AUV path planning based on sonar model | |
Li et al. | Three-dimensional path planning for AUVs in ocean currents environment based on an improved compression factor particle swarm optimization algorithm | |
Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
Yao et al. | A hierarchical architecture using biased min-consensus for USV path planning | |
Zhou et al. | Motion planning for an unmanned surface vehicle based on topological position maps | |
Yu et al. | Path planning of unmanned surface vessel in an unknown environment based on improved D* Lite algorithm | |
CN112612290A (zh) | 一种考虑洋流的水下航行器三维多任务路径规划方法 | |
Zhang et al. | A hybrid path planning algorithm considering AUV dynamic constraints based on improved A* algorithm and APF algorithm | |
CN113985893B (zh) | 面向复杂海洋环境的多无人艇系统协同路径规划方法 | |
Kim et al. | Any-angle path planning with limit-cycle circle set for marine surface vehicle | |
Li et al. | Dynamic route planning for a USV-UAV multi-robot system in the rendezvous task with obstacles | |
Paliwal | A survey of a-star algorithm family for motion planning of autonomous vehicles | |
Vahid et al. | Optimal path planning for unmanned surface vehicle using new modified local search ant colony optimization | |
Cai et al. | Improved BINN-Based Underwater Topography Scanning Coverage Path Planning for AUV in Internet of Underwater Things |
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 |