一种基于PF-RRT*算法的无人机航迹规划方法

阅读：评论：0

一种基于pf-rrt*算法的无人机航迹规划方法
技术领域
1.本发明涉及无人机决策技术领域，尤其涉及一种基于pf-rrt*算法的无人机航迹规划方法。

背景技术：

2.航迹规划是无人机任务规划系统的关键组成部分，它是指在综合考虑无人机能够成功完成预定任务的前提下，规划一条从起点到目标点的无碰撞飞行路径。然而，由于规划约束条件众多而且模糊性大，忽略地形和无人机操作性能等因素的影响，综合考虑敌方威胁，无人机航程等，成功规划出能够避开障碍物并顺利完成任务的飞行路径面临着新的挑战。无人机在执行复杂任务过程中，航迹规划的算法不能及时有效的计算出一条渐近最优航迹，将可能与障碍物碰撞并摧毁；为了提高航迹规划算法的搜索效率，使无人机飞行的航迹满足实时要求，通常要求无人机能够快速的规划出一条渐近最优航迹，这就要求航迹规划的算法更加高效，因此，航迹规划的研究就显得的格外重要，基于采样的算法因其在复杂环境中具有良好的搜索性能而受到广泛关注。其中，快速搜索随机树(rrt)因为避免了精确的环境模型，可以在更短的时间内到路径。它规划的路径由于搜索空间大、随机性强，并不是最优路径。为了弥补rrt算法的不足，提出了渐近最优快速搜索随机树(rrt*)算法。与rrt相比，rrt*增加了重新选择新节点的父节点的过程，并对新的子节点重新布线的过程。当迭代次数趋近于无穷时，rrt*一定能到最优解，但其存在着路径代价高，收敛速度慢的问题。

技术实现要素：

3.为解决现有技术的不足之处，本发明提出一种基于pf-rrt*算法的无人机航迹规划方法，将改进的人工势场函数与f-rrt*算法相结合，采用改进的人工势场函数引导双向随机搜索树的生长；引入了目标偏置策略，在采样过程中获得更高质量的采样点，实现了无人机的航迹规划，解决无人机在执行飞行任务中的航迹规划问题。
4.为实现上述目的，本发明所采取的技术方案是：
5.本发明提出了一种基于pf-rrt*算法的无人机航迹规划方法，包括以下步骤：
6.s1：规划无人机的飞行环境s，飞行环境s包括可飞行区域s
search
，障碍区域s
obs
；设置无人机航迹规划的起点q
start
、终点q
goal
、步长l；
7.s2：在飞行环境s中，以q
start
为根节点，创建快速搜索随机树t，随机树t只有1个初始节点q
start
，为随机树t中的初始节点创建基于位置的索引，父节点，父节点索引；
8.s3：在可飞行区域s
search
内，采用目标偏置策略的概率函数使目标点q
goal
具有一定的概率成为随机采样点；
9.s4：选取t树中距离q
rand
最近的节点q
nearest
，初始的节点q
nearest
为q
start
；
10.s5：采用改进的人工势场函数使q
rand
和q
goal
分别对q
nearest
产生潜在的引力，使障碍物对q
nearest
生成一个潜在的斥力，q
nearest
按照给定的步长l沿着三个力的合力方向生成新的
节点q
new
；
11.s6：检测q
nearest
和q
new
之间是否与障碍物碰撞；
12.s7：t生成新节点后，检测搜索随机树是否到达目标点，若未到达目标点则对s6输出节点q
new
利用二分法在障碍物附近生成新的父节点替换q
nearest
，并对新节点q
new
重新布线；然后，q
new
被加入到随机搜索树t中：执行s3-s6，进行生长；若到达目标点则q
new
和目标点q
goal
相连接；
13.s8：q
new
和目标点q
goal
连接后，得到一条由多个点构成的一条航迹，解决无人机飞行中转弯角度过大的问题。
14.所述目标偏置策略的概率函数如式(1)所示：
[0015][0016]
其中，rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点q
goal
生长。
[0017]
所述人工势场(apf)包括引力场函数u
att
(p)如式(2)所示、斥力场函数u
rep
(p)如式(3)所示，则合力势场函数如式(4)所示：
[0018][0019][0020]utotal
＝∑u
rep
+∑u
att
ꢀꢀ
(4)
[0021]
其中，p是无人机的一个航迹点，ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；
[0022]
其中，引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(5)和(6)表示，合力如式(7)所示：
[0023]
fatt(p)＝kaρg(p)
ꢀꢀ
(5)
[0024][0025]ftotal
＝∑f
rep
+∑f
att
ꢀꢀ
(7)
[0026]
当目标点附近存在障碍物时，无人机受到障碍物的斥力增加，且引力减少，无人机难以到达目标点，通过改进的引力势场函数、斥力场函数使无人机顺利到达目标点；
[0027]
改进的引力势场函数如式(8)所示、改进的斥力场函数如式(11)所示：
[0028]uatt
(p)＝u
att1
(p)+u
att2
(p)
ꢀꢀ
(8)
[0029]
其中：
[0030][0031][0032][0033]
改进的斥力场函数如式(11)所示：
[0034]
其中，u
att1
(p)和u
att2
(p)分别是q
goal
和q
rand
对q
nearest
的吸引力；ka是目标点产生的引力场增益常数，ρg(p
ner
,p
goal
)是q
nearest
和q
goal
之间的距离，kb是随机点产生的引力场增益常数，ρr(p
ner
,p
rand
)是q
nearest
和q
rand
之间的距离；p
ner
，p
goal
，p
obs
分别代表q
nearest
，qgoal和障碍物的位置；n是一个正整数，ρ(p
ner
,p
obs
)是q
nearest
距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(p
ner
,p
goal
)的减小而变小，避免斥力大于引力；
[0035]
t随机树采用生成随机点的函数，在无人机可飞行区域内随机生成一个采样点q
rand
，并且到搜索树中距离q
rand
最近的节点q
nearest
，结合改进的人工势场函数，在终点对q
nearest
产生引力f
att2
如式9所示，在q
rand
节点上生成对q
nearest
的潜在吸引力f
att1
如式10所示，障碍物对q
nearest
产生斥力f
rep
是不同障碍物对q
nearest
的斥力，根据平行四边形法则得到f
att1
，f
att2
，f
rep
的合力f
total
方向，q
new
沿着合力方向，按照给定的步长l生成新的节点q
new
；
[0036][0037][0038][0039]
其中，n
or
＝
▽
ρ(p,p
obs
)为q
nearest
和障碍物之间的方向矢量；n
rg
＝
‑▽
ρ(p,p
goal
)为q
nearest
和q
goal
的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解q
new
时，需要分别计算q
rand
和q
goal
对q
nearest
的引力，计算障碍物对q
nearest
的斥力，然后将合力f
total
分解为x轴和y轴两个方向的力，分别用f
x
，fy表示；设定q
nearest
的坐标为(xc，yc)，扩展步长由q
nearest
在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为f
max
，则无人机的扩展步长可以确定为：
[0040][0041]
其中，l是无人机的扩展步长，k是比例系数，可得到q
new
的坐标，令q
new
的坐标为
(xc，yc)，则q
new
的坐标方程为：
[0042][0043]
所述s6的具体过程为：
[0044]
以q
nearest
作为检测起点，以q
new
作为检测终点，把q
nearest
和q
new
之间的距离平均分成j段；由式(14)每次生成检测节点q
collision
的位置，计算q
collision
到距离q
collision
最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则q
nearest
和q
new
之间存在障碍物即与障碍物碰撞，重复s3-s5，重新寻一个新节点qnew；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点qnew；
[0045][0046]
k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为q
nearest
到q
new
的方向与x轴的夹角，x
qnearest
和y
qnearest
分别代表q
nearest
的横坐标和纵坐标，x
collision
和y
collision
分别代表求得的q
collision
横坐标和纵坐标。
[0047]
所述s7的具体过程为：
[0048]
t生成新节点后，计算q
new
与目标点q
goal
欧式距离d(q
new
,q
goal
)，检测d是否小于规定的阈值，即检测搜索随机树是否到达目标点，若d不小于规定的阈值则对步骤6输出节点q
new
利用二分法在障碍物附近生成新的父节点替换q
nearest
，并把新的父节点记作q
near
，使得q
new
到起点的路径代价减小，寻新的父节点后，在半径为r1的一个圆域内寻到一个以新节点q
new
作为父节点的子节点，使这个子节点到起点的路径代价减小；寻到子节点后，q
new
被加入到第一棵随机搜索树t中：执行步骤3-步骤6，进行生长；若d小于规定的阈值则q
new
和目标点q
goal
相连接。
[0049]
有益技术效果
[0050]
本发明提出一种基于pf-rrt*算法的无人机航迹规划方法，解决了rrt*算法冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率，优化了路径长度，加快了运算速度，具有良好的可操作性。具有以下有益技术效果：
[0051]
1、在rrt*算法中加入二分法在障碍物附近为新节点创建一个新的父节点，而不是在现有的随机树节点中寻父节点，降低了路径代价，规划出更平滑的轨迹。
[0052]
2、采用改进的apf缩短收敛时间。目标点和随机点分别吸引随机树，最靠近随机树的障碍物排斥随机树，新的节点沿着三种力合力的方向生长。
[0053]
3、结合现有的目标偏置策略。目标点成为具有一定概率的随机点，提高了随机点的质量，从而减少了迭代次数；改进的航迹规划算法解决了rrt*算法冗余点过多、迭代次数过多、路径过长等问题，提高了算法的搜索效率；提出的融合算法优化了路径长度，提高了运算速度，该方法便于实现，具有良好的可操作性。
附图说明
[0054]
图1为本发明实施例提供的一种基于pf-rrt*算法的无人机航迹规划方法流程图；
[0055]
图2为本发明实施例提供的一种基于pf-rrt*算法的无人机航迹规划方法地图示
意图；
[0056]
其中，图2(a)表示简单环境图，图2(b)表示较复杂环境图，图2(c)表示复杂环境图；
[0057]
图3为本发明实施例提供的随机树t生长过程中q
nearest
受力示意图；
[0058]
图4为本发明实施例提供的findnode示意图；
[0059]
图5为本发明实施例提供的createnode示意图；
[0060]
图6为本发明实施例提供的随机搜索树t重新布线示意图；
[0061]
图7为本发明实施例提供的一种基于pf-rrt*算法的无人机航迹规划方法示意图；
[0062]
其中，图7(a)表示本发明在简单环境下查航迹示意图，图7(b)表示本发明在较复杂环境下查航迹示意图，图7(c)表示本发明在复杂环境下查航迹示意图。
[0063]
图8为本发明实施例提供的简单环境仿真对比图；
[0064]
其中，图8(a)表示简单环境仿真对比中rrt*示意图，图8(b)表示简单环境仿真对比中q-rrt*示意图，图8(c)表示简单环境仿真对比中p-rrt*示意图，图8(d)表示简单环境仿真对比中f-rrt*示意图，图8(e)表示简单环境仿真对比中本发明示意图；
[0065]
图9为本发明实施例提供的简单环境仿真数据图；
[0066]
其中，图9(a)表示每种算法在简单环境中迭代次数平均值示意图，图9(b)表示每种算法在简单环境中路径长度平均值示意图，图9(c)表示每种算在简单环境中法运行时间平均值示意图。
[0067]
图10为本发明实施例提供的较复杂环境仿真对比图；
[0068]
其中，图10(a)表示较复杂环境仿真对比中rrt*示意图，图10(b)表示较复杂环境仿真对比中q-rrt*示意图，图10(c)表示较复杂环境仿真对比中p-rrt*示意图，图10(d)表示较复杂环境仿真对比中f-rrt*示意图，图10(e)表示较复杂环境仿真对比中本发明示意图；
[0069]
图11为本发明实施例提供的较复杂环境仿真数据图；
[0070]
其中，图11(a)表示每种算法在较复杂环境中迭代次数平均值示意图，图11(b)表示每种算法在较复杂环境中路径长度平均值示意图，图11(c)表示每种算法在较复杂环境中运行时间平均值示意图。
[0071]
图12为本发明实施例提供的复杂环境仿真对比图；
[0072]
其中，图12(a)表示复杂环境仿真对比中rrt*示意图，图12(b)表示复杂环境仿真对比中q-rrt*示意图，图12(c)表示复杂环境仿真对比中p-rrt*示意图，图12(d)表示复杂环境仿真对比中f-rrt*示意图，图12(e)表示复杂环境仿真对比中本发明示意图；
[0073]
图13为本发明实施例提供的复杂环境仿真数据图；
[0074]
其中，图13(a)表示每种算法在复杂环境中迭代次数平均值示意图，图13(b)表示每种算法在复杂环境中路径长度平均值示意图，图13(c)表示每种算法在复杂环境中运行时间平均值示意图。
具体实施方式
[0075]
下面结合附图和具体实施例对本发明内容做进一步说明；
[0076]
本发明提出一种基于pf-rrt*算法的无人机航迹规划方法，将改进的人工势场函
数与f-rrt*算法相结合，采用改进的人工势场函数引导双向随机搜索树的生长；并且引入了目标偏置策略，在采样过程中获得更高质量的采样点，实现了无人机的航迹规划，解决无人机在执行空战任务中的航迹规划问题，如图1所示，分为以下步骤：
[0077]
步骤1：对无人机航迹规划环境初始化；
[0078]
无人机航迹规划地图设置为100km*100km的二维空间；无人机飞行航迹的起始坐标设为(1,1)，终点设为(90,90)；如图2所示；图中圆圈表示障碍物，无人机无法通过该区域；
[0079]
步骤2：在可飞行区域s
search
内，采用目标偏置策略的概率函数使目标点q
goal
具有一定的概率成为随机采样点。目标偏置策略的概率函数如式18所示；
[0080][0081]
其中rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点q
goal
生长。
[0082]
步骤3：采用改进人工势场函数生成新节点
[0083]
人工势场(apf)包括引力场函数u
att
(p)如式19所示、斥力场函数u
req
(p)如式20所示；设定p是无人机的一个航迹点，则合力势场函数如式21所示：
[0084][0085][0086]utotal
＝∑u
rep
+σu
att
ꢀꢀ
(21)
[0087]
在其中ka是引力场增益常数，kr是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρg(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；引力和斥力的大小分别是引力场函数和斥力场函数的负梯度；如式22和23表示，合力如式24所示：
[0088]
fatt(p)＝kaρg(p)
ꢀꢀ
(22)
[0089][0090]ftotal
＝∑f
rep
+∑f
att
ꢀꢀ
(24)
[0091]
当目标点附近存在障碍物时，无人机受到障碍物的斥力将非常大，而且引力相对较小，无人机很难到达目标点，所以本发明提出改进的引力势场函数如式25所示、斥力场函数如式28所示：
[0092]uatt
(p)＝u
att1
(p)+u
att2
(p)
ꢀꢀ
(25)
[0093]
[0094][0095][0096]
其中，u
att1
(p)和u
att2
(p)分别是q
goal
和q
rand
对q
nearest
的吸引力。ka是目标点产生的引力场增益常数，ρg(p
ner
,p
goal
)是q
nearest
和q
goal
之间的距离，kb是随机点产生的引力场增益常数，ρr(p
ner
,p
rand
)是q
nearest
和q
rand
之间的距离。p
ner
，p
goal
，p
obs
分别代表q
nearest
，q
goal
和障碍物的位置；n是一个正整数，ρ(p
ner
,p
obs
)是q
nearest
距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(p
ner
,p
goal
)的减小而变小，避免斥力大于引力；ρ(p
ner
,p
obs
)是q
nearest
距其最近的障碍物之间的欧氏距离，ρg(p
ner
,p
goal
)是q
nearest
和q
goal
之间的距离；当无人机靠近障碍物时，障碍物产生的斥力会随着ρg(p
ner
,p
goal
)的减小而变小，这样避免了斥力大于引力的现象；
[0097]
流程中iter为迭代次数，maxiter为最大迭代次数，本发明程序达到最大迭代次数，程序将会停止；反之，随机搜索树进行生长；随机树t生长过程中q
nearest
受力示意图如图3所示，首先，t随机树采用目标偏置策略在无人机可飞行区域内生成随机点q
rand
，并且到随机树中距离q
rand
最近的节点q
nearest
；结合改进的人工势场函数，在目标点对q
nearest
产生引力f
att2
如式26所示，在q
rand
节点上生成对q
nearest
的潜在吸引力f
att1
如式27所示，障碍物对q
nearest
产生斥力f
rep
如式29所示。根据平行四边形法则得到f
att1
，f
att2
，f
rep
的合力f
total
方向，q
new
沿着合力方向，按照给定的步长l生成新的节点q
new
；
[0098][0099][0100][0101]
其中，n
or
＝
▽
ρ(p,p
obs
)为q
nearest
和障碍物之间的方向矢量；n
rg
＝
‑▽
ρ(p,p
goal
)为q
nearest
和q
goal
的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解q
new
时，首先，需要分别计算q
rand
和q
goal
对q
nearest
的引力，然后计算障碍物对q
nearest
的斥力，将合力f
total
分解为x轴和y轴两个方向的力，分别用f
x
，fy表示；设定q
nearest
的坐标为(xc，yc)，扩展步长由q
nearest
在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为f
max
，则无人机的扩展步长可以确定为：
[0102]
[0103]
l是无人机的扩展步长，k是比例系数，于是，可得到q
new
的坐标，令q
new
的坐标为(xc，yc)，则q
new
的坐标方程为：
[0104][0105]
步骤4：检测是否碰撞；
[0106]
检测q
nearest
和q
new
之间是否与障碍物碰撞，若q
nearest
和q
new
之间存在障碍物即与障碍物碰撞，则重复步骤2，重新寻一个新节点q
new
；反之则利用二分法为新节点创建新的父节点的过程；
[0107]
步骤5：为新节点创建新的父节点
[0108]
如图4所示的findnode示意图；在障碍物附近，首先要到q
new
的父节点q
nearest
。然后在障碍物附近到可以被优化的q
nearest
的父节点，把该点命名为q
reachest
，连接q
reachest
和q
new
。当q
reachest
的父节点parent(q
reachest
)不存在时，q
nearest
作为q
reachest
；如图5所示的createnode示意图；在创建节点的过程中，到q
reachest
后，在parent(q
reachest
))和新节点q
new
之间创建一个新节点q
create
，进一步优化了路径代价；并且引入参数d
dichotomy
作为二分法寻q
create
的终止条件。
[0109]
步骤6：重新布线，如图6所示的随机搜索树t1的更新子节点的示意图；首先以新节点q
new
为圆心，依据设定的半径画一个圆，将这个圆内的所有生长树上的节点作为新的子节点的备选节点；分别计算圆内的备选节点的原路径代价和备选节点以新节点q
new
为父节点到q
start
的欧氏距离之和；在图6(a)中，节点标号表示产生该节点的顺序，0节点是起点，9节点是新产生的节点q
new
，6节点是9节点的父节点q
nearest
，节点与节点之间连接的边上数字代表两个节点之间的欧氏距离；在创建q
reachest
后，再进行更新子节点：首先计算集合圆中每一个节点以q
new
(9节点)为父节点(q
nearest
除外)，并连接至q
start
(0节点)的总代价，若以q
new
为父节点的总代价小于不以q
new
为父节点的总代价，则修改该节点的父节点为q
new
，如图6(a)所示，在q
new
的所有节点中，当节点6以q
new
为父节点时，连接至q
start
的路径为6-9-5-1-0，路径总代价为12，而节点6不以q
new
为父节点时，例如连接至q
start
的路径为6-4-0，路径总代价为15，故修剪随机树，将节点6的父节点改为q
new
，如图6(b)所示；
[0110]
步骤7：随机树是否生长到目标节点
[0111]
t树对q
new
重新布线后，检测是否连接，若连接则执行步骤7；反之则t树重复步骤1-5进行生长；随机树在地图中生长过程如图7(a)、图7(b)及图7(c)所示，随机搜索树节点和节点之间的连接用浅线表示；随机搜索树节点和节点之间的连接用浅线表示；生成路径用深线表示；
[0112]
步骤8：随机树t生长到目标点后，得到一条由多个点构成的一条光滑的航迹，解决无人机飞行中转弯角度过大的问题；
[0113]
为进一步对本发明做详细说明，在简单环境中各算法的仿真结果如图8所示，深线为算法规划的无人机可行轨迹；其中无人机的可行轨迹用深线表示。在图8(a)中，由于rrt*在空间中随机采样，路径中出现了更多的拐点，得到了更长的路径。图8(b)显示了深度为2的q-rrt*的仿真图。与rrt*相比，其规划点路径拐点较少，且部分节点具有相同的父节点，因此路径更好。在图8(c)中，引入了rrt*和人工势场方法，使路径更加平滑。在图8(d)
中，f-rrt*算法规划了一条更短的路径。针对图8(e)环境简单的特点，改进的算法具有路径短、节点少等优点，能够满足无人机的实时性要求。
[0114]
每个算法在简单环境中计算初始解的仿真数据包括30次实验的平均运行时间、平均路径长度和平均迭代次数。在表1中，本发明30次实验数据的平均长度为126.4197，平均运行时间为4.3924，平均迭代次数为45次。与f-rrt*算法相比，所生成轨迹的平均长度减少了0.17％，平均迭代次数减少了66.16％。从图9中可以清楚地看到，尽管本文中算法的运行时间和迭代次数本质上与p-rrt*算法相同，路径更平滑。结果表明，在简单的环境下本发明的路径开代价大大降低，并且大大减少了算法迭代次数。
[0115]
表1简单环境中30次实验数据平均值
[0116][0117]
在较复杂的环境中，存在较多的障碍物，仿真结果如图10所示。与简单环境相比，这五种算法在较复杂的环境中具有更多的拐点。本发明规划的航迹优势依然明显。
[0118]
为了证明本发明具有更好的性能，在较复杂的环境中对每种算法获取30次实验结果，并记录了算法的运行时间、生成路径的长度和迭代次数。实验数据显示在表2和图11中。通过对图11分析，与其他算法相比，本发明的路径更短，运行时间更短，迭代次数更少。在表2中，本发明的平均路径长度为127.0054，平均运行时间为4.6533，平均迭代次数为133。与f-rrt*相比，平均运行时间和平均迭代次数分别减少了0.17％和24.86％。结合数据比较表明，本发明能够获得较好的路径。
[0119]
表2较复杂环境中30次实验数据平均值
[0120][0121]
在障碍物较为复杂环境中，本发明的优势更加明显；与简单环境，较复杂环境相比，复杂环境中障碍物更多，且大小不一，无人机航迹规划困难。但本发明仍然可以在复杂环境下更高效地规划轨迹，其优势是非常明显的。如图12所示，算法具有更少的冗余点和更平滑的路径；
[0122]
通过对表3的分析，本发明在复杂环境下生成初始解的平均路径长度为126.9231，平均运行时间为9.1732，平均迭代次数为52次，与f-rrt*算法相比，平均路径长度和迭代次数分别减少了0.24％和65.33％。对比图13，本发明虽然重新创建了新节点的父节点，但并不消耗太多时间，其平均运行时间与p-rrt*相似，这些数据表明本发明的性能指标有了明显的提高。因此，本发明规划的航迹迹是优越的。
[0123]
表3复杂环境中30次实验数据平均值
[0124][0125]
本发明考虑了完整路径的代价高、算法计算量大等因素的影响，实现了无人机飞行过程中算法收敛速度快，路径代价低动态航迹规划研究；因此，该方法能更灵活更快速地
生成路径代价更小的路径，提高了搜索效率；在以上分析的基础上，可以看出提出的方法可以使无人机在飞行方面更加的符合实际，即本发明设计的方法是有效的。

技术特征：

1.一种基于pf-rrt*算法的无人机航迹规划方法，其特征在于：包括以下步骤：s1：规划无人机的飞行环境s，飞行环境s包括可飞行区域s
search
，障碍区域s
obs
；设置无人机航迹规划的起点q
start
、终点q
goal
、步长l；s2：在飞行环境s中，以q
start
为根节点，创建快速搜索随机树t，随机树t只有1个初始节点q
start
，为随机树t中的初始节点创建基于位置的索引，父节点，父节点索引；s3：在可飞行区域s
search
内，采用目标偏置策略的概率函数使目标点q
goal
具有一定的概率成为随机采样点；s4：选取t树中距离q
rand
最近的节点q
nearest
，初始的节点q
nearest
为q
start
；s5：采用改进的人工势场函数使q
rand
和q
goal
分别对q
nearest
产生潜在的引力，使障碍物对q
nearest
生成一个潜在的斥力，q
nearest
按照给定的步长l沿着三个力的合力方向生成新的节点q
new
；s6：检测q
nearest
和q
new
之间是否与障碍物碰撞；s7：t生成新节点后，检测搜索随机树是否到达目标点，若未到达目标点则对s6输出节点q
new
利用二分法在障碍物附近生成新的父节点替换q
nearest
，并对新节点q
new
重新布线；q
new
被加入到随机搜索树t中：执行s3-s6，进行生长；若到达目标点则q
new
和目标点q
goal
相连接；s8：q
new
和目标点q
goal
连接后，得到一条由多个点构成的一条航迹。2.根据权利要求1所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述目标偏置策略的概率函数如式(1)所示：其中，rand()是0-1间的随机数，m是设定的目标偏置阈值，根据概率m的值来选择树的生长方向，当rand()值大于m时，随机树在采样空间中随机生长，反之，生长树则朝向目标点q
goal
生长。3.根据权利要求1所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述人工势场(apf)包括引力场函数u
att
(p)如式(2)所示、斥力场函数u
req
(p)如式(3)所示，则合力势场函数如式(4)所示：所示，则合力势场函数如式(4)所示：u
total
＝σu
rep
+∑u
att
ꢀꢀꢀꢀ
(4)其中，p是无人机的一个航迹点，k
a
是引力场增益常数，k
r
是斥力场增益常数，ρ0是航迹点p距离威胁范围的最大安全距离，ρ
g
(p)和ρ(p)分别表示点p与目标点和最近威胁中心的欧氏距离；其中，引力和斥力的大小分别是引力场函数和斥力场函数的负梯度，如式(5)和(6)表示，合力如式(7)所示：fatt(p)＝k
a
ρ
g
(p)
ꢀꢀꢀꢀ
(5)
f
total
＝∑f
rep
+∑f
att
ꢀꢀꢀꢀ
(7)通过改进的引力势场函数、斥力场函数使无人机到达目标点。4.根据权利要求3所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述改进的引力势场函数如式(8)所示、改进的斥力场函数如式(11)所示：u
att
(p)＝u
att1
(p)+u
att2
(p)
ꢀꢀꢀꢀ
(8)其中：其中：其中：5.根据权利要求3所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述改进的斥力场函数如式(11)所示：其中，u
att1(p)
和u
att2(p)
分别是q
goal
和q
rand
对q
nearest
的吸引力；k
a
是目标点产生的引力场增益常数，ρ
g
(p
ner
,p
goal
)是q
nearest
和q
goal
之间的距离，k
b
是随机点产生的引力场增益常数，ρ
r
(p
ner
,p
rand
)是q
nearest
和q
rand
之间的距离；p
ner
，p
goal
，p
obs
分别代表q
nearest
，q
goal
和障碍物的位置；n为正整数，ρ(p
ner
,p
obs
)是q
nearest
距其最近的障碍物之间的欧氏距离，当无人机靠近障碍物时，障碍物产生的斥力会随着ρ
g
(p
ner
,p
goal
)的减小而变小，避免斥力大于引力；t随机树采用生成随机点的函数，在无人机可飞行区域内随机生成采样点q
rand
，并且到搜索树中距离q
rand
最近的节点q
nearest
，结合改进的人工势场函数，在终点对q
nearest
产生引力f
att2
如式9所示，在q
rand
节点上生成对q
nearest
的潜在吸引力f
att1
如式10所示，障碍物对q
nearest
产生斥力f
rep
是不同障碍物对q
nearest
的斥力，根据平行四边形法则得到f
att1
，f
att2
，f
rep
的合力f
total
方向，q
new
沿着合力方向，按照给定的步长l生成新的节点q
new
；；；
其中，为q
nearest
和障碍物之间的方向矢量；为q
nearest
和q
goal
的方向矢量；当无人机接近目标点时，与其最近的障碍物之间的斥力就会变小，可以保证无人机能够到达目标点；在求解q
new
时，需要分别计算q
rand
和q
goal
对q
nearest
的引力，计算障碍物对q
nearest
的斥力，然后将合力f
total
分解为x轴和y轴两个方向的力，分别用f
x
，f
y
表示；设定q
nearest
的坐标为(x
c
，y
c
)，扩展步长由q
nearest
在x，y轴上受到的合力的分量决定；选择每个坐标轴上的合力分量最大值的绝对值作为f
max
，则无人机的扩展步长为：其中，l是无人机的扩展步长，k是比例系数，可得到q
new
的坐标，令q
new
的坐标为(x
c
，y
c
)，则q
new
的坐标方程为：6.根据权利要求1所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述s6的具体过程为：以q
nearest
作为检测起点，以q
new
作为检测终点，把q
nearest
和q
new
之间的距离平均分成j段；由式(14)每次生成检测节点q
collision
的位置，计算q
collision
到距离q
collision
最近的圆形障碍物圆心的欧式距离，若此欧式距离小于圆形障碍物的半径，则q
nearest
和q
new
之间存在障碍物即与障碍物碰撞，重复s3-s5，重新寻一个新节点q
new
；若此欧式距离大于圆形障碍物的半径，反之则输出当前的节点q
new
；k的初始值为k1，每次增加k2，增加到j停止，r为检测步长，θ为q
nearest
到q
new
的方向与x轴的夹角，x
qnearest
和y
qnearest
分别代表q
nearest
的横坐标和纵坐标，x
collision
和y
collision
分别代表求得的q
collision
横坐标和纵坐标。7.根据权利要求1所述的基于pf-rrt*算法的无人机航迹规划方法，其特征在于：所述s7的具体过程为：t生成新节点后，计算q
new
与目标点q
goal
欧式距离d(q
new
,q
goal
)，检测d是否小于设定的阈值，检测搜索随机树是否到达目标点，若d不小于规定的阈值则对步骤6输出节点q
new
利用二分法在障碍物附近生成新的父节点替换q
nearest
，并把新的父节点记作q
near
，使得q
new
到起点的路径代价减小，寻新的父节点后，在半径为r1的一个圆域内寻到一个以新节点q
new
作为父节点的子节点；寻到子节点后，q
new
被加入到第一棵随机搜索树t中：执行步骤3-步骤6，进行生长；若d小于规定的阈值则q
new
和目标点q
goal
相连接。

技术总结

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