CN114115362B - 一种基于双向apf-rrt*算法的无人机航迹规划方法 - Google Patents
一种基于双向apf-rrt*算法的无人机航迹规划方法 Download PDFInfo
- Publication number
- CN114115362B CN114115362B CN202111448012.7A CN202111448012A CN114115362B CN 114115362 B CN114115362 B CN 114115362B CN 202111448012 A CN202111448012 A CN 202111448012A CN 114115362 B CN114115362 B CN 114115362B
- Authority
- CN
- China
- Prior art keywords
- nearest
- new
- node
- unmanned aerial
- aerial vehicle
- 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
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 74
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000002457 bidirectional effect Effects 0.000 title claims abstract description 28
- 238000005070 sampling Methods 0.000 claims abstract description 10
- 238000001514 detection method Methods 0.000 claims description 12
- 239000013598 vector Substances 0.000 claims description 9
- 101150071716 PCSK1 gene Proteins 0.000 claims description 7
- 238000013459 approach Methods 0.000 claims description 5
- 230000004888 barrier function Effects 0.000 claims description 2
- 108091028140 FREP Proteins 0.000 claims 1
- 230000000694 effects Effects 0.000 abstract description 4
- 230000004927 fusion Effects 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 38
- 238000004088 simulation Methods 0.000 description 25
- 230000007613 environmental effect Effects 0.000 description 8
- 238000005457 optimization Methods 0.000 description 6
- 238000011160 research Methods 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 239000003795 chemical substances by application Substances 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000006378 damage Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/12—Target-seeking control
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
本发明公开了一种基于双向APF‑RRT*算法的无人机航迹规划方法，涉及智能体控制与决策技术领域，在RRT*算法中加入改进的APF，航迹规划效果好；在改进的APF的基础上，提出在双向RRT*算法中加入改进的人工势场来规划无人机的飞行航迹，算法选择质量好的节点加入到生长树中；改进的航迹规划算法解决了RRT*算法RRT*的采样不均匀，冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率；提出的融合算法优化了路径长度，提高了运算速度；该方法便于实现，具有良好的可操作性。
Description
技术领域
本发明涉及智能体控制与决策技术领域，尤其涉及一种基于双向APF-RRT*算法的无人机航迹规划方法。
背景技术
随着无人机技术的逐渐成熟，无人机以其高机动性、灵活方便和易于控制等优点在农业、军事和工业研究领域得到越来越广泛的应用；航迹规划在无人机控制系统中具有重要作用，其技术的发展受到了来自各行各业的重视；然而，无人机飞行任务日益复杂，飞行环境也在不断变化，寻找满足飞行条件的最优飞行路径面临着新的挑战；无人机在执行复杂任务过程中，航迹规划的算法不能及时有效的计算出一条渐近最优航迹，将会和障碍物碰撞摧毁；为了提高航迹规划算法的搜索效率，使无人机飞行的航迹满足实时要求，通常要求无人机能够快速的规划出一条渐近最优航迹，这就要求无人机航迹规划的算法更加高效，所以，无人机航迹规划的研究就显得的格外重要；无人机航迹规划的算法不但要快速规划从起始点到目标点的无人机航迹，而且应该保证航迹代价尽可能小；因此，在复杂环境下的无人机航迹规划就更加困难；航迹规划可以分为在已知环境中的静态航迹规划也叫全局规划，以及在部分未知情况下的动态航迹规划也可以叫做局部规划；本章主要研究无人机飞行的静态航迹规划；近年来，在无人机航迹规划方面取得了一些研究进展；例如，RRT*Smart算法，Q-RRT*和Informed-RRT*算法等；然而，RRT*Smart算法的解主要依赖于初始解的质量，违背了RRT*的均匀采样策略；而且Q-RRT*和Informed-RRT*算法存在着搜索时间长，路径代价高的问题。
发明内容
为解决现有技术的不足之处，本发明提出一种基于双向APF-RRT*算法的无人机航迹规划方法，将改进的APF(人工势场法)函数与双向RRT*算法(Rapidly-exploring RandomTrees，快速搜索随机树)相结合，采用改进的人工势场函数引导双向随机搜索树的生长，实现了无人机的航迹规划，解决无人机在执行空战任务中的航迹规划问题；
为解决上述技术问题，本发明所采取的技术方案是：
本发明提出了一种基于双向APF-RRT*算法的无人机航迹规划方法，分为以下步骤：
步骤1：规划无人机的飞行环境X，包括可飞行区域Xsearch，障碍区域Xobs；设置无人机航迹规划的起点qstart、终点qgoal、步长L；
步骤2：在飞行环境X中，分别以qstart和qgoal为根节点，同时创建两棵随机搜索树T1和T2，此时，T1和T2两棵树中均各自只有1个节点qstart和qgoal，为每棵树中的节点创建基于位置的索引，父节点，父节点索引；
步骤3：在可飞行区域Xsearch内，随机搜索树T1生成一个随机采样点qrand；
步骤4：选取T1树中距离qrand最近的节点qnearest，初始的节点qnearest为qstart；
步骤5：采用改进的人工势场函数使qrand和qgoal分别对qnearest产生潜在的引力，使障碍物对qnearest生成一个潜在的斥力，qnearest按照给定的步长L沿着三个力的合力方向生成新的节点qnew；
人工势场(APF)包括引力场函数Uatt(p)如式1所示、斥力场函数Ureq(p)如式2所示，则合力势场函数如式3所示：
Utotal＝∑Urep+∑Uatt (3)
其中，p是无人机的一个航迹点，ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；
引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(4)和(5)表示，合力如式(6)所示：
Fatt(p)＝kaρg(p) (4)
Ftotal＝∑Frep+∑Fatt (6)
当目标点附近存在障碍物时，无人机受到障碍物的斥力增加，且引力减少，无人机难以到达目标点，通过改进的引力势场函数、斥力场函数使无人机顺利到达目标点；
改进的引力势场函数如式7所示、改进的斥力场函数如式8所示：
其中，pner、pgoal、pobs分别代表qnearest，qgoal和障碍物的位置；n是一个正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，ρg(pner,pgoal)是qnearest和qgoal之间的距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，避免斥力大于引力；
T1搜索树采用生成随机点的函数，在无人机可飞行区域内随机生成一个采样点qrand，并且找到搜索树中距离qrand最近的节点qnearest，结合改进的人工势场函数，在qrand节点上生成对qnearest的潜在吸引力Fatt1，在终点对qnearest产生引力Fatt2如式(9)所示，障碍物对qnearest产生斥力Frep是不同障碍物对qnearest的斥力，根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
和/>是两个单位向量，分别为qnearest和qgoal的方向矢量，以及qnearest和障碍物之间的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变得很小，可以保证无人机能够到达目标点；在求解qnew时，需要分别计算qrand和qgoal对qnearest的引力，计算障碍物对qnearest的斥力，然后将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示；假设qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长可以确定为：
其中，L是无人机的扩展步长，k是比例系数，可得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
步骤6：检测qnearest和qnew之间是否与障碍物碰撞；
以qnearest作为检测起点，以qnew作为检测终点，把qnearest和qnew之间的距离平均分成j段；由式14每次生成检测节点qcollision的位置，计算qcollision到距离qcollision最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则qnearest和qnew之间存在障碍物即与障碍物碰撞，重复步骤3-步骤5，重新寻找一个新节点qnew；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点qnew；
k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为qnearest到qnew的方向与x轴的夹角，xqnearest和yqnearest分别代表qnearest的横坐标和纵坐标，xcollision和ycollision分别代表求得的qcollision横坐标和纵坐标；
步骤7：将步骤5中改进的APF(人工势场函数)与双向RRT*算法相结合；
T1生成新节点后，计算qnew与另一棵随机搜索树T2的新节点q'new欧式距离D(qnew,q'new)，检测D是否小于规定的阈值，即检测两棵随机搜索树是否连接，若D不小于规定的阈值则对步骤6输出节点qnew在以qnew为圆心，在半径为r1的一个圆域内找一个新的父节点替换qnearest，并把新的父节点记作qnear，使得qnew到起点的路径代价减小，寻找新的父节点后，同样在这个圆域内寻找到一个以新节点qnew作为父节点的子节点，使这个子节点到起点的路径代价减小；寻找到子节点后，qnew被加入到第一棵随机搜索树T1中，第二棵随机搜索树T2以同样的方式搜索：执行步骤3-步骤6，进行生长；若D小于规定的阈值则两棵随机搜索树相连接；
步骤8：两棵随机搜索树T1和T2连接后，得到一条由多个点构成的一条航迹；
步骤9：利用三次样条插值即把所有的航迹点分为n1个区间，每两个相邻的点构成一个区间，每个区间用三次多项式表示，即每两个相邻点之间用一条平滑的曲线的连接，所有区间的曲线相连接，获得光滑的航迹，解决无人机飞行中转弯角度过大的问题。
有益技术效果
本发明提出一种基于双向APF-RRT*算法的无人机航迹规划方法，具有以下有益技术效果：
1、在RRT*算法中加入改进的APF，航迹规划效果较好；
2、在改进的APF的基础上，提出在双向RRT*算法中加入改进的人工势场来规划无人机的飞行航迹，算法选择质量较好的节点加入到生长树中；改进的航迹规划算法解决了RRT*算法RRT*的采样不均匀，冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率；提出的融合算法优化了路径长度，提高了运算速度；该方法便于实现，具有良好的可操作性。
附图说明
图1为本发明实施例提供的一种基于双向APF-RRT*算法的无人机航迹规划方法流程图；
图2为本发明实施例提供的一种基于双向APF-RRT*算法的无人机航迹规划方法地图；
其中，(a)表示简单环境图，(b)表示较复杂环境图，(c)表示复杂环境图；
图3为本发明实施例提供的随机搜索树T1中q_nearest受力示意图；
图4为本发明实施例提供的随机搜索树T1的重新寻找父节点的示意图；
其中，(a)表示随机搜索树T1重新寻找父节点过程示意图，(b)表示随机搜索树T1重新寻找父节点后与新的父节点连接的示意图；
图5为本发明实施例提供的随机搜索树T1的更新子节点的示意图；
其中，(a)表示随机搜索树T1更新子节点过程示意图，(b)表示随机搜索树T1重更新子节点后与子节点连接的示意图；
图6为本发明实施例提供的随机搜索树T1和T2生长过程中qnearest受力示意图；
图7为本发明实施例提供的一种基于双向APF-RRT*算法的无人机航迹规划方法示意图；
其中，(a)表示本发明在简单环境下查找航迹示意图，(b)表示本发明在较复杂环境下查找航迹示意图，(c)表示本发明在复杂环境下查找航迹示意图；
图8为本发明实施例提供的简单环境仿真对比图；
其中，(a)表示简单环境仿真对比中RRT*示意图，(b)表示简单环境仿真对比中Informed-RRT*示意图，(c)表示简单环境仿真对比中Bi-RRT*示意图，(d)表示简单环境仿真对比中改进APF-RRT*示意图，(e)表示简单环境仿真对比中改进双向APF-RRT*示意图；
图9为本发明实施例提供的简单环境仿真数据图；
其中，(a)表示每种算法在简单环境中迭代次数平均值示意图，(b)表示每种算法在简单环境中路径长度平均值示意图，(c)表示每种算在简单环境中法运行时间平均值示意图；
图10为本发明实施例提供的较复杂环境仿真对比图；
其中，(a)表示较复杂环境仿真对比中RRT*示意图，(b)表示较复杂环境仿真对比中Informed-RRT*示意图，(c)表示较复杂环境仿真对比中Bi-RRT*示意图，(d)表示较复杂环境仿真对比中改进APF-RRT*示意图，(e)表示较复杂环境仿真对比中改进双向APF-RRT*示意图；
图11为本发明实施例提供的较复杂环境仿真数据图；
其中，(a)表示每种算法在较复杂环境中迭代次数平均值示意图，(b)表示每种算法在较复杂环境中路径长度平均值示意图，(c)表示每种算法在较复杂环境中运行时间平均值示意图；
图12为本发明实施例提供的复杂环境仿真对比图；
其中，(a)表示复杂环境仿真对比中RRT*示意图，(b)表示复杂环境仿真对比中Informed-RRT*示意图，(c)表示复杂环境仿真对比中Bi-RRT*示意图，(d)表示复杂环境仿真对比中改进APF-RRT*示意图，(e)表示复杂环境仿真对比中改进双向APF-RRT*示意图；
图13为本发明实施例提供的复杂环境仿真数据图；
其中，(a)表示每种算法在复杂环境中迭代次数平均值示意图，(b)表示每种算法在复杂环境中路径长度平均值示意图，(c)表示每种算法在复杂环境中运行时间平均值示意图；
图14为本发明实施例提供的三次样条插值优化后的路径图；
其中，(a)表示改进双向APF-RRT*在简单环境中航迹优化图，(b)表示改进双向APF-RRT*在较复杂环境中航迹优化图，(c)表示改进双向APF-RRT*在复杂环境中航迹优化图。
具体实施方式
下面结合附图和具体实施例对本发明内容做进一步说明；
本发明提出一种基于双向APF-RRT*算法的无人机航迹规划方法，将改进的人工势场函数与双向RRT*算法相结合，采用改进的人工势场函数引导双向随机搜索树的生长，实现了无人机的航迹规划，解决无人机在执行空战任务中的航迹规划问题，如图1所示，分为以下步骤：
步骤1：对无人机航迹规划环境初始化；
无人机航迹规划地图设置为100km*100km的二维空间；无人机飞行航迹的起始坐标设为(1,1)，终点设为(90,90)如图2一种基于双向APF-RRT*算法的无人机航迹规划方法地图所示；图中圆圈表示障碍物，无人机无法通过该区域；
步骤2：采用改进人工势场函数生成新节点；
人工势场(APF)包括引力场函数Uatt(p)如式1所示、斥力场函数Ureq(p)如式2所示；假设p是无人机的一个航迹点，则合力势场函数如式3所示：
Utotal＝∑Urep+∑Uatt (3)
在其中ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；引力和斥力的大小分别是引力场函数和斥力场函数的负梯度；如式(4)和(5)表示，合力如式(6)所示：
Fatt(p)＝kaρg(p) (4)
Ftotal＝∑Frep+∑Fatt (6)
然而，当目标点附近存在障碍物时，无人机受到障碍物的斥力将非常大，而且引力相对较小，无人机很难到达目标点，所以本发明提出改进的引力势场函数如式7所示、斥力场函数如式8所示：
其中，pner、qgoal、pobs分别代表qnearest，qgoal和障碍物的位置；n是一个正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，ρg(pner,pgoal)是qnearest和qgoal之间的距离；当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，这样避免了斥力大于引力的现象；
流程中i为迭代次数，Maxiter为最大迭代次数，本发明程序达到最大迭代次数，程序将会停止；反之，随机搜索树进行生长；新节点的生成方式如图3所示，首先，T1搜索树采用rand函数，在无人机可飞行区域内随机生成一个采样点qrand，并且找到搜索树中距离qrand最近的节点qnearest；结合改进的人工势场函数，在qrand节点上生成对qnearest的潜在吸引力Fatt1，在终点对qnearest产生引力Fatt2如式(9)所示，障碍物对qnearest产生斥力Frep是不同障碍物对qnearest的斥力，仅给定一个障碍物；根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
和/>是两个单位向量，分别为qnearest和qgoal的方向矢量，以及qnearest和障碍物之间的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变得很小，可以保证无人机能够到达目标点；在求解qnew时，首先，需要分别计算qrand和qgoal对qnearest的引力，然后计算障碍物对qnearest的斥力，然后将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示；假设qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长可以确定为：
L是无人机的扩展步长，k是比例系数，于是，可得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
步骤3：检测是否碰撞；
检测qnearest和qnew之间是否与障碍物碰撞，若qnearest和qnew之间存在障碍物即与障碍物碰撞，则重复步骤2，重新寻找一个新节点qnew；反之则进行重新寻找父节点的过程；
步骤4：对新节点重新寻找父节点；
图4为随机搜索树T1重新寻找父节点的示意图；在图4(a)中，节点标号表示产生该节点的顺序，0节点是起点，9节点是新产生的节点qnew，6节点是产生9节点的qnearest，节点与节点之间连接的边上数字代表两个节点之间的欧氏距离；在RRT算法寻找到新节点qnew后，重新开始选择父节点；首先以新节点qnew为圆心，依据设定的半径画一个圆，将这个圆内的所有生长树上的节点作为新的父节点的备选节点(不包括当前的父节点)；分别计算圆内的备选节点的原路径代价和备选节点到新节点qnew的欧氏距离之和；例如，qnew(9节点)经过节点qnearest(6节点)并连接至qstart(0节点)的路径为9-6-4-0，其代价为16；qnew经过节点5并连接至qstart的路径为9-5-1-0，其代价为11，选取其路径总代价最小的所对应的临近节点(5节点)作为qnew的新父节点，如图4(b)中所示；
步骤5：新的父节点和新节点是否存在障碍物；
检测新节点的父节点和新节点之间是否存在障碍物，若存在，则重复步骤4，对新节点重新寻找父节点，反之则完成一次迭代，并把qnew标号添加到随机生长树中；
步骤6：重新布线；
图5为随机搜索树T1的更新子节点的示意图；在完成重新寻找父节点过程之后，再进行更新子节点：首先计算集合圆中每一个节点以qnew(9节点)为父节点(qnearest除外)，并连接至qstart(0节点)的总代价，若以qnew为父节点的总代价小于不以qnew为父节点的总代价，则修改该节点的父节点为qnew，如图5(a)所示，在qnew的所有节点中，当节点6以qnew为父节点时，连接至qstart的路径为6-9-5-1-0，路径总代价为12，而节点6不以qnew为父节点时，例如连接至qstart的路径为6-4-0，路径总代价为15，故修剪随机树，将节点6的父节点改为qnew，如图5(b)所示；
步骤7：新节点是否与另一棵树连接；
T1树对qnew重新布线后，检测两棵树是否连接，若连接则程序停止；反之则T2树重复步骤1-6进行生长；随机搜索树T1和T2生长过程中qnearest受力示意图如图6所示；每棵随机生长树的终点和随机采样点均对qnearest产生引力，障碍物对qnearest生成斥力；根据平行四边形法则确定合力的方向，每棵生长树的新节点按照规定的步长沿着合力的方向生长；Fatt1和Fatt2分别为随机采样点和目标点对qnearest的引力，Frep为障碍物对qnearest的斥力；
两棵随机树生长在地图中生长过程如图7(a)(b)(c)所示，随机搜索树节点和节点之间的连接用浅色线表示；生成路径用深色线表示；两棵随机搜索树均加入改进的人工势场函数来引导随机树相向生长；
步骤8：两棵随机搜索树T1和T2连接后，得到一条由多个点构成的一条航迹；
步骤9：利用三次样条插值优化得到的航迹点，从而获得更光滑的航迹，解决无人机飞行中转弯角度过大的问题；
仿真比较：
在简单环境中各算法的仿真结果如图8所示，深色线为算法规划的无人机可行轨迹；图8中，RRT*在整个空间随机采样，航迹的拐点较多，路径较长；Informed-RRT*算法的仿真结果与RRT*算法相比，虽然路径长度看起来更长，但随机树增长的速度更快，花费的时间更少；尽管Informed-RRT*算法提高了搜索效率，但由于缺乏障碍物的排斥力的作用，算法在障碍物周围存在冗余点；Bi-RRT*双向随机搜索树生长较快，但路径不够平滑；改进APF-RRT*算法增加了障碍物的排斥力的作用，提高了算法的搜索效率；而本发明规划的路径更平滑、更短，更符合无人机的飞行要求；
本发明对每种算法在复杂环境下进行了30次测试，以验证算法的稳定性，并记录了算法的运行时间、生成路径长度和迭代次数；简单环境下的实验数据见图9；通过分析图9和表1的数据，与改进APF-RRT*算法相比，本发明生成的航迹平均长度减少了3.62％；与Bi-RRT*算法相比,平均运行时间减少了18.52％,平均迭代次数少7.69％,数据表明本发明可以明显减少随机点和迭代次数,提高算法的收敛速度,同时证明了本发明的稳定性；
表1简单环境中30次实验数据平均值
在较复杂环境中各算法的仿真结果如图10所示，本发明生成的路径代价减小；通过分析图11和表2中的数据，与改进APF-RRT*算法相比，本发明生成的航迹平均长度减少了2.69％；与Bi-RRT*算法相比，平均运行时间减少了49.55％，平均迭代次数减少了4.42％，表明本发明在较复杂的环境中具有更高的搜索效率，算法具有更好的稳定性；
表2较复杂环境中30次实验数据平均值
在障碍物较为复杂环境中，本发明的优势更加明显；虽然单向搜索方法优势很多，双向搜索时间更短，但路径并不好，如图12中Bi-RRT*所示；本发明在复杂环境下通过路径查找更加平滑，并实现了双向搜索；大大减少了搜索时间，减少了大量的冗余点；对图13和表3中的数据进行分析，本发明生成的航迹平均长度比改进APF-RRT*算法减少了2.69％；与Bi-RRT*算法相比，平均运行时间减少47.67％,平均迭代次数减少1.59％，因此本发明可以减少低质量随机点的数量，明显减少迭代次数；
表3复杂环境中30次实验数据平均值
在三种环境的仿真下，如图8、10和图12所示，节点没有进行优化；虽然生成了可行路径，但可行点多，路径长度大，路径曲率不能满足无人机飞行条件，不是最优路径；本发明采用三次样条插值算法对基于改进人工势场函数的双向RRT*算法生成的路径进行优化，优化结果如图14所示；为了解决路径中存在尖锐点和冗余点的问题，我们引入一种平滑的方法来去除冗余点，从而获得光滑的路径；图14中的浅色路径为平滑路径，可以清楚地看到，优化路径的冗余点被减少了；通过减少冗余点，使路径更平滑，更符合无人机的飞行要求；
本发明考虑了完整路径的平滑性、算法计算量大等因素的影响，实现了无人机飞行过程中算法收敛速度快，路径代价低动态航迹规划研究；因此，该方法能更灵活更快速地生成路径代价更小的路径；在以上分析的基础上，可以看出提出的方法可以使无人机在飞行方面更加的符合实际，即本发明设计的方法是有效的。
Claims (3)
1.一种基于双向APF-RRT*算法的无人机航迹规划方法,其特征在于：分为以下步骤：
步骤1：规划无人机的飞行环境X，包括可飞行区域Xsearch，障碍区域Xobs；设置无人机航迹规划的起点qstart、终点qgoal、步长L：
步骤2：在飞行环境X中，分别以qstart和qgoal为根节点，同时创建两棵随机搜索树T1和T2，此时，T1和T2两棵树中均各自只有1个节点qstart和qgoal，为每棵树中的节点创建基于位置的索引，父节点，父节点索引；
步骤3：在可飞行区域Xsearch内，随机搜索树T1生成一个随机采样点qrand；
步骤4：选取T1树中距离qrand最近的节点qnearest；
步骤5：采用改进的人工势场函数使qrand和qgoal分别对qnearest产生潜在的引力，使障碍物对qnearest生成一个潜在的斥力，qnearest按照给定的步长L沿着三个力的合力方向生成新的节点qnew；
步骤6：检测qnearest和qnew之间是否与障碍物碰撞；
步骤7：将步骤5中改进的人工势场函数APF与双向RRT*算法相结合；
步骤8：两棵随机搜索树T1和T2连接后，得到一条由多个点构成的一条航迹；
步骤9：利用三次样条插值把每两个相邻点之间用一条平滑的曲线的连接，所有区间的曲线相连接，获得光滑的航迹，解决无人机飞行中转弯角度过大的问题；
所述改进的人工势场函数APF包括引力场函数Uatt(p)如式1所示、斥力场函数Ureq(p)如式2所示，则合力势场函数如式3所示：
Utotal＝∑Urep+ΣUatt (3)
其中，p是无人机的一个航迹点，ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；
引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(4)和(5)表示，合力如式(6)所示：
Fatt(p)＝kaρg(p) (4)
Ftotal＝∑Frep+∑Fatt (6)
当目标点附近存在障碍物时，无人机受到障碍物的斥力增加，且引力减少，无人机难以到达目标点，通过改进的引力势场函数、斥力场函数使无人机顺利到达目标点；
改进的引力势场函数如式7所示、改进的斥力场函数如式8所示：
其中pner，pgoal，pobs分别代表qnearest，qgoal和障碍物的位置；n是一个正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，ρg(pner,pgoal)是qnearest和qgoal之间的距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，避免斥力大于引力；
T1搜索树采用生成随机点的函数，在无人机可飞行区域内随机生成一个采样点qrand，并且找到搜索树中距离qrand最近的节点qnearest，结合改进的人工势场函数，在qrand节点上生成对qnearest的潜在吸引力Fatt1，在终点对qnearest产生引力Fatt2如式(9)所示，障碍物对qnearest产生斥力Frep是不同障碍物对qnearest的斥力，根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
Frep(p)＝-▽Urep(p)＝Frep1nOR+Frep2nRG (9)
nOR＝▽ρ(p,pobs)，nRG＝-▽ρ(p,pgoal)是两个单位向量，分别为qnearest和qgoal的方向矢量，以及qnearest和障碍物之间的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变得很小，保证无人机能够到达目标点；在求解qnew时，需要分别计算qrand和qgoal对qnearest的引力，计算障碍物对qnearest的斥力，然后将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示，qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长确定为：
其中，L是无人机的扩展步长，k是比例系数，得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
所述检测qnearest和qnew之间是否与障碍物碰撞：
以qnearest作为检测起点，以qnew作为检测终点，把qnearest和qnew之间的距离平均分成j段；由式14每次生成检测节点qcollision的位置，计算qcollision到距离qcollision最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则qnearest和qnew之间存在障碍物即与障碍物碰撞，重复步骤3-步骤5，重新寻找一个新节点qnew；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点qnew；
k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为qnearest到qnew的方向与x轴的夹角，xqnearest和yqnearest分别代表qnearest的横坐标和纵坐标，xcollision和ycollision分别代表求得的qcollision横坐标和纵坐标；
所述步骤7的具体过程为：
步骤7：T1生成新节点后，计算qnew与另一棵随机搜索树T2的新节点q’new欧式距离D(qnew,q’new)，检测D是否小于规定的阈值，判断两棵随机搜索树是否连接，若D不小于规定的阈值则对步骤6输出节点qnew在以qnew为圆心，在半径为r1的一个圆域内找一个新的父节点替换qnearest，并把新的父节点记作qnear，使得qnew到起点的路径代价减小，寻找新的父节点后，同样在这个圆域内寻找到一个以新节点qnew作为父节点的子节点，使这个子节点到起点的路径代价减小；寻找到子节点后，qnew被加入到第一棵随机搜索树T1中，第二棵随机搜索树T2以同样的方式搜索：执行步骤3-步骤6；若D小于规定的阈值则两棵随机搜索树相连接。
2.如权利要求1所述的基于双向APF-RRT*算法的无人机航迹规划方法，其特征在于：所述节点qnearest初始为节点qstart。
3.如权利要求1所述的基于双向APF-RRT*算法的无人机航迹规划方法，其特征在于：所述样条插值是把所有的航迹点分为n1个区间，每两个相邻的点构成一个区间，每个区间用三次多项式表示。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111448012.7A CN114115362B (zh) | 2021-11-30 | 2021-11-30 | 一种基于双向apf-rrt*算法的无人机航迹规划方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111448012.7A CN114115362B (zh) | 2021-11-30 | 2021-11-30 | 一种基于双向apf-rrt*算法的无人机航迹规划方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114115362A CN114115362A (zh) | 2022-03-01 |
| CN114115362B true CN114115362B (zh) | 2023-12-26 |
Family
ID=80369025
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111448012.7A Active CN114115362B (zh) | 2021-11-30 | 2021-11-30 | 一种基于双向apf-rrt*算法的无人机航迹规划方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114115362B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114323028B (zh) * | 2022-03-16 | 2022-06-07 | 中南大学 | 自适应地图的路径规划方法、系统、设备及介质 |
| CN114625170B (zh) * | 2022-03-24 | 2023-05-12 | 中国民用航空飞行学院 | 一种山区火灾直升机救援飞行路径动态规划方法 |
| WO2023197092A1 (zh) * | 2022-04-11 | 2023-10-19 | 电子科技大学 | 一种基于改进rrt算法的无人机路径规划方法 |
| CN114610076B (zh) * | 2022-05-10 | 2022-07-29 | 四川大学 | 结合人工势场法与A-star算法角度约束的航迹规划方法 |
| CN115268456B (zh) * | 2022-08-10 | 2023-10-17 | 哈尔滨理工大学 | 一种动态变策略informed-RRT*的无人车路径规划方法 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109521794A (zh) * | 2018-12-07 | 2019-03-26 | 南京航空航天大学 | 一种多无人机航路规划及动态避障方法 |
| CN110609552A (zh) * | 2019-09-12 | 2019-12-24 | 哈尔滨工程大学 | 一种水下无人航行器编队平面航迹规划方法 |
| CN112684807A (zh) * | 2019-10-18 | 2021-04-20 | 成都凯天电子股份有限公司 | 无人机集群三维编队方法 |
| CN112987799A (zh) * | 2021-04-16 | 2021-06-18 | 电子科技大学 | 一种基于改进rrt算法的无人机路径规划方法 |
-
2021
- 2021-11-30 CN CN202111448012.7A patent/CN114115362B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109521794A (zh) * | 2018-12-07 | 2019-03-26 | 南京航空航天大学 | 一种多无人机航路规划及动态避障方法 |
| CN110609552A (zh) * | 2019-09-12 | 2019-12-24 | 哈尔滨工程大学 | 一种水下无人航行器编队平面航迹规划方法 |
| CN112684807A (zh) * | 2019-10-18 | 2021-04-20 | 成都凯天电子股份有限公司 | 无人机集群三维编队方法 |
| CN112987799A (zh) * | 2021-04-16 | 2021-06-18 | 电子科技大学 | 一种基于改进rrt算法的无人机路径规划方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114115362A (zh) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114115362B (zh) | 一种基于双向apf-rrt*算法的无人机航迹规划方法 | |
| CN109945881B (zh) | 一种蚁群算法的移动机器人路径规划方法 | |
| CN110262548B (zh) | 一种考虑抵达时间约束的无人机航迹规划方法 | |
| Zhang et al. | An improved path planning algorithm for unmanned aerial vehicle based on RRT-Connect | |
| CN110926477B (zh) | 一种无人机航路规划及避障方法 | |
| CN112904869B (zh) | 基于多树rrt的无人艇加权迭代路径规划方法及设备 | |
| CN109990787B (zh) | 一种机器人在复杂场景中规避动态障碍物的方法 | |
| CN112987799B (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
| CN108444489A (zh) | 一种改进rrt算法的路径规划方法 | |
| CN109685237B (zh) | 一种基于Dubins路径和分支限界的无人机航迹实时规划方法 | |
| CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
| CN110617818A (zh) | 一种无人机航迹生成方法 | |
| CN111381600B (zh) | 一种基于粒子群算法的uuv路径规划方法 | |
| CN112013846A (zh) | 一种结合动态步长rrt*算法和势场法的路径规划方法 | |
| CN107169557A (zh) | 一种对布谷鸟优化算法进行改进的方法 | |
| CN107607120A (zh) | 基于改进修复式Anytime稀疏A*算法的无人机动态航迹规划方法 | |
| CN108919818B (zh) | 基于混沌种群变异pio的航天器姿态轨道协同规划方法 | |
| WO2023197092A1 (zh) | 一种基于改进rrt算法的无人机路径规划方法 | |
| CN111930121A (zh) | 一种室内移动机器人的混合路径规划方法 | |
| CN109211242B (zh) | 一种融合rrt与蚁群算法的三维空间多目标路径规划方法 | |
| CN110726408A (zh) | 一种基于改进蚁群算法的移动机器人路径规划方法 | |
| CN111626500A (zh) | 一种基于改进人工蜂群算法的路径规划方法 | |
| CN110530373B (zh) | 一种机器人路径规划方法、控制器及系统 | |
| CN110954124A (zh) | 一种基于a*-pso算法的自适应路径规划方法及系统 | |
| Zhang et al. | Robot navigation based on improved A* algorithm in dynamic environment |
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 |