CN115167513A - 一种基于pf-rrt*算法的无人机航迹规划方法 - Google Patents

一种基于pf-rrt*算法的无人机航迹规划方法 Download PDF

Info

Publication number
CN115167513A
CN115167513A CN202210885243.2A CN202210885243A CN115167513A CN 115167513 A CN115167513 A CN 115167513A CN 202210885243 A CN202210885243 A CN 202210885243A CN 115167513 A CN115167513 A CN 115167513A
Authority
CN
China
Prior art keywords
new
nearest
node
aerial vehicle
unmanned aerial
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
Application number
CN202210885243.2A
Other languages
English (en)
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.)
Shenyang Institute Of Science And Technology
Shenyang Aerospace University
Original Assignee
Shenyang Institute Of Science And Technology
Shenyang Aerospace University
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 Shenyang Institute Of Science And Technology, Shenyang Aerospace University filed Critical Shenyang Institute Of Science And Technology
Priority to CN202210885243.2A priority Critical patent/CN115167513A/zh
Publication of CN115167513A publication Critical patent/CN115167513A/zh
Pending legal-status Critical Current

Links

Images

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/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
    • G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones

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

本发明公开了一种基于PF‑RRT*算法的无人机航迹规划方法，涉及无人机决策领域，在RRT*算法中加入二分法在障碍物附近为新节点创建一个新的父节点，与现有技术相比，降低了路径代价，规划出更平滑的轨迹；采用改进的APF算法缩短了收敛时间，目标点和随机点分别吸引随机树，靠近随机树的障碍物排斥随机树，新的节点沿着三种力合力的方向进行生长；结合目标偏置，目标点成为具有一定概率的随机点，提高了随机点的质量，从而减少了迭代次数；改进的航迹规划算法解决了RRT*算法冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率；提出的融合算法优化了路径长度，加快了运算速度；便于实现，具有良好的可操作性。

Description

一种基于PF-RRT*算法的无人机航迹规划方法
技术领域
本发明涉及无人机决策技术领域，尤其涉及一种基于PF-RRT*算法的无人机航迹规划方法。
背景技术
航迹规划是无人机任务规划系统的关键组成部分，它是指在综合考虑无人机能够成功完成预定任务的前提下，规划一条从起点到目标点的无碰撞飞行路径。然而，由于规划约束条件众多而且模糊性大，忽略地形和无人机操作性能等因素的影响，综合考虑敌方威胁，无人机航程等，成功规划出能够避开障碍物并顺利完成任务的飞行路径面临着新的挑战。无人机在执行复杂任务过程中，航迹规划的算法不能及时有效的计算出一条渐近最优航迹，将可能与障碍物碰撞并摧毁；为了提高航迹规划算法的搜索效率，使无人机飞行的航迹满足实时要求，通常要求无人机能够快速的规划出一条渐近最优航迹，这就要求航迹规划的算法更加高效，因此，航迹规划的研究就显得的格外重要，基于采样的算法因其在复杂环境中具有良好的搜索性能而受到广泛关注。其中，快速搜索随机树(RRT)因为避免了精确的环境模型，可以在更短的时间内找到路径。它规划的路径由于搜索空间大、随机性强，并不是最优路径。为了弥补RRT算法的不足，提出了渐近最优快速搜索随机树(RRT*)算法。与RRT相比，RRT*增加了重新选择新节点的父节点的过程，并对新的子节点重新布线的过程。当迭代次数趋近于无穷时，RRT*一定能找到最优解，但其存在着路径代价高，收敛速度慢的问题。
发明内容
为解决现有技术的不足之处，本发明提出一种基于PF-RRT*算法的无人机航迹规划方法，将改进的人工势场函数与F-RRT*算法相结合，采用改进的人工势场函数引导双向随机搜索树的生长；引入了目标偏置策略，在采样过程中获得更高质量的采样点，实现了无人机的航迹规划，解决无人机在执行飞行任务中的航迹规划问题。
为实现上述目的，本发明所采取的技术方案是：
本发明提出了一种基于PF-RRT*算法的无人机航迹规划方法，包括以下步骤：
S1：规划无人机的飞行环境S，飞行环境S包括可飞行区域Ssearch，障碍区域Sobs；设置无人机航迹规划的起点qstart、终点qgoal、步长L；
S2：在飞行环境S中，以qstart为根节点，创建快速搜索随机树T，随机树T只有1个初始节点qstart，为随机树T中的初始节点创建基于位置的索引，父节点，父节点索引；
S3：在可飞行区域Ssearch内，采用目标偏置策略的概率函数使目标点qgoal具有一定的概率成为随机采样点；
S4：选取T树中距离qrand最近的节点qnearest，初始的节点qnearest为qstart；
S5：采用改进的人工势场函数使qrand和qgoal分别对qnearest产生潜在的引力，使障碍物对qnearest生成一个潜在的斥力，qnearest按照给定的步长L沿着三个力的合力方向生成新的节点qnew；
S6：检测qnearest和qnew之间是否与障碍物碰撞；
S7：T生成新节点后，检测搜索随机树是否到达目标点，若未到达目标点则对S6输出节点qnew利用二分法在障碍物附近生成新的父节点替换qnearest，并对新节点qnew重新布线；然后，qnew被加入到随机搜索树T中：执行S3-S6，进行生长；若到达目标点则qnew和目标点qgoal相连接；
S8：qnew和目标点qgoal连接后，得到一条由多个点构成的一条航迹，解决无人机飞行中转弯角度过大的问题。
所述目标偏置策略的概率函数如式(1)所示：
Figure BDA0003765568750000021
其中，rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点qgoal生长。
所述人工势场(APF)包括引力场函数Uatt(p)如式(2)所示、斥力场函数Urep(p)如式(3)所示，则合力势场函数如式(4)所示：
Figure BDA0003765568750000022
Figure BDA0003765568750000023
Utotal＝∑Urep+∑Uatt (4)
其中，p是无人机的一个航迹点，ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；
其中，引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(5)和(6)表示，合力如式(7)所示：
Fatt(p)＝kaρg(p) (5)
Figure BDA0003765568750000031
Ftotal＝∑Frep+∑Fatt (7)
当目标点附近存在障碍物时，无人机受到障碍物的斥力增加，且引力减少，无人机难以到达目标点，通过改进的引力势场函数、斥力场函数使无人机顺利到达目标点；
改进的引力势场函数如式(8)所示、改进的斥力场函数如式(11)所示：
Uatt(p)＝Uatt1(p)+Uatt2(p) (8)
其中：
Figure BDA0003765568750000032
Figure BDA0003765568750000033
Figure BDA0003765568750000034
改进的斥力场函数如式(11)所示：
其中，Uatt1(p)和Uatt2(p)分别是qgoal和qrand对qnearest的吸引力；ka是目标点产生的引力场增益常数，ρg(pner,pgoal)是qnearest和qgoal之间的距离，kb是随机点产生的引力场增益常数，ρr(pner,prand)是qnearest和qrand之间的距离；pner，pgoal，pobs分别代表qnearest，qgoal和障碍物的位置；n是一个正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，避免斥力大于引力；
T随机树采用生成随机点的函数，在无人机可飞行区域内随机生成一个采样点qrand，并且找到搜索树中距离qrand最近的节点qnearest，结合改进的人工势场函数，在终点对qnearest产生引力Fatt2如式9所示，在qrand节点上生成对qnearest的潜在吸引力Fatt1如式10所示，障碍物对qnearest产生斥力Frep是不同障碍物对qnearest的斥力，根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
Figure BDA0003765568750000041
Figure BDA0003765568750000042
Figure BDA0003765568750000043
其中，nOR＝▽ρ(p,pobs)为qnearest和障碍物之间的方向矢量；nRG＝-▽ρ(p,pgoal)为qnearest和qgoal的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解qnew时，需要分别计算qrand和qgoal对qnearest的引力，计算障碍物对qnearest的斥力，然后将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示；设定qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长可以确定为：
Figure BDA0003765568750000044
其中，L是无人机的扩展步长，k是比例系数，可得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
Figure BDA0003765568750000051
所述S6的具体过程为：
以qnearest作为检测起点，以qnew作为检测终点，把qnearest和qnew之间的距离平均分成j段；由式(14)每次生成检测节点qcollision的位置，计算qcollision到距离qcollision最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则qnearest和qnew之间存在障碍物即与障碍物碰撞，重复S3-S5，重新寻找一个新节点qnew；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点qnew；
Figure BDA0003765568750000052
k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为qnearest到qnew的方向与x轴的夹角，xqnearest和yqnearest分别代表qnearest的横坐标和纵坐标，xcollision和ycollision分别代表求得的qcollision横坐标和纵坐标。
所述S7的具体过程为：
T生成新节点后，计算qnew与目标点qgoal欧式距离D(qnew,qgoal)，检测D是否小于规定的阈值，即检测搜索随机树是否到达目标点，若D不小于规定的阈值则对步骤6输出节点qnew利用二分法在障碍物附近生成新的父节点替换qnearest，并把新的父节点记作qnear，使得qnew到起点的路径代价减小，寻找新的父节点后，在半径为r1的一个圆域内寻找到一个以新节点qnew作为父节点的子节点，使这个子节点到起点的路径代价减小；寻找到子节点后，qnew被加入到第一棵随机搜索树T中：执行步骤3-步骤6，进行生长；若D小于规定的阈值则qnew和目标点qgoal相连接。
有益技术效果
本发明提出一种基于PF-RRT*算法的无人机航迹规划方法，解决了RRT*算法冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率，优化了路径长度，加快了运算速度，具有良好的可操作性。具有以下有益技术效果：
1、在RRT*算法中加入二分法在障碍物附近为新节点创建一个新的父节点，而不是在现有的随机树节点中寻找父节点，降低了路径代价，规划出更平滑的轨迹。
2、采用改进的APF缩短收敛时间。目标点和随机点分别吸引随机树，最靠近随机树的障碍物排斥随机树，新的节点沿着三种力合力的方向生长。
3、结合现有的目标偏置策略。目标点成为具有一定概率的随机点，提高了随机点的质量，从而减少了迭代次数；改进的航迹规划算法解决了RRT*算法冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率；提出的融合算法优化了路径长度，提高了运算速度，该方法便于实现，具有良好的可操作性。
附图说明
图1为本发明实施例提供的一种基于PF-RRT*算法的无人机航迹规划方法流程图；
图2为本发明实施例提供的一种基于PF-RRT*算法的无人机航迹规划方法地图示意图；
其中，图2(a)表示简单环境图，图2(b)表示较复杂环境图，图2(c)表示复杂环境图；
图3为本发明实施例提供的随机树T生长过程中qnearest受力示意图；
图4为本发明实施例提供的Findnode示意图；
图5为本发明实施例提供的Createnode示意图；
图6为本发明实施例提供的随机搜索树T重新布线示意图；
图7为本发明实施例提供的一种基于PF-RRT*算法的无人机航迹规划方法示意图；
其中，图7(a)表示本发明在简单环境下查找航迹示意图，图7(b)表示本发明在较复杂环境下查找航迹示意图，图7(c)表示本发明在复杂环境下查找航迹示意图。
图8为本发明实施例提供的简单环境仿真对比图；
其中，图8(a)表示简单环境仿真对比中RRT*示意图，图8(b)表示简单环境仿真对比中Q-RRT*示意图，图8(c)表示简单环境仿真对比中P-RRT*示意图，图8(d)表示简单环境仿真对比中F-RRT*示意图，图8(e)表示简单环境仿真对比中本发明示意图；
图9为本发明实施例提供的简单环境仿真数据图；
其中，图9(a)表示每种算法在简单环境中迭代次数平均值示意图，图9(b)表示每种算法在简单环境中路径长度平均值示意图，图9(c)表示每种算在简单环境中法运行时间平均值示意图。
图10为本发明实施例提供的较复杂环境仿真对比图；
其中，图10(a)表示较复杂环境仿真对比中RRT*示意图，图10(b)表示较复杂环境仿真对比中Q-RRT*示意图，图10(c)表示较复杂环境仿真对比中P-RRT*示意图，图10(d)表示较复杂环境仿真对比中F-RRT*示意图，图10(e)表示较复杂环境仿真对比中本发明示意图；
图11为本发明实施例提供的较复杂环境仿真数据图；
其中，图11(a)表示每种算法在较复杂环境中迭代次数平均值示意图，图11(b)表示每种算法在较复杂环境中路径长度平均值示意图，图11(c)表示每种算法在较复杂环境中运行时间平均值示意图。
图12为本发明实施例提供的复杂环境仿真对比图；
其中，图12(a)表示复杂环境仿真对比中RRT*示意图，图12(b)表示复杂环境仿真对比中Q-RRT*示意图，图12(c)表示复杂环境仿真对比中P-RRT*示意图，图12(d)表示复杂环境仿真对比中F-RRT*示意图，图12(e)表示复杂环境仿真对比中本发明示意图；
图13为本发明实施例提供的复杂环境仿真数据图；
其中，图13(a)表示每种算法在复杂环境中迭代次数平均值示意图，图13(b)表示每种算法在复杂环境中路径长度平均值示意图，图13(c)表示每种算法在复杂环境中运行时间平均值示意图。
具体实施方式
下面结合附图和具体实施例对本发明内容做进一步说明；
本发明提出一种基于PF-RRT*算法的无人机航迹规划方法，将改进的人工势场函数与F-RRT*算法相结合，采用改进的人工势场函数引导双向随机搜索树的生长；并且引入了目标偏置策略，在采样过程中获得更高质量的采样点，实现了无人机的航迹规划，解决无人机在执行空战任务中的航迹规划问题，如图1所示，分为以下步骤：
步骤1：对无人机航迹规划环境初始化；
无人机航迹规划地图设置为100km*100km的二维空间；无人机飞行航迹的起始坐标设为(1,1)，终点设为(90,90)；如图2所示；图中圆圈表示障碍物，无人机无法通过该区域；
步骤2：在可飞行区域Ssearch内，采用目标偏置策略的概率函数使目标点qgoal具有一定的概率成为随机采样点。目标偏置策略的概率函数如式18所示；
Figure BDA0003765568750000071
其中rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点qgoal生长。
步骤3：采用改进人工势场函数生成新节点
人工势场(APF)包括引力场函数Uatt(p)如式19所示、斥力场函数Ureq(p)如式20所示；设定p是无人机的一个航迹点，则合力势场函数如式21所示：
Figure BDA0003765568750000081
Figure BDA0003765568750000082
Utotal＝∑Urep+ΣUatt (21)
在其中ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；引力和斥力的大小分别是引力场函数和斥力场函数的负梯度；如式22和23表示，合力如式24所示：
Fatt(p)＝kaρg(p) (22)
Figure BDA0003765568750000083
Ftotal＝∑Frep+∑Fatt (24)
当目标点附近存在障碍物时，无人机受到障碍物的斥力将非常大，而且引力相对较小，无人机很难到达目标点，所以本发明提出改进的引力势场函数如式25所示、斥力场函数如式28所示：
Uatt(p)＝Uatt1(p)+Uatt2(p) (25)
Figure BDA0003765568750000084
Figure BDA0003765568750000085
Figure BDA0003765568750000086
其中，Uatt1(p)和Uatt2(p)分别是qgoal和qrand对qnearest的吸引力。ka是目标点产生的引力场增益常数，ρg(pner,pgoal)是qnearest和qgoal之间的距离，kb是随机点产生的引力场增益常数，ρr(pner,prand)是qnearest和qrand之间的距离。pner，pgoal，pobs分别代表qnearest，qgoal和障碍物的位置；n是一个正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，避免斥力大于引力；ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，ρg(pner,pgoal)是qnearest和qgoal之间的距离；当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，这样避免了斥力大于引力的现象；
流程中iter为迭代次数，Maxiter为最大迭代次数，本发明程序达到最大迭代次数，程序将会停止；反之，随机搜索树进行生长；随机树T生长过程中qnearest受力示意图如图3所示，首先，T随机树采用目标偏置策略在无人机可飞行区域内生成随机点qrand，并且找到随机树中距离qrand最近的节点qnearest；结合改进的人工势场函数，在目标点对qnearest产生引力Fatt2如式26所示，在qrand节点上生成对qnearest的潜在吸引力Fatt1如式27所示，障碍物对qnearest产生斥力Frep如式29所示。根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
Figure BDA0003765568750000091
Figure BDA0003765568750000092
Figure BDA0003765568750000093
其中，nOR＝▽ρ(p,pobs)为qnearest和障碍物之间的方向矢量；nRG＝-▽ρ(p,pgoal)为qnearest和qgoal的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解qnew时，首先，需要分别计算qrand和qgoal对qnearest的引力，然后计算障碍物对qnearest的斥力，将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示；设定qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长可以确定为：
Figure BDA0003765568750000101
L是无人机的扩展步长，k是比例系数，于是，可得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
Figure BDA0003765568750000102
步骤4：检测是否碰撞；
检测qnearest和qnew之间是否与障碍物碰撞，若qnearest和qnew之间存在障碍物即与障碍物碰撞，则重复步骤2，重新寻找一个新节点qnew；反之则利用二分法为新节点创建新的父节点的过程；
步骤5：为新节点创建新的父节点
如图4所示的Findnode示意图；在障碍物附近，首先要找到qnew的父节点qnearest。然后在障碍物附近找到可以被优化的qnearest的父节点，把该点命名为qreachest，连接qreachest和qnew。当qreachest的父节点Parent(qreachest)不存在时，qnearest作为qreachest；如图5所示的Createnode示意图；在创建节点的过程中，找到qreachest后，在Parent(qreachest))和新节点qnew之间创建一个新节点qcreate，进一步优化了路径代价；并且引入参数Ddichotomy作为二分法寻找qcreate的终止条件。
步骤6：重新布线，如图6所示的随机搜索树T1的更新子节点的示意图；首先以新节点qnew为圆心，依据设定的半径画一个圆，将这个圆内的所有生长树上的节点作为新的子节点的备选节点；分别计算圆内的备选节点的原路径代价和备选节点以新节点qnew为父节点到qstart的欧氏距离之和；在图6(a)中，节点标号表示产生该节点的顺序，0节点是起点，9节点是新产生的节点qnew，6节点是9节点的父节点qnearest，节点与节点之间连接的边上数字代表两个节点之间的欧氏距离；在创建qreachest后，再进行更新子节点：首先计算集合圆中每一个节点以qnew(9节点)为父节点(qnearest除外)，并连接至qstart(0节点)的总代价，若以qnew为父节点的总代价小于不以qnew为父节点的总代价，则修改该节点的父节点为qnew，如图6(a)所示，在qnew的所有节点中，当节点6以qnew为父节点时，连接至qstart的路径为6-9-5-1-0，路径总代价为12，而节点6不以qnew为父节点时，例如连接至qstart的路径为6-4-0，路径总代价为15，故修剪随机树，将节点6的父节点改为qnew，如图6(b)所示；
步骤7：随机树是否生长到目标节点
T树对qnew重新布线后，检测是否连接，若连接则执行步骤7；反之则T树重复步骤1-5进行生长；随机树在地图中生长过程如图7(a)、图7(b)及图7(c)所示，随机搜索树节点和节点之间的连接用浅色线表示；随机搜索树节点和节点之间的连接用浅色线表示；生成路径用深色线表示；
步骤8：随机树T生长到目标点后，得到一条由多个点构成的一条光滑的航迹，解决无人机飞行中转弯角度过大的问题；
为进一步对本发明做详细说明，在简单环境中各算法的仿真结果如图8所示，深色线为算法规划的无人机可行轨迹；其中无人机的可行轨迹用深色线表示。在图8(a)中，由于RRT*在空间中随机采样，路径中出现了更多的拐点，得到了更长的路径。图8(b)显示了深度为2的Q-RRT*的仿真图。与RRT*相比，其规划点路径拐点较少，且部分节点具有相同的父节点，因此路径更好。在图8(c)中，引入了RRT*和人工势场方法，使路径更加平滑。在图8(d)中，F-RRT*算法规划了一条更短的路径。针对图8(e)环境简单的特点，改进的算法具有路径短、节点少等优点，能够满足无人机的实时性要求。
每个算法在简单环境中计算初始解的仿真数据包括30次实验的平均运行时间、平均路径长度和平均迭代次数。在表1中，本发明30次实验数据的平均长度为126.4197，平均运行时间为4.3924，平均迭代次数为45次。与F-RRT*算法相比，所生成轨迹的平均长度减少了0.17％，平均迭代次数减少了66.16％。从图9中可以清楚地看到，尽管本文中算法的运行时间和迭代次数本质上与P-RRT*算法相同，路径更平滑。结果表明，在简单的环境下本发明的路径开代价大大降低，并且大大减少了算法迭代次数。
表1简单环境中30次实验数据平均值
Figure BDA0003765568750000121
在较复杂的环境中，存在较多的障碍物，仿真结果如图10所示。与简单环境相比，这五种算法在较复杂的环境中具有更多的拐点。本发明规划的航迹优势依然明显。
为了证明本发明具有更好的性能，在较复杂的环境中对每种算法获取30次实验结果，并记录了算法的运行时间、生成路径的长度和迭代次数。实验数据显示在表2和图11中。通过对图11分析，与其他算法相比，本发明的路径更短，运行时间更短，迭代次数更少。在表2中，本发明的平均路径长度为127.0054，平均运行时间为4.6533，平均迭代次数为133。与F-RRT*相比，平均运行时间和平均迭代次数分别减少了0.17％和24.86％。结合数据比较表明，本发明能够获得较好的路径。
表2较复杂环境中30次实验数据平均值
Figure BDA0003765568750000131
在障碍物较为复杂环境中，本发明的优势更加明显；与简单环境，较复杂环境相比，复杂环境中障碍物更多，且大小不一，无人机航迹规划困难。但本发明仍然可以在复杂环境下更高效地规划轨迹，其优势是非常明显的。如图12所示，算法具有更少的冗余点和更平滑的路径；
通过对表3的分析，本发明在复杂环境下生成初始解的平均路径长度为126.9231，平均运行时间为9.1732，平均迭代次数为52次，与F-RRT*算法相比，平均路径长度和迭代次数分别减少了0.24％和65.33％。对比图13，本发明虽然重新创建了新节点的父节点，但并不消耗太多时间，其平均运行时间与P-RRT*相似，这些数据表明本发明的性能指标有了明显的提高。因此，本发明规划的航迹迹是优越的。
表3复杂环境中30次实验数据平均值
Figure BDA0003765568750000141
本发明考虑了完整路径的代价高、算法计算量大等因素的影响，实现了无人机飞行过程中算法收敛速度快，路径代价低动态航迹规划研究；因此，该方法能更灵活更快速地生成路径代价更小的路径，提高了搜索效率；在以上分析的基础上，可以看出提出的方法可以使无人机在飞行方面更加的符合实际，即本发明设计的方法是有效的。

Claims (7)

1.一种基于PF-RRT*算法的无人机航迹规划方法，其特征在于：包括以下步骤：
S1：规划无人机的飞行环境S，飞行环境S包括可飞行区域Ssearch，障碍区域Sobs；设置无人机航迹规划的起点qstart、终点qgoal、步长L；
S2：在飞行环境S中，以qstart为根节点，创建快速搜索随机树T，随机树T只有1个初始节点qstart，为随机树T中的初始节点创建基于位置的索引，父节点，父节点索引；
S3：在可飞行区域Ssearch内，采用目标偏置策略的概率函数使目标点qgoal具有一定的概率成为随机采样点；
S4：选取T树中距离qrand最近的节点qnearest，初始的节点qnearest为qstart；
S5：采用改进的人工势场函数使qrand和qgoal分别对qnearest产生潜在的引力，使障碍物对qnearest生成一个潜在的斥力，qnearest按照给定的步长L沿着三个力的合力方向生成新的节点qnew；
S6：检测qnearest和qnew之间是否与障碍物碰撞；
S7：T生成新节点后，检测搜索随机树是否到达目标点，若未到达目标点则对S6输出节点qnew利用二分法在障碍物附近生成新的父节点替换qnearest，并对新节点qnew重新布线；qnew被加入到随机搜索树T中：执行S3-S6，进行生长；若到达目标点则qnew和目标点qgoal相连接；
S8：qnew和目标点qgoal连接后，得到一条由多个点构成的一条航迹。
2.根据权利要求1所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述目标偏置策略的概率函数如式(1)所示：
Figure FDA0003765568740000011
其中，rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点qgoal生长。
3.根据权利要求1所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述人工势场(APF)包括引力场函数Uatt(p)如式(2)所示、斥力场函数Ureq(p)如式(3)所示，则合力势场函数如式(4)所示：
Figure FDA0003765568740000021
Figure FDA0003765568740000022
Utotal＝ΣUrep+∑Uatt (4)
其中，p是无人机的一个航迹点，ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；
其中，引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(5)和(6)表示，合力如式(7)所示：
Fatt(p)＝kaρg(p) (5)
Figure FDA0003765568740000023
Ftotal＝∑Frep+∑Fatt (7)
通过改进的引力势场函数、斥力场函数使无人机到达目标点。
4.根据权利要求3所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述改进的引力势场函数如式(8)所示、改进的斥力场函数如式(11)所示：
Uatt(p)＝Uatt1(p)+Uatt2(p) (8)
其中：
Figure FDA0003765568740000024
Figure FDA0003765568740000025
Figure FDA0003765568740000031
5.根据权利要求3所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述改进的斥力场函数如式(11)所示：
其中，Uatt1(p)和Uatt2(p)分别是qgoal和qrand对qnearest的吸引力；ka是目标点产生的引力场增益常数，ρg(pner,pgoal)是qnearest和qgoal之间的距离，kb是随机点产生的引力场增益常数，ρr(pner,prand)是qnearest和qrand之间的距离；pner，pgoal，pobs分别代表qnearest，qgoal和障碍物的位置；n为正整数，ρ(pner,pobs)是qnearest距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(pner,pgoal)的减小而变小，避免斥力大于引力；
T随机树采用生成随机点的函数，在无人机可飞行区域内随机生成采样点qrand，并且找到搜索树中距离qrand最近的节点qnearest，结合改进的人工势场函数，在终点对qnearest产生引力Fatt2如式9所示，在qrand节点上生成对qnearest的潜在吸引力Fatt1如式10所示，障碍物对qnearest产生斥力Frep是不同障碍物对qnearest的斥力，根据平行四边形法则得到Fatt1，Fatt2，Frep的合力Ftotal方向，qnew沿着合力方向，按照给定的步长L生成新的节点qnew；
Figure FDA0003765568740000032
Figure FDA0003765568740000033
Figure FDA0003765568740000034
其中，
Figure FDA0003765568740000035
为qnearest和障碍物之间的方向矢量；
Figure FDA0003765568740000036
为qnearest和qgoal的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解qnew时，需要分别计算qrand和qgoal对qnearest的引力，计算障碍物对qnearest的斥力，然后将合力Ftotal分解为x轴和y轴两个方向的力，分别用Fx，Fy表示；设定qnearest的坐标为(xc，yc)，扩展步长由qnearest在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为Fmax，则无人机的扩展步长为：
Figure FDA0003765568740000041
其中，L是无人机的扩展步长，k是比例系数，可得到qnew的坐标，令qnew的坐标为(xc，yc)，则qnew的坐标方程为：
Figure FDA0003765568740000042
6.根据权利要求1所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述S6的具体过程为：
以qnearest作为检测起点，以qnew作为检测终点，把qnearest和qnew之间的距离平均分成j段；由式(14)每次生成检测节点qcollision的位置，计算qcollision到距离qcollision最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则qnearest和qnew之间存在障碍物即与障碍物碰撞，重复S3-S5，重新寻找一个新节点qnew；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点qnew；
Figure FDA0003765568740000043
k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为qnearest到qnew的方向与x轴的夹角，xqnearest和yqnearest分别代表qnearest的横坐标和纵坐标，xcollision和ycollision分别代表求得的qcollision横坐标和纵坐标。
7.根据权利要求1所述的基于PF-RRT*算法的无人机航迹规划方法，其特征在于：
所述S7的具体过程为：
T生成新节点后，计算qnew与目标点qgoal欧式距离D(qnew,qgoal)，检测D是否小于设定的阈值，检测搜索随机树是否到达目标点，若D不小于规定的阈值则对步骤6输出节点qnew利用二分法在障碍物附近生成新的父节点替换qnearest，并把新的父节点记作qnear，使得qnew到起点的路径代价减小，寻找新的父节点后，在半径为r1的一个圆域内寻找到一个以新节点qnew作为父节点的子节点；寻找到子节点后，qnew被加入到第一棵随机搜索树T中：执行步骤3-步骤6，进行生长；若D小于规定的阈值则qnew和目标点qgoal相连接。
CN202210885243.2A 2022-07-26 2022-07-26 一种基于pf-rrt*算法的无人机航迹规划方法 Pending CN115167513A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210885243.2A CN115167513A (zh) 2022-07-26 2022-07-26 一种基于pf-rrt*算法的无人机航迹规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210885243.2A CN115167513A (zh) 2022-07-26 2022-07-26 一种基于pf-rrt*算法的无人机航迹规划方法

Publications (1)

Publication Number Publication Date
CN115167513A true CN115167513A (zh) 2022-10-11

Family

ID=83496119

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210885243.2A Pending CN115167513A (zh) 2022-07-26 2022-07-26 一种基于pf-rrt*算法的无人机航迹规划方法

Country Status (1)

Country Link
CN (1) CN115167513A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115331131A (zh) * 2022-10-17 2022-11-11 四川腾盾科技有限公司 一种无人机任务规划辅助决策方法
CN117400269A (zh) * 2023-12-14 2024-01-16 湖南大学 一种基于双向采样和虚拟势场引导的机械臂路径规划方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115331131A (zh) * 2022-10-17 2022-11-11 四川腾盾科技有限公司 一种无人机任务规划辅助决策方法
CN117400269A (zh) * 2023-12-14 2024-01-16 湖南大学 一种基于双向采样和虚拟势场引导的机械臂路径规划方法
CN117400269B (zh) * 2023-12-14 2024-04-12 湖南大学 一种基于双向采样和虚拟势场引导的机械臂路径规划方法

Similar Documents

Publication Publication Date Title
CN115167513A (zh) 一种基于pf-rrt*算法的无人机航迹规划方法
CN114115362B (zh) 一种基于双向apf-rrt*算法的无人机航迹规划方法
CN109945881B (zh) 一种蚁群算法的移动机器人路径规划方法
CN110262548B (zh) 一种考虑抵达时间约束的无人机航迹规划方法
CN107063255B (zh) 一种基于改进果蝇优化算法的三维航路规划方法
Zhang et al. An improved path planning algorithm for unmanned aerial vehicle based on RRT-Connect
CN104406593A (zh) 一种确定无人机航路最优路径的方法
CN112904869B (zh) 基于多树rrt的无人艇加权迭代路径规划方法及设备
CN112666981B (zh) 基于原鸽群动态群组学习的无人机集群动态航路规划方法
CN111930121B (zh) 一种室内移动机器人的混合路径规划方法
CN105512769A (zh) 基于遗传规划的无人机航迹规划系统及方法
CN108919818B (zh) 基于混沌种群变异pio的航天器姿态轨道协同规划方法
CN111024085B (zh) 一种具有端点方向和时间约束的无人机航迹规划方法
CN109211242B (zh) 一种融合rrt与蚁群算法的三维空间多目标路径规划方法
CN115509239B (zh) 一种基于空地信息共享的无人车路径规划方法
WO2023197092A1 (zh) 一种基于改进rrt算法的无人机路径规划方法
Cheng et al. Path planning based on immune genetic algorithm for UAV
CN116400737B (zh) 一种基于蚁群算法的安全路径规划系统
CN117850471A (zh) 三维环境下考虑雷达威胁的多智能体协同航迹规划方法及其规划系统
CN116893689A (zh) 一种无人机自动避障航线规划方法和系统
CN116626608A (zh) 一种面向组网雷达假航迹的无人干扰机控制策略生成方法
CN114740873B (zh) 一种基于多目标改进粒子群算法的自主式水下机器人的路径规划方法
CN114815875B (zh) 一种基于集合满射鸽群智能优化的无人机集群编队控制器调参方法
Cheng et al. Route planning of mixed ant colony algorithm based on Dubins path
CN116107329A (zh) 复杂环境中分布式蜂群的路径规划方法

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