一种用于无人机自动避撞的最优常值导引指令求解方法技术

本发明专利技术公开了一种用于无人机自动避撞的最优常值导引指令求解方法，属于无人机飞行控制技术领域，通过基于无人机与侵入机相对几何关系的碰撞检测，首先利用几何方法和过载约束求解常值导引指令可行域并通过粒子群算法进行避撞完成时间估计，接着建立最优常值导引指令的目标评价函数，通过遍历的方法求解使得目标函数取最小值的导引指令，即为最优常值导引指令。本发明专利技术能够实现无人机自动避撞，完成避撞时间在线估计，并且实现避撞机动飞行过程能耗最少。

An optimal constant value guidance method for unmanned vehicle automatic collision avoidance

The invention discloses a method for UAV automatic collision avoidance optimal constant guidance instruction method, which belongs to the technical field of UAV flight control, collision detection based on the geometric relation between intrusive machine and UAV, firstly using a geometric method and overload constraint constant guidance command and feasible collision avoidance estimated completion time the particle swarm algorithm, the optimal value function of objective function guiding instructions are then established, by solving the traversal method makes the objective function of guidance instruction from the minimum, is the optimal constant guidance command. The invention can realize the automatic collision avoidance of the UAV, complete the on-line estimation of collision avoidance time, and realize the minimum energy consumption during the collision avoidance maneuver.

【技术实现步骤摘要】
一种用于无人机自动避撞的最优常值导引指令求解方法
本专利技术属于无人机飞行控制
，具体涉及一种用于无人机自动避撞的最优常值导引指令求解方法。
技术介绍
随着无人机在侦查、搜救、运输、军事等多个领域的广泛使用，其飞行活动量的不断增加对空域环境内的其他飞行器以及地面第三方带来很大的安全隐患。无人机与有人机共享空域飞行是未来的发展趋势，因而避撞撞问题也成为制约无人机发展的关键挑战之一。2015年，飞行员遭遇无人驾驶飞机的次数，已经是2014年的2.7倍，超过600次。无人驾驶飞机对飞航安全威胁越来越大。其主要原因是无人机不具备自动避撞能力，自动避撞技术已成为当前无人机研究的热点。在无人机避撞方面的研究，现有的基于最优比例导引的无人机自动避撞方法虽然简化了导引指令的求解过程，但这类以视线角为基础的导引指令仍然是非线性的，而且每一时刻的导引指令需要计算当前无人机飞行状态信息，这些使得避撞时间的估计模型没有解析解。现有的避撞时间估计的方法精确度较差，容易导致避撞失败。
技术实现思路
针对基于比例导引的无人机自动避撞过程中非线性导引指令峰值较大，姿态跟踪性能较差以及避撞时间难以精确估计的问题，本专利技术提供了一种用于无人机自动避撞的最优常值导引指令求解方法，设计合理，克服了现有技术的不足，具有良好的效果。为了实现上述目的，本专利技术采用如下技术方案：一种用于无人机自动避撞的最优常值导引指令求解方法，包括以下步骤：步骤1：求解常值导引指令可行域的上、下界；步骤1.1：求解常值导引指令可行域的下界；初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，当避撞机与侵入机两无人机的相对速度Vrel(t)在碰撞锥内，即视线角λ(t)与相对速度方位角ψrel(t)的偏差绝对值|ε(t)|小于碰撞锥的半顶角θ(t)，如公式(1)所示，则两无人机会发生碰撞；|λ(t)-ψrel(t)|＝|ε(t)|＜θ(t)(1)；利用避撞机的机载传感器获得此时避撞机的飞行状态信息，包括避撞机初始位置(x0,y0)、飞行速度V和航向角ψ(t)；利用侵入机的机载传感器获得此时侵入机的飞行状态信息，包括侵入机初始位置(xOB,yOB)、飞行速度VOB和航向角ψOB，根据避撞机的飞行状态信息和侵入机的飞行状态信息得出两无人机的相对距离RT(t)，如公式(2)所示；其中，RS为给定的常值安全距离；避撞机与侵入机的相对速度Vrel(t)表达式如公式(3)所示：Vrel(t)＝Vcos(ψrel-ψ(t))+VOBcos(π+ψOB-ψrel(t))(3)；相对速度方位角ψrel(t)表达式如公式(4)所示：视线角λ(t)表达式如公式(5)所示：检测到两无人机将要发生碰撞时，如果不采取避撞机动，记两无人机在时间T0后发生碰撞；如果采取避撞机动，则两无人机相对速度Vrel(t)能在有限时间T1内与碰撞锥边界重合，T1满足式(6)；T1≤T0(6)；导引指令的下界amin应使得不等式(2)、(6)等号成立；避撞机的协调转弯公式如(7)所示：其中，为避撞机的航向角变化率；φ(t)为避撞机的滚转角指令；a(t)为常值导引指令，g为重力加速度；由于避撞过程中只改变避撞机的速度方向，速度的大小不变，因此避撞机的飞行速度V为常数；根据式(7)可以得知避撞机的航向角变化率也为常值；任意时刻避撞机的航向角ψ(t)可由下式(8)求得；将式(6)和(8)代入式(2)可求解导引指令可行域的下界amin；步骤1.2：求解常值导引指令可行域的上界；导引指令的上界amax受避撞机过载能力制约，导引指令上界取避撞机最大的滚转角φ(t)max；步骤2：基于粒子群算法的避撞时间T的估计；步骤3：建立最优常值导引指令的目标函数f(X)，通过遍历常值导引指令可行域的方法求解目标函数的最小值f(X)min，则目标函数最小值f(X)min对应的导引指令值即为最优常值导引指令。优选地，在步骤2中，具体包括如下步骤：步骤2.1：基于粒子群算法的避撞时间T1求解；步骤2.1.1：粒子群初始化初始化一群粒子的种群粒子个数、粒子随机位置以及速度；由于求解避撞时间的模型为关于时间t的非线性方程组，因此粒子的维度为1；步骤2.1.2：计算每个粒子的适应度取适应度函数如公式(9)所示：fitness1(t)＝|ε(t)-θ(t)|(9)；其中，ε(t)为相对速度方位角ψrel(t)和视线角λ(t)的差值，θ(t)为碰撞锥的半顶角，可由式(10)求得：联立式(1)、(3)、(4)、(5)、(10)可求得fitness1(t)；步骤2.1.3：记录每个粒子的最优位置对每个粒子，将当前的适应度fitness1(t)与其历史最好位置pj的适应度fitness1(pj)比较，若fitness1(t)＜fitness1(pj)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；步骤2.1.4：记录粒子群的最优位置对于每个粒子，将当前的适应度fitness1(t)与其全局经历的最好位置Pg的适应度fitness1(pg)比较，若fitness1(t)＜fitness1(pg)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；步骤2.1.5：对粒子的速度和位置进行更新；速度vi更新公式如(11)所示vi(t+1)＝wvi(t)+c1r1[pi,j-xi(t)]+c2r2[pg-xi(t)](11)；其中，vi(t+1)和vi(t)分别为t+1时刻和t时刻第i个粒子的速度；pi,j为第i个粒子本身的最好位置；pg为整个粒子群最好位置；xi(t)为t时刻第i个粒子的位置，权重因子w采用惯性权重；ci和ri均为常数；权重因子w更新公式如(12)所示：其中，wmax、wmin分别为权重因子w的最大值和最小值，均为常数；l为当前迭代步数，lmax为最大迭代步数；粒子位置xi更新公式如(13)所示：xi(t+1)＝xi(t)+vi(t+1)(13)；其中，xi(t+1)和xi(t)分别为t+1时刻和t时刻第i个粒子的位置；步骤2.1.6：若达到最大迭代次数或者适应度达到预定的标准，则求解结束，返回此时的最优个体即避撞时间T1；步骤2.2：基于粒子群算法的避撞时间T2估计到达T1时刻之后，两无人机的相对速度已跟踪到碰撞锥的边界，然后避撞机维持当前的航向角继续追踪避撞点，利用粒子群算法求解避撞机在常值导引指令导引下沿着碰撞锥边界直至到达碰撞点这段时间T2；步骤2.2.1：根据避撞时间T1的估计过程获得初始条件并初始化粒子群算法主要包括T1时刻的避撞机的位置(x(T1),y(T1))、避撞机的航向角ψ(T1)、侵入机的位置(xOB(T1),yOB(T1))、侵入机的航向角ψOB；步骤2.2.2：利用粒子群算法对T2时间求解；具体包括如下步骤：步骤2.2.2.1：将步骤2.1.2中式(9)的适应度函数更换为公式(14)；其中，xT1(t)和yT1(t)为T1时刻后避撞机的位置；和为T1时刻后侵入机的位置，可通过式(15)求解步骤2.2.2.2：记录每个粒子的最优位置；步骤2.2.2.3：记录粒子群的最优位置；步骤2.2.2.4：对粒子的速度和位置进行更新；步骤2.1.6：若达到最大迭代次数或者适应度达到本文档来自技高网...

【技术保护点】
一种用于无人机自动避撞的最优常值导引指令求解方法，其特征在于，包括以下步骤：步骤1：求解常值导引指令可行域的上、下界；步骤1.1：求解常值导引指令可行域的下界；初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，当避撞机与侵入机两无人机的相对速度V

【技术特征摘要】
1.一种用于无人机自动避撞的最优常值导引指令求解方法，其特征在于，包括以下步骤：步骤1：求解常值导引指令可行域的上、下界；步骤1.1：求解常值导引指令可行域的下界；初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，当避撞机与侵入机两无人机的相对速度Vrel(t)在碰撞锥内，即视线角λ(t)与相对速度方位角ψrel(t)的偏差绝对值|ε(t)|小于碰撞锥的半顶角θ(t)，如公式(1)所示，则两无人机会发生碰撞；|λ(t)-ψrel(t)|＝|ε(t)|＜θ(t)(1)；利用避撞机的机载传感器获得此时避撞机的飞行状态信息，包括避撞机初始位置(x0,y0)、飞行速度V和航向角ψ(t)；利用侵入机的机载传感器获得此时侵入机的飞行状态信息，包括侵入机初始位置(xOB,yOB)、飞行速度VOB和航向角ψOB，根据避撞机的飞行状态信息和侵入机的飞行状态信息得出两无人机的相对距离RT(t)，如公式(2)所示；其中，RS为给定的常值安全距离；避撞机与侵入机的相对速度Vrel(t)表达式如公式(3)所示：Vrel(t)＝Vcos(ψrel-ψ(t))+VOBcos(π+ψOB-ψrel(t))(3)；相对速度方位角ψrel(t)表达式如公式(4)所示：视线角λ(t)表达式如公式(5)所示：检测到两无人机将要发生碰撞时，如果不采取避撞机动，记两无人机在时间T0后发生碰撞；如果采取避撞机动，则两无人机相对速度Vrel(t)能在有限时间T1内与碰撞锥边界重合，T1满足式(6)；T1≤T0(6)；导引指令的下界amin应使得不等式(2)、(6)等号成立；避撞机的协调转弯公式如(7)所示：其中，为避撞机的航向角变化率；φ(t)为避撞机的滚转角指令；a(t)为常值导引指令，g为重力加速度；由于避撞过程中只改变避撞机的速度方向，速度的大小不变，因此避撞机的飞行速度V为常数；根据式(7)可以得知避撞机的航向角变化率也为常值；任意时刻避撞机的航向角ψ(t)可由下式(8)求得；将式(6)和(8)代入式(2)可求解导引指令可行域的下界amin；步骤1.2：求解常值导引指令可行域的上界；导引指令的上界amax受避撞机过载能力制约，导引指令上界取避撞机最大的滚转角φ(t)max；步骤2：基于粒子群算法的避撞时间T的估计；步骤3：建立最优常值导引指令的目标函数f(X)，通过遍历常值导引指令可行域的方法求解目标函数的最小值f(X)min，则目标函数最小值f(X)min对应的导引指令值即为最优常值导引指令。2.根据权利要求1所述的用于无人机自动避撞的最优常值导引指令求解方法，其特征在于，在步骤2中，具体包括如下步骤：步骤2.1：基于粒子群算法的避撞时间T1求解；步骤2.1.1：粒子群初始化初始化一群粒子的种群粒子个数、粒子随机位置以及速度；由于求解避撞时间的模型为关于时间t的非线性方程组，因此粒子的维度为1；步骤2.1.2：计算每个粒子的适应度取适应度函数如公式(9)所示：fitness1(t)＝|ε(t)-θ(t)|(9)；其中，ε(t)为相对速度方位角ψrel(t)和视线角λ(t)的差值，θ(t)为碰撞锥的半顶角，可由式(10)求得：联立式(1)、(3)、(4)、(5)、(10)可求得fitness1(t)；步骤2.1.3：记录每个粒子的最优位置对每个粒子，将当前的适应度fitness1(t)与其历史最好位置pj的适应度fitness1(pj)比较，若fitness1(t)＜fitness1(pj)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；步骤2.1.4：记录粒子群的最优位置对于每个粒子，将当前的适应度fitness1(t)与其全局经历的最好位置Pg的适应度fitness1(pg)比较，若fitness1(t)＜fitness1(pg)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；步骤2.1.5：对粒子的速度和位置进行更新；速度vi更新公式如(11)所示vi(t+1)＝wvi(t)+c1r1[pi,j-xi(t)]+c2r2[pg-xi(t)](11)；其中，vi(t+1)和vi(t)分别为t+1时刻和t时刻第i个粒子的速度；pi,j为第i个粒子本身的最好位置；pg为...

【专利技术属性】
技术研发人员：盖文东，张宁，张婧，张桂林，李玉霞，
申请(专利权)人：山东科技大学，
类型：发明
国别省市：山东,37