CN117193296A - 一种基于高安全性的改进a星无人艇路径规划方法 - Google Patents
一种基于高安全性的改进a星无人艇路径规划方法 Download PDFInfo
- Publication number
- CN117193296A CN117193296A CN202311143107.7A CN202311143107A CN117193296A CN 117193296 A CN117193296 A CN 117193296A CN 202311143107 A CN202311143107 A CN 202311143107A CN 117193296 A CN117193296 A CN 117193296A
- Authority
- CN
- China
- Prior art keywords
- container
- sub
- data
- path
- grid
- 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 34
- 230000004888 barrier function Effects 0.000 claims abstract description 29
- 238000004364 calculation method Methods 0.000 claims description 10
- 125000004122 cyclic group Chemical group 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 6
- 230000004927 fusion Effects 0.000 claims description 4
- 230000006870 function Effects 0.000 description 9
- 238000010586 diagram Methods 0.000 description 7
- 230000001174 ascending effect Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 231100001261 hazardous Toxicity 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于高安全性的改进A星无人艇路径规划方法,包括生成地图:通过深度相机采取周围环境的深度信息和激光雷达扫描到的环境信息,将两部分的数据进行融合,进而生成规划用的二维栅格占据地图;确定危险区域:根据接收到栅格数据并作进行处理,寻找障碍物边界栅格信息,找到危险区域的范围;找出路径:根据找到危险区域的范围,采用改进A星算法进行路径规划,直至获得符合条件的路径;控制行驶;本发明旨在提供一种基于高安全性的改进A星无人艇路径规划方法,通过改进全局算法A星的代价函数,考虑路径的安全性问题,能够保证无人艇航行路线具备高安全性和稳定的特点。
Description
技术领域
本发明涉及无人艇控制技术领域,尤其涉及一种基于高安全性的改进A星无人艇路径规划方法。
背景技术
无人艇是一种具有环境感知、自主航行能力,并能够自主完成相应任务的小型海上平台,近几年来被广泛的应用在海上科学考察、海上搜救和水质监测等领域,然而在实际应用场景下,要求无人艇在未知海域执行任务时具备躲避障碍物的能力,使其完成海上作业的任务。
目前应用在无人艇上的主要方法是基于传统的全局+局部思想,全局规划是根据起点和终点,根据传感器对周围信息的感知信息,规划一条安全的路径作为局部规划的参考,局部规划是以全局路径点作为局部目标点来进行规划,实现动态躲避障碍物,如A星+DWA、RRT*+TEB等算法。
现实的海洋环境是十分复杂的,不仅包含静态障碍物,同时受海面浪流、其他动态障碍物的影响,因此避碰和保证高安全性成为影响USV自主航行的关键技术因素。在传统无人艇规划算法中,主要应用的是全局+局部两个算法结合,全局规划主要是通过传感器感知周围静态障碍物信息进行路径规划,给出一系列的局部目标点。针对于传统的A星算法找到的最优路径,很有可能在实际复杂的海洋环境中是能通过的,但是由于控制的效果往往会有偏差,很有可能在航行过程中存在很高的碰撞风险,尤其是对于自动巡航的无人艇来说,危险更大。
发明内容
本发明的目的在于提出一种基于高安全性的改进A星无人艇路径规划方法,通过改进全局算法A星的代价函数,考虑路径的安全性问题,能够保证无人艇航行路线具备高安全性和稳定的特点。
为达此目的,本发明采用以下技术方案:一种基于高安全性的改进A星无人艇路径规划方法,包括下述步骤:
生成地图:通过深度相机采取周围环境的深度信息和激光雷达扫描到的环境信息,将两部分的数据进行融合,进而生成规划用的二维栅格占据地图;
确定危险区域:根据接收到栅格数据并作进行处理,寻找障碍物边界栅格信息,找到危险区域的范围;
找出路径:根据找到危险区域的范围,采用改进A星算法进行路径规划,直至获得符合条件的路径;
控制行驶:根据获得符合条件的路径和激光雷达扫描到的环境信息,将路径和环境信息进行局部规划并生成控制指令,将控制指令传递至无人艇,无人艇按照规划轨迹行驶。
优选的,在所述生成地图的步骤中,具体包括:
设采集到的栅格地图宽度,即x轴方向为m,高度,即y轴方向为v,对应的栅格数据整型数组为α,地图的栅格分辨率为1,障碍物总个数为k;
设定多个容器包括:
容器A:存储障碍物栅格数据;
容器B:存储所有障碍物左边界栅格数据;
容器C_i:存储第i组障碍物的矩形栅格区域数据;
容器D_i:存储第i组障碍物的矩形区域对应的危险栅格数据;
容器1_left至容器k_left:存储对应序号的障碍物的左边界栅格数据;
容器1_right至容器k_right:存储对应序号的障碍物的右边界栅格数据;
容器1至容器k:存储对应序号的障碍物的边界栅格数据。
优选的,在所述确定危险区域的步骤中,具体包括下述步骤:
步骤A21:遍历栅格数据整型数组α,寻找存在障碍物的栅格数据,将存在障碍物的栅格数据存入容器A中;
步骤A22:遍历容器A中的栅格数据,寻找所有障碍物的左边界的栅格数据,将所有障碍物的左边界的栅格数据存入容器B中;
步骤A23:遍历容器B中的栅格数据,寻找每个障碍物的左边界顶点的栅格数据,设有k个障碍物,则将k个障碍物分别存入容器1left至容器k_left中;
步骤A24:遍历容器1_left至容器k_left的栅格数据,对每个容器i_left进行循环遍历,将符合预设的循环遍历的点存入容器i_right;
步骤A25:根据容器1_left至容器n_left的格栅数据以及容器1_right至容器n_right的格栅数据,进行格栅数据融合获得每个障碍物的边界顶点的栅格数据,将每个障碍物的边界顶点的栅格数据分别存入容器1至容器n;
步骤A26:根据容器1至容器n的格栅数据,确定危险栅格区域的格栅数据,并将危险栅格区域的格栅数据放入容器D中。
优选的,在所述步骤A21中,具体为:
设i=0,从0开始循环遍历数组α,每次循环都会判断α[i]的值是否为1,当α[i]的值为1时,代入下列公式:
xi=i/m+1,yi=i%m+1;求出对应的xi和yi,将xi,yi存入到容器A中,执行i++;当α[i]的值不为1时,不作任何操作,执行i++。
优选的,在所述步骤A22中,具体为:
设xi,yi为容器中A的第i个元素对应的坐标,将x轴坐标减1,y坐标保持不变,判断(xi-1,yi)是否不属于容器A内,若是则将第i个元素存入容器B;否则,不做处理。
优选的,在所述步骤A23中,具体包括下列子步骤:
子步骤A231:设容器B中有n个元素,令i=1,i≤n;
子步骤A232:选取容器B的第一个元素,用(x_,y_)表示;
子步骤A233:初始化令j=1,容器B的第j个元素,用(xj,yj)表示,j≤Smax,Smax=size(容器B)-1;
子步骤A234:获取第一个元素与第j个元素之间的欧式距离d,计算公式为:
子步骤A235:根据第一个元素与第j个元素之间的欧式距离d的计算结果,将第j个元素的坐标值存入容器i_left;
若将xj,yj的值存入i_left,令x_=xj,y_=yj,删除容器B中第j个元素(xj,yj),更新Smax的值,判断j是否大于等于更新后的Smax的值,若否,则执行j++,返回执行子步骤A234;若是,则执行下一子步骤A236;
若判断j是否大于等于更新后的Smax的值,若是,则执行下一子步骤A236;若否,则执行j++,返回执行子步骤A234;
子步骤A236:执行i++,若i<n且j≠-1,回到子步骤A232,反之则结束循环,进入子步骤A237;
子步骤A237:循环操作直至筛选完容器B内的所有格栅数据,获得k个障碍物的左边界栅格数据容器i_left。
优选的,在所述步骤A24中,具体包括下列子步骤:
子步骤A241:设容器i_left有n个元素,令j=0,j的最大值为n-1;
子步骤A242:取i_left的第j个元素,用(xj,yj)表示;
子步骤A243:令xj=xj+1,判断(xj,yj)是否属于容器A,若(xj,yj)不属于容器A,则将(xj-1,yj)存入容器i_right中,并进入下一子步骤,否则重复子步骤A243;
子步骤A244:若j=n-1,则结束,否则j=j+1,返回至子步骤A242。
优选的,在所述步骤A26中,具体包括下列子步骤:
子步骤A261:设船的宽度为L,设Dz=βL;其中其中Dz为安全距离,β为增益系数;设二维栅格占据地图中存在多个存储障碍物的边界栅格数据;
子步骤A262:计算获得每两个障碍物中不属于同个障碍物的两两边界格栅数据的欧式距离,取最小的欧式距离为dmin;
子步骤A263:根据下列公式,判断是否存在危险区域:
子步骤A264:根据子步骤A263,以所有存在危险区域的两个障碍物进行编组,记录总组数为p和每一组对应的最小欧氏距离dimin,令i=1,i≤p;
子步骤A265:选取第i组障碍物;
子步骤A266:取存在障碍物的所有边缘的坐标值(x1、x2、x3、...xn、y1、y2、y3、...yn),xn表示存在障碍物的最大x轴坐标,yn表示存在障碍物的最大y轴坐标;将坐标值代入下列公式计算获得矩形区域Oi:
将矩形区域Oi的格栅数据存入容器Ci中;
子步骤A267:遍历容器Ci,将其中的障碍物栅格取出;
子步骤A268:
令j=0,j≤jmax,jmax=size(Ci)-1;取容器Ci的第j个元素(xj,yj);
(xj,yj)与取得dimin对应的栅格坐标(x3,y3)和(x4,y4)作欧式距离运算,获得dj:
若dj≤γDz,γ为系数,将(xj,yj)存入容器D中,将(xj,yj)代入下列公式计算获得整数N:
N=(yj-1)*m+xj-1;
使用d=dj+1替换栅格数组α[N]对应的栅格值0,执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;若dj>γDz,则直接执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;
子步骤A269:若i<p,i++,返回子步骤A265;否则遍历数据容器Di,删除Di中重复的元素。
优选的,所述改进的A星算法总流程的步骤中,包括下列子步骤:
子步骤A31:根据传递过来的栅格地图数据和规划的起终点,初始化算法;
子步骤A32:根据代价函数f(n)=g(n)+h(n)+I(n),式中的I(n)为危险区域的代价函数;
其中a为常数,d为自变量,d为当前选取的格栅节点对应的格栅值;利用常规的A星算法找路径点的遍历方式寻找路径点,直至找到的路径点为目标点;
子步骤A33:从目标点开始往回找对应节点的父节点,并将点存容器中,直至找到的节点的父节点为起点,算法结束。
本发明的一个技术方案的有益效果:通过在传统A星的代价函数基础上添加危险区域约束项,使得A星在规划时考虑路径的安全性,来确保传递给控制部分的路径是具备高安全性的,以此达到良好的避障效果。本发明通过提出基于危险区域改进代价函数的A星无人艇规划算法;提出采用循环遍历方式处理栅格数据,寻找障碍物边界栅格集合的方法;提出基于障碍物边界栅格集合作欧式距离的方法寻找危险区域。
附图说明
图1是本发明一个实施例无人艇路径规划总流程的示意图;
图2是本发明一个实施例容器A数据的示意图;
图3是本发明一个实施例容器B数据的示意图;
图4是本发明一个实施例障碍物左边界的示意图;
图5是本发明一个实施例障碍物右边界的示意图;
图6是本发明一个实施例障碍物顶点边界栅格的示意图;
图7是本发明一个实施例障碍物栅格标记的示意图;
图8是本发明一个实施例障碍物欧式距离的示意图;
图9是本发明一个实施例障碍物矩形区域的示意图;
图10是本发明一个实施例替换后的栅格数组的示意图;
图11是本发明一个实施例危险区域的示意图;
图12是本发明一个实施例确定危险区域步骤的流程示意图;
图13是本发明一个实施例常规A星搜索路径方法的流程示意图;
图14是本发明一个实施例行进路径的示意图。
具体实施方式
下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。
下面详细描述本发明的实施例,所述实施例的示例在附图中示出。下面通过参考附图描述的实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。
在本发明的描述中,除非另有说明,“多个”的含义是两个或两个以上。
参阅图1至图14所示,一种基于高安全性的改进A星无人艇路径规划方法,包括下述步骤:
生成地图:通过深度相机采取周围环境的深度信息和激光雷达扫描到的环境信息,将两部分的数据进行融合,进而生成规划用的二维栅格占据地图;二维栅格占据地图包含障碍物位置、可通行区域等信息。
确定危险区域:根据接收到栅格数据并作进行处理,寻找障碍物边界栅格信息,找到危险区域的范围;
找出路径:根据找到危险区域的范围,采用改进A星算法进行路径规划,直至获得符合条件的路径;
控制行驶:根据获得符合条件的路径和激光雷达扫描到的环境信息,将路径和环境信息进行局部规划并生成控制指令,将控制指令传递至无人艇,无人艇按照规划轨迹行驶。
本发明通过在传统A星的代价函数基础上添加危险区域约束项,使得A星在规划时考虑路径的安全性,来确保传递给控制部分的路径是具备高安全性的,以此达到良好的避障效果。基于高安全性的改进A星算法主要有两个部分,第一部分是在危险区域的代价函数中添加一个约束项,该约束主要的作用是保证A星在进行全局规划时充分考虑路径的安全性,能够保证路径远离障碍物,尽量不通过危险区,为无人艇安全通过提供了一定的保证;第二部分是针对栅格数据进行处理,找到危险区域,使得路径尽可能地远离危险区,符合高安全性的要求,确保无人艇自主巡航安全可靠。
优选的,在所述生成地图的步骤中,具体包括:
设采集到的栅格地图宽度,即x轴方向为m,高度,即y轴方向为v,对应的栅格数据整型数组为α,地图的栅格分辨率为1,障碍物总个数为k;
设定多个容器包括:
容器A:存储障碍物栅格数据;
容器B:存储所有障碍物左边界栅格数据;
容器C_i:存储第i组障碍物的矩形栅格区域数据;
容器D_i:存储第i组障碍物的矩形区域对应的危险栅格数据;
容器1_left至容器k_left:存储对应序号的障碍物的左边界栅格数据;
容器1_right至容器k_right:存储对应序号的障碍物的右边界栅格数据;
容器1至容器k:存储对应序号的障碍物的边界栅格数据。
具体地,在所述确定危险区域的步骤中,具体包括下述步骤:
步骤A21:遍历栅格数据整型数组α,寻找存在障碍物的栅格数据,将存在障碍物的栅格数据存入容器A中;
步骤A22:遍历容器A中的栅格数据,寻找所有障碍物的左边界的栅格数据,将所有障碍物的左边界的栅格数据存入容器B中;
步骤A23:遍历容器B中的栅格数据,寻找每个障碍物的左边界顶点的栅格数据,设有k个障碍物,则将k个障碍物分别存入容器1_left至容器k_left中;
步骤A24:遍历容器1_left至容器k_left的栅格数据,对每个容器i_left进行循环遍历,将符合预设的循环遍历的点存入容器i_right;
步骤A25:根据容器1_left至容器n_left的格栅数据以及容器1_right至容器n_right的格栅数据,进行格栅数据融合获得每个障碍物的边界顶点的栅格数据,将每个障碍物的边界顶点的栅格数据分别存入容器1至容器n;具体的融合方式就是将容器i_left、容器i_right依次存入容器i即可;得到每个障碍物边界顶点栅格数据如图6所示,图6中灰色格栅表示障碍物边界顶点。
步骤A26:根据容器1至容器n的格栅数据,确定危险栅格区域的格栅数据,并将危险栅格区域的格栅数据放入容器D中。
优选的,在所述步骤A21中,具体为:
设i=0,从0开始循环遍历数组α,每次循环都会判断α[i]的值是否为1,当α[i]的值为1时,代入下列公式:
xi=i/m+1,yi=i%m+1;求出对应的xi和yi,将xi,yi存入到容器A中,执行i++;当α[i]的值不为1时,不作任何操作,执行i++。
由于i是升序增加的,根据i的值代入上述公式计算出来的xi的值和yi的值也是升序的,也就说明容器A中的元素是按照升序的方式进行存储的,对应栅格图中的顺序就是如图2中黑色栅格按照自左向右、自下而上的顺序存储数据。
优选的,在所述步骤A22中,具体为:
设xi,yi为容器中A的第i个元素对应的坐标,将x轴坐标减1,y坐标保持不变,判断(xi-1,yi)是否不属于容器A内,若是则将第i个元素存入容器B;否则,不做处理。
由于i是升序增加的,对应存入容器B的元素也是按升序的方式进行存储的,对应栅格图中顺序就是如图3所示的浅灰色栅格按照自左向右、自下而上的顺序存储数据。
具体地,在所述步骤A23中,具体包括下列子步骤:
子步骤A231:设容器B中有n个元素,令i=1,i≤n;
子步骤A232:选取容器B的第一个元素,用(x_,y_)表示;
子步骤A233:初始化令j=1,容器B的第j个元素,用(xj,yj)表示,j≤Smax,Smax=size(容器B)-1;
子步骤A234:获取第一个元素与第j个元素之间的欧式距离d,计算公式为:
子步骤A235:根据第一个元素与第j个元素之间的欧式距离d的计算结果,将第j个元素的坐标值存入容器i_left;
若将xj,yj的值存入i_1eft,令x_=xj,y_=yj,删除容器B中第j个元素(xj,yj),更新Smax的值,判断j是否大于等于更新后的Smax的值,若否,则执行j++,返回执行子步骤A234;若是,则执行下一子步骤A236;
若判断j是否大于等于更新后的Smax的值,若是,则执行下一子步骤A236;若否,则执行j++,返回执行子步骤A234;
子步骤A236:执行i++,若i<n且j≠-1,回到子步骤A232,反之则结束循环,进入子步骤A237;
子步骤A237:循环操作直至筛选完容器B内的所有格栅数据,获得k个障碍物的左边界栅格数据容器i_left。得到的障碍物左边界顶点栅格数据如图4所示,图4中每个障碍物左边界用灰色栅格表示。
优选的,在所述步骤A24中,具体包括下列子步骤:
子步骤A241:设容器i_left有n个元素,令j=0,j的最大值为n-1;
子步骤A242:取i_left的第j个元素,用(xj,yj)表示;
子步骤A243:令xj=xj+1,判断(xj,yj)是否属于容器A,若(xj,yj)不属于容器A,则将(xj-1,yj)存入容器i_right中,并进入下一子步骤,否则重复子步骤A243;
子步骤A244:若j=n-1,则结束,否则j=j+1,返回至子步骤A242。
经过上述步骤之后就可以得到障碍物右边界栅格数,图5中每个障碍物左边界用灰色栅格表示。
具体地,在所述步骤A26中,具体包括下列子步骤:
子步骤A261:设船的宽度为L,设Dz=βL;其中其中Dz为安全距离,β为增益系数;设二维栅格占据地图中存在多个存储障碍物的边界栅格数据;假设如图4所示的三个障碍物,容器1存储着第一个障碍物的边界栅格坐标、容器2存储第二个障碍物的边界栅格坐标、容器3存储第三个障碍物的边界栅格坐标,三个障碍物边界栅格坐标的标记如图7所示;
子步骤A262:计算获得每两个障碍物中不属于同个障碍物的两两边界格栅数据的欧式距离,取最小的欧式距离为dmin;
如按照图7中所示的障碍物1、2来计算它们的dmin;
障碍物1边界栅格坐标为(x1,y1).....(x8,y8),障碍物2边界栅格坐标为(x9,y9).....(x15,y15),让障碍物1的(x1,y1).....(x8,y8)的每一个边界栅格(xi,yi)与障碍物2的所有边界栅格(x9,y9).....(x15,y15)都按照公示作一次欧氏距离运算(i从1开始取到8),等做完所有的欧式距离运算之后,比较所有的欧式距离,取最小的为dmin。
同理按照上述方法也可以求得障碍物1与障碍物3之间的dmin1和障碍物2、3之间的dmin2,如图8所示。
子步骤A263:根据下列公式,判断是否存在危险区域:
子步骤A264:根据子步骤A263,以所有存在危险区域的两个障碍物进行编组,记录总组数为p和每一组对应的最小欧氏距离dimin,令i=1,i≤p;
子步骤A265:选取第i组障碍物;如障碍物1与障碍物2为第i组障碍物;
子步骤A266:取存在障碍物的所有边缘的坐标值(x1、x2、x3、...xn、y1、y2、y3、...yn),xn表示存在障碍物的最大x轴坐标,yn表示存在障碍物的最大y轴坐标;将坐标值代入下列公式计算获得矩形区域Oi:
将矩形区域Oi的格栅数据存入容器Ci中。
例如图8所示,假设障碍物1、障碍物2之间符合危险区域产生的条件,则其dmin对应的矩形区域如图9所示,对应的矩形区域Oi的栅格数据存入容器Ci。
子步骤A267:遍历容器Ci,将其中的障碍物栅格取出;
子步骤A268:
令j=0,j≤jmax,jmax=size(Ci)-1;取容器Ci的第j个元素(xj,yj);
(xj,yj)与取得dimin对应的栅格坐标(x3,y3)和(x4,y4)作欧式距离运算,获得dj:
若dj≤γDz,γ为系数,将(xj,yj)存入容器D中,将(xj,yj)代入下列公式计算获得整数N:
N=(yj-1)*m+xj-1;
使用d=dj+1替换栅格数组α[N]对应的栅格值0,如图9所示,栅格数组进行替换后的结果如图10所示,执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;若dj>γDz,则直接执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;
子步骤A269:若i<p,i++,返回子步骤A265;否则遍历数据容器Di,删除Di中重复的元素。例如图9所示的矩形区域所对应的危险区域如图11标柱斜线的框所示。
优选的,所述改进的A星算法总流程的步骤中,包括下列子步骤:
子步骤A31:根据传递过来的栅格地图数据和规划的起终点,初始化算法;
子步骤A32:根据代价函数f(n)=g(n)+h(n)+I(n),式中的I(n)为危险区域的代价函数;
其中a为常数,d为自变量,d为当前选取的格栅节点对应的格栅值;利用常规的A星算法找路径点的遍历方式寻找路径点,直至找到的路径点为目标点;子步骤A33:从目标点开始往回找对应节点的父节点,并将点存容器中,直至找到的节点的父节点为起点,算法结束。搜索出来的路径如图14所示,常规搜索出来的路径如图14中的右边的路径,而改进A星搜索出来的路径如图14中的左边的路径。
在本说明书的描述中,参考术语“实施例”、“示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理,而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释,本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式,这些方式都将落入本发明的保护范围之内。
Claims (9)
1.一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,包括下述步骤:
生成地图:通过深度相机采取周围环境的深度信息和激光雷达扫描到的环境信息,将两部分的数据进行融合,进而生成规划用的二维栅格占据地图;
确定危险区域:根据接收到栅格数据并作进行处理,寻找障碍物边界栅格信息,找到危险区域的范围;
找出路径:根据找到危险区域的范围,采用改进A星算法进行路径规划,直至获得符合条件的路径;
控制行驶:根据获得符合条件的路径和激光雷达扫描到的环境信息,将路径和环境信息进行局部规划并生成控制指令,将控制指令传递至无人艇,无人艇按照规划轨迹行驶。
2.根据权利要求1所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述生成地图的步骤中,具体包括:
设采集到的栅格地图宽度,即x轴方向为m,高度,即y轴方向为v,对应的栅格数据整型数组为α,地图的栅格分辨率为1,障碍物总个数为k;
设定多个容器包括:
容器A:存储障碍物栅格数据;
容器B:存储所有障碍物左边界栅格数据;
容器C_i:存储第i组障碍物的矩形栅格区域数据;
容器D_i:存储第i组障碍物的矩形区域对应的危险栅格数据;
容器1_left至容器k_left:存储对应序号的障碍物的左边界栅格数据;
容器1_right至容器k_right:存储对应序号的障碍物的右边界栅格数据;
容器1至容器k:存储对应序号的障碍物的边界栅格数据。
3.根据权利要求2所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述确定危险区域的步骤中,具体包括下述步骤:
步骤A21:遍历栅格数据整型数组α,寻找存在障碍物的栅格数据,将存在障碍物的栅格数据存入容器A中;
步骤A22:遍历容器A中的栅格数据,寻找所有障碍物的左边界的栅格数据,将所有障碍物的左边界的栅格数据存入容器B中;
步骤A23:遍历容器B中的栅格数据,寻找每个障碍物的左边界顶点的栅格数据,设有k个障碍物,则将k个障碍物分别存入容器1_left至容器k_left中;
步骤A24:遍历容器1_left至容器k_left的栅格数据,对每个容器i_left进行循环遍历,将符合预设的循环遍历的点存入容器i_right;
步骤A25:根据容器1_left至容器n_left的格栅数据以及容器1_right至容器n_right的格栅数据,进行格栅数据融合获得每个障碍物的边界顶点的栅格数据,将每个障碍物的边界顶点的栅格数据分别存入容器1至容器n;
步骤A26:根据容器1至容器n的格栅数据,确定危险栅格区域的格栅数据,并将危险栅格区域的格栅数据放入容器D中。
4.根据权利要求3所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述步骤A21中,具体为:
设i=0,从0开始循环遍历数组α,每次循环都会判断α[i]的值是否为1,当α[i]的值为1时,代入下列公式:
xi=i/m+1,yi=i%m+1;求出对应的xi和yi,将xi,yi存入到容器A中,执行i++;当α[i]的值不为1时,不作任何操作,执行i++。
5.根据权利要求3所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述步骤A22中,具体为:
设xi,yi为容器中A的第i个元素对应的坐标,将x轴坐标减1,y坐标保持不变,判断(xi-1,yi)是否不属于容器A内,若是则将第i个元素存入容器B;否则,不做处理。
6.根据权利要求3所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述步骤A23中,具体包括下列子步骤:
子步骤A231:设容器B中有n个元素,令i=1,i≤n;
子步骤A232:选取容器B的第一个元素,用(x_,y_)表示;
子步骤A233:初始化令j=1,容器B的第j个元素,用(xj,yj)表示,j≤Smax,Smax=size(容器B)-1;
子步骤A234:获取第一个元素与第j个元素之间的欧式距离d,计算公式为:
子步骤A235:根据第一个元素与第j个元素之间的欧式距离d的计算结果,将第j个元素的坐标值存入容器i_left;
若将xj,yj的值存入i_left,令x_=xj,y_=yj,删除容器B中第j个元素(xj,yj),更新Smax的值,判断j是否大于等于更新后的Smax的值,若否,则执行j++,返回执行子步骤A234;若是,则执行下一子步骤A236;
若判断j是否大于等于更新后的Smax的值,若是,则执行下一子步骤A236;若否,则执行j++,返回执行子步骤A234;
子步骤A236:执行i++,若i<n且j≠-1,回到子步骤A232,反之则结束循环,进入子步骤A237;
子步骤A237:循环操作直至筛选完容器B内的所有格栅数据,获得k个障碍物的左边界栅格数据容器i_left。
7.根据权利要求3所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述步骤A24中,具体包括下列子步骤:
子步骤A241:设容器i_left有n个元素,令j=0,j的最大值为n-1;
子步骤A242:取i_left的第j个元素,用(xj,yj)表示;
子步骤A243:令xj=xj+1,判断(xj,yj)是否属于容器A,若(xj,yj)不属于容器A,则将(xj-1,yj)存入容器i_right中,并进入下一子步骤,否则重复子步骤A243;
子步骤A244:若j=n-1,则结束,否则j=j+1,返回至子步骤A242。
8.根据权利要求3所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,在所述步骤A26中,具体包括下列子步骤:
子步骤A261:设船的宽度为L,设Dz=βL;其中Dz为安全距离,β为增益系数;设二维栅格占据地图中存在多个存储障碍物的边界栅格数据;
子步骤A262:计算获得每两个障碍物中不属于同个障碍物的两两边界格栅数据的欧式距离,取最小的欧式距离为dmin;
子步骤A263:根据下列公式,判断是否存在危险区域:
子步骤A264:根据子步骤A263,以所有存在危险区域的两个障碍物进行编组,记录总组数为p和每一组对应的最小欧氏距离dimin,令i=1,i≤p;
子步骤A265:选取第i组障碍物;
子步骤A266:取存在障碍物的所有边缘的坐标值(x1、x2、x3、...xn、y1、y2、y3、...yn),xn表示存在障碍物的最大x轴坐标,yn表示存在障碍物的最大y轴坐标;将坐标值代入下列公式计算获得矩形区域Oi:
将矩形区域Oi的格栅数据存入容器Ci中;
子步骤A267:遍历容器Ci,将其中的障碍物栅格取出;
子步骤A268:
令j=0,j≤jmax,jmax=size(Ci)-1;取容器Ci的第j个元素(xj,yj);
(xj,yj)与取得dimin对应的栅格坐标(x3,y3)和(x4,y4)作欧式距离运算,获得dj:
若dj≤γDz,γ为系数,将(xj,yj)存入容器D中,将(xj,yj)代入下列公式计算获得整数N:
N=(yj-1)*m+xj-1;
使用d=dj+1替换栅格数组α[N]对应的栅格值0,执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;若dj>γDz,则直接执行j++,若j≠jmax,返回到计算欧氏距离dj的步骤,反之进入下一步;
子步骤A269:若i<p,i++,返回子步骤A265;否则遍历数据容器Di,删除Di中重复的元素。
9.根据权利要求1所述的一种基于高安全性的改进A星无人艇路径规划方法,其特征在于,所述改进的A星算法总流程的步骤中,包括下列子步骤:
子步骤A31:根据传递过来的栅格地图数据和规划的起终点,初始化算法;
子步骤A32:根据代价函数f(n)=g(n)+h(n)+I(n),式中的I(n)为危险区域的代价函数;
其中a为常数,d为自变量,d为当前选取的格栅节点对应的格栅值;利用常规的A星算法找路径点的遍历方式寻找路径点,直至找到的路径点为目标点;
子步骤A33:从目标点开始往回找对应节点的父节点,并将点存容器中,直至找到的节点的父节点为起点,算法结束。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311143107.7A CN117193296A (zh) | 2023-09-05 | 2023-09-05 | 一种基于高安全性的改进a星无人艇路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311143107.7A CN117193296A (zh) | 2023-09-05 | 2023-09-05 | 一种基于高安全性的改进a星无人艇路径规划方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117193296A true CN117193296A (zh) | 2023-12-08 |
Family
ID=88995440
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311143107.7A Pending CN117193296A (zh) | 2023-09-05 | 2023-09-05 | 一种基于高安全性的改进a星无人艇路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117193296A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117848345A (zh) * | 2024-01-08 | 2024-04-09 | 广东工业大学 | 一种步进式采用优化无人艇路径规划方法 |
CN117870653A (zh) * | 2024-03-13 | 2024-04-12 | 中国科学技术大学 | 一种二维差分欧几里得符号距离场地图的建立与更新方法 |
-
2023
- 2023-09-05 CN CN202311143107.7A patent/CN117193296A/zh active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117848345A (zh) * | 2024-01-08 | 2024-04-09 | 广东工业大学 | 一种步进式采用优化无人艇路径规划方法 |
CN117870653A (zh) * | 2024-03-13 | 2024-04-12 | 中国科学技术大学 | 一种二维差分欧几里得符号距离场地图的建立与更新方法 |
CN117870653B (zh) * | 2024-03-13 | 2024-05-14 | 中国科学技术大学 | 一种二维差分欧几里得符号距离场地图的建立与更新方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Tang et al. | Geometric A-star algorithm: An improved A-star algorithm for AGV path planning in a port environment | |
CN117193296A (zh) | 一种基于高安全性的改进a星无人艇路径规划方法 | |
US11688285B2 (en) | Dynamic collision avoidance method for unmanned surface vessel based on route replanning | |
CN109828607B (zh) | 一种面向不规则障碍物的无人机路径规划方法及系统 | |
CN110609552B (zh) | 一种水下无人航行器编队平面航迹规划方法 | |
CN108459503B (zh) | 一种基于量子蚁群算法的无人水面艇航迹规划方法 | |
Yahja et al. | Framed-quadtree path planning for mobile robots operating in sparse environments | |
CN110398250B (zh) | 一种无人艇全局路径规划方法 | |
CN111829527B (zh) | 一种基于深度强化学习且顾及海洋环境要素的无人船路径规划方法 | |
CN114089762B (zh) | 一种基于强化学习的水空两栖无人航行器路径规划方法 | |
CN111721296B (zh) | 一种水下无人航行器数据驱动路径规划方法 | |
Thompson et al. | Efficient lidar-based object segmentation and mapping for maritime environments | |
CN111412918B (zh) | 无人艇全局安全路径规划方法 | |
Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
CN114839968A (zh) | 一种水面无人艇路径规划方法 | |
Du et al. | An optimized path planning method for coastal ships based on improved DDPG and DP | |
CN114387822B (zh) | 船舶避碰方法 | |
Vidal et al. | Optimized environment exploration for autonomous underwater vehicles | |
CN114610046A (zh) | 一种考虑动态水深的无人艇动态安全轨迹规划方法 | |
Zhao et al. | Global path planning of unmanned vehicle based on fusion of A∗ algorithm and Voronoi field | |
CN114088094A (zh) | 一种无人艇的智能航路规划方法及系统 | |
CN117311160A (zh) | 一种基于人工智能的自动控制系统和控制方法 | |
CN117289301A (zh) | 一种未知越野场景下空地无人平台协同路径规划方法 | |
Kapetanović et al. | A side-scan sonar data-driven coverage planning and tracking framework | |
Xie et al. | Random patrol path planning for unmanned surface vehicles in shallow waters |
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 |