一种固定翼无人机的轨迹规划方法与流程

本发明涉及一种固定翼无人机的轨迹规划方法，属于考虑路径信息的飞行器航迹规划控制策略领域。

背景技术：

固定翼无人机因其低成本，可自主飞行等优良特性，目前在军事和民用领域得到了广泛应用。固定翼无人机航迹规划在固定翼无人机控制策略中显得十分重要，它是一个多目标优化问题，定义为在一定环境和任务目标下，寻找从起始点到目标点的最优飞行路线，同时避开各种威胁源。

常用的航迹规划方法可分为几何方法、启发式搜索方法、势场法等。其中，几何方法首先对环境进行几何建模，后依据一定的最优策略，选择某种搜索算法得到可行解，但当任务空间发生变化，需对任务空间重新遍历，故而不适合动态航迹规划；启发式搜索法包括a*算法，粒子群算法，遗传算法等经典算法，这类方法会随着搜索空间的扩大，这类算法的计算复杂度会呈爆炸式增长，故而实时性不好；势场法中典型的方法是人工势场法，其优点是计算量小，速度快，但其容易陷入局部最优。还有些方法只适用于离线规划，但外界环境因素是不确定的，且固定翼无人机受限于动力学约束，如最大转弯角和检测半径等，因此在应用过程中具有很大的局限性。

固定翼无人机一般具有飞行速度快、飞行高度较高、载重量大、航程长等优势而得到广泛应用，但其在航行过程中频繁转弯代价高。目前，轨迹规划的方法主要是基于单一的动态避障或者静态避障方法，缺乏有效的鉴别障碍物方法，以及没能够合理的发挥出动态避障或者静态避障方法的优势。

技术实现要素：

有鉴于此，本发明提供一种固定翼无人机的轨迹规划方法，通过预判式碰撞检测方法对障碍物进行甄别，同时提出了静态避障和动态避障相结合的轨迹规划方法，以满足不确定环境下的固定翼无人机实时避障的要求和固定翼无人机动力学约束，使其能够达到了良好的动态航迹规划效果。

为达到上述目的，本发明提供如下技术方案：

一种固定翼无人机的轨迹规划方法，包括如下步骤：

步骤一、根据固定翼无人机基本参数设定固定参数，初始化任务要求和相关控制参数等；

步骤二、固定翼无人机通过传感器实时检测飞行前方状况，根据预判式碰撞检测方法，判断障碍物是否真正对固定翼无人机构成威胁；

步骤三、根据实时检测的威胁结果，采取不同的控制策略进行飞行；

步骤四、重复步骤二和步骤三，控制固定翼无人机飞行，直到到达目标位置。

进一步，所述步骤一具体为：所述步骤一具体为：建立xoy二维平面坐标系，将固定翼无人机的基本参数——固定翼无人机的起始位置p0、最小速度vm0、最大速度vm1、最大加速度a、最大转弯角φ、最小安全距离r，传感器最大检测半径r等设定为控制策略的固定参数；初始化任务要求——目标位置p1，同时，初始化基于滚动时域控制策略的模糊粒子群优化方法的滚动时域控制步数t、粒子群最大迭代次数k、粒子群数量n、采样时间δt。

进一步，步骤二所述的预判式碰撞检测方法具体为：(1)使用传感器实时检测同一障碍物的多个轮廓点，筛选出其中相邻连线为凸的多个轮廓点，连接为一个凸多边形，并求该凸多边形的最小外接圆圆心坐标为oi，半径为ri，其中i表示第i个障碍物；(2)如果di-ri≤r，此时视障碍物i为潜在威胁源。di表示固定翼无人机当前位置p与障碍物最小外接圆圆心oi之间的距离；(3)当潜在威胁源满足时，将障碍物i视为真正的威胁源，将继续向此方向飞行视为不可飞区域，其中，ψi为障碍物i和固定翼无人机当前位置连线与x轴正方向的夹角，αi为固定翼无人机当前位置到障碍物i外接圆两切线的夹角，为固定翼无人机当前位置与障碍物i相对速度vi与x轴正方向的夹角；(4)将固定翼无人机当前飞行速度v分解到相对速度vi方向，设为并判断是否近似相等，即为静态障碍物威胁，否则为动态障碍物威胁，其中ε为人为设定的误差范围。

进一步，所述步骤三的具体控制策略为：当没有检测到障碍物威胁时，固定翼无人机朝目标位置方向最大输出前进；当检测到静态障碍物威胁时，采用基于滚动时域控制策略的模糊粒子群优化方法(r-rhc-fpso)进行静态避障飞行控制；当检测到动态障碍物威胁时，采用分段线性人工势场法(plapf)完成动态避障飞行控制。

进一步，基于滚动时域控制策略的模糊粒子群优化方法(r-rhc-fpso)的静态避障飞行控制具体如下：

(1)建立固定翼无人机的动力学模型如下：

其中，i2和o2表示二维单位矩阵和零矩阵，p(k+j|k)、v(k+j|k)、a(k+j|k)分别为第k时刻预测得到的第k+j时刻的固定翼无人机位置向量、速度向量和加速度向量，当其中的j＝0时，分别等于第k时刻的固定翼无人机位置向量p(k)、速度向量v(k)和加速度向量a(k)。

(2)利用粒子滤波算法，用随机分布在固定翼无人机当前位置附近的粒子替代固定翼无人机进行预测估计，用粒子预测位置与障碍物和目标位置的距离来构建最优化目标函数，更新粒子滤波参数，计算n个粒子在之后步数t内的位置和速度，同时，判断每一个粒子的每一步是否在可行域②内，如果不在，则直接删除该粒子。

其中，φ为固定翼无人机转角，d代表当前可检测到的区域，f代表不可飞区域。

(3)选择最优的粒子路径，计算出预测的加速度a(k+j|k)，代回固定翼无人机动力学模型①，指导更新固定翼无人机下一时刻的位置。

进一步，分段线性人工势场法(plapf)的动态避障飞行控制具体如下：

(1)由固定翼无人机到目标点的引力势场和固定翼无人机自身速度势场构建第k时刻的引力势场ua，

即：

ua(k)＝εqρ²g(k)+εv||v(k)||²③

其中，ρg(k)＝||p(k)-p1||表示第k时刻固定翼无人机与目标位置的距离，εq，εv为人为设定的引力增益因数。

(2)由障碍物势场和固定翼无人机与障碍物之间的相对速度势场构建第k时刻的斥力势场uri，即：

其中，ρi(k)表示第k时刻固定翼无人机到障碍物i之间的距离，ηq、ηv为人为设定的斥力增益因数。

(3)将引力势场和斥力势场进行分段线性处理，并计算第k时刻的固定翼无人机与目标点以及检测半径内障碍物i分别相对于x轴的夹角α(k)和θi(k)。

(4)根据公式⑤，指导更新固定翼无人机下一时刻的位置。

其中，px、py分别为固定翼无人机位置p在x轴和y轴方向上的分量。

本发明的有益效果在于：本发明提供了一种固定翼无人机的轨迹规划方法，通过对通过预判式碰撞检测算法结合滚动时域预测方式能使固定翼无人机提前判断障碍物是否构成威胁，及早进行规避操作，减少固定翼无人机进行大机动运动带来的损耗，同时，利用模糊粒子群实时优化求解子问题以实现静态避障，利用分段线性人工势场法完成动态避障，能够有效实现航迹规划，提升其规划速度、稳定性与实时避障能力。

附图说明

为了使本发明的目的、技术方案，本发明提供如下附图进行说明：

图1为一种固定翼无人机的轨迹规划方法流程图；

图2为预判式碰撞检测方法示意图；

图3为实施例1的轨迹规划结果；

图4为实施例2的轨迹规划结果。

具体实施方式

下面将结合附图1，对本发明的优选实施例进行详细的描述。

实施例1：一种固定翼无人机的静态避障轨迹规划方法，包括如下步骤：

步骤一、根据固定翼无人机基本参数设定固定参数，初始化任务要求和相关控制参数等；

步骤二、固定翼无人机通过传感器实时检测飞行前方状况，根据预判式碰撞检测方法，判断障碍物是否真正对固定翼无人机构成威胁；

步骤三、采用基于滚动时域控制策略的模糊粒子群优化方法(r-rhc-fpso)进行控制；

步骤四、重复步骤二和步骤三，控制固定翼无人机飞行，直到到达目标位置。

进一步，所述步骤一具体为：建立xoy二维平面坐标系，按照表1设定参数。

进一步，结合附图2，步骤二所述的预判式碰撞检测方法具体为：(1)使用传感器实时检测同一障碍物的多个轮廓点，筛选出其中相邻连线为凸的多个轮廓点，连接为一个凸多边形，并求该凸多边形的最小外接圆圆心坐标为oi，半径为ri，其中i表示第i个障碍物；(2)如果di-ri≤r，此时视障碍物i为潜在威胁源。di表示固定翼无人机当前位置p与障碍物最小外接圆圆心oi之间的距离；(3)当潜在威胁源满足时，将障碍物i视为真正的威胁源，将继续向此方向飞行视为不可飞区域，其中，ψi为障碍物i和固定翼无人机当前位置连线与x轴正方向的夹角，αi为固定翼无人机当前位置到障碍物i外接圆两切线的夹角，为固定翼无人机当前位置与障碍物i相对速度vi与x轴正方向的夹角；(4)将固定翼无人机当前飞行速度v分解到相对速度vi方向，设为并判断是否近似相等，即为静态障碍物威胁，否则为动态障碍物威胁，其中ε为人为设定的误差范围。

采用matlab编程仿真，可以得到如图3所示的规划线路结果。

实施例2：一种固定翼无人机的动态避障轨迹规划方法，包括如下步骤：

步骤一、根据固定翼无人机基本参数设定固定参数，初始化任务要求和相关控制参数等；

步骤二、固定翼无人机通过传感器实时检测飞行前方状况，根据预判式碰撞检测方法，判断障碍物是

否真正对固定翼无人机构成威胁；

步骤三、根据实时检测的威胁结果，采取不同的控制策略进行飞行；

步骤四、重复步骤二和步骤三，控制固定翼无人机飞行，直到到达目标位置。

进一步，所述步骤一具体为：建立xoy二维平面坐标系，按照表1设定参数。

进一步，结合附图2，步骤二所述的预判式碰撞检测方法具体为：(1)使用传感器实时检测同一障碍物的多个轮廓点，筛选出其中相邻连线为凸的多个轮廓点，连接为一个凸多边形，并求该凸多边形的最小外接圆圆心坐标为oi，半径为ri，其中i表示第i个障碍物；(2)如果di-ri≤r，此时视障碍物i为潜在威胁源。di表示固定翼无人机当前位置p与障碍物最小外接圆圆心oi之间的距离；(3)当潜在威胁源满足时，将障碍物i视为真正的威胁源，将继续向此方向飞行视为不可飞区域，其中，ψi为障碍物i和固定翼无人机当前位置连线与x轴正方向的夹角，αi为固定翼无人机当前位置到障碍物i外接圆两切线的夹角，为固定翼无人机当前位置与障碍物i相对速度vi与x轴正方向的夹角；(4)将固定翼无人机当前飞行速度v分解到相对速度vi方向，设为并判断是否近似相等，即为静态障碍物威胁，否则为动态障碍物威胁，其中ε为人为设定的误差范围。

进一步，所述步骤三的具体控制策略为：当没有检测到障碍物威胁时或者检测到静态障碍物威胁时，采用基于滚动时域控制策略的模糊粒子群优化方法(r-rhc-fpso)进行静态避障飞行控制；当检测到动态障碍物威胁时，采用分段线性人工势场法(plapf)完成动态避障飞行控制。

采用matlab编程仿真，可以得到如图4所示的规划线路结果。

通过实施案例可见，本发明能够有效的规避障碍物，实现固定翼无人机的轨迹规划。

表1本发明实施例的相关设定参数

最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其做出各种各样的改变，而不偏离本发明权利要求书所限定的范围。