CN111060109B - 一种基于改进a星算法的无人艇全局路径规划方法 - Google Patents
一种基于改进a星算法的无人艇全局路径规划方法 Download PDFInfo
- Publication number
- CN111060109B CN111060109B CN202010004408.1A CN202010004408A CN111060109B CN 111060109 B CN111060109 B CN 111060109B CN 202010004408 A CN202010004408 A CN 202010004408A CN 111060109 B CN111060109 B CN 111060109B
- Authority
- CN
- China
- Prior art keywords
- path
- node
- value
- longitude
- latitude
- 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 30
- 239000011159 matrix material Substances 0.000 claims description 5
- 238000005192 partition Methods 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 3
- 238000007670 refining Methods 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 3
- 238000012217 deletion Methods 0.000 claims 1
- 230000037430 deletion Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Processing Or Creating Images (AREA)
Abstract
本发明提供的是一种基于改进A星算法的无人艇全局路径规划方法,涉及路径规划领域。本发明改变传统A星算法邻点搜索策略,扩大搜索邻域,对传统启发式函数进行改进,结合分权策略,加入角度因素,使路径搜索时得到的结果偏向分布在起始点与目标点连线附近,提高搜索效率。本发明利用电子海图获取海洋地理信息,将改进A星算法与动态栅格法相结合,通过栅格动态细化构建网格地图模型,应用改进A星算法寻找路径,使路径精度逐步达到精度要求,再通过平滑路径处理,进一步减少多余的路径节点。
Description
技术领域
本发明涉及路径规划技术领域,具体为一种基于改进A星算法的无人艇全局路径规划方法。
背景技术
无人艇作为一种无人操作的水面舰艇,主要用于执行危险和不适于有人船只执行的任务,在众多领域得到广泛应用。目前,无人艇正在向着智能化发展,可以通过指令下达的方式进行自主航行及作业,但是复杂海洋环境中无人艇的自航技术还有待提高。为了提高无人艇的智能化水平,完成在各种复杂环境中的特定任务,无人艇必须要具备自主的规划能力。路径规划是无人智能平台领域需要解决的关键问题之一,在某些层面上反映了无人艇智能化程度的高低。
电子海图是以数字信息表示的,描写海域地理要素和航海要素,利用电子海图全局信息,规划出一条避开所有已知静态障碍物,并且到达目的点的路径,从而指导无人艇更高效地完成路径规划任务。A星算法是静态路网中求解最短路径最有效的直接搜索方法,结合Dijkstra算法与最佳优先算法BFS优点,综合考虑起始点到当前节点的真实代价与当前节点到终点的估计代价,在保证搜索效率的同时,得到最优的路径。
发明内容
为了解决以上问题,本发明提供一种基于改进A星算法的无人艇全局路径规划方法,本发明改变传统A星算法邻点搜索策略,扩大搜索邻域,对传统启发式函数进行改进,结合分权策略,加入角度因素,使路径搜索时得到的结果偏向分布在起始点与目标点连线附近,提高搜索效率。本发明利用电子海图获取海洋地理信息,将改进A星算法与动态栅格法相结合,通过栅格动态细化构建网格地图模型,应用改进A星算法寻找路径,使路径精度逐步达到精度要求,再通过平滑路径处理,进一步减少多余的路径节点,为达此目的,本发明提供一种基于改进A星算法的无人艇全局路径规划方法,具体包括如下步骤:
(1)获取全局海图信息,设置无人艇起点与终点位置的经纬度信息,判断无人艇起点或终点位置是否为不可通行区域,若是则重新设置起点与终点的经纬度信息;
(2)构建网格地图,确定经纬度范围,以经纬度纵横比动态栅格细化海图,形成M*N的网格海图,将起点与终点的经纬度坐标转换为笛卡尔坐标;判断起点与终点之间的连线是否通过不可通行区域,若不存在不可通行区域,则路径即为起点与终点的连线,否则进行改进A星算法寻找路径;
(3)改进A星算法寻找路径,创建OPEN集与CLOSE集,OPEN集用于存放待检测的节点,CLOSE集用于存放已检测过的节点;OPEN集与CLOSE集中每个节点有父节点、F值、G值、H值;父节点为路径中通过该节点所经过的上一节点,G值为从起点移动到当前节点的代价,H值为从当前节点移动到终点的估计代价,F值是G值与H值之和;遍历OPEN集,查找F值最小的节点,将此节点作为当前要处理的节点;若将终点加入到OPEN集中,则路径已经找到,回溯路径,将从终点开始沿着父节点移动直至起点的节点依次加入PATH集;若未将终点加入到OPEN集中且OPEN集已空,则未找到路径;
(4)判断是否达到导航精度要求,即单位网格边长实际经纬度数,若达到要求且找到路径,则进行平滑路径处理;若达到要求但未找到路径,则表明当前分辨率下无法找到路径;若未达到导航精度要求但找到路径,按找到的路径的范围再次栅格化,若未达到导航精度要求也未找到路径则将M与N乘以同一倍数,以原经纬度范围再次栅格化;
(5)平滑路径,删去PATH集中保存的共线的节点以及多余转折点,只保留必要转折点的位置,并利用网格图的经纬度范围大小,M值与N值,将PATH集中节点横纵坐标转换为经纬度坐标。
作为本发明进一步改进,步骤(1)所述获取海图全局信息,是通过ArcMap平台将电子海图转换为可应用于Matlab软件处理的shapefile格式,应用map工具箱可读取相应图层的.shp文件的数据和信息,以及进行相应图层的可视化显示。
作为本发明进一步改进,步骤(2)所采用的动态栅格细化,首先在整个海图进行粗划分,找到一条较为粗略的路径,在此路径的基础上再次细划分,M值为经度划分的网格数,N值为纬度划分的网格数,比较海图经度范围和纬度范围,取较大者划分为固定格数,再按比例划分较小者。完成寻径后,若未达到导航精度要求但找到路径,按找到的路径的范围再次栅格化,否则将M与N乘以倍数,以原经纬度范围再次栅格化将M与N乘以倍数,以原经纬度范围再次栅格化。
作为本发明进一步改进,步骤(3)所述改进A星算法寻找路径,遍历OPEN集,查找F值最小的节点,若出现多个节点F值最小值相同的情况,选择H值较小者,若F值与H值皆相等,优先选择后加入OPEN集的节点;将此节点作为当前要处理的节点,依据扩展的16邻域矩阵,以当前节点下方开始顺时针顺序查找该节点相邻的节点,若当前节点为奇数节点,即横纵坐标之和为奇数的节点,以相反的顺序查找当前节点的邻点,忽略已经加入CLOSE集或者不可通行的节点,删去受阻碍的位于对角线位置的邻点,如当前节点的上方或右方不可通行,则右上方节点不可选;如果相邻节点不在OPEN集中,将此相邻节点加入OPEN集,并且把当前节点设置为节点的父节点,记录该节点的F,G和H值;如果此相邻节点已在OPEN集,并且经由当前节点到达此相邻节点G值小于原G值,则把此相邻节点父节点设置为当前节点,并重新计算G值与F值;若将终点加入到OPEN集中,则路径已经找到,回溯路径,将从终点开始沿着父节点移动直至起点的节点依次加入PATH集;若未将终点加入到OPEN集中且OPEN集已空,则未找到路径。
作为本发明进一步改进,步骤(3)中计算H值的启发式函数的改进,曼哈顿距离是当前节点与终点横纵坐标差的绝对值之和,是A星算法常用的代价估计,为避免邻域中代价相同而造成的路径不确定性和抖动现象,将横纵坐标差加权后取和,X,Y为当前节点与终点横坐标之差和纵坐标之差的绝对值;以终点为坐标原点,以X=Y作为分割线,两侧分区对引导作用的要求相反,进一步将max(X,Y)与min(X,Y)加权求和,引入角度信息cross,即起点到终点的矢量与当前节点到终点的矢量的叉乘的模,X2,Y2为起点与终点横坐标之差和纵坐标之差的绝对值,D为起点与终点的距离,cross=abs(X*X2-Y*Y2),结合距离和角度的新启发式函数H=a*max(X,Y)+b*min(X,Y)+c*cross/D。
作为本发明进一步改进,步骤(5)中所述剔除共线节点与多余转折点,是从节点N1开始,即从PATH集中第一个节点开始,利用矢量叉乘,依据正弦值,依次判断相邻的3个节点是否共线,剔除共线节点;记Line(i,j)为剔除共线节点后PATH集中的转折点Ni与Nj的连线,先令Ni为PATH集中最后一个节点,依次检查Line(i,j)(j=1,2,…i-1)是否存在不可通行区,若不存在,则剔除此两节点间多余转折点,并令i=j,否则令i=i-1,再次检查Line(i,j)(j=1,2,…i-1)是否存在不可通行区,直至再无多余转折点。
本发明的原理:所述无人艇全局路径方法,利用电子海图的海洋地理信息,将改进A星算法与动态栅格法相结合,通过栅格动态细化构建网格地图模型,应用改进A星算法寻找路径,使路径精度逐步达到精度要求;再通过平滑路径处理,进一步减少多余的路径节点。
本发明与现有技术相比的优点在于:
(1)本发明构建栅格地图的方法,不同于将电子海图以图片形式灰度化后,再以一定阈值二值化的方法,应用map工具箱,对.shp矢量文件进行栅格化处理,不丢失细小的要素。采用动态栅格细化的方法,不简单地以固定值M、N,将海图直接划分为M*N的网格,若M、N值选取过小,即不可通行区域膨胀过大,出现被错误判定为无法找到路径的情况,或因无法达到导航精度的要求,并不能得到最优的路径;若M、N值选取过大,即构建地图过大,将导致A星寻径算法运算量增加,运行时间变长。
(2)本发明采用基于改进A星算法的路径规划方法,改变传统8邻域搜索,采用16邻域搜索扩展搜索方向;奇数节点改变搜索顺序;启用新的启发式函数,引入分权与角度信息;若出现多个节点F值最小值相同的情况,选择H值较小者,若F值与H值皆相等,优先选择后加入OPEN集的节点,避免邻域中代价相同而造成的路径不确定性和抖动现象;再通过平滑路径处理,进一步减少多余的路径节点。
附图说明
图1为无人艇全局路径规划流程图;
图2为改进A星算法流程图;
图3为16邻域搜索方向示意图;
图4为两节点通行判断示意图;
图5为全局路径规划仿真实验图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述:
本发明提供一种基于改进A星算法的无人艇全局路径规划方法,本发明改变传统A星算法邻点搜索策略,扩大搜索邻域,对传统启发式函数进行改进,结合分权策略,加入角度因素,使路径搜索时得到的结果偏向分布在起始点与目标点连线附近,提高搜索效率。本发明利用电子海图获取海洋地理信息,将改进A星算法与动态栅格法相结合,通过栅格动态细化构建网格地图模型,应用改进A星算法寻找路径,使路径精度逐步达到精度要求,再通过平滑路径处理,进一步减少多余的路径节点。
如图1所示,具体实现步骤如下:
(1)获取海图全局信息,是通过ArcMap平台,将基于S-57数字化海道测量数据传输标准的电子海图利用esri-s57-viewer插件转换为可应用于Matlab软件处理的shapefile格式文件,应用map工具箱shaperead函数读取相应图层(如陆地区域)的.shp文件的数据,以及shapeinfo函数读取相应图层.shp文件的信息,geoshow函数进行相应图层可视化显示。
(2)初次划分时,利用shapeinfo读取到的海图信息获知海图的经纬度范围。运用map工具箱vec2mtx函数将读取到的.shp文件数据转换为M*N的矩阵。将起点与终点的经纬度坐标转换为笛卡尔坐标。具体实现步骤为:
(a)确定栅格化区域的经纬度范围,LONMAX、LONMIN分别为经度最大值、最小值,LATMAX、LATMIN分别为纬度最大值、最小值;
(b)比较经度范围和纬度范围,确定划分网格数,取较大者划分为100格,再按比例划分较小者,M值为经度划分的网格数,N值为纬度划分的网格数;
(c)若前次栅格化网格图未寻找到路径,令M值与N值为原M值与原N值的2倍;
(d)运用map工具箱vec2mtx函数从西向开始将经度范围划分网格数M,从南向开始将纬度范围划分网格数N,划分M*N网格地图并确定xstep与ystep,单位栅格横向边长实际代表的经度度数xstep为栅格化区域经度范围差除以划分网格数M,单位栅格纵向边长实际代表的纬度度数ystep为栅格化区域纬度范围差除以划分网格数N;
(e)将起点与终点的经纬度坐标转换为笛卡尔坐标,经纬度坐标(lon,lat),横坐标x为ceil((lon-LONMIN)/xstep),纵坐标y为ceil((lat-LATMIN)/ystep);
(f)判断起点与终点之间的连线是否通过不可通行区域,若不存在不可通行区域,则路径即为起点与终点的连线,否则进行改进A星算法寻找路径。
(3)如图2所示,改进A星算法寻找路径的具体实现步骤如下:
(a)检测起点与终点网格节点是否被膨胀为不可通行区,若是则无法找到路径,返回查找失败标志符。
(b)创建OPEN集与CLOSE集,OPEN集与CLOSE集中每个节点有父节点、F值、G值、H值,查找节点以结构体struct形式保存,结构体成员为父节点坐标、F值、G值与H值,将起点存入OPEN集;
(c)遍历OPEN集中保存的节点,查找F值最小的节点,若出现多个节点F值最小值相同的情况,选择H值较小者,若F值与H值皆相等,优先选择后加入OPEN集的节点;将此节点作为当前要处理的节点;
(d)依据扩展的16邻域矩阵,16邻域矩阵如图3所示,以图3标识数字表明顺序查找该节点相邻的节点,若当前节点为奇数节点,即横纵坐标之和为奇数的节点,以相反的顺序查找当前节点的邻点,忽略已经加入CLOSE集或者不可通行的节点,删去受阻碍的位于对角线位置的邻点,如当前节点的上方或右方不可通行,则右上方节点不可选;
(e)如果相邻节点不在OPEN集中,将此相邻节点加入OPEN集,并且把当前节点设置为节点的父节点,记录该节点的F,G和H值;X,Y为当前节点与终点横坐标之差和纵坐标之差的绝对值;X2,Y2为起点与终点横坐标之差和纵坐标之差的绝对值;D为起点与终点的距离;起点到终点的矢量与当前节点到终点的矢量的叉乘的模cross=abs(X*X2-Y*Y2);结合距离和角度的新启发式函数H=a*max(X,Y)+b*min(X,Y)+c*cross/D,最后经比较选取a=6,b=10,c=6;
(f)如果此相邻节点已在OPEN集,并且经由当前节点到达此相邻节点G值小于原G值,则把此相邻节点父节点设置为当前节点,并重新计算G值与F值;
(g)若将终点加入到OPEN集中,则路径已经找到,创建PATH集,回溯路径,将从终点开始沿着父节点移动直至起点的节点依次加入PATH集;若未将终点加入到OPEN集中且OPEN集已空,则未找到路径,返回查找失败标志符。
(4)判断是否达到导航精度要求,即xstep与ystep两者都不大于要求的导航精度STEP,STEP可按需求自己设定,则转步骤(5);若未达到要求,按以下步骤处理:
(a)查询路径查找失败标志符以判断是否寻找到路径,若找到路径,转步骤(4-b),否则转步骤(4-c);
(b)若找到路径,依据PATH集中路径节点横纵坐标最大值与最小值XMAX、XMIN、YMAX、YMIN,重新确定栅格化区域,最小纬度LATMIN为原最小纬度LATMIN加ystep与(YMIN-1)的乘积,最大纬度LATMAX为原最大纬度LATMAX加ystep与YMAX的乘积,最小经度LONMIN为原最小经度LONMIN加xstep与(XMIN-1)的乘积,最大经度LONMAX为原最大经度LONMAX加xstep与XMAX的乘积,转步骤(2-a),以此经纬度范围再次划分网格;
(c)若未找到路径,转步骤(2-c),令M值与N值为原M值与原N值的2倍,以原经纬度范围再次划分M*N网格。
(5)查询标识符以判断是否寻找到路径,若未找到路径,则结束程序,否则进行平滑路径,删去PATH集中保存的共线的节点以及多余转折点,只保留必要转折点的位置。平滑路径具体实现步骤如下:
(a)从起点开始,利用矢量叉乘,依据正弦值,依次判断相邻的3个节点是否共线,剔除共线节点;
任意相邻的3个节点的坐标分别为(xi-1,yi-1)、(xi,yi)和(xi+1,yi+1)
节点Ni-1到节点Ni的矢量为(xi-xi-1,yi-yi-1)
节点Ni到节点Ni+1的矢量为(xi+1-xi,yi+1-yi)
若(xi-xi-1)*(yi+1-yi)-(xi+1-xi)*(yi-yi-1)为0,则节点Ni-1,Ni和Ni+1共线,删去中间节点Ni,否则判断节点Ni,Ni+1和Ni+2是否共线;
(b)先令Ni为PATH中最后一个节点,依次检查节点Ni与Nj(j=1,2…n)是否存在不可通行区,若存在,则令i=i-1,若不存在,则删除此两节点间多余转折点,同时令i=j,再次检查Ni与Nj(j=1,2…i-1)是否存在不可通行区,直至再无多余转折点。
(c)将坐标转换为经纬度坐标,路径节点N(x,y)坐标转换经纬度坐标(lon,lat),并保存;
经度lon=LONMIN+xstep*(x-0.5)
纬度lat=LATMIN+ystep*(y-0.5)
其中,LONMIN、LATMIN最后次栅格化区域经度最小值、纬度最小值。
(6)判断任意两个节点Ni与Nj是否存在不可通行区,具体实现步骤如下:
(a)确定搜索方向与直线方程,若两节点横坐标之差的绝对值大于纵坐标之差的绝对值采用横向搜索,否则采用纵向搜索,交换节点横纵坐标;
(b)步骤(a)结束后,两坐标为(xi,yi)与(xj,yj),计算直线方程为y=k*x+b,k=(yj-yi)/(xj-xi),b=yi-k*xi;
(b)如图4所示,xk以固定步长1,依据直线方程y=k*x+b依次计算出坐标(xk,yk),依据此交点判断是否经过不可通行区;
(c)若此交点坐标为两个网格共享边线上的节点,若是横向搜索,此时判别(xk-0.5,floor(yk+0.5))与(xk+0.5,floor(yk+0.5))是否为不可通行区,只要有一个网格不可通行,即节点Ni与Nj连线存在不可通行区;若是纵向搜索,此时判别(floor(yk+0.5),xk-0.5)与(floor(yk+0.5),xk+0.5)是否为不可通行区,只要有一个网格不可通行,即节点Ni与Nj连线存在不可通行区;
(d)若此交点为4个网格的交点,若是横向搜索,此时判别(xk-0.5,floor(yk)),(xk-0.5,floor(yk)+1),(xk+0.5,floor(yk)+1)与(xk+0.5,floor(yk))是否为不可通行区,只要有一个网格不可通行,即节点Ni与Nj连线存在不可通行区;若是纵向搜索,此时判别(floor(yk),xk-0.5),(floor(yk)+1,xk-0.5),(floor(yk)+1,xk+0.5)与(floor(yk),xk+0.5)是否为不可通行区,只要有一个网格不可通行,即节点Ni与Nj连线存在不可通行区。
仿真效果
本发明利用电子海图的海洋地理信息,将改进A星算法与动态栅格法相结合,通过栅格动态细化构建网格地图模型,应用改进A星算法寻找路径,使路径精度逐步达到精度要求;再通过平滑路径处理,进一步减少多余的路径节点。
以CN580505马友石至小铲岛电子海图作为数据来源,以东经113.8843°北纬22.5115°为起点,以东经113.9378°E北纬22.4977°为终点,图5为仿真实验图,灰色区域为不可通行区,黑色部分为栅格细化分后网格地图中代表不可通行区的部分,下表为起点至终点的路径规划中生成的导航点:
表1导航点生成表
以上所述,仅是本发明的较佳实施例而已,并非是对本发明作任何其他形式的限制,而依据本发明的技术实质所作的任何修改或等同变化,仍属于本发明所要求保护的范围。
Claims (3)
1.一种基于改进A星算法的无人艇全局路径规划方法,具体包括如下步骤,其特征在于:
(1)获取全局海图信息,设置无人艇起点与终点位置的经纬度信息,判断无人艇起点或终点位置是否为不可通行区域,若是则重新设置起点与终点的经纬度信息;
(2)构建网格地图,确定经纬度范围,以经纬度纵横比动态栅格细化海图,形成M*N的网格海图,将起点与终点的经纬度坐标转换为笛卡尔坐标;判断起点与终点之间的连线是否通过不可通行区域,若不存在不可通行区域,则路径即为起点与终点的连线,否则进行改进A星算法寻找路径;
步骤(2)所采用的动态栅格细化,首先在整个海图进行粗划分,找到一条较为粗略的路径,在此路径的基础上再次细划分,M值为经度划分的网格数,N值为纬度划分的网格数,比较海图经度范围和纬度范围,取较大者划分为固定格数,再按比例划分较小者,完成寻径后,若未达到导航精度要求但找到路径,按找到的路径的范围再次栅格化,若未达到导航精度要求也未找到路径则将M与N乘以倍数,以原经纬度范围再次栅格化;
具体如下:
(a)确定栅格化区域的经纬度范围,LONMAX、LONMIN分别为经度最大值、最小值,LATMAX、LATMIN分别为纬度最大值、最小值;
(b)比较经度范围和纬度范围,确定划分网格数,取较大者划分为100格,再按比例划分较小者,M值为经度划分的网格数,N值为纬度划分的网格数;
(c)若前次栅格化网格图未寻找到路径,令M值与N值为原M值与原N值的2倍;
(d)运用map工具箱vec2mtx函数从西向开始将经度范围划分网格数M,从南向开始将纬度范围划分网格数N,划分M*N网格地图并确定xstep与ystep,单位栅格横向边长实际代表的经度度数xstep为栅格化区域经度范围差除以划分网格数M,单位栅格纵向边长实际代表的纬度度数ystep为栅格化区域纬度范围差除以划分网格数N;
(e)将起点与终点的经纬度坐标转换为笛卡尔坐标,经纬度坐标(lon,lat),横坐标x为ceil((lon-LONMIN)/xstep),纵坐标y为ceil((lat-LATMIN)/ystep);
(f)判断起点与终点之间的连线是否通过不可通行区域,若不存在不可通行区域,则路径即为起点与终点的连线,否则进行改进A星算法寻找路径;
(3)改进A星算法寻找路径,创建OPEN集与CLOSE集,OPEN集用于存放待检测的节点,CLOSE集用于存放已检测过的节点;OPEN集与CLOSE集中每个节点有父节点、F值、G值、H值;父节点为路径中通过该节点所经过的上一节点,G值为从起点移动到当前节点的代价,H值为从当前节点移动到终点的估计代价,F值是G值与H值之和;遍历OPEN集,查找F值最小的节点,将此节点作为当前要处理的节点;若将终点加入到OPEN集中,则路径已经找到,回溯路径,将从终点开始沿着父节点移动直至起点的节点依次加入PATH集;若未将终点加入到OPEN集中且OPEN集已空,则未找到路径;
步骤(3)所述改进A星算法寻找路径,遍历OPEN集,查找F值最小的节点,若出现多个节点F值最小值相同的情况,选择H值较小者,若F值与H值皆相等,优先选择后加入OPEN集的节点;将此节点作为当前要处理的节点,依据扩展的16邻域矩阵,以当前节点下方开始顺时针顺序查找该节点相邻的节点,若当前节点为奇数节点,即横纵坐标之和为奇数的节点,以相反的顺序查找当前节点的邻点,忽略已经加入CLOSE集或者不可通行的节点,删去受阻碍的位于对角线位置的邻点,如当前节点的上方或右方不可通行,则右上方节点不可选;如果相邻节点不在OPEN集中,将此相邻节点加入OPEN集,并且把当前节点设置为节点的父节点,记录该节点的F,G和H值;如果此相邻节点已在OPEN集,并且经由当前节点到达此相邻节点G值小于原G值,则把此相邻节点父节点设置为当前节点,并重新计算G值与F值;若将终点加入到OPEN集中,则路径已经找到,回溯路径,将从终点开始沿着父节点移动直至起点的节点依次加入PATH集;若未将终点加入到OPEN集中且OPEN集已空,则未找到路径;
步骤(3)中计算H值的启发式函数的改进,曼哈顿距离是当前节点与终点横纵坐标差的绝对值之和,是A星算法常用的代价估计,为避免邻域中代价相同而造成的路径不确定性和抖动现象,将横纵坐标差加权后取和,X,Y为当前节点与终点横坐标之差和纵坐标之差的绝对值;以终点为坐标原点,以X=Y作为分割线,两侧分区对引导作用的要求相反,进一步将max(X,Y)与min(X,Y)加权求和,引入角度信息cross,即起点到终点的矢量与当前节点到终点的矢量的叉乘的模,X2,Y2为起点与终点横坐标之差和纵坐标之差的绝对值,D为起点与终点的距离,cross=abs(X*X2-Y*Y2),结合距离和角度的新启发式函数H=a*max(X,Y)+b*min(X,Y)+c*cross/D;
(4)判断是否达到导航精度要求,即单位网格边长实际经纬度数,若达到要求且找到路径,则进行平滑路径处理;若达到要求但未找到路径,则表明当前分辨率下无法找到路径;若未达到导航精度要求但找到路径,按找到的路径的范围再次栅格化,若未达到导航精度要求也未找到路径则将M与N乘以同一倍数,以原经纬度范围再次栅格化;
其中判断是否达到导航精度要求,即xstep与ystep两者都不大于要求的导航精度STEP,其中xstep为单位栅格横向边长实际代表的经度度数,ystep为单位栅格纵向边长实际代表的纬度度数,STEP按需求自己设定,则转步骤(5);若未达到要求,按以下步骤处理:
(a)查询路径查找失败标志符以判断是否寻找到路径,若找到路径,转步骤(4)的(b)步骤,否则转步骤(4)的(c)步骤;
(b)若找到路径,依据PATH集中路径节点横纵坐标最大值与最小值XMAX、XMIN、YMAX、YMIN,重新确定栅格化区域,最小纬度LATMIN为原最小纬度LATMIN加ystep与(YMIN-1)的乘积,最大纬度LATMAX为原最大纬度LATMAX加ystep与YMAX的乘积,最小经度LONMIN为原最小经度LONMIN加xstep与(XMIN-1)的乘积,最大经度LONMAX为原最大经度LONMAX加xstep与XMAX的乘积,转步骤(2)的(a)步骤,以此经纬度范围再次划分网格;
(c)若未找到路径,转步骤(2)的(c)步骤,令M值与N值为原M值与原N值的2倍,以原经纬度范围再次划分M*N网格;
(5)平滑路径,删去PATH集中保存的共线的节点以及多余转折点,只保留必要转折点的位置,并利用网格图的经纬度范围大小,M值与N值,将PATH集中节点横纵坐标转换为经纬度坐标。
2.根据权利要求1所述的一种基于改进A星算法的无人艇全局路径规划方法,其特征在于:步骤(1)所述获取全局海图信息,是通过ArcMap平台将电子海图转换为可应用于Matlab软件处理的shapefile格式,应用map工具箱读取相应图层的.shp文件的数据和信息,以及进行相应图层的可视化显示。
3.根据权利要求1所述的一种基于改进A星算法的无人艇全局路径规划方法,其特征在于:步骤(5)中所述删去PATH集中保存的共线的节点以及多余转折点,是从节点N1开始,即从PATH集中第一个节点开始,利用矢量叉乘,依据正弦值,依次判断相邻的3个节点是否共线,剔除共线节点;记Line(i,j)为剔除共线节点后PATH集中的转折点Ni与Nj的连线,先令Ni为PATH集中最后一个节点,依次检查Line(i,j),j=1,2,…i-1是否存在不可通行区,若不存在,则剔除此两节点间多余转折点,并令i=j,否则令i=i-1,再次检查Line(i,j),j=1,2,…i-1是否存在不可通行区,直至再无多余转折点。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010004408.1A CN111060109B (zh) | 2020-01-03 | 2020-01-03 | 一种基于改进a星算法的无人艇全局路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010004408.1A CN111060109B (zh) | 2020-01-03 | 2020-01-03 | 一种基于改进a星算法的无人艇全局路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111060109A CN111060109A (zh) | 2020-04-24 |
CN111060109B true CN111060109B (zh) | 2021-08-27 |
Family
ID=70304872
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010004408.1A Active CN111060109B (zh) | 2020-01-03 | 2020-01-03 | 一种基于改进a星算法的无人艇全局路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111060109B (zh) |
Families Citing this family (28)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111693049B (zh) * | 2020-05-20 | 2022-02-11 | 五邑大学 | 一种无人船覆盖投食的动态路径规划方法及设备 |
CN111854754B (zh) * | 2020-06-19 | 2023-01-24 | 北京三快在线科技有限公司 | 无人机航线规划方法、装置、无人机及存储介质 |
CN111982129B (zh) * | 2020-08-24 | 2022-03-08 | 哈尔滨工业大学 | 一种基于月面数字高程地图的综合全局路径规划方法 |
CN112180955B (zh) * | 2020-08-26 | 2024-02-20 | 国网安徽省电力有限公司淮南供电公司 | 自动巡检无人机的基于视觉反馈的二次复查方法和系统 |
CN112197778A (zh) * | 2020-09-08 | 2021-01-08 | 南京理工大学 | 基于改进a*算法的轮式机场巡界机器人路径规划方法 |
CN112113573B (zh) * | 2020-09-18 | 2021-08-10 | 武汉理工大学 | 一种面向单艘无人测量船艇覆盖路径规划方法 |
CN112529243A (zh) * | 2020-10-22 | 2021-03-19 | 西安超越申泰信息科技有限公司 | 一种基于路网图像的流向地图路径优化展示方法 |
CN112683275B (zh) * | 2020-12-24 | 2023-11-21 | 长三角哈特机器人产业技术研究院 | 一种栅格地图的路径规划方法 |
CN112762942B (zh) * | 2021-01-06 | 2023-05-26 | 集美大学 | 一种航标航路导航推荐方法、终端设备及存储介质 |
CN112683278B (zh) * | 2021-01-08 | 2024-01-30 | 东南大学 | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 |
CN112908027A (zh) * | 2021-02-03 | 2021-06-04 | 芜湖泊啦图信息科技有限公司 | 基于停车场内主定位点特征路径构建的控制算法及其系统 |
CN112985408B (zh) * | 2021-02-25 | 2022-02-11 | 南京航空航天大学 | 一种路径规划优化方法及系统 |
CN113124874A (zh) * | 2021-04-12 | 2021-07-16 | 北京理工大学 | 一种用于无人装备的时间协同航迹规划方法 |
CN113156970B (zh) * | 2021-05-08 | 2023-06-09 | 珠海一微半导体股份有限公司 | 一种通行区域的路径融合规划方法、机器人及芯片 |
CN113804209B (zh) * | 2021-05-27 | 2024-01-09 | 中国人民解放军战略支援部队信息工程大学 | 一种四角格网高精度长距离越野路径规划方法 |
CN113341992B (zh) * | 2021-06-18 | 2023-10-27 | 广东工业大学 | 一种无人艇多任务路径规划方法 |
CN113657636B (zh) * | 2021-06-18 | 2024-03-01 | 广东电网有限责任公司佛山供电局 | 一种电网运行方式图的自动规划生成算法 |
CN113655786B (zh) * | 2021-06-29 | 2023-09-19 | 航天科工深圳(集团)有限公司 | 一种基于非洲蜂智能算法的无人艇艇群控制方法 |
CN113625709A (zh) * | 2021-07-22 | 2021-11-09 | 中国舰船研究设计中心 | 一种水面无人艇避障方法 |
CN113776535A (zh) * | 2021-08-31 | 2021-12-10 | 中国舰船研究设计中心 | 一种基于栅格化电子海图的无人艇航路规划方法 |
CN113932812B (zh) * | 2021-09-27 | 2024-09-13 | 北京邮电大学 | 路径规划方法、装置、电子设备和存储介质 |
CN113804213B (zh) * | 2021-10-14 | 2024-05-28 | 南京芯传汇电子科技有限公司 | 一种AStar快速路径规划改进算法 |
CN114323045B (zh) * | 2021-12-24 | 2024-04-19 | 浙江中控技术股份有限公司 | 路径规划方法及装置 |
CN114485611B (zh) * | 2021-12-28 | 2024-04-26 | 中科星图股份有限公司 | 基于北斗网格码的三维空间最短路径规划方法和装置 |
CN115639827B (zh) * | 2022-12-22 | 2023-03-21 | 华北科技学院(中国煤矿安全技术培训中心) | 一种结构化环境下机器人路径规划方法和系统 |
CN117709839B (zh) * | 2024-02-04 | 2024-04-23 | 华芯(嘉兴)智能装备有限公司 | 基于任务时限要求的天车路径规划方法及装置 |
CN117851655B (zh) * | 2024-03-05 | 2024-05-28 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 基于多算法耦合的船舶航迹缺失数据补全方法及系统 |
CN118112576B (zh) * | 2024-04-29 | 2024-07-12 | 广州大学 | 一种基于声呐定位和智能算法的海底设备定位和搜寻方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107197806A (zh) * | 2017-05-17 | 2017-09-26 | 武汉理工大学 | 基于无人艇的弹性栅格式定点自动投饵方法 |
CN110045731A (zh) * | 2019-03-26 | 2019-07-23 | 深圳先进技术研究院 | 一种路径规划方法、电子装置及计算机可读存储介质 |
CN110487279A (zh) * | 2019-08-27 | 2019-11-22 | 东南大学 | 一种基于改进a*算法的路径规划方法 |
-
2020
- 2020-01-03 CN CN202010004408.1A patent/CN111060109B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107197806A (zh) * | 2017-05-17 | 2017-09-26 | 武汉理工大学 | 基于无人艇的弹性栅格式定点自动投饵方法 |
CN110045731A (zh) * | 2019-03-26 | 2019-07-23 | 深圳先进技术研究院 | 一种路径规划方法、电子装置及计算机可读存储介质 |
CN110487279A (zh) * | 2019-08-27 | 2019-11-22 | 东南大学 | 一种基于改进a*算法的路径规划方法 |
Non-Patent Citations (3)
Title |
---|
"Bio-inspired Algorithm for Path Planning of Terrestrial Robot Using Aerial Images";Erasmo Gabriel Martinez-Soltero;《2018 IEEE Latin American Conference on Computational Intelligence (LA-CCI)》;20181231;正文第1-5页 * |
室外自主移动机器人AMOR的导航技术;杨唐文等;《计算机工程与应用》;20080801(第22期);正文第206-209页 * |
面向对象的自主车越野路径规划的设计和实现;翁妙凤,潘峻;《计算机研究与发展》;19960731(第07期);正文第533-540页 * |
Also Published As
Publication number | Publication date |
---|---|
CN111060109A (zh) | 2020-04-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111060109B (zh) | 一种基于改进a星算法的无人艇全局路径规划方法 | |
US6259988B1 (en) | Real-time mission adaptable route planner | |
Wilkie et al. | Transforming GIS data into functional road models for large-scale traffic simulation | |
CN110006430B (zh) | 一种航迹规划算法的优化方法 | |
CN106441303A (zh) | 一种基于可搜索连续邻域a*算法的路径规划方法 | |
CN106980633B (zh) | 室内地图数据的生成方法及装置 | |
CN112113573B (zh) | 一种面向单艘无人测量船艇覆盖路径规划方法 | |
CN101241507A (zh) | 一种地图寻路方法及系统 | |
CN110196059B (zh) | 一种无人艇全局路径规划方法 | |
CN109737965B (zh) | 一种三维虚拟场景下的导航路径最优选择的方法 | |
CN111721296B (zh) | 一种水下无人航行器数据驱动路径规划方法 | |
JP6810432B2 (ja) | 物体の条件に応じてモードを切り換えることができるcnn基盤で軍事目的、スマートフォン又は仮想走行に使用される疑似3dバウンディングボックスを検出する方法及びこれを利用した装置 | |
US20010016796A1 (en) | Device and method for changing map information | |
CN114721401A (zh) | 一种基于改进a*算法的高效路径规划方法 | |
KR20090035589A (ko) | 잠수정을 위한 경로를 결정하는 방법 | |
CN112923940A (zh) | 路径规划方法、装置、处理设备、移动设备及存储介质 | |
RU2296368C2 (ru) | Способ отсечения линии и способ отображения трехмерного изображения на основе этого способа | |
US11846517B2 (en) | Vector tile navigation | |
CN114404985A (zh) | 虚拟角色的路径规划方法及装置、电子设备、存储介质 | |
CN110375763A (zh) | 搜索兴趣点的方法、装置及智能终端 | |
CN116892943A (zh) | 一种基于改进的jps算法的极地船舶路径规划方法及系统 | |
JP4513455B2 (ja) | 3次元海底地形図表示装置及び方法 | |
CN112509132B (zh) | 一种面向船舶应用的立体路径计算方法 | |
CN113776535A (zh) | 一种基于栅格化电子海图的无人艇航路规划方法 | |
JP2022123401A (ja) | 経路計画装置、移動体、経路計画方法及びプログラム |
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 |