CN103676944B - 基于Dubins路径和稀疏A*搜索的无人机航迹规划方法 - Google Patents

Abstract

本发明涉及一种基于Dubins路径和稀疏A*搜索的无人机航迹规划方法（简称Dubins‑SAS航迹规划方法），属于航迹自主规划技术领域。本方法将Dubins路径与稀疏A*搜索算法相结合，采用Dubins路径长度作为稀疏A*搜索算法的启发函数，并利用该启发函数搜索空间中的节点，实现无人机的航迹规划。本发明能够更加准确的估计节点间的距离，提高了启发函数（也称为估计代价）的准确性，可以减少完成路径搜索所需扩展的节点数，进而减少航迹规划规划的耗时，提高了无人机航迹规划的效率。

Description

基于Dubins路径和稀疏A*搜索的无人机航迹规划方法

技术领域

本发明涉及一种基于Dubins路径和稀疏A*搜索的无人机航迹规划方法（简称Dubins-SAS航迹规划方法），属于航迹自主规划技术领域。

背景技术

无人机（UAV）是无人驾驶飞机(UnmannedAerialVehicles)的简称，它是一种由动力驱动，机上无人驾驶，可重复使用的航空器。与载人飞机相比，它具有体积小、造价低、使用方便、对作战环境要求低、战场生存能力较强等优点，使其比有人飞机更适合执行枯燥、恶劣、危险（Dull，Dirty，orDangerous）任务。在科学技术的推动和作战需求的牵引下，无人驾驶飞机已逐渐成为当今各国武器装备发展的重点，并在战争中扮演越来越重要的角色。

航迹规划作为无人机系统中任务规划子系统的重要功能，是提高无人机系统实际作战效能的关键，备受世界各国重视。自20世纪50年代以来，美、英、德等国家开始进行航路规划方法的研究，但受当时技术水平和应用需求的限制，航迹规划基本上依赖于规划人员的手工操作。直到20世纪80年代初，防空体系日益完善，人工航路规划的各种缺陷逐渐显现出来。同时，由于卫星通讯技术的发展，航迹规划人员获得的信息也更多，例如通过卫星、无人机等侦察手段可以得到敌方防空阵地的精确信息；通过数字地形图可以获得相关地区地形信息。为了最大限度减小无人机被敌方雷达发现的概率，航迹规划必须依据已知信息为无人机规划出满足性能约束的安全飞行航迹。由于其中涉及因素太多，各种因素之间需要相互权衡，传统的手工规划方式难以完成如此复杂的任务。因此，迫切需求对自动航迹规划技术开展相关研究。八十年代中后期，美国投入大量的人力物力进行自动航迹规划技术的研究，并取得了一定成功。其中最具代表性的是系统控制技术公司为空射巡航导弹开发的自动航迹规划模块和波音航空航天公司开发的基于人工智能的任务规划软件。九十年代以来，NASA和美国军方联合开展了一项名为自主掠地飞行(AutomatedNapoftheEarth，ANOE)的研究计划，该计划的主要内容是利用机载传感器获取环境信息结合导航系统数据，实时规划掠地飞行(NapoftheEarth，NOE)的最优轨迹，并给出沿最优轨迹飞行的导引控制指令。ANOE实际上研制的正是一个实时航迹规划系统。

航迹规划就是在综合考虑无人机到达时间、油耗、威胁以及飞行区域等因素的前提下，为无人机规划出最优或者是满意的飞行航迹，以保证圆满地完成飞行任务。国内外学者根据各自的学科背景和专业领域，提出了多种航迹规划方法。可依据不同的标准对这些方法进行分类，例如按照使用方法一般可以分为基于最优控制的轨迹优化、基于几何学的路径规划和基于类比的航迹规划；按照规划决策的计算方法可以分为传统最优式规划法和智能启发式规划法；按照规划在作战中的时序可分为离线预先规划和在线实时规划。其中，A*搜索算法是一种经典的智能启发式搜索算法，但采用A*搜索算法为无人机、飞机等飞行器规划航迹时难以处理其运动学约束与任务约束。为此，Szczerba等对A*搜索算法进行了改进，提出了稀疏A*搜索（SparseA*Search，SAS）算法，该算法在搜索过程中利用约束修剪搜索空间，并可搜索得到一条满足约束可飞航迹，可实现无人机的航迹规划。然而，稀疏A*搜索算法在搜索过程中以节点与目标位置的直线距离作为启发函数（也称估计代价），对于存在最小转弯半径等运动学约束的飞行航迹是不准确的，因此虽然标准稀疏A*搜索算法可以实现无人机的航迹规划，但该算法在搜索过程中存在大量的无效搜索节点，影响了航迹规划效率。

而随着信息技术和网络技术的发展，快速响应、先敌制人对于现代战争愈发重要，因此对航迹规划的快速性要求愈发苛刻。同时，算法的实时性对无人机在线航迹规划更为重要，因为在环境变化时，能否快速的为无人机在线规划出新的航迹，直接关系到无人机的存亡和任务的成败。可见，提高航迹规划的效率对无人机具有极其重要的意义。

Dubins路径是指在恒定速度和限定曲率的条件下，由给定位置与姿态角的初始点到达给定位置与姿态角的目标点的最短路径，该最短路径的存在性最早由Dubins用几何学方法证明，并推出其构成形式只可能为直线段或以无人机最小转弯半径为半径的圆弧段。由于Dubins路径是考虑了运动学约束后的路径，其比直线路径更加接近无人机飞行的真实路径。

发明内容

本发明的目的是为提高无人机航迹规划效率，使无人机能够对环境或任务的改变做出快速响应，提出了一种基于Dubins路径和稀疏A*搜索方法的无人机快速航迹规划方法（简称Dubins-SAS）。

本发明的技术原理为：将Dubins路径与稀疏A*搜索算法相结合，采用Dubins路径长度作为稀疏A*搜索算法的启发函数，并利用该启发函数搜索空间中的节点，实现无人机的航迹规划。

一种基于Dubins路径和稀疏A*搜索的航迹规划方法，其实现步骤如下：

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

在应用本发明所述方法进行航迹规划过程中，有如下定义：当以节点A为当前节点，以本发明步骤5-6方法扩展时，得到的节点B、C为节点A的子节点，节点A为节点B、C的父节点，其中父节点与其子节点间的航迹长度为航迹规划所设置的步长d。通常，将步长大小设置为无人机的最小航迹段长度，即无人机在开始改变飞行姿态前需保持直线飞行的最短直线距离。

步骤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个小扇弧。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轴负半轴

利用公式(4)或(5)计算均可。

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，重新确定下一次搜索的当前节点。

有益效果

本发明结合Dubins路径和稀疏A*搜索算法，以Dubins路径长度作为稀疏A*搜索算法的启发函数，能够更加准确的估计节点间的距离，也就提高了启发函数（也称为估计代价）的准确性，因此可以减少完成路径搜索所需扩展的节点数，进而减少航迹规划规划的耗时，从而提高了无人机航迹规划的效率。

附图说明

图1为本发明所述的基于Dubins路径和稀疏A*搜索算法的无人机快速航迹规划方法流程图；

图2为本发明方法中不同终点所对应的Dubins路径飞行策略；

图3为具体实施方式中无障碍环境下本发明提出的Dubins-SAS航迹规划方法所规划的航迹（左a）与搜索节点分布图（右b）；

图4为具体实施方式中无障碍环境下传统稀疏A*搜索算法所规划的航迹（左a）与搜索节点分布图（右b）；

图5为具体实施方式中有障碍环境下本发明提出的Dubins-SAS航迹规划方法所规划的航迹（左a）与搜索节点分布图（右b）；

图6为具体实施方式中有障碍环境下传统稀疏A*搜索算法规划的航迹（左a）与搜索节点分布图（右b）。

具体实施方式

为了更好的说明本发明的目的与优点，下面通过无人机航迹规划实例，结合附图与表格对本发明做进一步说明，并通过与传统航迹规划方法结果进行比较，对本发明的综合性能进行验证分析。

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

例如，获取得到无人机飞行区域为5km×5km，威胁区域为以（1000m，1000m）为中心，以500m为半径的圆形区域及其内部；设置无人机的起始点位于原点（0，0），飞行目标点为（3000m，3000m），初始方向角为90°，步长为200m，无人机最大转弯角为11.26°，航迹最大允许长度d_max为6km。

下述步骤中也以上述初始条件为例进行说明。

步骤2：创建OPEN表和CLOSED表，并对其初始化，即将起始点放入OPEN表，同时将CLOSED表置空。

根据步骤1中获取的信息和设置的初始条件，此时OPEN表中包含一个节点信息（即初始点信息），具体包括节点的坐标值、父节点的坐标值、节点的总代价值、节点到起始点的真实代价值、节点到目标点的估计代价值，具体数据为[0，0，0，-200，4333.14，0，4333.14]。而CLOSED表此时为空。

步骤3：判断OPEN表是否为空，若为空，则航迹搜索失败并结束搜索；若不为空，则取出OPEN表中代价值最小的节点作为当前节点，并放入CLOSED表中。

例如，以步骤2的结果为例执行该步骤时，OPEN表不为空，因此取出OPEN表中代价值最小的节点作为当前节点，即[0，0，0，-200，4333.14，0，4333.14]，并将此节点放入CLOSED表中。执行完此步骤后，OPEN表为空，而CLOSED表中包含节点[0，0，0，-200，4333.14，0，4333.14]。

步骤4：判断当前节点到达目标点的长度是否小于步长。若满足则将目标节点的父节点指针指向当前节点，并且结束搜索过程，然后从目标点根据父节点依次回溯到起始节点，从而得到从起始点到目标的最小代价路径；否则，执行步骤5。

以步骤3的结果为例执行该步骤时，当前节点距目标点的长度（4242.6m）大于步长（200m），因此继续执行后续步骤。而如果后续再执行此步骤时，若当前节点距目标点距离小于200m，则结束搜索过程。

步骤5：创建步骤3确定的当前节点的待扩展空间，并将待扩展空间分割为多个区间，计算每个区间中待扩展节点的代价值。

以步骤3的结果为例进行说明，此时当前节点信息为[0，0，0，-200，4333.14，0，4333.14]。

1）创建当前节点待扩展空间，即为以[0,0]为圆心，以200m为半径，以过[0，0]点且呈90°的直线为对称轴，角度为22.52°的的扇弧。

2）分割待扩展空间，即上述扇弧等分为4个小扇弧。

3）对每个小扇弧，以小扇弧的中心点作为待扩展节点，并计算每个待扩展节点的代价值。代价值为起始点到该待扩展节点的真实代价值和由该待扩展节点到目标点的估计代价值之和。其中估计代价值根据待扩展节点到目标点的Dubins路径长度计算得到。Dubins路径长度的计算方法如下：

通过坐标变换，使得无人机的起始点位于原点，无人机的初始飞行方向角为0°（沿x轴正向）。表1和附图2给出了无人机实现终端方向自由时的Dubins路径飞行所需要采用的飞行策略，即根据目标点在二维平面中的位置，从原点到达目标点T的最短路径所对应的飞行策略。飞行路径包含直线飞行航迹段L和以最小转弯半径转弯飞行的圆弧航迹段C+或C-，其中C+表示逆时针飞行的圆弧段，C-表示顺时针飞行的圆弧段。另外，C₁表示以O₁（0，R）为圆心、以R为半径的圆，C₂表示以O₂（0，-R）为圆心、以R为半径的圆。

设θ₁为O₁T与x轴的夹角，θ₂为O₂T与x轴的夹角，P₁为过目标点与圆C₁的切点，P₂为过目标点与圆C₂的切点。根据目标位置不同和其对应的Dubins路径飞行策略，Dubins路径的具体计算公式见公式（1）—（7）。

以上述得到的4个小圆弧中最上方的小圆弧为例进行代价计算，此小圆弧中点坐标可经过几何关系计算得到为[-29.4，197.8]。然后计算该点到起始点的真实代价值为200，和该点到目标点的Dubins路径长度对应的估计代价值为4301.6。

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

以步骤5所得到的待扩展节点为例，由于对于该待扩展节点D（x）+DubinsD（x）=4501.6<6000，因此将该待扩展节点的父节点指向步骤3中的当前节点[0，0]，并将此节点的完整描述（即[-29.4，197.8，0，0，4501.6，200，4301.6]）作为一行存入OPEN表中。将步骤5中得到的所有待扩展节点都判断与存完/舍弃后，然后返回步骤3。

实施实例：

（1）无障碍环境下无人机航迹规划实例

实例中的相关参数为：无人机飞行速度V为100m/s，最小转弯半径R为1km，最短直飞距离（即步长d）为200m，最大转弯角为11.26°，节点扩展时的扇形空间划分数N取为5。

首先对无人机航迹规划的环境初始化，包括设置无人机的初始位置、初始方向角和目标位置，本实例中无人机的起始点位于原点（0，0），初始飞行方向角为90°，目标点坐标为（2500，0）。

然后，利用本发明所述的基于Dubins路径和稀疏A*搜索算法的快速航迹规划方法，为无人机规划从起点到达目标的最优航迹，规划的航迹结果如图3（左）所示。图3（右）为Dubins-SAS算法在搜索最优航迹过程中搜索过的节点分布图，分布图中每个点表示一个搜索节点。为了体现本发明的优势与目的，对该实例采用传统的稀疏A*搜索算法实现无人机航迹规划来与本发明所述方法进行比较，传统的稀疏A*搜索算法规划的结果见图4，其中左图为规划的航迹结果，右图为航迹搜索过程中搜索过的节点分布图。

表2无障碍环境下算法仿真结果对比

	传统稀疏A*搜索算法	本发明所述的Dubins-SAS算法
			规划的航迹长度/m	3265	3259
规划过程中搜索节点数	292	135
			航迹规划耗时/s	0.1163	0.0835

根据实例的计算结果，采用直线距离的传统稀疏A*搜索算法由于估计代价的偏差较大，因此在搜索过程中搜索的节点较多，对于该实例共搜索了292个节点后才得到可飞路径，而采用本发明所述的Dubins-SAS方法只需搜索135个节点即可得到可飞路径，搜索节点数减少53.77%。传统稀疏A*搜索算法与本发明所述的Dubins-SAS算法得到的航迹长度相当，分别为3265m、3259m，但Dubins-SAS的航迹规划时间为0.0835s，相比于传统稀疏A*搜索算法完成航迹规划所用的0.1163s，减少了28.20%。可见本发明所述的基于Dubins和稀疏A*搜索算法的快速航迹规划方法比传统SAS算法是实现无人机航迹规划所需搜索的节点数减少，从而减小了无人机航迹规划的耗时，即提高了无人机规划航迹的效率，达到了本发明所预期的目的。

（2）有障碍环境下无人机航迹规划实例

实例中的相关参数为：无人机飞行速度V为100m/s，最小转弯半径R为1km，最短直飞距离（即步长d）为200m，最大转弯角为11.26°，节点扩展时的扇形空间划分数N取为5。

首先对无人机航迹规划的环境初始化，包括设置无人机的初始位置、初始方向角、目标位置和障碍物位置及其覆盖区域，本实例中无人机的起始点位于原点（0，0），初始飞行方向角为90°，目标点坐标为（2500，0），障碍物为3个圆盘形区域，障碍物位置与大小如图5、图6中圆形区域所示。

然后，利用本发明所述的基于Dubins路径和稀疏A*搜索算法的快速航迹规划方法，为无人机规划一条从起点到达目标点，并能够有效回避环境中障碍物和满足飞行约束的最优航迹，规划的航迹结果如图5（左）所示。图5（右）为Dubins-SAS算法在搜索最优航迹过程中搜索过的节点分布图，分布图中每个点表示一个搜索节点。与实例1相似，为了体现本发明的优势与目的，对该实例采用传统的稀疏A*搜索算法进行航迹规划，以用于与本发明所述方法进行比较，传统的稀疏A*搜索算法规划的航迹结果见图6，其中左图为规划的航迹结果，右图为稀疏A*搜索算法在搜索航迹过程中搜索过的节点分布图。

表3有障碍环境下算法仿真结果对比

根据实例的计算结果，在该有障碍物存在的环境下，传统稀疏A*搜索算法完成无人机的避障航迹规划共搜索了7410个节点，耗时14.8535s，规划得到的可飞避障航迹长度为2934m；而Dubins-SAS算法只需搜索484个节点，耗时0.4088s，即可得到可飞的避障航迹，航迹长度为2967m。可见Dubins-SAS算法对启发函数估计准确度的提高可以减少了大量的搜索节点（对于本实例减少了93.47%），搜索的效率得以提升，航迹规划时间只有传统稀疏A*搜索算法的2.75%。另外，对比无障碍物环境与有障碍物环境下的结果，本发明所述的Dubins-SAS算法在有障碍物时的规划效率提高更为明显。而真实环境下，无人机的飞行环境中往往存在威胁区或不可飞区域，因此Dubins-SAS算法更具有实用性。同时，利用Dubins-SAS算法进行航迹规划的时间很短，尤其适用于无人机的在线航迹规划。

根据前述的无人机航迹规划实例仿真结果与分析可见，本发明所述的基于Dubins路径和稀疏A*搜索算法的快速航迹规划方法在航迹规划过程中搜索节点的数量小于传统的稀疏A*搜索算法，从而减少航迹规划的耗时，提高了无人机航迹规划的效率，具有快速完成无人机航迹规划的能力，因此本发明具有很强的工程实用性，并且基本实现了预期的发明目的。

以上的具体描述，是对发明的目的、技术方案和有益效果的进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施实例，仅用于解释本发明，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

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，重新确定下一次搜索的当前节点。

2.根据权利要求1所述的基于Dubins路径和稀疏A*搜索的无人机航迹规划方法，其特征在于，在应用所述方法进行航迹规划过程中，有如下定义：当以节点A为当前节点，以步骤5-6方法扩展时，得到的节点B、C为节点A的子节点，节点A为节点B、C的父节点，父节点与其子节点间的航迹长度为航迹规划所设置的步长d，其大小为无人机的最小航迹段长度，即无人机在开始改变飞行姿态前需保持直线飞行的最短直线距离。

3.根据权利要求1所述的基于Dubins路径和稀疏A*搜索的无人机航迹规划方法，其特征在于：N的值越大航迹规划成功率越高，计算量和占用的存储空间越大。