[发明专利]基于Dubins路径和稀疏A*搜索的无人机航迹规划方法

权利要求书

1.基于Dubins路径和稀疏A*搜索的无人机航迹规划方法，其特征在于：包括如下步骤：

步骤1：获取无人机飞行环境信息，包括飞行区域、威胁信息；设置无人机航迹规划的初始条件，包括无人机航迹规划的起始点、目标点、初始方向角、步长d、无人机最大转弯角和航迹的最大允许长度d_max；

步骤2：设计创建OPEN表和CLOSED表，并对其初始化；

本发明方法进行航迹规划过程中，涉及以下三类节点：

（1）封闭节点：已经被扩展的节点；

（2）开放节点：已经被产生但尚未扩展的节点；

（3）尚未产生的节点；

所述CLOSED表存放封闭节点；所述OPEN表存放开放节点；OPEN表和CLOSED表以行列的形式，一行表示一个节点，节点在OPEN表和CLOSED表中存放形式相同，每个节点存放的信息包括自身坐标值、父节点的坐标值、自身总代价值、自身到起始点的真实代价值、自身到目标点的估计代价值；OPEN表和CLOSED表各有五列，每列为各节点同类型的信息；

初始化时，起始点作为开放节点放入OPEN表，同时将CLOSED表置空；

步骤3：判断OPEN表是否为空，若为空，则航迹搜索失败并结束搜索；若不为空，则以OPEN表中代价值最小的节点作为当前节点，并将其移出OPEN表，放入CLOSED表中；然后执行步骤4；

步骤4：比较当前节点到达目标点的长度与步长二者的大小；若小于或者等于步长则将目标节点的父节点指针指向当前节点，结束搜索过程，然后从目标点开始根据每一级父节点依次回溯到起始节点，得到从起始点到目标的最小代价路径；若大于步长则执行步骤5；

步骤5：创建步骤3所确定的当前节点的待扩展空间，并将待扩展空间分割为多个区间，计算每个区间中待扩展节点的代价值；具体过程如下：

步骤5.1，创建当前节点待扩展空间：以步骤3确定的当前节点为圆心，以步长d为半径，以进入当前节点的方向为对称轴、以两倍于无人机最大转弯角为扩展角度所构成的扇弧即为当前节点待扩展空间；

步骤5.2，分割待扩展空间：把待扩展扇弧区分为N个小扇弧；

步骤5.3，以步骤5.2得到的每一个待扩展小扇弧的中心点作为待扩展节点，因此每个小扇弧中包含一个待扩展节点；计算每个扇弧的待扩展节点的总代价值；所述总代价值为起始点到该待扩展节点的真实代价值和由该待扩展节点到目标点的估计代价值之和；其中估计代价值根据待扩展节点到目标点的Dubins路径长度计算得到；

所述Dubins路径针对不同的运动对象有不同的形式，本发明适用对象为前向飞行的无人机，并且是针对无人机起始点位置与方向给定、目标点位置给定而进入方向自由情况下的航迹规划，Dubins路径长度的具体计算方法为：

假设无人机最小转弯半径为R，并以无人机当前节点为原点，以无人机当前飞行速度为x轴正方向建立坐标系；

表1给出了无人机实现终端方向自由时的Dubins路径飞行所采用的飞行策略，即根据目标点在二维平面中的位置，从原点到达目标点的最短路径所对应的飞行策略；飞行策略包含直线飞行航迹段L和以最小转弯半径转弯飞行的圆弧航迹段C+或C-，其中C+表示逆时针飞行的圆弧段，C-表示顺时针飞行的圆弧段；另外，C₁表示以O₁（0，R）为圆心、以R为半径的圆，C₂表示以O₂（0，-R）为圆心、以R为半径的圆；

表1终端方向自由时Dubins路径对应的飞行策略

设目标点为T，θ₁为O₁T与x轴的夹角，θ₂为O₂T与x轴的夹角，P₁为过目标点与圆C₁的切点，P₂为过目标点与圆C₂的切点；根据目标位置不同和其对应的Dubins路径飞行策略，得到Dubins路径DubinsD的具体计算公式为：

a)目标点位于x轴正半轴

DubinsD＝|OT|    (1)

b-1)目标点位于圆C₁上

DubinsD＝R·(π/2+θ₁)    (2)其中，θ₁取值范围需转化到[-π/2,3π/2)；

b-2)目标点位于圆C₂上

DubinsD＝R·(π/2-θ₂)    (3)其中，θ₂取值范围需转化到[-3π/2,π/2)；

c-1)目标点位于上半平面圆C₁外

DubinsD＝|P₁T|+R·(π/2+θ-∠TO₁P₁)    (4)其中，θ取值范围需转化到(-π/2,3π/2)；

c-2)目标点位于下半平面圆C2外

DubinsD＝|P₂T|+R·(π/2-θ-∠TO₂P₂)    (5)其中，θ取值范围需转化到(-3π/2,π/2)；

c-3)目标点位于x轴负半轴

DubinsD＝|P₁T|+R·(π/2+θ-∠TO₁P₁)

或者DubinsD＝|P₂T|+R·(π/2-θ-∠TO₂P₂)；

d-1)目标点位于圆C₁内

DubinsD＝R·(2π-∠TO₂O₃-∠TO₂O₁+∠TO₃O₂)    (6)其中，∠TO₂O₁需转化到(-π/2,π/2)；O₃为左半平面中与圆C₂相切

且过目标点的圆的圆心；

d-2)目标点位于圆C₂内

DubinsD＝R·(2π-∠TO₁O₄-∠TO₁O₂+∠TO₄O₂)    (7)其中，∠TO₁O₂需转化到(-π/2,π/2)；O₄为左半平面中与圆C₁相切且过目标点的圆的圆心；

步骤6：分别对步骤5中的每一个小扇弧中待扩展节点进行判断，判读是否满足最大航迹长度约束D（x）+DubinsD（x）≤d_max，其中D（x）是从起始点到待扩展节点x所经过的真实距离，DubinsD（x）是从待扩展节点x到目标点的Dubins路径长度；若满足最大航迹长度约束，则将当前节点作为待扩展节点x的父节点，并将待扩展节点x插入OPEN表；否则舍弃该待扩展节点x；完成对当前节点的所有待扩展节点的判断后，返回执行步骤3，重新确定下一次搜索的当前节点。