[发明专利]一种基于改进RRT算法的无人机航迹规划方法

权利要求书

1.一种基于改进RRT算法的无人机航迹规划方法，其特征在于步骤如下：

步骤1：通过问题描述，明确规划空间的表示方法，飞行航迹的表示方法，以及飞行航迹的约束条件：

规划空间的表示方法：在三维空间中进行航迹规划，设(x,y,z)为规划空间某点坐标，x,y分别表示经纬度，z表示海拔高度；则离散化空间可表示为集合：

Ω＝{(x,y,z)|0≤x≤Xmax,0≤y≤Ymax,0≤z≤Zmax}

为简化问题，设定巡飞弹在固定高度巡飞，将三维空间路径搜索问题简化为二维问题；

飞行航迹的表示方法：采用飞行器空间位置的时间序列来表示，即飞行过程中的任一时刻，飞行器的运动状态表现为三维空间中的航迹位置；

飞行航迹的约束条件：(1)最小航迹段长度，即飞行器在开始改变飞行姿态前必须保持直飞的最短距离，由机动能力和导航要求决定；设飞行航迹中的直线段由{l_i|i＝1,...,n}组成，最小航迹段长度为l_min，该约束可表示为：l_i≥l_min，其中i＝1,...,n；(2)最小转弯半径，最小转弯半径约束和最大转弯角约束起同样的约束作用，限制飞行器在机动能力范围内转弯；(3)最大总航程，它取决于所携带的燃料以及任务执行时间限制，设飞行航迹由{l_i|i＝1,...,n}组成，最大总航程为L_max，则该约束可写成

步骤2：对于给定的状态空间、出发点和目标点，给定的出发点为树的根节点，采用快速扩展随机树算法RRT通过逐渐增加叶节点的方式生成随机扩展树：

令X_start为出发点，X_end为目标点，A为规划空间，在空间A中寻找一条从出发点X_start到目标点X_end的连续航迹f(a)，且满足

在增加叶节点时，设定飞行步长为L，确定暂时目标点X_target，在现有的节点中寻找距离暂时目标点X_target最近的叶节点X_new，如果在向X_target行进过程中遇到障碍或进入威胁区，则重新选择暂时目标点X_target，继续迭代计算；如果行进到X_target过程中没遇到障碍或威胁，则添加X_new到随机树中，建立节点之间的链接关系；

节点扩展公式如下：

迭代计算，直到暂时目标点X_target到目标点X_end的距离小到某一个阀值e，随机树生长完成，其判断条件如下：

distance(X_end,X_new)≤e

步骤3：引入无人机转弯角度约束条件：

在规划空间扩展节点时，不遍历该区域内每一个单元，采用扇形区域，α表示节点扩展角，在区间范围内调整α，满足扩展节点最少，节点扩展运算速度最短的α，即得到最优的“扇形区域角度”；所述的θ表示最大转弯角，取值范围与转弯半径约束条件对应，最小转弯半径对应最大转弯角；

步骤4：加入总航程约束：

给定当前节点X_new，当满足下式中条件时，将X_new加入到树分支中：

D(x)+SL(x)≤L_max

其中，L_max为无人机的最大航程，D(x)是从起始位置X_start到X_new经过的真实航程，SL(x)是从X_new到目标点X_end的直线距离，此直线距离小于航迹实际要经过的路径长度；若不满足上式条件，则此树分支为无效分支路径；

步骤5：冗余节点裁剪：

设经过步骤2-4后求解的节点序列为{X_start,...X_j...,X_end}，其中X_end为终点位置，将经过冗余节点剪裁后的节点序列集合记为Φ，设Φ初始为空，令j＝N，N为节点的总数量，则节点剪裁过程如下：首先将X_j添加到Φ，i∈[1,...,j-1]，循环检查(X_i,X_j)之间的连线是否存在障碍或者威胁，如果存在，则令i＝i+1；否则，只要检测出第一个没有障碍的节点X_i，就停止循环，令j＝i，并将X_i加入Φ；重复上述循环过程，直到j＝1时结束；

步骤6：气动数据采用风洞吹风实验数据，控制系统采用BTT控制设计参数，航路点坐标系采用北天东坐标系，坐标原点取出发点(0,0,0)，X轴指向北为正，Y轴位于铅垂面指向天向为正，Z轴垂直于0XY组成的平面，组成右手坐标系，指向东为正；

将步骤1到步骤5规划得到的航迹点作为飞控模型航路点进行飞行器六自由度飞行，得到的实际飞行轨迹即为无人机的航迹。