CN107608372A - 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法 - Google Patents

Abstract

本发明公开了一种基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，其特征是，包括如下步骤：1）环境建模；2）规划单机航迹集；3）规划多机协同航迹。这种方法结合多无人机初末速度方向、能加快多无人机航迹规划算法速度、可以节约无人机燃油、能平滑无人机航程航迹，在多无人机协同航迹规划中具有广泛的应用前景。

Description

一种基于改进RRT算法与改进PH曲线相结合的多无人机协同 航迹规划方法

技术领域

本发明涉及无人机航迹搜索领域，具体是一种基于改进快速扩展随机树RRT(Rapidly-exploring Random Tree，简称RRT)算法与改进毕达哥拉斯矢端曲线(Pythagorean Hodograph，简称PH曲线)相结合的多无人机协同航迹规划方法。

背景技术

快速扩展随机树RRT算法第一次提出是在1998年由美国科学S.M.LaValle提出，其基 本原理是在任务空间内以节点生长的方式构建随机树，主要优点是能够在复杂环境中快速找 到路径规划问题的可行解。

毕达哥拉斯矢端PH曲线,是由Farouki和Sakkalis在1990年研究等距曲线的过程中率 先提出，他们将PH曲线定义为多项式曲线的形式，并且PH曲线的矢端矢量满足勾股条件。

随着环境和任务的日益复杂性，单架无人机已经不法完成所需任务需求，需要通过多架 无人机合作来完成，多无人机协同是指多架无人机通过采取协作的方式完成某项任务。航迹 规划是最大限度的利用地形信息，综合考虑无人机导航精度和机动能力的限制，在最短的时 间内计算出从现在位置到指定位置的最优或次优的飞行轨迹，能使无人机回避威胁环境，安 全地完成预定任务。

假设N架无人机分别处于不同起始位置S₁，S₂，….，S_N,要求在同一时刻t₁到达同一目 标点D，实施打击且代价最小，协同航迹问题多假设为多机同时到达问题，通常使用两种方 法使得无人机能够同时到达目标：一种为通过调节无人机的飞行速度，航程较大的无人机的 速度较大，航程较小的无人机的速度较小；另一种是修正航迹长度，使得每架无人机的航程 大致相同。

发明内容

本发明的目的针对现有技术的不足，而提供一种基于改进RRT算法与改进PH曲线相结合 的多无人机协同航迹规划方法。这种方法结合多无人机初末速度方向、能加快多无人机航迹 规划算法速度、可以节约无人机燃油、能平滑无人机航程航迹，在多无人机协同航迹规划中 具有广泛的应用前景。

实现本发明目的的技术方案是：

一种基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，包括如下步 骤：

1)环境建模：假设无人机进行等高飞行，结合无人机飞行中自然威胁分布构建环境模型， 自然威胁主要是山川地形威胁；

2)规划单机航迹集：首先根据起始点和终止点，运用改进PH曲线算法，得到6个控制 点，分别为b₀,b₁,b₂,b₃,b₄,b₅；然后选取规划得到的6个控制点中的b₂和b₃作为新起始点和新 终止点，并在新起始点和新终止点之间多次运用改进RRT算法规划得到单机航迹集；

3)规划多机协同航迹：根据多无人机协同航迹规划原理以及多无人机协同航迹规划的代 价函数，在规划航迹集中选取一条符合多无人机协同规划原理以及航迹代价函数的最小航迹。

步骤1)中所述自然威胁为：某飞行高度上对无人机飞行有可能造成威胁的山峰和高地 在水平截图面上的截面是圆周，则

其中，d_Tmin表示地形所允许的最靠近距离，d_Tmax表示地形能影响的最远距离，d_T为某 一时刻无人机距离地形中心轴距离。

步骤2)包括：

(1)根据任务要求的起始点和终止点，运用改进PH曲线算法，得到6个控制点，分别为 b₀,b₁,b₂,b₃,b₄,b₅，具体为：

PH曲线贝塞尔(Bezier)形式的多项式为

路径r(q)的一阶导数为

采用艾米特(Hermite)插值法，确定基准路径的形状，起始点和终止点分别为(x_s,y_s)和

(x_f,y_f)，Bezier曲线的6个控制点b₀,b₁,b₂,b₃,b₄,b₅为

其中：φ_s表示起始点方位角，φ_R表示无人机转弯角，φ_f表示终止点方位角，常数m₀∈[1,+∞]， 常数m₃∈[1,+∞]，常数m₅∈[1,+∞]；

(2)选取步骤(1)得到的6个控制点中的b₂和b₃作为新的起始点和新的终止点，并在新起 始点和新终止点之间运用改进RRT算法规划得到单无人机航迹集，由于RRT算法具有随机性， 所以运用改进RRT算法得到三条航迹集，具体为：

经典RRT算法在选择q_rand时需要遍历当前随机树的所有节点并且计算相应的欧式距离， 因此减少q_rand的选取次数，可以降低计算运行时间，运用基于威胁值和航路代价构建随机点 空间的概率矩阵作为航迹规划的先验知识，然后使用轮盘赌算法优先拓展概率大(适应度值 高)的随机点，设置随机搜索空间的航路代价值如公式(5)所示：

J_L＝K*[(A+B)/C]² (5)

其中：K为常数，A为起点到路径点的距离，B为目标点带路径点的距离，C为起点到目标点 的距离；

设置随机搜索空间威胁代价值如公式(6)所示：

J_T＝K/(D-d)² (6)

其中：K为常数，D为路径上点到障碍物距离，d为绝对杀伤区的距离；

设置随机点的适应度值如公式(7)所示：

P_ij＝1/(λ₁*J_L+λ₂+J_T) (7)

其中：P_ij表示点(i，j)的适应度值，λ₁、λ₂分别表示J_L和J_T的权重系数；

使用轮盘赌算法优先拓展概率大(适应度值高)的随机点，个体被选中的概率与其适应 度函数值成正比，群体全部个体的适当度值由一张轮盘来代表设群体大小为n，个体i的适 应度为F_i，则个体i被选中的概率如公式(8)所示：

(3)航迹平滑处理：采用平滑算法与滑窗滤波相结合的形式对单无人机航迹进行航迹平滑处 理，首先运用平滑算法，从起始点开始，每三个航迹点考虑一次最小转弯半径约束，如果不 满足约束条件，即单机最小转弯半径，则删除三个航迹点的中间节点，插入满足约束的临界 节点，虽然平滑算子解决了转弯半径约束的问题，但没有解决航段出现左右波动的问题，因 此，接着采用滑动平均法滤波，滤波法是指通过对各航迹节点进行滤波处理，削弱波动性， 滑动平均是对一定长度移动窗口的数据进行加权平均，设第k个航迹点为v(k)，对其前后各 i个点加权平均得u(k)：

其中，W为常数是窗口宽度，w(j)是窗口加权函数。

步骤3)所述的规划多机协同航迹具体为：

路径总长度在无法躲避的障碍物中的路径长度是航迹规划结果的重要指标，设f_ij为路径 在威胁区代价，D_ij为路径总长度燃油消耗代价，假设共有N架无人机，每架无人机单独规划 出M条备选航迹，每一架无人机的速度v_i∈[v_min v_max]，那么T∈[L_i/v_max L_i/v_min]，其中L_i是某 架无人机的第i条航迹；对于某架无人机的M条航迹，预计到达目标的时间则是M个时间范 围的并集S_t，那么N架无人机共同到达的时间交集S＝S_t，1I S_t,2I K I S_t,n，那么多无人机协同 航迹规划的代价函数如公式(10)：

J＝min(λ₁∑(w_kf_ij+w_DD_ij)+λ₂T) (10)

其中，J为航路总代价值；λ₁，λ₂为系数；w_k>0为安全代价权，w_D>0为路径长度代 价权；f_ij为安全代价；D_ij为航线段的长度，作为燃油消耗的代价指标；T∈S。

这种方法与现有技术相比的优点在于：

(1)利用了改进PH曲线的曲率连续，曲线平滑以及有理特性，充分考虑无人机初、末 速度方向，可实现无人机路径规划，具有弯曲能量小，曲线上曲率和长度均有闭合解等优点；

(2)改进RRT算法省去了选取随机点时太多的无用空间搜索，缩短了航迹航程，节约 了燃油燃料，且能成功躲避障碍物；

(3)采用了平滑算法和滑窗滤波相结合的航迹平滑处理方法，能够很好的解决无人机 转弯半径约束的问题且削弱了航段出现左右波动的问题。

这种方法结合多无人机初末速度方向、能加快多无人机航迹规划算法速度、可以节约无 人机燃油、能平滑无人机航程航迹，在多无人机协同航迹规划中具有广泛的应用前景。

附图说明

图1为实施例中的方法流程示意图；

图2为实施例中的山川地形威胁示意图；

图3为实施例中的随机搜索空间航路代价值示意图；

图4为实施例中的随机搜索空间威胁代价值示意图；

图5为实施例中的轮盘赌选择示意图；

图6为实施例中的航迹平滑处理示意图；

图7为实施例中的经典RRT算法的多机单目标备选航迹集仿真图；

图8为实施例中的经典RRT算法最优飞行航迹与飞行速度仿真图；

图9为实施例中的改进RRT算法和改进PH曲线相结合算法的单目标备选航迹集仿真图；

图10为实施例中改进RRT算法和改进PH曲线相结合算法的最优飞行航迹与飞行速度仿 真图。

具体实施方式

下面结合附图和实施例对本发明内容作进一步的阐述，但不是对本发明的限定。

参照图1，一种基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法， 包括如下步骤：

1)环境建模：假设无人机进行等高飞行，结合无人机飞行中自然威胁分布构建环境模型， 自然威胁主要是山川地形威胁；

2)规划单机航迹集：首先根据起始点和终止点，运用改进PH曲线算法，得到6个控制 点，分别为b₀,b₁,b₂,b₃,b₄,b₅；然后选取规划得到的6个控制点中的b₂和b₃作为新起始点和新 终止点，并在新起始点和新终止点之间多次运用改进RRT算法规划得到单机航迹集；

3)规划多机协同航迹：根据多无人机协同航迹规划原理以及多无人机协同航迹规划的代 价函数，在规划航迹集中选取一条符合多无人机协同规划原理以及航迹代价函数的最小航迹。

步骤1)中所述自然威胁为：某飞行高度上对无人机飞行有可能造成威胁的山峰和高地 在水平截图面上的截面是圆周，如图2所示，

其中，d_Tmin表示地形所允许的最靠近距离，d_Tmax表示地形能影响的最远距离，d_T为某 一时刻无人机距离地形中心轴距离。

步骤2)包括：

(1)根据任务要求的起始点和终止点，运用改进PH曲线算法，得到6个控制点，分别为 b₀,b₁,b₂,b₃,b₄,b₅，具体为：

PH曲线贝塞尔(Bezier)形式的多项式为

路径r(q)的一阶导数为

采用艾米特(Hermite)插值法，确定基准路径的形状，起始点和终止点分别为(x_s,y_s)和

(x_f,y_f)，Bezier曲线的6个控制点b₀,b₁,b₂,b₃,b₄,b₅为

其中：φ_s表示起始点方位角，φ_R表示无人机转弯角，φ_f表示终止点方位角，常数m₀∈[1,+∞]， 常数m₃∈[1,+∞]，常数m₅∈[1,+∞]；

(2)选取步骤(1)得到的6个控制点中的b₂和b₃作为新的起始点和新的终止点，并在新起 始点和新终止点之间运用改进RRT算法规划得到单无人机航迹集，由于RRT算法具有随机性， 所以运用改进RRT算法得到三条航迹集，具体为：

经典RRT算法在选择q_rand时需要遍历当前随机树的所有节点并且计算相应的欧式距离， 因此减少q_rand的选取次数，可以降低计算运行时间，运用基于威胁值和航路代价构建随机点 空间的概率矩阵作为航迹规划的先验知识，然后使用轮盘赌算法优先拓展概率大(适应度值 高)的随机点。如图3所示，设置随机搜索空间航路代价值如公式(5)所示：

J_L＝K*[(A+B)/C]² (5)

其中：K为常数，A为起点到路径点的距离，B为目标点带路径点的距离，C为起点到目标点 的距离；

如图4所示，设置随机搜索空间威胁代价值如公式(6)所示：

J_T＝K/(D-d)² (6)

其中：K为常数，D为路径上点到障碍物距离，d为绝对杀伤区的距离；

设置随机点的适应度值如公式(7)所示：

P_ij＝1/(λ₁*J_L+λ₂+J_T) (7)

其中：P_ij表示点(i，j)的适应度值，λ₁、λ₂分别表示J_L和J_T的权重系数；

如图5所示，使用轮盘赌算法优先拓展概率大(适应度值高)的随机点，个体被选中的 概率与其适应度函数值成正比，群体全部个体的适当度值由一张轮盘来代表设群体大小为n， 个体i的适应度为F_i，则个体i被选中的概率如公式(8)所示：

(3)航迹平滑处理：如图6所示，采用平滑算法与滑窗滤波相结合的形式对单机航迹进行航 迹平滑处理，首先运用平滑算法，从起始点开始，每三个航迹点考虑一次最小转弯半径约束， 如果不满足约束条件，即单机最小转弯半径，则删除三个航迹点的中间节点，插入满足约束 的临界节点，虽然平滑算子解决了转弯半径约束的问题，但没有解决航段出现左右波动的问 题，因此，接着采用滑动平均法滤波，滤波法是指通过对各航迹节点进行滤波处理，削弱波 动性，滑动平均是对一定长度移动窗口的数据进行加权平均，设第k个航迹点为v(k)，对其 前后各i个点加权平均得u(k)：

其中，W为常数是窗口宽度，w(j)是窗口加权函数。

步骤3)所述的规划多机协同航迹具体为：

路径总长度在无法躲避的障碍物中的路径长度是航迹规划结果的重要指标，设f_ij为路径 在威胁区代价，D_ij为路径总长度燃油消耗代价，假设共有N架无人机，每架无人机单独规划 出M条备选航迹，每一架无人机的速度v_i∈[v_min v_max]，那么T∈[L_i/v_max L_i/v_min]，其中L_i是某 架无人机的第i条航迹；对于某架无人机的M条航迹，预计到达目标的时间则是M个时间范 围的并集S_t，那么N架无人机共同到达的时间交集S＝S_t，1∩S_t,2∩...∩S_t,n，那么多无人机协同 航迹规划的代价函数如公式(10)：

J＝min(λ₁∑(w_kf_ij+w_DD_ij)+λ₂T) (10)

其中，J为航路总代价值；λ₁，λ₂为系数；w_k>0为安全代价权，w_D>0为路径长度代 价权；f_ij为安全代价；D_ij为航线段的长度，作为燃油消耗的代价指标；T∈S。

参照图2，本例中的环境建模如下

某飞行高度上对无人机飞行有可能造成威胁的山峰和高地在水平截图面上的截面是圆 周，则

其中，d_Tmin表示地形所允许的最靠近距离，d_Tmax表示地形能影响的最远距离，d_T为某 一时刻无人机距离地形中心轴距离。

参照图3，本例中设置随机搜索空间的航路代价值如下所示：

J_L＝K*[(A+B)/C]²

其中：K为常数，A为起点到路径点的距离，B为目标点带路径点的距离，C为起点到目 标点的距离；

参照图4，本例中设置随机搜索空间威胁代价值如公式下所示：

J_T＝K/(D-d)²

其中：K为常数，D为路径上点到障碍物距离，d为绝对杀伤区的距离；

参照图5，本例中使用轮盘赌算法优先拓展概率大(适应度值高)的随机点，个体被选 中的概率与其适应度函数值成正比，群体全部个体的适当度值由一张轮盘来代表设群体大小 为n，个体i的适应度为F_i，则个体i被选中的概率如下所示：

参照图6，本例采用平滑算法与滑窗滤波相结合的形式对单机航迹进行航迹平滑处理， 首先运用平滑算法，从起始点开始，每三个航迹点考虑一次最小转弯半径约束，如果不满足 约束条件，即单机最小转弯半径，则删除三个航迹点的中间节点，插入满足约束的临界节点， 虽然平滑算子解决了转弯半径约束的问题，但没有解决航段出现左右波动的问题，因此，接 着采用滑动平均法滤波，滤波法是指通过对各航迹节点进行滤波处理，削弱波动性，滑动平 均是对一定长度移动窗口的数据进行加权平均，设第k个航迹点为v(k)，对其前后各i个点 加权平均得u(k)：

其中，W为常数是窗口宽度，w(j)是窗口加权函数。

参照图7和图8，设定三架无人机分别从不同位置起飞，同时到达同一终点。采用RRT 算法规划多无人机协同航迹规划，如图7所示，三架无人机分别规划三条不同航迹；如图8 所示，依据多无人机协同航迹规划原理和多无人机协同航迹规划代价函数，得到每一架无人 机的协同航迹。

参照图9和图10，设定三架无人机分别从不同位置起飞，同时到达同一终点。采用本例 采取的方案规划多无人机协同航迹规划，选取初、末速度方向为90°。如图9所示，三架无人 机分别规划三条不同航迹；如图10所示，依据多无人机协同航迹规划原理和多无人机协同航 迹规划代价函数，得到每一架无人机的协同航迹。

参照表1和表2，表1为实施例中的基于RRT算法的多无人机协同航迹规划协调预计到 达时间、协调速度、航迹长度，表2为实施例中的基于改进PH曲线和改进RRT算法相结合的多无人机协同航迹规划协调预计到达时间、协调速度、航迹长度。

仿真结果表明：改进算法的多机协同到达时间(ETA)缩短了17.88％，UAV1航迹长度缩 短了5.22％，UAV2航迹长度缩短了20.25％，UAV3航迹长度缩短了13.60％。

本例的改进算法与RRT算法仿真结果及分析：

设定三架无人机分别从不同位置起飞，同时到达同一终点。采用RRT算法规划多无人机 协同航迹规划，如图7所示，三架无人机分别规划三条不同航迹；如图8所示，依据多无人 机协同航迹规划原理和多无人机协同航迹规划代价函数，得到每一架无人机的协同航迹；

设定三架无人机分别从不同位置起飞，同时到达同一终点。采用本例采取的方案规划多 无人机协同航迹规划，选取初、末速度方向为90°，如图9所示，三架无人机分别规划三条 不同航迹；如图10所示，依据多无人机协同航迹规划原理和多无人机协同航迹规划代价函数， 得到每一架无人机的协同航迹；

通过将本例的改进算法与RRT算法进行仿真对比，仿真结果表明：改进算法的多机协同 到达时间(ETA)缩短了17.88％，UAV1航迹长度缩短了5.22％，UAV2航迹长度缩短了20.25％， UAV3航迹长度缩短了13.60％。如表1，2所示，具有较好的应用价值。

表1

表2。

Claims (4)

1.一种基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，其特征是，包括如下步骤：

1)环境建模：假设无人机进行等高飞行，结合无人机飞行中自然威胁分布构建环境模型，自然威胁主要是山川地形威胁；

2)规划单机航迹集：首先根据起始点和终止点，运用改进PH曲线算法，得到6个控制点，分别为b₀,b₁,b₂,b₃,b₄,b₅；然后选取规划得到的6个控制点中的b₂和b₃作为新起始点和新终止点，并在新起始点和新终止点之间多次运用改进RRT算法规划得到单机航迹集；

3)规划多机协同航迹：根据多无人机协同航迹规划原理以及多无人机协同航迹规划的代价函数，在规划航迹集中选取一条符合多无人机协同规划原理以及航迹代价函数的最小航迹。

2.根据权利要求1所述的基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，其特征是，步骤1)中所述自然威胁为：某飞行高度上对无人机飞行有可能造成威胁的山峰和高地在水平截图面上的投影截面是圆周，则

其中，d_Tmin表示地形所允许的最靠近距离，d_Tmax表示地形能影响的最远距离，d_T为某一时刻无人机距离地形中心轴距离。

3.根据权利要求1所述的基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，其特征是，步骤2)包括：

(1)根据任务要求的起始点和终止点，运用改进PH曲线算法，得到6个控制点，分别为b₀,b₁,b₂,b₃,b₄,b₅，具体为：

PH曲线贝塞尔(Bezier)形式的多项式为

路径r(q)的一阶导数为

采用艾米特(Hermite)插值法，确定基准路径的形状，起始点和终止点分别为(x_s,y_s)和(x_f,y_f)，Bezier曲线的6个控制点b₀,b₁,b₂,b₃,b₄,b₅为

其中：φ_s表示起始点方位角，φ_R表示无人机转弯角，φ_f表示终止点方位角，常数m₀∈[1,+∞]，常数m₃∈[1,+∞]，常数m₅∈[1,+∞]；

(2)选取步骤(1)得到的6个控制点中的b₂和b₃作为新的起始点和新的终止点，并在新的起始点和新的终止点之间运用改进RRT算法规划得到单无人机的航迹集，由于RRT算法具有随机性，所以三次运用改进RRT算法得到三条航迹作，并将三条单无人机航迹作为一个单无人机航迹集，具体为：

设置随机搜索空间的航路代价值如公式(5)所示：

J_L＝K*[(A+B)/C]² (5)

其中：K为常数，A为起点到路径点的距离，B为目标点带路径点的距离，C为起点到目标点的距离；

设置随机搜索空间威胁代价值如公式(6)所示：

J_T＝K/(D-d)² (6)

其中：K为常数，D为路径上点到障碍物距离，d为绝对杀伤区的距离；

设置随机点的适应度值如公式(7)所示：

P_ij＝1/(λ₁*J_L+λ₂+J_T) (7)

其中：P_ij表示点(i，j)的适应度值，λ₁、λ₂分别表示J_L和J_T的权重系数；

使用轮盘赌算法优先拓展概率大即适应度值高的随机点，个体被选中的概率与其适应度函数值成正比，群体全部个体的适当度值由一张轮盘来代表设群体大小为n，个体i的适应度为F_i，则个体i被选中的概率如公式(8)所示：

(3)航迹平滑处理：采用平滑算法与滑窗滤波相结合的形式对单无人机航迹进行航迹平滑处理，首先运用平滑算法，从起始点开始，每三个航迹点考虑一次最小转弯半径约束，如果不满足约束条件，即单无人机最小转弯半径，则删除三个航迹点的中间节点，插入满足约束的临界节点，接着采用滑动平均法滤波，滤波法是指通过对各航迹节点进行滤波处理，削弱波动性，滑动平均是对一定长度移动窗口的数据进行加权平均，设第k个航迹点为v(k)，对其前后各i个点加权平均得u(k)：

其中，W为常数是窗口宽度，w(j)是窗口加权函数。

4.根据权利要求1所述的基于改进RRT算法与改进PH曲线相结合的多无人机协同航迹规划方法，其特征是，步骤3)所述的规划多机协同航迹具体为：

设f_ij为路径在威胁区代价，D_ij为路径总长度燃油消耗代价，假设共有N架无人机，每架无人机单独规划出M条备选航迹，每一架无人机的速度v_i∈[v_min v_max]，那么T∈[L_i/v_max L_i/v_min]，其中L_i是某架无人机的第i条航迹；对于某架无人机的M条航迹，预计到达目标的时间则是M个时间范围的并集S_t，那么N架无人机共同到达的时间交集S＝S_t，1I S_t,2I K I S_t,n，那么多无人机协同航迹规划的代价函数如公式(10)：

J＝min(λ₁∑(w_kf_ij+w_DD_ij)+λ₂T) (10)

其中，J为航路总代价值；λ₁，λ₂为系数；w_k>0为安全代价权，w_D>0为路径长度代价权；f_ij为安全代价；D_ij为航线段的长度，作为燃油消耗的代价指标；T∈S。