CN111561933A - 双重改进a星最短航路规划方法 - Google Patents

双重改进a星最短航路规划方法 Download PDF

Info

Publication number
CN111561933A
CN111561933A CN202010552031.3A CN202010552031A CN111561933A CN 111561933 A CN111561933 A CN 111561933A CN 202010552031 A CN202010552031 A CN 202010552031A CN 111561933 A CN111561933 A CN 111561933A
Authority
CN
China
Prior art keywords
node
new
point
cost
search
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.)
Granted
Application number
CN202010552031.3A
Other languages
English (en)
Other versions
CN111561933B (zh
Inventor
王晶
刘洋
牛元龙
周利军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xian Aisheng Technology Group Co Ltd
Original Assignee
Xian Aisheng Technology Group Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Xian Aisheng Technology Group Co Ltd filed Critical Xian Aisheng Technology Group Co Ltd
Priority to CN202010552031.3A priority Critical patent/CN111561933B/zh
Publication of CN111561933A publication Critical patent/CN111561933A/zh
Application granted granted Critical
Publication of CN111561933B publication Critical patent/CN111561933B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation

Abstract

本发明提供了一种双重改进A星最短航路规划方法，根据安全区位置信息，禁飞区位置信息，飞机性能信息，无人机行驶航段信息，使得无人机在安全区内规避各种禁飞区，形成一条安全最短飞行航路。本发明所提出的双重改进A星最短航路规划算法，能根据复杂的空域信息和禁飞区信息进行快速的航路规划，能根据再次在已经规划好得航路中再次使用A星算法，将短更优航路规划出来，非常有效的减少了因步长和搜索角度设置而生成的中间过渡航点。

Description

双重改进A星最短航路规划方法
技术领域
本发明涉及自动航路规划领域，尤其是涉及一种A星算法在复杂情况下的最短航路规划方法。
背景技术
随着无人机技术的飞速发展，反无人机技术也在逐步增强，无人机在战场上的生存环境变得日益恶化，对无人机智能自动导航，威胁规避提出了更高的要求。
A星算法是一种在图形平面上，有多个节点的路径，求出最低通过成本的算法。A星算法作为一种启发式搜索算法，常被用于游戏中的NPC移动计算。通过对A星算法的改进使得应用在无人机的航路规划中。
无人机在飞行过程中，可以通过多种手段实时检测出对无人机飞行产生威胁的区域，无人机针对实时发现的威胁区域需要在完成任务的前提下，实时调整自身航线，降低被击落风险。完成整个过程，需要无人机能够根据动态采集周围环境，实现实时自主航路规划。
发明内容
为了克服现有技术的不足，本发明提供一种双重改进A星最短航路规划方法，本发明是一种无人机最短航路规划算法，根据安全区位置信息，禁飞区位置信息，飞机性能信息，无人机行驶航段信息，使得无人机在安全区内规避各种禁飞区，形成一条安全最短飞行航路。
本发明解决其技术问题所采用的技术方案的步骤如下：
步骤1：计算最小安全距离：将飞机转弯半径和飞机大小看成质点，所需要的最小安全距离Ls为最大速度的最小转弯半径Rmin与飞机翼展LA之和，即：
Ls＝Rmin+LA
步骤2：安全区和禁飞区重生成：为了将飞机和飞机运动特性看成质点，将对禁飞区区域Sf按照最小安全距离LS扩大得到新禁飞区SfL，安全区区域Ss按照最小安全距离LS缩小得到新安全区SsL；
所述扩大的方法分为两种情况：
1)当禁飞区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi)，求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li加上最小安全距离Ls作为新距离Lis，Lis＝Li+Ls，，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成了区域扩大；
Figure BDA0002542899480000021
n为凸多边形总个数；
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当禁飞区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形扩大的步骤完成每个凸多边形的区域扩大，多个被扩大后的凸多边形的构成区域的并集，形成扩大后的凹多边形；
所述缩小的方法分为两种情况：
1)当安全区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi),求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li减去最小安全距离Ls作为新距离Lis，Lis＝Li-Ls，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成区域缩小；
Figure BDA0002542899480000022
n为凸多边形总个数
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当安全区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形缩小的步骤完成每个凸多边形的区域缩小，多个被缩小后的凸多边形的构成区域的并集，形成缩小后的凹多边形；
步骤3：确定A星算法搜索方向数目Nd和搜索步长Ds：
A星搜索方向数目Nd为4-360，每个方向间隔45度，搜索步长DS取最小转弯半径Rmin；
步骤4：确定重复点的判断依据：
历史搜索节点nodei包括自身位置，父节点指针，花费代价，历史代价，估计代价；
新加入节点noden包括自身位置，父节点指针，花费代价，历史代价，估计代价；
依次通过历史搜索过的节点nodei和新节点noden的自身位置，计算节点nodei和新节点noden的之间距离D(i,n)，当距离D(i,n)小于步长Ds的一半，即D(i,n)<Ds/2，则认为新节点noden和历史节点nodei为重复点，将不再作为新节点参与计算；
步骤5：确定禁飞区相交依据：新点noden在新禁飞区SfL内，或者新点noden与新点的父节点nodep的连线与新禁飞区SfL存在交点，两个条件满足其一，则认为与禁飞区相交；
步骤6：确定安全区相交依据：新点noden在新安全区SsL外，或者新点noden与新点的父节点nodep的连线和新安全区SsL存在交点；
步骤7：确定有效搜索点判断依据：根据步骤4判断不是重复点，或根据步骤5或步骤6判断不和禁飞区或者安全区相交；当两个条件都满足，认为当前搜索点为有效点；
步骤8：确定A星算法花费代价F(i)：
花费代价F(i)等于从起始点到当前有效搜索点的位置所历史代价G(i)与该位置到目标位置的估计代价H(i)之和，即：
F(i)＝G(i)+H(i)；
步骤9：父节点nodep生成：
开始指定起始点为第一个父节点nodep，通过父节点nodep的位置(xp,yp)和当前节点需搜索方向角度Dn，计算新节点noden位置(xn,yn)，通过步骤7的有效搜索点的判断依据，判断新节点noden是否为有效搜索节点，如果不是有效搜索节点，则改变搜索方向角度，并重新计算；否则是有效搜索节点，将有效搜索新节点noden加入有效数队列openlist中，并根据步骤8计算每个有效搜索点noden的花费代价F(n)值，查找openlist中所有搜索点，将F(n)最小的节点返回，当做新的父节点nodep，并从openlist中移除F(n)最小的节点，将新的父节点nodep加入已搜索队列closelist中；
新节点noden位置(xn,yn)的计算公式为：
Xn＝Xp+sin(Dn)*Ds
Yn＝Yp+cos(Dn)*Ds
步骤10：新节点noden生成：
根据父节点nodep位置(xp,yp)、当前节点的搜索方向角度Dn和搜索步长DS，计算出新节点noden的位置(xn,yn)，将新节点noden的父节点指针设置为nodep，方便后期逆向查找；根据步骤8分别计算新节点noden的花费代价F(n)，历史代价G(n)，估计代价H(n)；
其中，Dn＝D0+360/Nd*Ni；
Dn为当前节点的搜索方向角度，即本次新节点和父节点的连线和正北方向的夹角为搜索方向角度；
D0为本次目标点相对父节点的初始角度；
Ni为当前方向数目；
G(n)＝G(p)+C(n)
G(n)为新节点历史代价，取从起始点到新节点总距离；
G(p)为父节点历史代价，取从起始点到父节点总距离；
C(n)从父节点到新节点的花费代价，取父节点到新节点的距离；
F(n)＝G(n)+H(n)
H(n)为新节点估计代价，取从新节点直接到目标点的距离；
步骤11：确定搜索结束依据：
当目标点变成为有效搜索节点时，则规划成功，进入步骤12；如果目标点不是有效搜索节点时，进入步骤9，重新搜寻父节点，并改变新父节点，添加新目标点；
当有效队列openlist中所有有效节点都被移除，搜寻父节点失败，则规划失败，结束本次航线段规划，并返回空航路；
步骤12：规划航线初始生成：
当规划成功后，对终止时父节点nodep反向寻找生成该节点的父节点，根据当前节点存储的父节点的指针寻找父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点从起始位置开始依次连接起来，构成的节点集合为生成的初始规划航路；
步骤13：A星重构优化航路：按照初始规划航线的节点集合，作为A星搜索的新点集；
步骤14：花费代价重新计算F(i)，父节点重新指定：
按照顺序取节点nodep，当作父节点，依次取出节点nodep后面的节点nodei，根据步骤7判断当前节点nodei的有效性，当nodei为无效节点时，跳过本次节点，取出下个节点nodei进行判断，当nodei为有效节点时，重新计算父节点nodep直接扩展到nodei的历史代价G(pi)，如果代价G(pi)小于nodei中原始代价G(i)，则改变节点nodei中G(i)＝G(pi)，并更新节点nodei中父节点指针为nodep，代价G(pi)大于等于nodei中原始代价G(i)，则nodei中任何值不改变；
G(pi)＝G(p)+C(pi)
G(pi)为新节点历史代价，取从起始点到新节点总距离；
G(p)为父节点历史代价，取从起始点到父节点总距离；
C(pi)从父节点到新节点的花费代价，取父节点到新节点的距离；
步骤15：航路规划完成：通过对终止时父节点nodep反向寻找生成该节点的父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点，从起始位置开始依次连接，连接起来构成的位置集合为生成的最短规划航路。
本发明的有益效果在于所提出的双重改进A星最短航路规划算法，具有以下优点：
1.能根据复杂的空域信息和禁飞区信息进行快速的航路规划；
2.能根据再次在已经规划好得航路中再次使用A星算法，将短更优航路规划出来，非常有效的减少了因步长和搜索角度设置而生成的中间过渡航点。
附图说明
图1是本发明飞行航线、安全区、禁飞区相对关系图。
图2是本发明将航线段分别单独使用A星算法所搜索过的所有点图。
图3是本发明航线规划初始航点图。
图4是本发明再次使用A星算法规划的航点图。
图5是本发明生成最终规划后航路图。
图6是本发明本发明双重改进A星最短航路规划算法实现流程图。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
步骤1：计算最小安全距离：将飞机转弯半径和飞机大小看成质点，所需要的最小安全距离Ls为最大速度的最小转弯半径Rmin与飞机翼展LA之和，即：
Ls＝Rmin+LA
步骤2：安全区和禁飞区重生成：为了将飞机和飞机运动特性看成质点，将对禁飞区区域Sf按照最小安全距离LS扩大得到新禁飞区SfL，安全区区域Ss按照最小安全距离LS缩小得到新安全区SsL；
所述扩大的方法分为两种情况：
1)当禁飞区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi)，求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li加上最小安全距离Ls作为新距离Lis，Lis＝Li+Ls，，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成了区域扩大；
Figure BDA0002542899480000061
n为凸多边形总个数；
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当禁飞区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形扩大的步骤完成每个凸多边形的区域扩大，多个被扩大后的凸多边形的构成区域的并集，形成扩大后的凹多边形；
所述缩小的方法分为两种情况：
1)当安全区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi),求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li减去最小安全距离Ls作为新距离Lis，Lis＝Li-Ls，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成区域缩小；
Figure BDA0002542899480000071
n为凸多边形总个数
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当安全区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形缩小的步骤完成每个凸多边形的区域缩小，多个被缩小后的凸多边形的构成区域的并集，形成缩小后的凹多边形；
步骤3：确定A星算法搜索方向数目Nd和搜索步长Ds：
A星搜索方向数目Nd为4-360，搜索方向数目越多，搜索步长越短，航线越优化但搜索时间越长，本发明取8个方向，每个方向间隔45度，搜索步长DS取最小转弯半径Rmin，能够以一个较快的速度收敛到期望结果；
步骤4：确定重复点的判断依据：
历史搜索节点nodei包括自身位置，父节点指针，花费代价，历史代价，估计代价。
新加入节点noden包括自身位置，父节点指针，花费代价，历史代价，估计代价。
依次通过历史搜索过的节点nodei和新节点noden的自身位置，计算节点nodei和新节点noden的之间距离D(i,n)，当距离D(i,n)小于步长Ds的一半，即D(i,n)<Ds/2，则认为新节点noden和历史节点nodei为重复点，将不再作为新节点参与计算；
步骤5：确定禁飞区相交依据：新点noden在新禁飞区SfL内，或者新点noden与新点的父节点nodep的连线与新禁飞区SfL存在交点，两个条件满足其一，则认为与禁飞区相交；
步骤6：确定安全区相交依据：新点noden在新安全区SsL外，或者新点noden与新点的父节点nodep的连线和新安全区SsL存在交点；
步骤7：确定有效搜索点判断依据：根据步骤4判断不是重复点，或根据步骤5或步骤6判断不和禁飞区或者安全区相交；当两个条件都满足，认为当前搜索点为有效点；
步骤8：确定A星算法花费代价F(i)：
花费代价F(i)等于从起始点到当前有效搜索点的位置所历史代价G(i)与该位置到目标位置的估计代价H(i)之和，即：
F(i)＝G(i)+H(i)；
步骤9：父节点nodep生成：
开始指定起始点为第一个父节点nodep，通过父节点nodep的位置(xp,yp)和当前节点需搜索方向角度Dn，计算新节点noden位置(xn,yn)，通过步骤7的有效搜索点的判断依据，判断新节点noden是否为有效搜索节点，如果不是有效搜索节点，则改变搜索方向角度，并重新计算；否则是有效搜索节点，将有效搜索新节点noden加入有效数队列openlist中，并根据步骤8计算每个有效搜索点noden的花费代价F(n)值，查找openlist中所有搜索点，将F(n)最小的节点返回，当做新的父节点nodep，并从openlist中移除F(n)最小的节点，将新的父节点nodep加入已搜索队列closelist中；
新节点noden位置(xn,yn)的计算公式为：
Xn＝Xp+sin(Dn)*Ds
Yn＝Yp+cos(Dn)*Ds
步骤10：新节点noden生成：
根据父节点nodep位置(xp,yp)、当前节点的搜索方向角度Dn和搜索步长DS，计算出新节点noden的位置(xn,yn)，将新节点noden的父节点指针设置为nodep，方便后期逆向查找；根据步骤8分别计算新节点noden的花费代价F(n)，历史代价G(n)，估计代价H(n)。
其中，Dn＝D0+360/Nd*Ni
Dn为当前节点的搜索方向角度，即本次新节点和父节点的连线和正北方向的夹角为搜索方向角度；
D0为本次目标点相对父节点的初始角度。
Ni为当前方向数目；
G(n)＝G(p)+C(n)
G(n)为新节点历史代价，取从起始点到新节点总距离；
G(p)为父节点历史代价，取从起始点到父节点总距离；
C(n)从父节点到新节点的花费代价，取父节点到新节点的距离；
F(n)＝G(n)+H(n)
H(n)为新节点估计代价，取从新节点直接到目标点的距离。
步骤11：确定搜索结束依据：
当目标点变成为有效搜索节点时，则规划成功，进入步骤12；如果目标点不是有效搜索节点时，进入步骤9，重新搜寻父节点，并改变新父节点，添加新目标点；
当有效队列openlist中所有有效节点都被移除，搜寻父节点失败，则规划失败，结束本次航线段规划，并返回空航路；
步骤12：规划航线初始生成：
当规划成功后，对终止时父节点nodep反向寻找生成该节点的父节点，根据当前节点存储的父节点的指针寻找父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点从起始位置开始依次连接起来，构成的节点集合为生成的初始规划航路；
步骤13：A星重构优化航路：按照初始规划航线的节点集合，作为A星搜索的新点集；
步骤14：花费代价重新计算F(i)，父节点重新指定：
按照顺序取节点nodep，当作父节点，依次取出节点nodep后面的节点nodei，根据步骤7判断当前节点nodei的有效性，当nodei为无效节点时，跳过本次节点，取出下个节点nodei进行判断，当nodei为有效节点时，重新计算父节点nodep直接扩展到nodei的历史代价G(pi)，如果代价G(pi)小于nodei中原始代价G(i)，则改变节点nodei中G(i)＝G(pi)，并更新节点nodei中父节点指针为nodep，代价G(pi)大于等于nodei中原始代价G(i)，则nodei中任何值不改变；
G(pi)＝G(p)+C(pi)
G(pi)为新节点历史代价，取从起始点到新节点总距离。
G(p)为父节点历史代价，取从起始点到父节点总距离。
C(pi)从父节点到新节点的花费代价，取父节点到新节点的距离。
步骤15：航路规划完成：通过对终止时父节点nodep反向寻找生成该节点的父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点，从起始位置开始依次连接，连接起来构成的位置集合为生成的最短规划航路。

Claims (2)

1.一种双重改进A星最短航路规划方法，其特征在于包括下述步骤：
步骤1：计算最小安全距离：将飞机转弯半径和飞机大小看成质点，所需要的最小安全距离Ls为最大速度的最小转弯半径Rmin与飞机翼展LA之和，即：
Ls＝Rmin+LA
步骤2：安全区和禁飞区重生成：为了将飞机和飞机运动特性看成质点，将对禁飞区区域Sf按照最小安全距离LS扩大得到新禁飞区SfL，安全区区域Ss按照最小安全距离LS缩小得到新安全区SsL；
所述扩大的方法分为两种情况：
1)当禁飞区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi)，求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li加上最小安全距离Ls作为新距离Lis，Lis＝Li+Ls，，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成了区域扩大；
Figure FDA0002542899470000011
n为凸多边形总个数；
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当禁飞区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形扩大的步骤完成每个凸多边形的区域扩大，多个被扩大后的凸多边形的构成区域的并集，形成扩大后的凹多边形；
所述缩小的方法分为两种情况：
1)当安全区区域为凸多边形，将每个顶点转换为直角坐标(Pxi,Pyi),求凸多边形的中心点(Px,Py)，计算凸多边形的每个顶点i到中心点的距离Li和凸多边形的每个顶点和中心点的连线与正北方向的夹角形成的角度Ai，在距离Li减去最小安全距离Ls作为新距离Lis，Lis＝Li-Ls，通过新距离Lis和初始角度Ai，计算每个新顶点位置(Pnxi,Pnyi)，将每个新顶点位置(Pnxi,Pnyi)转换为地理坐标并依次连接，即可完成区域缩小；
Figure FDA0002542899470000021
n为凸多边形总个数
Pnxi＝px+sin(Ai)*Lis
Pnyi＝py+cos(Ai)*Lis
2)当安全区区域为凹多边形，先将凹多边形拆分成多个凸多边形，再按照凸多边形缩小的步骤完成每个凸多边形的区域缩小，多个被缩小后的凸多边形的构成区域的并集，形成缩小后的凹多边形；
步骤3：确定A星算法搜索方向数目Nd和搜索步长Ds：
A星搜索方向数目Nd为4-360，每个方向间隔45度，搜索步长DS取最小转弯半径Rmin；
步骤4：确定重复点的判断依据：
历史搜索节点nodei包括自身位置，父节点指针，花费代价，历史代价，估计代价；
新加入节点noden包括自身位置，父节点指针，花费代价，历史代价，估计代价；
依次通过历史搜索过的节点nodei和新节点noden的自身位置，计算节点nodei和新节点noden的之间距离D(i,n)，当距离D(i,n)小于步长Ds的一半，即D(i,n)<Ds/2，则认为新节点noden和历史节点nodei为重复点，将不再作为新节点参与计算；
步骤5：确定禁飞区相交依据：新点noden在新禁飞区SfL内，或者新点noden与新点的父节点nodep的连线与新禁飞区SfL存在交点，两个条件满足其一，则认为与禁飞区相交；
步骤6：确定安全区相交依据：新点noden在新安全区SsL外，或者新点noden与新点的父节点nodep的连线和新安全区SsL存在交点；
步骤7：确定有效搜索点判断依据：根据步骤4判断不是重复点，或根据步骤5或步骤6判断不和禁飞区或者安全区相交；当两个条件都满足，认为当前搜索点为有效点；
步骤8：确定A星算法花费代价F(i)：
花费代价F(i)等于从起始点到当前有效搜索点的位置所历史代价G(i)与该位置到目标位置的估计代价H(i)之和，即：
F(i)＝G(i)+H(i)；
步骤9：父节点nodep生成：
开始指定起始点为第一个父节点nodep，通过父节点nodep的位置(xp,yp)和当前节点需搜索方向角度Dn，计算新节点noden位置(xn,yn)，通过步骤7的有效搜索点的判断依据，判断新节点noden是否为有效搜索节点，如果不是有效搜索节点，则改变搜索方向角度，并重新计算；否则是有效搜索节点，将有效搜索新节点noden加入有效数队列openlist中，并根据步骤8计算每个有效搜索点noden的花费代价F(n)值，查找openlist中所有搜索点，将F(n)最小的节点返回，当做新的父节点nodep，并从openlist中移除F(n)最小的节点，将新的父节点nodep加入已搜索队列closelist中；
新节点noden位置(xn,yn)的计算公式为：
Xn＝Xp+sin(Dn)*Ds
Yn＝Yp+cos(Dn)*Ds
步骤10：新节点noden生成：
根据父节点nodep位置(xp,yp)、当前节点的搜索方向角度Dn和搜索步长DS，计算出新节点noden的位置(xn,yn)，将新节点noden的父节点指针设置为nodep，方便后期逆向查找；根据步骤8分别计算新节点noden的花费代价F(n)，历史代价G(n)，估计代价H(n)；
其中，Dn＝D0+360/Nd*Ni；
Dn为当前节点的搜索方向角度，即本次新节点和父节点的连线和正北方向的夹角为搜索方向角度；
D0为本次目标点相对父节点的初始角度；
Ni为当前方向数目；
G(n)＝G(p)+C(n)
G(n)为新节点历史代价，取从起始点到新节点总距离；
G(p)为父节点历史代价，取从起始点到父节点总距离；
C(n)从父节点到新节点的花费代价，取父节点到新节点的距离；
F(n)＝G(n)+H(n)
H(n)为新节点估计代价，取从新节点直接到目标点的距离；
步骤11：确定搜索结束依据：
当目标点变成为有效搜索节点时，则规划成功，进入步骤12；如果目标点不是有效搜索节点时，进入步骤9，重新搜寻父节点，并改变新父节点，添加新目标点；
当有效队列openlist中所有有效节点都被移除，搜寻父节点失败，则规划失败，结束本次航线段规划，并返回空航路；
步骤12：规划航线初始生成：
当规划成功后，对终止时父节点nodep反向寻找生成该节点的父节点，根据当前节点存储的父节点的指针寻找父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点从起始位置开始依次连接起来，构成的节点集合为生成的初始规划航路；
步骤13：A星重构优化航路：按照初始规划航线的节点集合，作为A星搜索的新点集；
步骤14：花费代价重新计算F(i)，父节点重新指定：
按照顺序取节点nodep，当作父节点，依次取出节点nodep后面的节点nodei，根据步骤7判断当前节点nodei的有效性，当nodei为无效节点时，跳过本次节点，取出下个节点nodei进行判断，当nodei为有效节点时，重新计算父节点nodep直接扩展到nodei的历史代价G(pi)，如果代价G(pi)小于nodei中原始代价G(i)，则改变节点nodei中G(i)＝G(pi)，并更新节点nodei中父节点指针为nodep，代价G(pi)大于等于nodei中原始代价G(i)，则nodei中任何值不改变；
G(pi)＝G(p)+C(pi)
G(pi)为新节点历史代价，取从起始点到新节点总距离；
G(p)为父节点历史代价，取从起始点到父节点总距离；
C(pi)从父节点到新节点的花费代价，取父节点到新节点的距离；
步骤15：航路规划完成：通过对终止时父节点nodep反向寻找生成该节点的父节点，一直寻找直到父节点为起始节点时，将寻找出来的父节点，从起始位置开始依次连接，连接起来构成的位置集合为生成的最短规划航路。
2.根据权利要求1所述的一种双重改进A星最短航路规划方法，其特征在于：
所述步骤3中，A星搜索方向取8个方向。
CN202010552031.3A 2020-06-17 2020-06-17 双重改进a星最短航路规划方法 Active CN111561933B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010552031.3A CN111561933B (zh) 2020-06-17 2020-06-17 双重改进a星最短航路规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010552031.3A CN111561933B (zh) 2020-06-17 2020-06-17 双重改进a星最短航路规划方法

Publications (2)

Publication Number Publication Date
CN111561933A true CN111561933A (zh) 2020-08-21
CN111561933B CN111561933B (zh) 2023-03-31

Family

ID=72070061

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010552031.3A Active CN111561933B (zh) 2020-06-17 2020-06-17 双重改进a星最短航路规划方法

Country Status (1)

Country Link
CN (1) CN111561933B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113257045A (zh) * 2021-07-14 2021-08-13 四川腾盾科技有限公司 一种基于大型固定翼无人机电子围栏的无人机控制方法
CN113721653A (zh) * 2021-08-09 2021-11-30 陕西工业职业技术学院 一种飞行器航迹实时规划系统
CN115930973A (zh) * 2023-02-08 2023-04-07 中国民航大学 一种无人机航路规划方法及装置
CN116386389A (zh) * 2023-03-21 2023-07-04 中国南方航空股份有限公司 带限制的民航航路规划方法
CN117572894A (zh) * 2024-01-16 2024-02-20 中国人民解放军陆军航空兵学院 一种无人机指定区域内按时到达航线规划方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121517A1 (en) * 2008-11-10 2010-05-13 Electronics And Telecommunications Research Institute Method and apparatus for generating safe path of mobile robot
CN104075717A (zh) * 2014-01-21 2014-10-01 武汉吉嘉伟业科技发展有限公司 一种基于改进a*算法的无人机航线规划算法
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
US20170358212A1 (en) * 2016-06-10 2017-12-14 ETAK Systems, LLC Anti-drone flight protection systems and methods
CN109298720A (zh) * 2018-09-30 2019-02-01 鲁东大学 一种植保无人机航线规划方法
CN110850891A (zh) * 2019-11-11 2020-02-28 西北工业大学 一种基于空间和时间协同的多无人机动态航路规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100121517A1 (en) * 2008-11-10 2010-05-13 Electronics And Telecommunications Research Institute Method and apparatus for generating safe path of mobile robot
CN104075717A (zh) * 2014-01-21 2014-10-01 武汉吉嘉伟业科技发展有限公司 一种基于改进a*算法的无人机航线规划算法
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
US20170358212A1 (en) * 2016-06-10 2017-12-14 ETAK Systems, LLC Anti-drone flight protection systems and methods
CN109298720A (zh) * 2018-09-30 2019-02-01 鲁东大学 一种植保无人机航线规划方法
CN110850891A (zh) * 2019-11-11 2020-02-28 西北工业大学 一种基于空间和时间协同的多无人机动态航路规划方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
CHO, J等: "On the area of the polygon determined by the short diagonals of a convex polygon", 《ACTA MATHEMATICA HUNGARICA》 *
MATTEI M 等: "Smooth flight trajectory planning in the presence of no-fly zones and obstacles", 《JOURNAL OF GUIDANCE, CONTROL, AND DYNAMICS》 *
池彩虹: "多机协同航路规划", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 *
黄雄 等: "基于A*算法的高超声速飞行器航迹规划方法", 《计算机仿真》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113257045A (zh) * 2021-07-14 2021-08-13 四川腾盾科技有限公司 一种基于大型固定翼无人机电子围栏的无人机控制方法
CN113721653A (zh) * 2021-08-09 2021-11-30 陕西工业职业技术学院 一种飞行器航迹实时规划系统
CN113721653B (zh) * 2021-08-09 2024-01-19 陕西工业职业技术学院 一种飞行器航迹实时规划系统
CN115930973A (zh) * 2023-02-08 2023-04-07 中国民航大学 一种无人机航路规划方法及装置
CN116386389A (zh) * 2023-03-21 2023-07-04 中国南方航空股份有限公司 带限制的民航航路规划方法
CN116386389B (zh) * 2023-03-21 2023-12-29 中国南方航空股份有限公司 带限制的民航航路规划方法
CN117572894A (zh) * 2024-01-16 2024-02-20 中国人民解放军陆军航空兵学院 一种无人机指定区域内按时到达航线规划方法
CN117572894B (zh) * 2024-01-16 2024-03-22 中国人民解放军陆军航空兵学院 一种无人机指定区域内按时到达航线规划方法

Also Published As

Publication number Publication date
CN111561933B (zh) 2023-03-31

Similar Documents

Publication Publication Date Title
CN111561933B (zh) 双重改进a星最短航路规划方法
CN110320933B (zh) 一种巡航任务下无人机避障运动规划方法
US11262764B2 (en) Computer-implemented method and a system for defining a path for a vehicle within an environment with obstacles
Szczerba et al. Robust algorithm for real-time route planning
CN106125764B (zh) 基于a*搜索的无人机路径动态规划方法
US6259988B1 (en) Real-time mission adaptable route planner
WO2019042295A1 (zh) 一种无人驾驶路径规划方法、系统和装置
CN106774425B (zh) 一种无人机飞行导航的方法及系统
CN109685237B (zh) 一种基于Dubins路径和分支限界的无人机航迹实时规划方法
CN107741232B (zh) 一种测量船的航路规划方法及装置
Ablavsky et al. Optimal search for a moving target-A geometric approach
CN112947594A (zh) 一种面向无人机的航迹规划方法
Jenie et al. Three-dimensional velocity obstacle method for UAV deconflicting maneuvers
CN113759936B (zh) 一种适用于动态目标跟踪的比例导引法与人工势场法相结合的路径规划方法
Weiss et al. Global real-time path planning for uavs in uncertain environment
Li et al. A 3D path planning approach for quadrotor UAV navigation
D’Amato et al. Smooth path planning for fixed-wing aircraft in 3d environment using a layered essential visibility graph
Si et al. TOM-odometry: A generalized localization framework based on topological map and odometry
CN113252039B (zh) 面向地形辅助导航的粒子群快速匹配方法
Zollars et al. Simplex Solutions for Optimal Control Flight Paths in Urban Environments
Zollars et al. Simplex optimal control methods for urban environment path planning
CN114444267A (zh) 一种飞行计划航段水平航迹预测方法
Kulbiej Autonomous Vessels' Pathfinding Using Visibility Graph
Zollars et al. Simplex Methods for Optimal Control of Unmanned Aircraft Flight Trajectories
Cuciniello et al. Real time optimal path generation with mission and vehicle maneuvering constraints

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
CB02 Change of applicant information

Address after: No.34, Fenghui South Road, Xi'an, Shaanxi 710065

Applicant after: Xi'an Aisheng Technology Group Co.,Ltd.

Address before: No.34, Fenghui South Road, Xi'an, Shaanxi 710065

Applicant before: XI'AN AISHENG TECHNOLOGY GROUP Co.,Ltd.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant