[发明专利]一种面向突发威胁的无人机航迹规划算法

权利要求书

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的操作；之后重复上述过程，不断产生新的路径扩展点，直到到达目标位置，构建出一条完整的路径。