本发明属于飞行器航迹规划技术研究领域。尤其涉及综合考虑无人机到达时间、油耗、威胁、以及飞行区域等因素的前提下找到一条能够保证无人机安全飞行的飞行航迹。该航迹规划方法能在大范围的真实环境中规划出满足各种约束的安全航迹，可广泛应用于各类无人飞行器的航迹规划。
背景技术：
随着航空技术的发展，无人机在军用与民用领域的应用不断扩大，如：敌情侦察、察-打-评、协同打击、地形勘探、地理测绘、高压巡检等。无人机执行的任务复杂多样，为提高其生存能力，必须实现自主飞行。航迹规划是无人机自主飞行的关键技术之一。
无人机的航迹规划的目的是找到能够保证无人机安全突防的飞行航迹，既要尽量减少被敌防空设施捕获和摧毁的概率，又要降低坠毁的概率，同时还要满足各种约束条件。对于多飞行器协同作战，为了提高执行任务的成功率，多个无人机之间的协作与安全性也很重要，具体地，无人机航迹规划需要考虑包括安全性、航迹约束、协作性以及实时性要求。
航迹规划是一个np问题，对其直接求解往往会导致组合爆炸，为了降低其算法复杂度，国内外学者提出了各种规划方法，包括基于概率图搜索的规划方法、基于栅格搜索的规划方法、基于人工势场的规划方法以及基于进化计算的规划方法。本发明属于一种基于快速扩展随机树(rrt)的无人机航迹规划方法。
技术实现要素：
要解决的技术问题
为了避免现有技术的不足之处，本发明针对无人机在复杂战场环境条件下对航迹规划算法的快速性要求，基于快速扩展随机树(rrt)方法提出了一种快速高效且满足无人机性能指标约束的工程化航迹规划算法。
技术方案
一种基于改进rrt算法的无人机航迹规划方法，其特征在于步骤如下：
步骤1：通过问题描述，明确规划空间的表示方法，飞行航迹的表示方法，以及飞行航迹的约束条件：
规划空间的表示方法：在三维空间中进行航迹规划，设(x,y,z)为规划空间某点坐标，x,y分别表示经纬度，z表示海拔高度；则离散化空间可表示为集合：
ω＝{(x,y,z)|0≤x≤xmax,
0≤y≤ymax,0≤z≤zmax}
为简化问题，设定巡飞弹在固定高度巡飞，将三维空间路径搜索问题简化为二维问题；
飞行航迹的表示方法：采用飞行器空间位置的时间序列来表示，即飞行过程中的任一时刻，飞行器的运动状态表现为三维空间中的航迹位置；
飞行航迹的约束条件：(1)最小航迹段长度，即飞行器在开始改变飞行姿态前必须保持直飞的最短距离，由机动能力和导航要求决定；设飞行航迹中的直线段由{li|i＝1,...,n}组成，最小航迹段长度为lmin，该约束可表示为：li≥lmin(i＝1,...,n)；(2)最小转弯半径，最小转弯半径约束和最大转弯角约束起同样的约束作用，限制飞行器在机动能力范围内转弯；(3)最大总航程，它取决于所携带的燃料以及任务执行时间限制，设飞行航迹由{li|i＝1,...,n}组成，最大总航程为lmax，则该约束可写成
步骤2：对于给定的状态空间、出发点和目标点，给定的出发点为树的根节点，采用快速扩展随机树算法rrt通过逐渐增加叶节点的方式生成随机扩展树：
令xstart为出发点，xend为目标点，a为规划空间，在空间a中寻找一条从出发点xstart到目标点xend的连续航迹f(a)，且满足
在增加叶节点时，设定飞行步长为l，确定暂时目标点xtarget，在现有的节点中寻找距离暂时目标点xtarget最近的叶节点xnew，如果在向xtarget行进过程中遇到障碍或进入威胁区，则重新选择暂时目标点xtarget，继续迭代计算；如果行进到xtarget过程中没遇到障碍或威胁，则添加xnew到随机树中，建立节点之间的链接关系；
节点扩展公式如下：
迭代计算，直到暂时目标点xtarget到目标点xend的距离小到某一个阀值e，随机树生长完成，其判断条件如下：
distance(xend,xnew)≤e
步骤3：引入无人机转弯角度约束条件：
在规划空间扩展节点时，不遍历该区域内每一个单元，采用扇形区域，α表示节点扩展角，
步骤4：加入总航程约束：
给定当前节点xnew，当满足下式中条件时，将xnew加入到树分支中：
d(x)+sl(x)≤lmax
其中，lmax为无人机的最大航程，d(x)是从起始位置xstart到xnew经过的真实航程，sl(x)是从xnew到目标点xend的直线距离，此直线距离小于航迹实际要经过的路径长度；若不满足上式条件，则此树分支为无效分支路径；
步骤5：冗余节点裁剪：
设经过步骤2-4后求解的节点序列为{xstart,...xj...,xend}，其中xend为终点位置，将经过冗余节点剪裁后的节点序列集合记为φ，设φ初始为空，令j＝n，n为节点的总数量，则节点剪裁过程如下：首先将xj添加到φ，i∈[1,...,j-1]，循环检查(xi,xj)之间的连线是否存在障碍或者威胁，如果存在，则令i＝i+1；否则，只要检测出第一个没有障碍的节点xi，就停止循环，令j＝i，并将xi加入φ；重复上述循环过程，直到j＝1时结束；
步骤6：气动数据采用风洞吹风实验数据，控制系统采用btt控制设计参数，航路点坐标系采用北天东坐标系，坐标原点取出发点(0,0,0)，x轴指向北为正，y轴位于铅垂面指向天向为正，z轴垂直于0xy组成的平面，组成右手坐标系，指向东为正；将步骤1到步骤5规划得到的航迹点作为飞控模型航路点进行飞行器六自由度飞行，得到的实际飞行轨迹即为无人机的航迹。
有益效果
本发明针对无人机在复杂战场环境条件下对航迹规划的工程化要求，基于基本快速扩展随机树算法框架，充分借鉴经典启发式稀疏a*算法思想，改进了树节点扩展方式，引入转弯半径、转弯角、总航程约束条件，有效减少了规划空间内采样点密度、运算时间和航迹点个数，设计了冗余节点裁剪方法，降低了巡飞弹转弯频率和机动性要求，缩短了航程，算法易于工程实现，可用于无人机快速航迹规划，也可为其他飞行器进行快速航迹规划提供参考。
附图说明
图1最小转弯半径和最大转弯角具有转换关系
图2rrt的节点扩展示意图
图3rrt算法流程图
图4节点扩展扇形区域
图5无人机六自由度控制模型示意图
图6冗余节点剪裁前后对比
具体实施方式
现结合实施例、附图对本发明作进一步描述：
该算法基于rrt算法框架下，借鉴稀疏a*算法思想，改进了树节点扩展方式，引入了转弯半径、转弯角以及总航程约束条件，设计了冗余节点裁剪方法，有效减少了规划空间内采样点密度、运算时间和航迹点个数，降低了巡飞弹转弯频率，缩短了航程。
本发明所提出的无人机航迹规划方法能够规划出一条满足要求的航迹，避免分层规划的复杂性，提高了无人机航迹规划的效率以及工程实用性。
本发明所提出的无人机航迹规划方法总体流程为：
(1)问题建模
航迹规划的目的和方法根据执行任务的类型确定。
规划空间的表示方法：在三维空间中进行航迹规划，设(x,y,z)为规划空间某点坐标，x,y分别表示经纬度，z表示海拔高度。则离散化空间可表示为集合：
ω＝{(x,y,z)|0≤x≤xmax,
0≤y≤ymax,0≤z≤zmax}(1)
为简化问题，设定无人机在固定高度巡飞，将三维空间路径搜索问题简化为二维问题。
飞行航迹的约束条件：(1)最小航迹段长度。即飞行器在开始改变飞行姿态前必须保持直飞的最短距离，由机动能力和导航要求决定。设飞行航迹中的直线段由{li|i＝1,...,n}组成，最小航迹段长度为lmin，该约束可表示为：li≥lmin(i＝1,...,n)；(2)最小转弯半径。最小转弯半径约束和最大转弯角约束起同样的约束作用，限制飞行器在机动能力范围内转弯，如下图1所示。(3)最大总航程。它取决于所携带的燃料以及任务执行时间限制。设飞行航迹由{li|i＝1,...,n}组成，最大总航程为lmax，则该约束可写成
(2)快速扩展随机树算法(rrt)
对于一个给定的状态空间、出发点和目标点，给定的出发点为树的根节点，通过逐渐增加叶节点的方式生成随机扩展树。令xstart为出发点，xend为目标点，a为规划空间。在空间a中寻找一条从出发点xstart到目标点xend的连续航迹f(a)，且满足
在增加叶节点时，设定飞行步长为l，确定暂时目标点xtarget，在现有的节点中寻找距离暂时目标点xtarget最近的叶节点xnew，如果在向xtarget行进过程中遇到障碍或进入威胁区，则重新选择暂时目标点xtarget，继续迭代计算。如果行进到xtarget过程中没遇到障碍或威胁，则添加xnew到随机树中，建立节点之间的链接关系。其示意图如下图2所示：
节点扩展公式如下：
迭代计算，直到暂时目标点xtarget到目标点xend的距离小到某一个阀值e，随机树生长完成，其判断条件如下：
distance(xend,xnew)≤e(3)
其算法流程图如下图3所示。
(3)改进随机点的选择
基本rrt算法，在树节点扩展时随机在空间采点，这种无序性降低了有效路径的生成。借鉴稀疏a*算法(sas)稀疏化方法，同时，引入转弯角度约束条件。在规划空间扩展节点时，并不遍历该区域内每一个位置单元，只考虑其中的若干个扇面。这样可以降低空间采样密度，使树状路径稀疏化，有效加快路径生成速度。
如图4所示，θ表示最大转弯角，取值范围与转弯半径约束条件对应，最小转弯半径对应最大转弯角；α表示节点扩展角，通过调整α角度，将扩展节点导向有效空白区域，加快运算速度，提高搜索效率。
(4)加入总航程约束
由于飞行器携带燃料有限，其总航程或航时均有要求，因此在进行航迹规划时需要将航程约束考虑进去。给定当前节点xnew，当满足下述公式(5)中条件时，将xnew加入到树分支中。此时，d(x)是从起始位置xstart到xnew经过的真实航程，sl(x)是从xnew到目标点xend的直线距离，此直线距离小于航迹实际要经过的路径长度。若不满足公式(5)中条件，则此树分支为无效分支路径。此方法可以有效避免过长路径的生成。
d(x)+sl(x)≤dmax(5)
(5)冗余节点筛选
为减少导航误差，飞行器远距离飞行时一般不希望迂回飞行和频繁转弯。设经过rrt算法求解的原始节点序列为{node1,...noden}，其中noden为终点位置，将经过冗余节点剪裁后的节点序列集合记为φ，设φ初始为空，令j＝n，则基本的节点剪裁过程如下：首先将nodej添加到φ，i∈[1,...,j-1]，循环检查(nodei,nodej)之间的连线是否存在障碍或者威胁，如果存在，则令i＝i+1；否则，只要检测出第一个没有障碍的节点wpi，就停止循环，令j＝i，并将nodei加入φ。重复上述循环过程，直到j＝1时结束，可以快速实现rrt冗余节点的剪裁。
增加航程约束后冗余节点裁剪前后结果对比如下表1所示，经过10次计算后取平均值，剪裁后计算时间减少25％，航路点减少78％，航程减少10％，这非常有利于巡飞弹的快速实时航迹规划。
表1.冗余节点剪裁前后数据对比
具体实施例：
第一步：通过问题描述，建立无人机航迹规划的数学模型，明确规划空间的表示方法，飞行航迹的表示方法，以及飞行航迹的约束条件；
第二步：如图3所示，对于给定的状态空间、出发点和目标点，给定的出发点为树的根节点，通过逐渐增加叶节点的方式生成随机扩展树(rrt)；
第三步：引入无人机转弯角度约束条件，在规划空间扩展节点时，不遍历该区域内每一个单元，只考虑其中满足约束条件的扇面如图4所示，θ表示最大转弯角，取值范围与转弯半径约束条件对应，最小转弯半径对应最大转弯角；α表示节点扩展角，通过调整α角度，将扩展节点导向有效空白区域，加快运算速度，提高搜索效率，从而降低空间采用密度，加快航迹生成速度；
第四步：加入总航程约束，由于飞行器携带燃料有限，其总航程或航时均有要求，因此在进行航迹规划时需要将航程约束考虑进去。给定当前节点xnew，当满足下述公式(5)中条件时，将xnew加入到树分支中。
第五步：冗余节点裁剪，裁剪方式为：设经过rrt算法求解的原始节点序列为{node1,...noden}，其中noden为终点位置，将经过冗余节点剪裁后的节点序列集合记为φ，设φ初始为空，令j＝n，则基本的节点剪裁过程如下：首先将nodej添加到φ，i∈[1,...,j-1]，循环检查(nodei,nodej)之间的连线是否存在障碍或者威胁，如果存在，则令i＝i+1；否则，只要检测出第一个没有障碍的节点wpi，就停止循环，令j＝i，并将nodei加入φ。重复上述循环过程，直到j＝1时结束，可以快速实现rrt冗余节点的剪裁。
第六步：建立六自由度飞行动力学仿真模型，如下图5所示。气动数据采用风洞吹风实验数据，控制系统采用btt控制设计参数。