CN113050684B - 一种面向突发威胁的无人机航迹规划算法 - Google Patents

Abstract

本发明公开了一种面向突发威胁的无人机航迹规划算法。该算法在Dubins路径的基础上建立路径扩展点评估函数，通过引入路径长度评估因子和威胁评估因子对路径扩展点进行选择，从而可以有效的降低路径搜索点的数量。同时结合启发式搜索的思想，对可能出现的路径长度代价和威胁代价进行评估，达到了缩短路径长度的目的。仿真表明，多因素Dubins算法在突发威胁场景下，可以规划出较短的路径，同时相比于传统的航迹规划算法具有较少的路径搜索点，并且得到的路径符合无人机实际飞行时的航向变化。

Description

一种面向突发威胁的无人机航迹规划算法

技术领域

本发明属于无人机领域，特别涉及突发威胁场景中的无人机航迹规划算法。

背景技术

无人机航迹规划方法大致分为两类：一类是全局路径规划或称为离线路径规划，另一类是局部路径规划或称为在线路径规划。全局路径规划方法通常根据已知的环境或过去对环境的感知信息生成一条优化的路径，但是这种方法无法应对未知或突发威胁的情况。而局部路径规划算法不需要环境先验信息，在面对突发威胁时通过机载传感器提供的信息实现动态的航迹规划与路线调整。由于在突发威胁场景下，无人机无法得知全局威胁区域位置分布情况，导致全局路径规划算法无法工作。在此情况下只能采用局部路径规划，当威胁区域处于无人机的探测范围内，无人机才能做出反应，并通过在线航迹规划算法进行实时的航迹计算与调整。

目前，研究人员对航迹规划已经做了大量的工作，并提出了多种航迹规划算法。Dijkstra算法被广泛应用于解决最短路径问题。它将单个节点标记为源节点，然后找到从源节点到图中所有其它节点的最短路径，从而生成最短路径树。该算法交互式地考虑了每一个最短路径距离较近的位置点，能够寻找到最优路径，但是该算法在某些拓扑结构中会存在重复的位置点搜索，导致路径搜索速度相对较慢。A*算法将起始点到目标点的地图进行网格化划分，再对状态空间中的每一个网格即位置点进行评估选择直到找到目标点。但是A*算法很难有效地处理运动学和动力学约束条件下的无人机路径规划。为了解决此问题，稀疏A*搜索(Sparse A*Search，SAS)方法能够引入运动学和任务的各种约束条件，并利用它们来缩小搜索空间。然而，SAS中的估计代价通常是基于从节点到终端的直线距离，在最小转弯半径的约束条件下，并不能准确的度量基于A*算法规划的路径，这种不准确的度量会导致在搜索的过程中更多不必要的节点需要扩展，降低了该方法的效率。人工势场法在引力和斥力这两种合力势场的影响下，机器人在避开威胁区域的同时可以移动到目的点。然而人工势场法容易陷入局部极小值并且存在目标不可达的不足。在此情况下，会导致路径曲线持续震荡，极大地增加路径搜索时间。

随着研究的不断深入，上述几种方法之间没有了绝对的界限，研究人员发现虽然每种算法都存在各自的缺点，但是通过将多重算法进行结合可以很好的将各自算法的优点发挥出来。

发明内容

本发明的目的是在突发威胁场景中，提出一种实时的无人机航迹规划算法。为了实现该算法，本发明所采用的步骤是：

步骤1：首先将起始点当作当前路径扩展点，生成到目标点的路径。如果在此方向上不存在突发威胁区域，则将目标点P_f加入到CLOSE表中，构建成一条从起始点到目标点的路径曲线，算法运行结束。如果在此方向上存在突发威胁区域，则生成当前路径扩展点到威胁区域的多条Dubins曲线。

步骤2：通过MFDA算法估价函数计算并选出代价值最小的点作为下一步的路径扩展点，将此路径扩展点加入到CLOSE表中。

步骤3：将步骤2中选取的路径扩展点作为当前的位置，然后重新执行步骤1的操作。之后重复上述过程，不断产生新的路径扩展点，直到到达目标位置，构建出一条完整的路径。

附图说明

图1是本发明的MFDA算法航迹规划过程；

图2是本发明的扩展点选择示意图；

图3是本发明的LSR路径类型示意图；

图4是本发明的仿真结果图。

具体实施方式

下面结合附图和实施例对本发明作进一步详细描述。

本方法首先假设了以下无人机集群飞行参数：

1、无人机集群通过互连通方式进行通信，在无人机受控范围内，所有无人机均能够较低延时地获取其他无人机位置、姿态等信息。

2、领航者无人机能够通过超声波传感器、激光雷达等方式感知突发威胁位置信息。

3、节点表示为(x_i，y_i，ψ_i)，其中x_i表示无人机在X平面映射的横坐标，y_i时表示在Y平面映射的纵坐标，ψ_i表示无人机的航向。

3、无人机的最小转弯半径为ρ，即无人机最小以转弯半径为ρ的圆调整航向。

4、起始点位置为p_i，航向为ψ_i，这些信息由无人机飞控通过GPS等方式获取。

5、目标点位置为p_f，航向为ψ_f，这些信息由无人机飞控通过GPS等方式获取。

6、将威胁区域表示为(x_threat，y_threat，r_threat，G_threat)，其中(x_threat，y_threat)代表威胁区域的中心坐标，r_threat代表威胁区域半径，G_threat代表威胁级别。

以上述条件为基础，本发明提出的无人机集群队形保持算法已在MATLAB中实现具体的案例，实现结果证明了该方法的有效性，具体实施步骤为：

步骤1：路径扩展点环境搭建。

无人机的最小转弯半径为ρ，即无人机最小以转弯半径为ρ的圆调整航向。起始点位置为p_i，航向为ψ_i，目标点位置为p_f，航向为ψ_f。在无人机起始位置和目标位置之间存在一个半径为r_threat的威胁区域。并且在最小转弯半径和航向的限制条件下，Dubins路径是从起始位置到达目标位置的最短路径。另一方面，将威胁区域表示为(x_threat，y_threat，r_threat，G_threat)，其中(x_threat，y_threat)代表威胁区域的中心坐标，r_threat代表威胁区域半径，G_threat代表威胁级别，威胁等级越大，表明对无人机可能造成的伤害程度越高，并且无人机距离威胁区域越近，遭遇威胁的可能性越大。

步骤2：路径扩展点类型。

当无人机飞行方向相较于威胁区域处于逆时针方向时，按照附图2所示有两条Dubins路径可以绕过威胁区域到达目标点位置，分别是路径p_ip_n1p_f和路径p_ip_n2p_f，它们分别属于LSR类型和LSL类型。当无人机飞行方向相较于威胁区域处于顺时针方向时，有两条Dubins路径可以绕过威胁区域到达目标点位置，它们分别属于RSL类型和RLR类型。

步骤3：路径扩展点选择

将直线路径与威胁区域圆的切点作为A*算法的下一步扩展节点，将节点p_n1和节点p_n2均加入进OPEN表中，通过评估函数分别计算两个节点的代价值并进行比较，选择出代价值最小的节点作为下一步的路径扩展节点，同时将此节点添加到CLOSE表中。

(1)估价函数的建立

MFDA算法同时考虑路径代价和威胁代价对无人机轨迹的影响，估价函数建立如下：

f(n)＝g(n)+h(n) (1)

g(n)＝w₁·Distance+w₂·Hazard (2)

其中，w₁表示距离代价的影响因子，w₂表示威胁代价的影响因子，同时满足w₁+w₂＝1。按照附图3所示Distance距离由两端弧长和一段直线长度构成，下面分别计算LSR和RLS两种类型的Dubins路径距离。

(2)切点坐标计算

起始节点(x_i，y_i，ψ_i)，求解圆O_i和圆O_threat的切线Q_iP_n的坐标。

计算圆O_i的圆心坐标。

威胁区域O_threat的圆心坐标已知，并在此基础上生成直线航迹Q_iP_n。

因此，向量O_threatO_i逆时针旋转β₁角度与向量O_threatP_n重合，向量O_threatO_i的模L已知，之后按照比例

进行缩放可以得到向量O_threatP_n，从而计算出切点P_n坐标，同理可得到切点Q_i坐标。

(3)指定路径代价值计算

当无人机在位置为p_i，航向为ψ_i准备绕过威胁区域O_threat时，选择切点P_n或者p′_n作为A*算法的下一步路径扩展节点，因此需要对P_n和p′_n节点的代价值通过估价函数进行计算。在这里计算P_n节点的代价值，同理可计算得到。首先计算g(n)：

Distance＝P_iQ_i+Q_iP_n (8)

其中节点Q_i、P_n的位置已由前述步骤计算可知，因此P_iQ_i和O_iP_n的长度也可计算得到。接下来计算Hazard，距离威胁区域越近，所受到的威胁就越大，假设把不经过威胁区域的航迹路径所受到的威胁设置为0，因此只有绕威胁区域飞行的圆弧段航迹才会受到威胁。从当前节点到下一个扩展节点的威胁代价值为：

Hazard(P_n-1P_n)＝Hazard(P_n-1Q_n-1)+Haxard(Q_n-1P_n) (9)

因此Hazard(Q_n-1P_n)＝0，Hazard(P_n-1P_n)＝Hazard(P_n-1Q_n-1)，其中，

因此，

(4)预估代价值计算

计算h(n)，同样将距离代价和威胁代价的预评估影响因子分别用w₁、w₂表示。当无人机以P_n或p′_n点切入威胁区域，则无人机分别以P_nQ_nP_f路径或P′_nQ′_nP_f路径以指定位姿到达目标点，则P_nQ_n和P′_nQ′_n分别预示了无人机绕过威胁区域的飞行轨迹，所以h(n)表示为：

h(P_n)＝w₁·distance′(P_nP_f)+w₂·Hazard′(P_nP_f) (12)

其中，

本发明申请书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种面向突发威胁的无人机航迹规划算法，所采用的步骤是：

步骤1：首先将起始点当作当前路径扩展点，生成到目标点的路径；如果在此方向上不存在突发威胁区域，则将目标点P_f加入到CLOSE表中，构建成一条从起始点到目标点的路径曲线，算法运行结束；如果在此方向上存在突发威胁区域，则生成当前路径扩展点到威胁区域的多条Dubins曲线，具体方法：

(1)路径扩展点环境搭建

无人机的最小转弯半径为ρ，即无人机最小以转弯半径为ρ的圆调整航向，起始点位置为p_i，航向为ψ_i，目标点位置为p_f，航向为ψ_f，在无人机起始位置和目标位置之间存在一个半径为r_threat的威胁区域，并且在最小转弯半径和航向的限制条件下，Dubins路径是从起始位置到达目标位置的最短路径；另一方面，将威胁区域表示为(x_threat，y_threat，r_threat，G_threat)，其中(x_threat，y_threat)代表威胁区域的中心坐标，r_threat代表威胁区域半径，G_threat代表威胁级别，威胁等级越大，表明对无人机可能造成的伤害程度越高，并且无人机距离威胁区域越近，遭遇威胁的可能性越大；

(2)路径扩展点类型

当无人机飞行方向相较于威胁区域处于逆时针方向时，有两条Dubins路径可以绕过威胁区域到达目标点位置，分别是路径p_ip_n1p_f和路径p_ip_n2p_f，它们分别属于LSR类型和LSL类型；当无人机飞行方向相较于威胁区域处于顺时针方向时，有两条Dubins路径可以绕过威胁区域到达目标点位置，它们分别属于RSL类型和RLR类型；

步骤2：通过多因素Dubins算法估价函数计算并选出代价值最小的点作为下一步的路径扩展点，将此路径扩展点加入到CLOSE表中；其中，路径扩展点的代价计算与选取的具体方法为：

(1)估价函数的建立

多因素Dubins算法同时考虑路径代价和威胁代价对无人机轨迹的影响，估价函数建立如下：

f(n)＝g(n)+h(n) (1)

g(n)＝w₁·Distance+w₂·Hazard (2)

其中，w₁表示距离代价的影响因子，w₂表示威胁代价的影响因子，同时满足w₁+w₂＝1，Distance距离由两端弧长和一段直线长度构成，下面分别计算LSR和RLS两种类型的Dubins路径距离；

(2)切点坐标计算

起始节点(x_i，y_i，ψ_i)，求解圆O_i和圆O_threat的切线Q_iP_n的坐标，

计算圆O_i的圆心坐标为

其中，ρ表示无人机的最小转弯半径；

威胁区域O_threat的圆心坐标已知，并在此基础上生成直线航迹Q_iP_n，

因此，向量O_threatO_i逆时针旋转β₁角度与向量O_threatP_n重合，向量O_threatO_i的模L已知，之后按照比例

进行缩放可以得到向量O_threatP_n，从而计算出切点P_n坐标，同理可得到切点Q_i坐标；

(3)指定路径代价值计算

当无人机在位置为p_i，航向为ψ_i准备绕过威胁区域O_threat时，选择切点P_n或者p′_n作为A*算法的下一步路径扩展节点，因此需要对P_n和p′_n节点的代价值通过估价函数进行计算；在这里计算P_n节点的代价值，同理可计算得到；首先计算g(n)为

其中节点Q_i、P_n的位置已由前述步骤计算可知，因此

和O_iP_n的长度也可计算得到；接下来计算Hazard，距离威胁区域越近，所受到的威胁就越大，假设把不经过威胁区域的航迹路径所受到的威胁设置为0，因此只有绕威胁区域飞行的圆弧段航迹才会受到威胁；从当前节点到下一个扩展节点的威胁代价值为

因此Hazard(Q_n-1P_n)＝0，

其中，

因此，

(4)预估代价值计算

计算h(n)，同样将距离代价和威胁代价的预评估影响因子分别用w₁、w₂表示；当无人机以P_n或p′_n点切入威胁区域，则无人机分别以P_nQ_nP_f路径或P′_nQ′_nP_f路径以指定位姿到达目标点，则

和

分别预示了无人机绕过威胁区域的飞行轨迹，所以h(n)表示为

h(P_n)＝w₁·distance′(P_nP_f)+w₂·Hazard′(P_nP_f) (12)

其中，

步骤3：将步骤2中选取的路径扩展点作为当前的位置，然后重新执行步骤1的操作；之后重复上述过程，不断产生新的路径扩展点，直到到达目标位置，构建出一条完整的路径。

Images