CN108444482B - 一种无人机自主寻路避障方法及*** - Google Patents
一种无人机自主寻路避障方法及*** Download PDFInfo
- Publication number
- CN108444482B CN108444482B CN201810619904.0A CN201810619904A CN108444482B CN 108444482 B CN108444482 B CN 108444482B CN 201810619904 A CN201810619904 A CN 201810619904A CN 108444482 B CN108444482 B CN 108444482B
- Authority
- CN
- China
- Prior art keywords
- path
- point
- node
- grid
- plane
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 39
- 230000003068 static effect Effects 0.000 claims abstract description 14
- 230000008569 process Effects 0.000 claims abstract description 13
- 230000008859 change Effects 0.000 claims abstract description 10
- 238000012545 processing Methods 0.000 claims abstract description 10
- 238000004364 calculation method Methods 0.000 claims abstract description 5
- 238000009499 grossing Methods 0.000 claims abstract description 4
- 230000006870 function Effects 0.000 claims description 19
- 230000004888 barrier function Effects 0.000 claims description 10
- 238000013507 mapping Methods 0.000 claims description 5
- 239000003086 colorant Substances 0.000 claims description 3
- 238000012805 post-processing Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 12
- 238000010845 search algorithm Methods 0.000 description 5
- 230000007547 defect Effects 0.000 description 2
- 230000009467 reduction Effects 0.000 description 2
- 230000001154 acute effect Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000006467 substitution reaction Methods 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
-
- 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/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
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)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种无人机自主寻路避障方法及***。本发明方法,包括:采集障碍物的位置信息;通过栅格法进行三维环境建模,并将其分割成若干个栅格,分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理,通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型;基于A*算法在所述二维栅格模型上做出全局静态路径规划;在全局静态路径规划后,进行局部动态路径规划;通过贝塞尔曲线对规划出的线路点进行轨迹跟踪,完成对路径轨迹的平滑处理;本发明通过全局静态路径规划和局部动态路径规划,使得无人机可以及时发现环境的动态变化带来的障碍物位置的改变,计算过程简便快捷,解决了传统飞行线路容易碰障碍物的问题。
Description
技术领域
本发明涉及无人机路径规划和轨迹跟踪领域,尤其涉及一种无人机自主寻路避障方法及***。
背景技术
无人机可以在复杂环境下进行各种需求的任务,由于环境的复杂多变性,导致无人机在执行任务时会遇到各种各样的问题。其中最主要的问题就是由于多变的环境导致的无人机周围障碍物增多,使得无人机无法正确的绕过障碍物执行任务。直接直线绕过障碍物也可能会产生碰撞桨的事故。由于环境的复杂多变性,静态的避障寻路已经不能满足时刻变化的复杂环境,为此必须采用一种实时监测出环境变化,实时变更路径的一种动态规划方法。
在路径规划和轨迹跟踪领域,各种算法层出不穷。现有的相关算法由于多数都是处理静态地图和环境,虽然在静态环境下的表现都很优秀,但是在动态环境领域表现的有欠缺。
发明内容
根据上述提出的技术问题,而提供一种在全局路径规划后,进行局部动态规划,更好地处理环境变化的无人机自主寻路避障方法及***。本发明采用的技术手段如下:
一种无人机自主寻路避障方法,包括如下步骤:
S1、采集障碍物的位置信息;
S2、通过栅格法进行三维环境建模,并将其分割成若干个栅格,分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理,通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型;
S3、基于A*算法在所述二维栅格模型上做出全局静态路径规划,用于寻找起始点与最终点之间的最优的全局参考路径;
S4、在全局静态路径规划后,进行局部动态路径规划,用于在处理完突发的动态环境变化后,尽快回归至原先规划好的路径;
进一步地,所述局部动态规划后,包括:S5、通过贝塞尔曲线对规划出的线路点进行轨迹跟踪,完成对路径轨迹的平滑处理。
进一步地,所述步骤S1具体包括:
S101、通过传感器获取障碍物在传感器坐标系下的位置信息;
S102、通过坐标变换将所述传感器坐标系下的位置信息转换到无人机坐标系下;
S103、通过导航解算将无人机坐标系下的位置信息导入惯性坐标系下,获得动态的障碍物位置信息。
进一步地,所述通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型具体包括:
S201、过起始点及目标终点,生成垂直于yz平面的判断平面;
S202、将xy平面的二维栅格映射至所述判断平面,
当起始点的水平高度高于目标终点的水平高度时:
当起始点的水平高度低于目标终点的水平高度时:
当起始点的水平高度高于目标终点的水平高度一致时,判断平面平行于xy平面,
Pz=Ez=Sz (3)
其中,P1(Py,Pz)表示判断平面上任一点P的坐标在yz平面的投影点,S1(Sy,Sz)表示起始点S在yz平面的投影点,E1(Ey,Ez)表示目标终点S在yz平面的投影点,
结合公式(1)(2)(3)得到判断平面中任一点的坐标,
S203、设判断平面中某点坐标Px=Bx,Py=By,通过公式(4)判断二维栅格中的障碍物栅格和自由栅格,其中:
若Pz>Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为自由栅格,
若Pz<Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为障碍物栅格,
B(Bx,By,Bz)表示三维障碍物栅格坐标。
进一步地,所述步骤S3具体包括如下步骤:
S301、将建立的二维栅格模型映射到二维数组中,将所述障碍物栅格在该数组中对应的元素被赋值为1,自由栅格在数组中的对应元素被赋值为0;
S302、将起始点加入开启集OpenList中,并将起始点信息结构体的地址赋值给当前节点父指针,
其中,所述开启集用于存储待选栅格节点信息;
S303、将当前节点放入关闭集CloseList中,所述关闭集用于存储已经搜索选择过的、不能被再次搜索的栅格节点信息;
S304、在二维数组中搜索当前节点邻域内的所有自由栅格计算各节点的代价函数的值,并与开启集中的节点进行比较,若开启集已经存在该节点,则比较各自的g(k)值,如果该节点的g(k)值更小,则将开启集中的g(k)与f(k)更新,并将开启集中所存储该节点的父指针指向当前节点,若开启集中没有该节点,则将该节点加入开启集,如果目标终点被加入开启集中,则执行步骤S306,否则执行步骤S305,
其中,g(k),起始点到待选节点的k的路径代价;f(k),待选节点k的总代价值;
所述代价函数f(n)通过如下表达式表述:
f(n)=g(n)+h(n),
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;
S305、从开启集中选取具有最小f(k)的节点,将当前节点指针指向当前节点,执行S303;
S306、从目标节点开始,通过追溯父节点指针确定最终路径,全局路径规划完成,输出路径结果。
进一步地,所述步骤S4具体包括如下步骤:
S401、读取路径点并判断是否到达目标终点,若是,则结束流程,若否,则执行S402;
S402、判断当前节点领域内环境是否改变,若是,则进行局部动态路径规划,执行S403,若否,则返回S401,继续进行路径点跟踪,
其中,所述动态路径规划通过如下算法所示:
f(n)=g(n)+h(n)+x(n)
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离;
S403、动态路径规划后,判断是否回归全局路径,若是,则返回S401,继续进行路径点跟踪,若否,则继续进行动态路径规划,直到回归到全局路径中;
路径点逐点判断后,到达目标终点,完成全局路径中局部动态路径规划。
一种无人机自主寻路避障***,包括:
采集单元,用于采集路径障碍物的位置信息;
预处理单元,用于对采集的信息进行三维环境建模并降维成二维栅格模型;路径规划单元,包括
其中,全局规划模块,采用A*算法在二维栅格生成起始点与最终点之间的最优的全局参考路径;
局部规划模块,采用改进并具有无记忆回归的A*算法进行局部路径最优规划;
后期处理模块,采用贝塞尔曲线对规划出的线路点进行轨迹跟踪,使轨迹平滑。
本发明对无人机的环境进行三维环境建模,通过判断平面对其进行降模得到二维栅格模型,基于A*算法在二维栅格模型上做出全局路径规划,再通过局部动态规划对突发的动态环境变化进行重新规划,再通过贝塞尔曲线对规划出的线路点进行平滑规划,从而使得无人机可以及时发现环境的动态变化带来的障碍物位置的改变,局部动态规划响应速度快,实时性好,计算过程简便快捷,解决了传统飞行线路容易碰障碍物的问题,基于上述理由本发明可在无人机路径规划和轨迹跟踪领域广泛推广。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍,显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动性的前提下,还可以根据这些附图获得其他的附图。
图1为本发明无人机自主寻路避障方法流程图;
图2为本发明无人机自主寻路避障装置图;
图3为本发明三维建模示意图,其中,(a)为起始点高于目标终点情况下的示意图,(b)为起始点低于目标终点情况下的示意图;
图4为本发明判断平面切割栅格示意图,其中,(a)为起始点高于目标终点情况下的示意图,(b)为起始点低于目标终点情况下的示意图;
图5为本发明二维栅格模型示意图,其中,(a)为起始点高于目标终点情况下的示意图,(b)为起始点低于目标终点情况下的示意图;
图6为本发明全局静态路径规划的路径搜索过程示意图;
图7为本发明局部动态路径规划的路径规划流程图;
图8为本发明通过贝塞尔曲线对线路点平滑规划示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
如图1所示,一种无人机自主寻路避障方法,包括如下步骤:
S2、通过栅格法进行三维环境建模,并将其分割成若干个栅格,分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理,通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型;
S3、基于A*算法在所述二维栅格模型上做出全局静态路径规划,用于寻找起始点与最终点之间的最优的全局参考路径;
S4、在全局静态路径规划后,进行局部动态路径规划,用于在处理完突发的动态环境变化后,尽快回归至原先规划好的路径;
S5、通过贝塞尔曲线对规划出的线路点进行轨迹跟踪,完成对路径轨迹的平滑处理。
如图2所示,一种无人机自主寻路避障***,包括:
采集单元,用于采集路径障碍物的位置信息;
预处理单元,用于对采集的信息进行三维环境建模并降维成二维栅格模型;
路径规划单元,包括
其中,全局规划模块,采用A*算法在二维栅格生成起始点与最终点之间的最优的全局参考路径;
局部规划模块,采用改进并具有无记忆回归的A*算法进行局部路径最优规划;
后期处理模块,采用贝塞尔曲线对规划出的线路点进行轨迹跟踪,使轨迹平滑。
实施例1,其中,步骤S1包括:通过激光和超声波传感器获取障碍物在传感器坐标系下的位置信息,通过坐标变化将其转换到无人机坐标系下,再通过导航***将其在惯性坐标系下的位置表示出来,从而获得动态的障碍物的位置信息。
采用单元分解建模法将规划空间分为若干个形状规则的基本单元,每个基本单元都被划分成包含障碍物的节点与不包含障碍物的节点,这些节点可以直观的描述环境信息并便于计算机存储,同时各节点方便拆分,便于规划算法处理运算。
栅格法进行三维环境建模,是将有限范围的三维规划空间分割成等大的单元,如图3(a)(b)所示,将三维空间分割成n×n×n个栅格,本实施例采用30×30×30个栅格,每个栅格为一个立方体,每个栅格的中心作为规划算法的一个计算单位,其中包含障碍物的障碍物栅格赋值为1,并用深色处理,不包含障碍物的自由栅格赋值为0,用浅色处理,每个单位栅格的中心将作为该栅格的坐标。
如图4(a)(b)所示,过起始点及目标终点,生成垂直于yz平面的判断平面F,判断平面F包含起始点S与目标终点E,并切过高度高于判断平面F的障碍物栅格;
如图5(a)(b)所示,将xy平面的二维栅格映射至所述判断平面F,判断平面F的单位栅格大小由判断平面F与xy平面的夹角和xy平面的单位栅格大小确定。图中包括,判断平面F与xy平面所成的锐角α,起始点与目标终点连线的延长线与y轴的交点C(Cy,0),
如图3、4、5中(a)图所示,当起始点的水平高度高于目标终点的水平高度时:
如图3、4、5中(b)图所示,当起始点的水平高度低于目标终点的水平高度时:
当起始点的水平高度高于目标终点的水平高度一致时,判断平面平行于xy平面,
Pz=Ez=Sz (3)
其中,P1(Py,Pz)表示判断平面上任一点P的坐标在yz平面的投影点,S1(Sy,Sz)表示起始点S在yz平面的投影点,E1(Ey,Ez)表示目标终点S在yz平面的投影点,
结合公式(1)(2)(3)得到判断平面中任一点的坐标,
S203、设判断平面中某点坐标Px=Bx,Py=By,通过公式(4)判断二维栅格中的障碍物栅格和自由栅格,其中:
若Pz>Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为自由栅格,
若Pz<Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为障碍物栅格,
B(Bx,By,Bz)表示三维障碍物栅格坐标。
A*算法的本质是贪心算法与启发式搜索算法的结合,所以A*算法结合了贪心算法以及启发式搜索算法的优点,继承了二者的特性。A*算法与贪心算法相比,其在状态空间规模庞大的情形下,搜索效率有显著提高,与最佳优先搜索算法相比,A*算法因为结合了贪心算法的“寻优”性,逐步趋于最优地进行搜索,克服了最佳优先搜索算法启发信息单一的缺点,理论上能在状态空间内搜索到最优解。
A*算法的核心在于代价函数的设计,代价函数的通用表达式如公式(5)所示。
f(n)=g(n)+h(n) (5)
其中f(n)为每个节点的代价函数,g(n)表示起始点到节点n的代价,h(n)表示从节点到目标点的启发函数,针对不同情形的问题,应重点设计启发函数h(n)。当h(n)设计为小于等于当前节点到目标终点的真实最小代价,A*算法在理论上就能在规划空间内找到最优路径,当前节点到目标终点的真实最小代价事先无法估计与计算,且h(n)要求与真实代价最大限度地接近,因此要保证A*算法寻路的准确性,h(n)必须结合实际问题本身精心设计。
如图6所示,利用A*算法进行路径规划的过程中,假设以栅格法建立环境模型,A*算法在栅格模型的状态空间中通过代价函数计算出待选择节点的代价值,将其存入开启集中。从开启集中选出代价最小的节点,将其存入关闭集,以选入关闭集的节点为基础继续进行扩展搜索,在每次扩展搜索的过程中进行代价值得更新,保证每个节点的父节点指针都是从起点到这个节点的最优路径,当A*算法搜索到终点并将其存入关闭集后,利用每个节点的父节点指针进行回溯,A*算法的路径搜索过程完成。
h(n)选择曼哈顿距离,有:
h(n)=D*(|n.x-goal.x|+|n.y-goal.y|)
A*算法在全局路径规划的过程中,实际是通过检查降维后的二维栅格节点的信息,并计算为代价值,在软件仿真过程中,各栅格节点所包含的信息将存储为一个结构体的形式,便于程序访问调用。该结构体如下代码所示。
结构体中的成员变量g,f,h,分别表示g(),f(),h(),fatherpoint为节点父指针,指向当前节点的父节点的结构体地址,当A*算法的节点搜索结束后,通过父节点指针回溯各节点的父节点,完成全局路径规划。
如图7所示,基于A*算法在降维模型上的全局路径规划过程如下:
S301、将建立的二维栅格模型映射到二维数组中,将所述障碍物栅格在该数组中对应的元素被赋值为1,自由栅格在数组中的对应元素被赋值为0;
S302、将起始点加入开启集OpenList中,并将起始点信息结构体的地址赋值给当前节点父指针,
其中,所述开启集用于存储待选栅格节点信息;
S303、将当前节点放入关闭集CloseList中,所述关闭集用于存储已经搜索选择过的、不能被再次搜索的栅格节点信息;
S304、在二维数组中搜索当前节点邻域内的所有自由栅格计算各节点的代价函数的值,并与开启集中的节点进行比较,若开启集已经存在该节点,则比较各自的g(k)值,如果该节点的g(k)值更小,则将开启集中的g(k)与f(k)更新,并将开启集中所存储该节点的父指针指向当前节点,若开启集中没有该节点,则将该节点加入开启集,如果目标终点被加入开启集中,则执行步骤S306,否则执行步骤S305,
其中,g(k),起始点到待选节点的k的路径代价;f(k),待选节点k的总代价值;
所述代价函数f(n)通过如下表达式表述:
f(n)=g(n)+h(n),
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;
S305、从开启集中选取具有最小f(k)的节点,将当前节点指针指向当前节点,执行S303;
S306、从目标节点开始,通过追溯父节点指针确定最终路径,全局路径规划完成,输出路径结果。
如图7所示,动态路径规划流程,
S401、读取路径点并判断是否到达目标终点,若是,则结束流程,若否,则执行S402;
S402、判断当前节点领域内环境是否改变,若是,则进行局部动态路径规划,执行S403,若否,则返回S401,继续进行路径点跟踪,
其中,所述动态路径规划通过如下算法所示:
f(n)=g(n)+h(n)+x(n)
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离;
S403、动态路径规划后,判断是否回归全局路径,若是,则返回S401,继续进行路径点跟踪,若否,则继续进行动态路径规划,直到回归到全局路径中;
路径点逐点判断后,到达目标终点,完成全局路径中局部动态路径规划。
由于传统的避障在检测到障碍物之后,沿着直线直接进行避障,容易撞到无人机,导致任务失败,所以本节采用基于贝塞尔曲线的轨迹规划方法,进行绕过障碍物的轨迹跟踪。
如图8所示,二阶贝塞尔曲线,由绕过P0,P1,P2这三个点组成的一个贝赛尔曲线:
B(t)=(1-t)2P0+2t(1-t)P1+t2P2,t∈[0,1] (6)
三阶贝塞尔曲线的一般性的通用公式:
通过用贝塞尔曲线的方式对前面规划出的线路点,进行一个轨迹跟踪,使得轨迹更平滑,更可控,具有极好的适用性。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (6)
1.一种无人机自主寻路避障方法,其特征在于,包括:如下步骤:
S1、采集障碍物的位置信息;
S2、通过栅格法进行三维环境建模,并将其分割成若干个栅格,分别对包含障碍物的栅格和不包含障碍物的栅格进行不同颜色的处理,通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型;
S3、基于A*算法在所述二维栅格模型上做出全局静态路径规划,用于寻找起始点与最终点之间的最优的全局参考路径;
S4、在全局静态路径规划后,进行局部动态路径规划,用于在处理完突发的动态环境变化后,尽快回归至原先规划好的路径;
S5、通过贝塞尔曲线对规划出的线路点进行轨迹跟踪,完成对路径轨迹的平滑处理;
动态路径规划如下算法所示:
f(n)=g(n)+h(n)+x(n)
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;
x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离。
2.根据权利要求1所述的方法,其特征在于,所述步骤S1具体包括:
S101、通过传感器获取障碍物在传感器坐标系下的位置信息;
S102、通过坐标变换将所述传感器坐标系下的位置信息转换到无人机坐标系下;
S103、通过导航解算将无人机坐标系下的位置信息导入惯性坐标系下,获得动态的障碍物位置信息。
3.根据权利要求2所述的方法,其特征在于,所述通过过起始点及目标终点的判断平面分割栅格,得到二维栅格模型具体包括:
S201、过起始点及目标终点,生成垂直于yz平面的判断平面;
S202、将xy平面的二维栅格映射至所述判断平面,
当起始点的水平高度高于目标终点的水平高度时:
当起始点的水平高度低于目标终点的水平高度时:
当起始点的水平高度高于目标终点的水平高度一致时,判断平面平行于xy平面,
Pz=Ez=Sz (3)
其中,P1(Py,Pz)表示判断平面上任一点P的坐标在yz平面的投影点,S1(Sy,Sz)表示起始点S在yz平面的投影点,E1(Ey,Ez)表示目标终点S在yz平面的投影点,
结合公式(1)(2)(3)得到判断平面中任一点的坐标,
S203、设判断平面中某点坐标Px=Bx,Py=By,通过公式(4)判断二维栅格中的障碍物栅格和自由栅格,其中:
若Pz>Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为自由栅格,
若Pz<Bz,则平面F上以点P(Px,Py,Pz)为中心的栅格为障碍物栅格,
B(Bx,By,Bz)表示三维障碍物栅格坐标。
4.根据权利要求3所述的方法,其特征在于,所述步骤S3具体包括如下步骤:
S301、将建立的二维栅格模型映射到二维数组中,将所述障碍物栅格在该数组中对应的元素被赋值为1,自由栅格在数组中的对应元素被赋值为0;
S302、将起始点加入开启集OpenList中,并将起始点信息结构体的地址赋值给当前节点父指针,
其中,所述开启集用于存储待选栅格节点信息;
S303、将当前节点放入关闭集CloseList中,所述关闭集用于存储已经搜索选择过的、不能被再次搜索的栅格节点信息;
S304、在二维数组中搜索当前节点邻域内的所有自由栅格计算各节点的代价函数的值,并与开启集中的节点进行比较,若开启集已经存在该节点,则比较各自的g(k)值,如果该节点的g(k)值更小,则将开启集中的g(k)与f(k)更新,并将开启集中所存储该节点的父指针指向当前节点,若开启集中没有该节点,则将该节点加入开启集,如果目标终点被加入开启集中,则执行步骤S306,否则执行步骤S305,
其中,g(k),起始点到待选节点的k的路径代价;f(k),待选节点k的总代价值;
所述代价函数f(n)通过如下表达式表述:
f(n)=g(n)+h(n),
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;
S305、从开启集中选取具有最小f(k)的节点,将当前节点指针指向当前节点,执行S303;
S306、从目标节点开始,通过追溯父节点指针确定最终路径,全局路径规划完成,输出路径结果。
5.根据权利要求4所述的方法,其特征在于,所述步骤S4具体包括如下步骤:
S401、读取路径点并判断是否到达目标终点,若是,则结束流程,若否,则执行S402;
S402、判断当前节点领域内环境是否改变,若是,则进行局部动态路径规划,执行S403,若否,则返回S401,继续进行路径点跟踪,
S403、动态路径规划后,判断是否回归全局路径,若是,则返回S401,继续进行路径点跟踪,若否,则继续进行动态路径规划,直到回归到全局路径中;
路径点逐点判断后,到达目标终点,完成全局路径中局部动态路径规划。
6.一种权利要求1所述无人机自主寻路避障方法的***,包括:
采集单元,用于采集路径障碍物的位置信息;
预处理单元,用于对采集的信息进行三维环境建模并降维成二维栅格模型;
路径规划单元,包括
其中,全局规划模块,采用A*算法在二维栅格生成起始点与最终点之间的最优的全局参考路径;
局部规划模块,采用改进并具有无记忆回归的A*算法进行局部路径最优规划;
后期处理模块,采用贝塞尔曲线对规划出的线路点进行轨迹跟踪,使轨迹平滑;
无记忆回归的A*算法如下所示:
f(n)=g(n)+h(n)+x(n)
g(n)表示起始点到节点n的路径代价,g(n)采用一种基于闵可夫斯基距离的公式进行计算,计算各个方向的代价不同从而使得路径代价不一样,h(n)表示从节点到目标点的启发函数;h(n)表示的是曼哈顿距离|x1-x2|+|y1-y2|;
x(n)表示的是新规划的节点到原来静态规划的路径的欧氏距离。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810619904.0A CN108444482B (zh) | 2018-06-15 | 2018-06-15 | 一种无人机自主寻路避障方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810619904.0A CN108444482B (zh) | 2018-06-15 | 2018-06-15 | 一种无人机自主寻路避障方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108444482A CN108444482A (zh) | 2018-08-24 |
CN108444482B true CN108444482B (zh) | 2021-10-22 |
Family
ID=63207123
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810619904.0A Expired - Fee Related CN108444482B (zh) | 2018-06-15 | 2018-06-15 | 一种无人机自主寻路避障方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108444482B (zh) |
Families Citing this family (60)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109163613A (zh) * | 2018-08-28 | 2019-01-08 | 联誉信息股份有限公司 | 一种智能打靶机器人 |
CN109634304B (zh) * | 2018-12-13 | 2022-07-15 | 中国科学院自动化研究所南京人工智能芯片创新研究院 | 无人机飞行路径规划方法、装置和存储介质 |
CN111380526B (zh) * | 2018-12-27 | 2022-04-05 | 北京航迹科技有限公司 | 一种路径确定的***和方法 |
US10948300B2 (en) | 2018-12-27 | 2021-03-16 | Beijing Voyager Technology Co., Ltd. | Systems and methods for path determination |
CN109781126A (zh) * | 2018-12-30 | 2019-05-21 | 芜湖哈特机器人产业技术研究院有限公司 | 一种基于矢量法的全局静态路径规划方法 |
CN111435256A (zh) * | 2019-01-12 | 2020-07-21 | 上海航空电器有限公司 | 一种基于栅格地图的飞行器自动地形规避方法 |
CN109636077B (zh) * | 2019-02-19 | 2022-11-25 | 浙江大学 | 基于双重局部位姿变换的变节点装配路径规划方法 |
CN109947123B (zh) * | 2019-02-27 | 2021-06-22 | 南京航空航天大学 | 一种基于视线导引律的无人机路径跟踪与自主避障方法 |
CN109871022A (zh) * | 2019-03-18 | 2019-06-11 | 江苏科技大学 | 一种面向两栖无人搜救器的智能路径规划及避障方法 |
CN111721279A (zh) * | 2019-03-21 | 2020-09-29 | 国网陕西省电力公司商洛供电公司 | 一种适用于输电巡检工作的末端路径导航方法 |
CN110108284B (zh) * | 2019-05-24 | 2020-10-30 | 西南交通大学 | 一种顾及复杂环境约束的无人机三维航迹快速规划方法 |
CN110260875A (zh) * | 2019-06-20 | 2019-09-20 | 广州蓝胖子机器人有限公司 | 一种全局规划路径的方法、全局规划器以及存储介质 |
CN110282553B (zh) * | 2019-06-26 | 2020-11-17 | 上海应用技术大学 | 桥式起重机路径规划方法及*** |
CN110262568B (zh) * | 2019-07-19 | 2021-10-22 | 深圳市道通智能航空技术股份有限公司 | 一种基于目标跟踪的无人机避障方法、装置及无人机 |
CN110320933B (zh) * | 2019-07-29 | 2021-08-10 | 南京航空航天大学 | 一种巡航任务下无人机避障运动规划方法 |
CN110568862A (zh) * | 2019-09-29 | 2019-12-13 | 苏州浪潮智能科技有限公司 | 一种无人机飞行路径规划方法、装置及相关设备 |
CN110618702B (zh) * | 2019-10-18 | 2022-06-24 | 中国电子科技集团公司电子科学研究院 | 一种无人机集群三维曲线路径跟踪方法、装置和存储介质 |
CN110609560A (zh) * | 2019-10-29 | 2019-12-24 | 广州高新兴机器人有限公司 | 移动机器人避障规划方法及计算机存储介质 |
CN110687923B (zh) * | 2019-11-08 | 2022-06-17 | 深圳市道通智能航空技术股份有限公司 | 无人机长距离循迹飞行方法、装置、设备及存储介质 |
CN111111187B (zh) * | 2019-11-28 | 2023-07-14 | 玩心(北京)网络科技有限公司 | 基于网格的在线游戏寻路方法和装置 |
CN110849385B (zh) * | 2019-11-28 | 2023-09-22 | 的卢技术有限公司 | 基于双层启发搜索共轭梯度下降的轨迹规划方法及*** |
CN111024082B (zh) * | 2019-12-02 | 2021-12-17 | 深圳优地科技有限公司 | 一种规划机器人局部路径的方法、装置及机器人 |
CN110887503B (zh) * | 2019-12-06 | 2021-10-15 | 广州文远知行科技有限公司 | 移动轨迹模拟方法、装置、设备及介质 |
CN110986951B (zh) * | 2019-12-11 | 2023-03-24 | 广州市技田信息技术有限公司 | 一种基于惩罚权重的路径规划方法、导航格以及栅格地图 |
CN111176273B (zh) * | 2019-12-24 | 2022-03-11 | 中国科学院合肥物质科学研究院 | 无人机动平台在多域地形环境下的全局路径规划方法 |
CN111369991B (zh) * | 2020-02-17 | 2023-12-12 | 南京邮电大学 | 一种支持自然语言指令的移动控制方法及其*** |
DE102020105793A1 (de) | 2020-03-04 | 2021-09-09 | Volocopter Gmbh | Bahnplanungsverfahren und Bahnplanungsalgorithmus für ein Fluggerät |
CN111338359B (zh) * | 2020-04-30 | 2022-11-01 | 武汉科技大学 | 一种基于距离判断和角度偏转的移动机器人路径规划方法 |
CN111552296B (zh) * | 2020-05-14 | 2021-03-26 | 宁波智能装备研究院有限公司 | 一种基于弯曲柱坐标系的局部平滑轨迹规划方法 |
CN114041097B (zh) * | 2020-05-27 | 2024-05-17 | 深圳市大疆创新科技有限公司 | 无人飞行器的航线平滑处理方法、装置及控制终端 |
CN111707269B (zh) * | 2020-06-23 | 2022-04-05 | 东南大学 | 一种三维环境下的无人机路径规划方法 |
CN111780777B (zh) * | 2020-07-13 | 2022-10-21 | 江苏中科智能制造研究院有限公司 | 一种基于改进a*算法和深度强化学习的无人车路径规划方法 |
CN111811517A (zh) * | 2020-07-15 | 2020-10-23 | 中国科学院上海微***与信息技术研究所 | 一种动态局部路径规划方法及*** |
CN111897361B (zh) * | 2020-08-05 | 2023-08-22 | 广州市赛皓达智能科技有限公司 | 一种无人机自主航线规划方法及其*** |
CN111897341A (zh) * | 2020-08-05 | 2020-11-06 | 三一专用汽车有限责任公司 | 泊车路径规划方法、装置和计算机可读存储介质 |
CN112484726B (zh) * | 2020-09-30 | 2022-08-30 | 天津基点科技有限公司 | 一种基于三维模型的无人机路径规划方法 |
CN112327939B (zh) * | 2020-10-15 | 2024-04-12 | 广东工业大学 | 一种城市街区环境下高层消防多无人机协同路径规划方法 |
CN112629549B (zh) * | 2020-11-27 | 2024-02-23 | 三峡大学 | 混凝土拱坝缆机运行轨迹线规划方法 |
CN112591410A (zh) * | 2020-11-27 | 2021-04-02 | 江苏科技大学 | 一种基于全向轮的物料传输***及其控制方法 |
CN113758484B (zh) * | 2020-11-30 | 2024-07-16 | 北京京东乾石科技有限公司 | 路径规划方法和装置 |
CN113000397B (zh) * | 2021-01-28 | 2023-03-17 | 珠海迪沃航空工程有限公司 | 一种基于动态图像识别的螺栓抓取检测装置及控制*** |
CN112975953B (zh) * | 2021-01-28 | 2022-06-03 | 珠海迪沃航空工程有限公司 | 一种机械手运动轨迹规划方法及螺栓抓取检测*** |
CN112902969B (zh) * | 2021-02-03 | 2023-08-01 | 重庆大学 | 一种无人机在数据收集过程中的路径规划方法 |
CN113093787B (zh) * | 2021-03-15 | 2022-09-13 | 西北工业大学 | 一种基于速度场的无人机轨迹规划方法 |
CN113155132B (zh) * | 2021-04-18 | 2024-04-12 | 吴亮亮 | 一种用于温室大棚的无人机路径规划方法及*** |
CN113296536B (zh) * | 2021-05-24 | 2022-04-05 | 哈尔滨工业大学 | 一种基于a*与凸优化算法的无人机三维避障算法 |
CN113341970A (zh) * | 2021-06-01 | 2021-09-03 | 苏州天准科技股份有限公司 | 智能巡检导航避障***、方法、存储介质和巡检车 |
CN113496547A (zh) * | 2021-06-22 | 2021-10-12 | 华南理工大学 | 实物保护***的最薄弱路径识别方法、装置、设备和介质 |
CN113467469B (zh) * | 2021-07-23 | 2024-01-23 | 中国核动力研究设计院 | 一种基于bim技术的物项吊运空间轨迹规划方法及*** |
CN113467483B (zh) * | 2021-08-23 | 2022-07-26 | 中国人民解放军国防科技大学 | 动态环境中基于时空栅格地图的局部路径规划方法及装置 |
CN113759905A (zh) * | 2021-08-30 | 2021-12-07 | 北京盈迪曼德科技有限公司 | 机器人路径规划方法、装置及机器人 |
CN114035572B (zh) * | 2021-10-09 | 2023-08-01 | 凤凰智能电子(杭州)有限公司 | 一种割草机器人的避障巡回方法及*** |
CN114152263B (zh) * | 2021-11-11 | 2024-04-16 | 上海应用技术大学 | 路径规划方法、***、电子设备及存储介质 |
CN114313878B (zh) * | 2021-11-19 | 2024-04-19 | 江苏科技大学 | 一种面向物料传输平台的运动学建模与路径规划方法 |
CN114485611B (zh) * | 2021-12-28 | 2024-04-26 | 中科星图股份有限公司 | 基于北斗网格码的三维空间最短路径规划方法和装置 |
CN115578525B (zh) * | 2022-09-06 | 2024-03-29 | 中国能源建设集团云南火电建设有限公司 | 一种复杂环境的工程选线优化***及方法 |
CN116105741B (zh) * | 2023-04-07 | 2023-07-11 | 南京航天宏图信息技术有限公司 | 一种多目标三维动态路径规划方法和装置 |
CN117308945B (zh) * | 2023-08-17 | 2024-04-09 | 成川科技(苏州)有限公司 | 基于准确交通管控的无人天车动态路径规划方法及*** |
CN116893687B (zh) * | 2023-08-21 | 2024-01-23 | 广东工业大学 | 一种复杂环境下改进lazy theta*的无人机路径规划方法 |
CN117631618A (zh) * | 2023-11-08 | 2024-03-01 | 南京国电南自维美德自动化有限公司 | 一种dcs逻辑组态画面连线的实时优化方法及*** |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102359784A (zh) * | 2011-08-01 | 2012-02-22 | 东北大学 | 一种室内移动机器人自主导航避障***及方法 |
CN103528585A (zh) * | 2013-09-26 | 2014-01-22 | 中北大学 | 一种不等距分割可通行区域的路径规划方法 |
CN104075717A (zh) * | 2014-01-21 | 2014-10-01 | 武汉吉嘉伟业科技发展有限公司 | 一种基于改进a*算法的无人机航线规划算法 |
CN106325294A (zh) * | 2016-08-22 | 2017-01-11 | 上海交通大学 | 基于贝塞尔曲线转接的无人机轨迹平滑方法 |
CN106647769A (zh) * | 2017-01-19 | 2017-05-10 | 厦门大学 | 基于a*提取引导点的agv路径跟踪与避障协调方法 |
CN106774310A (zh) * | 2016-12-01 | 2017-05-31 | 中科金睛视觉科技(北京)有限公司 | 一种机器人导航方法 |
CN106980320A (zh) * | 2017-05-18 | 2017-07-25 | 上海思岚科技有限公司 | 机器人充电方法及装置 |
CN107229998A (zh) * | 2017-05-31 | 2017-10-03 | 江苏德胜智业信息技术有限公司 | 一种无人机自主寻路策略方法 |
CN107328418A (zh) * | 2017-06-21 | 2017-11-07 | 南华大学 | 移动机器人在陌生室内场景下的核辐射探测路径自主规划方法 |
CN107883962A (zh) * | 2017-11-08 | 2018-04-06 | 南京航空航天大学 | 一种多旋翼无人机在三维环境下的动态航路规划方法 |
-
2018
- 2018-06-15 CN CN201810619904.0A patent/CN108444482B/zh not_active Expired - Fee Related
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102359784A (zh) * | 2011-08-01 | 2012-02-22 | 东北大学 | 一种室内移动机器人自主导航避障***及方法 |
CN103528585A (zh) * | 2013-09-26 | 2014-01-22 | 中北大学 | 一种不等距分割可通行区域的路径规划方法 |
CN104075717A (zh) * | 2014-01-21 | 2014-10-01 | 武汉吉嘉伟业科技发展有限公司 | 一种基于改进a*算法的无人机航线规划算法 |
CN106325294A (zh) * | 2016-08-22 | 2017-01-11 | 上海交通大学 | 基于贝塞尔曲线转接的无人机轨迹平滑方法 |
CN106774310A (zh) * | 2016-12-01 | 2017-05-31 | 中科金睛视觉科技(北京)有限公司 | 一种机器人导航方法 |
CN106647769A (zh) * | 2017-01-19 | 2017-05-10 | 厦门大学 | 基于a*提取引导点的agv路径跟踪与避障协调方法 |
CN106980320A (zh) * | 2017-05-18 | 2017-07-25 | 上海思岚科技有限公司 | 机器人充电方法及装置 |
CN107229998A (zh) * | 2017-05-31 | 2017-10-03 | 江苏德胜智业信息技术有限公司 | 一种无人机自主寻路策略方法 |
CN107328418A (zh) * | 2017-06-21 | 2017-11-07 | 南华大学 | 移动机器人在陌生室内场景下的核辐射探测路径自主规划方法 |
CN107883962A (zh) * | 2017-11-08 | 2018-04-06 | 南京航空航天大学 | 一种多旋翼无人机在三维环境下的动态航路规划方法 |
Non-Patent Citations (3)
Title |
---|
Two Layers Solving Strategy for Path Planning of Unmanned Aerial Vehicle;Kangning Zhu, er al;《2016 8th International Conference on Intelligent Human-Machine Systems and Cybernetics》;20161231;第69-73页 * |
何雨枫.室内微小型无人机路径规划算法研究.《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》.2014, * |
室内微小型无人机路径规划算法研究;何雨枫;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20140115;第9-53页 * |
Also Published As
Publication number | Publication date |
---|---|
CN108444482A (zh) | 2018-08-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108444482B (zh) | 一种无人机自主寻路避障方法及*** | |
CN107436148B (zh) | 一种基于多地图的机器人导航方法及装置 | |
JP6828044B2 (ja) | ルート逸脱認識方法、端末、および記憶媒体 | |
CN106371445B (zh) | 一种基于拓扑地图的无人车规划控制方法 | |
EP3109842B1 (en) | Map-centric map matching method and apparatus | |
CN108225358B (zh) | 交通工具导航 | |
US11300964B2 (en) | Method and system for updating occupancy map for a robotic system | |
CN113286985A (zh) | 一种路径规划方法和路径规划装置 | |
CN110220521A (zh) | 一种高精地图的生成方法和装置 | |
EP4116935A2 (en) | High-definition map creation method and device, and electronic device | |
CN114440916B (zh) | 一种导航方法、装置、设备及存储介质 | |
Chen et al. | Research on Ship Meteorological Route Based on A‐Star Algorithm | |
CN113587933A (zh) | 一种基于分支定界算法的室内移动机器人定位方法 | |
CN113819917A (zh) | 自动驾驶路径规划方法、装置、设备及存储介质 | |
Li et al. | Robust localization for intelligent vehicles based on compressed road scene map in urban environments | |
CN108803659B (zh) | 基于魔方模型的多窗口启发式三维空间路径规划方法 | |
CN114494329A (zh) | 用于移动机器人在非平面环境自主探索的导引点选取方法 | |
WO2022099620A1 (zh) | 三维点云分割方法和装置、可移动平台 | |
CN116772846A (zh) | 一种无人机航迹规划方法、装置、设备及介质 | |
CN115239899B (zh) | 位姿图生成方法、高精地图生成方法和装置 | |
CN116009552A (zh) | 一种路径规划方法、装置、设备及存储介质 | |
Yan et al. | Path planning based on constrained delaunay triangulation | |
CN114839975A (zh) | 一种自主探索式语义地图构建方法及*** | |
KR102624644B1 (ko) | 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법 | |
Jia et al. | Automatic ship routing with high reliability and efficiency between two arbitrary points at sea |
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 | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20211022 |