本发明公开了一种用于无人机自动避撞的最优常值导引指令求解方法，属于无人机飞行控制技术领域，通过基于无人机与侵入机相对几何关系的碰撞检测，首先利用几何方法和过载约束求解常值导引指令可行域并通过粒子群算法进行避撞完成时间估计，接着建立最优常值导引指令的目标评价函数，通过遍历的方法求解使得目标函数取最小值的导引指令，即为最优常值导引指令。本发明能够实现无人机自动避撞，完成避撞时间在线估计，并且实现避撞机动飞行过程能耗最少。
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)所示；
避撞机与侵入机的相对速度Vrel(t)表达式如公式(3)所示：Vrel(t)＝Vcos(ψrel-ψ(t))+VOBcos(π+ψOB-ψrel(t)) (3)；
相对速度方位角ψrel(t)表达式如公式(4)所示：视线角λ(t)表达式如公式(5)所示：
检测到两无人机将要发生碰撞时，如果不采取避撞机动，记两无人机在时间T0后发生碰撞；如果采取避撞机动，则两无人机相对速度Vrel(t)能在有限时间T1内与碰撞锥边界重合，T1满足式(6)；
导引指令的下界amin应使得不等式(2)、(6)等号成立；
其中， 为避撞机的航向角变化率；φ(t)为避撞机的滚转角；a(t)为常值导引指令，g为重力加速度；由于避撞过程中只改变避撞机的速度方向，速度的大小不变，因此避撞机的飞行速度V为常数；根据式(7)可以得知避撞机的航向角变化率 也为常值；
任意时刻避撞机的航向角ψ(t)可由下式(8)求得；
将式(6)和(8)代入式(2)可求解导引指令可行域的下界amin；
导引指令的上界amax受避撞机过载能力制约，导引指令上界取避撞机最大的滚转角φ(t)max；
步骤3：建立最优常值导引指令的目标函数f(X)，通过遍历常值导引指令可行域的方法求解目标函数的最小值f(X)min，则目标函数最小值f(X)min对应的导引指令值即为最优常值导引指令。
2.根据权利要求1所述的用于无人机自动避撞的最优常值导引指令求解方法，其特征在于，在步骤2中，具体包括如下步骤：步骤2.1：基于粒子群算法的避撞时间T1求解；
初始化一群粒子的种群粒子个数、粒子随机位置以及速度；由于求解避撞时间的模型为关于时间t的非线性方程组，因此粒子的维度为1；
fitness1(t)＝|ε(t)-θ(t)| (9)；
其中，ε(t)为相对速度方位角ψrel(t)和视线角λ(t)的差值，θ(t)为碰撞锥的半顶角，可由式(10)求得：其中，RT(t)为两无人机的相对距离，RS为给定的常值安全距离；
联立式(1)、(3)、(4)、(5)、(10)可求得fitness1(t)；
对每个粒子，将当前的适应度fitness1(t)与其历史最好位置pj的适应度fitness1(pj)比较，若fitness1(t)＜fitness1(pj)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；
对于每个粒子，将当前的适应度fitness1(t)与其全局经历的最好位置Pg的适应度fitness1(pg)比较，若fitness1(t)＜fitness1(pg)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；
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均为常数；
其中，wmax、wmin分别为权重因子w的最大值和最小值，均为常数；l为当前迭代步数，lmax为最大迭代步数；
xi(t+1)＝xi(t)+vi(t+1) (13)；
其中，xi(t+1)和xi(t)分别为t+1时刻和t时刻第i个粒子的位置；
步骤2.1.6：若达到最大迭代次数或者适应度达到预定的标准，则求解结束，返回此时的最优个体即避撞时间T1；
到达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.1.6：若达到最大迭代次数或者适应度达到预定的标准，则求解结束，返回此时的最优个体即避撞时间T2；
步骤2.3：总的避撞完成时间T由式(16)求解：T＝T1+T2 (16)。
3.根据权利要求1所述的用于无人机自动避撞的最优常值导引指令求解方法，其特征在于，在步骤3中，具体包括如下步骤：步骤3.1：计算两无人机的最小间距RTmin与安全距离RS的差值ES；
根据姿态控制回路响应过程可近似估计最终相对速度Vrel(t)与碰撞锥边界的夹角ξ(T1+t0)，其表达式如公式(17)所示：其中，a(T1)为T1时刻的导引指令，φ(T1)为T1时刻的滚转角，t0为姿态控制回路的响应时间，可由式(18)求解；
根据余弦定理可求出ES，其表达式如公式(19)所示：步骤3.2：通过单位里程耗油量与避撞过程的航迹长度S的乘积估计整个避撞过程的耗油量作为能耗指标P；
步骤3.2.1：求取离散航迹点间距之和近似为T1时间段避撞机航迹的长度S1；
使两无人机的相对速度Vrel(t)跟踪到碰撞锥边界的避撞机航迹，即T1时间段的避撞机航迹；取0.1s为采样时间间隔，对避撞机航迹进行离散化，S1可由式(20)求得步骤3.2.2：求取避撞机维持T1时刻航向角水平飞行追踪避撞点的航迹S2，可由式(21)求得其中，x(T)和y(T)为避撞完成时的避撞机位置，x(T1)和y(T1)为T1时刻避撞机的位置；
步骤3.2.3：求取避撞机避撞航迹总长度S，可由式(22)求得；
步骤3.2.4：求取避撞过程的总能耗，可由式(23)求得；
步骤3.3：建立最优常值导引指令的目标函数f(X)，由式(24)所示：其中，Xj为评价指标，ωj为给定的权重系数，m为评价指标的个数，且满足式(25)；
步骤3.4：将T1、ES和P经过如式(26)所示的量化处理得到 和将量化处理后的数据 和 代入式(24)，通过遍历的方法得到f(X)min对应的导引指令即为最优常值导引指令。
一种用于无人机自动避撞的最优常值导引指令求解方法
技术领域
[0001] 本发明属于无人机飞行控制技术领域，具体涉及一种用于无人机自动避撞的最优常值导引指令求解方法。
背景技术
[0002] 随着无人机在侦查、搜救、运输、军事等多个领域的广泛使用，其飞行活动量的不断增加对空域环境内的其他飞行器以及地面第三方带来很大的安全隐患。无人机与有人机共享空域飞行是未来的发展趋势，因而避撞撞问题也成为制约无人机发展的关键挑战之一。2015年，飞行员遭遇无人驾驶飞机的次数，已经是2014年的2.7倍，超过600次。无人驾驶飞机对飞航安全威胁越来越大。其主要原因是无人机不具备自动避撞能力，自动避撞技术已成为当前无人机研究的热点。
[0003] 在无人机避撞方面的研究，现有的基于最优比例导引的无人机自动避撞方法虽然简化了导引指令的求解过程，但这类以视线角为基础的导引指令仍然是非线性的，而且每一时刻的导引指令需要计算当前无人机飞行状态信息，这些使得避撞时间的估计模型没有解析解。现有的避撞时间估计的方法精确度较差，容易导致避撞失败。
发明内容
[0004] 针对基于比例导引的无人机自动避撞过程中非线性导引指令峰值较大，姿态跟踪性能较差以及避撞时间难以精确估计的问题，本发明提供了一种用于无人机自动避撞的最优常值导引指令求解方法，设计合理，克服了现有技术的不足，具有良好的效果。
[0005] 为了实现上述目的，本发明采用如下技术方案：
[0006] 一种用于无人机自动避撞的最优常值导引指令求解方法，包括以下步骤：
[0007] 步骤1：求解常值导引指令可行域的上、下界；
[0008] 步骤1.1：求解常值导引指令可行域的下界；
[0009] 初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，当避撞机与侵入机两无人机的相对速度Vrel(t)在碰撞锥内，即视线角λ(t)与相对速度方位角ψrel(t)的偏差绝对值|ε(t)|小于碰撞锥的半顶角θ(t)，如公式(1)所示，则两无人机会发生碰撞；
[0010] |λ(t)-ψrel(t)|＝|ε(t)|＜θ(t) (1)；
[0011] 利用避撞机的机载传感器获得此时避撞机的飞行状态信息，包括避撞机初始位置(x0,y0)、飞行速度V和航向角ψ(t)；利用侵入机的机载传感器获得此时侵入机的飞行状态信息，包括侵入机初始位置(xOB,yOB)、飞行速度VOB和航向角ψOB，根据避撞机的飞行状态信息和侵入机的飞行状态信息得出两无人机的相对距离RT(t)，如公式(2)所示；
[0012]
[0013] 其中，RS为给定的常值安全距离；
[0014] 避撞机与侵入机的相对速度Vrel(t)表达式如公式(3)所示：
[0015] Vrel(t)＝Vcos(ψrel-ψ(t))+VOBcos(π+ψOB-ψrel(t)) (3)；
[0016] 相对速度方位角ψrel(t)表达式如公式(4)所示：
[0017]
[0018] 视线角λ(t)表达式如公式(5)所示：
[0019]
[0020] 检测到两无人机将要发生碰撞时，如果不采取避撞机动，记两无人机在时间T0后发生碰撞；如果采取避撞机动，则两无人机相对速度Vrel(t)能在有限时间T1内与碰撞锥边界重合，T1满足式(6)；
[0021] T1≤T0 (6)；
[0022] 导引指令的下界amin应使得不等式(2)、(6)等号成立；
[0023] 避撞机的协调转弯公式如(7)所示：
[0024]
[0025] 其中，为避撞机的航向角变化率；φ(t)为避撞机的滚转角；a(t)为常值导引指令，g为重力加速度；由于避撞过程中只改变避撞机的速度方向，速度的大小不变，因此避撞机的飞行速度V为常数；根据式(7)可以得知避撞机的航向角变化率 也为常值；
[0026] 任意时刻避撞机的航向角ψ(t)可由下式(8)求得；
[0027]
[0028] 将式(6)和(8)代入式(2)可求解导引指令可行域的下界amin；
[0029] 步骤1.2：求解常值导引指令可行域的上界；
[0030] 导引指令的上界amax受避撞机过载能力制约，导引指令上界取避撞机最大的滚转角φ(t)max；
[0031] 步骤2：基于粒子群算法的避撞时间T的估计；
[0032] 步骤3：建立最优常值导引指令的目标函数f(X)，通过遍历常值导引指令可行域的方法求解目标函数的最小值f(X)min，则目标函数最小值f(X)min对应的导引指令值即为最优常值导引指令。
[0033] 优选地，在步骤2中，具体包括如下步骤：
[0034] 步骤2.1：基于粒子群算法的避撞时间T1求解；
[0035] 步骤2.1.1：粒子群初始化
[0036] 初始化一群粒子的种群粒子个数、粒子随机位置以及速度；由于求解避撞时间的模型为关于时间t的非线性方程组，因此粒子的维度为1；
[0037] 步骤2.1.2：计算每个粒子的适应度
[0038] 取适应度函数如公式(9)所示：
[0039] fitness1(t)＝|ε(t)-θ(t)| (9)；
[0040] 其中，ε(t)为相对速度方位角ψrel(t)和视线角λ(t)的差值，θ(t)为碰撞锥的半顶角，可由式(10)求得：
[0041]
[0042] 联立式(1)、(3)、(4)、(5)、(10)可求得fitness1(t)；
[0043] 步骤2.1.3：记录每个粒子的最优位置
[0044] 对每个粒子，将当前的适应度fitness1(t)与其历史最好位置pj的适应度fitness1(pj)比较，若fitness1(t)＜fitness1(pj)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；
[0045] 步骤2.1.4：记录粒子群的最优位置
[0046] 对于每个粒子，将当前的适应度fitness1(t)与其全局经历的最好位置Pg的适应度fitness1(pg)比较，若fitness1(t)＜fitness1(pg)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度；
[0047] 步骤2.1.5：对粒子的速度和位置进行更新；
[0048] 速度vi更新公式如(11)所示
[0049] vi(t+1)＝wvi(t)+c1r1[pi,j-xi(t)]+c2r2[pg-xi(t)] (11)；
[0050] 其中，vi(t+1)和vi(t)分别为t+1时刻和t时刻第i个粒子的速度；pi,j为第i个粒子本身的最好位置；pg为整个粒子群最好位置；xi(t)为t时刻第i个粒子的位置，权重因子w采用惯性权重；ci和ri均为常数；
[0051] 权重因子w更新公式如(12)所示：
[0052]
[0053] 其中，wmax、wmin分别为权重因子w的最大值和最小值，均为常数；l为当前迭代步数，lmax为最大迭代步数；
[0054] 粒子位置xi更新公式如(13)所示：
[0055] xi(t+1)＝xi(t)+vi(t+1) (13)；
[0056] 其中，xi(t+1)和xi(t)分别为t+1时刻和t时刻第i个粒子的位置；
[0057] 步骤2.1.6：若达到最大迭代次数或者适应度达到预定的标准，则求解结束，返回此时的最优个体即避撞时间T1；
[0058] 步骤2.2：基于粒子群算法的避撞时间T2估计
[0059] 到达T1时刻之后，两无人机的相对速度已跟踪到碰撞锥的边界，然后避撞机维持当前的航向角继续追踪避撞点，利用粒子群算法求解避撞机在常值导引指令导引下沿着碰撞锥边界直至到达碰撞点这段时间T2；
[0060] 步骤2.2.1：根据避撞时间T1的估计过程获得初始条件并初始化粒子群算法[0061] 主要包括T1时刻的避撞机的位置(x(T1),y(T1))、避撞机的航向角ψ(T1)、侵入机的位置(xOB(T1),yOB(T1))、侵入机的航向角ψOB；
[0062] 步骤2.2.2：利用粒子群算法对T2时间求解；具体包括如下步骤：
[0063] 步骤2.2.2.1：将步骤2.1.2中式(9)的适应度函数更换为公式(14)；
[0064]
[0065] 其中，xT1(t)和yT1(t)为T1时刻后避撞机的位置； 和 为T1时刻后侵入机的位置，可通过式(15)求解
[0066]
[0067] 步骤2.2.2.2：记录每个粒子的最优位置；
[0068] 步骤2.2.2.3：记录粒子群的最优位置；
[0069] 步骤2.2.2.4：对粒子的速度和位置进行更新；
[0070] 步骤2.1.6：若达到最大迭代次数或者适应度达到预定的标准，则求解结束，返回此时的最优个体即避撞时间T2；
[0071] 步骤2.3：总的避撞完成时间T由式(16)求解：
[0072] T＝T1+T2 (16)。
[0073] 优选地，在步骤3中，具体包括如下步骤：
[0074] 步骤3.1：计算两无人机的最小间距RTmin与安全距离RS的差值ES；
[0075] 根据姿态控制回路响应过程可近似估计最终相对速度Vrel(t)与碰撞锥边界的夹角ξ(T1+t0)，其表达式如公式(17)所示：
[0076]
[0077] 其中，a(T1)为T1时刻的导引指令，φ(T1)为T1时刻的滚转角，t0为姿态控制回路的响应时间，可由式(18)求解；
[0078]
[0079] 根据余弦定理可求出ES，其表达式如公式(19)所示：
[0080]
[0081] 步骤3.2：通过单位里程耗油量与避撞过程的航迹长度S的乘积估计整个避撞过程的耗油量作为能耗指标P；
[0082] 步骤3.2.1：求取离散航迹点间距之和近似为T1时间段避撞机航迹的长度S1；
[0083] 使两无人机的相对速度Vrel(t)跟踪到碰撞锥边界的避撞机航迹，即T1时间段的避撞机航迹；取0.1s为采样时间间隔，对避撞机航迹进行离散化，S1可由式(20)求得[0084]
[0085] 步骤3.2.2：求取避撞机维持T1时刻航向角水平飞行追踪避撞点的航迹S2，可由式(21)求得
[0086]
[0087] 其中，x(T)和y(T)为避撞完成时的避撞机位置，x(T1)和y(T1)为T1时刻避撞机的位置；
[0088] 步骤3.2.3：求取避撞机避撞航迹总长度S，可由式(22)求得。
[0089] S＝S1+S2 (22)；
[0090] 步骤3.2.4：求取避撞过程的总能耗，可由式(23)求得。
[0091] P＝C*S (23)；
[0092] 其中，C是单位里程的耗油量，为常数；
[0093] 步骤3.3：建立最优常值导引指令的目标函数f(X)，由式(24)所示：
[0094]
[0095] 其中，Xj为评价指标，ωj为给定的权重系数，m为评价指标的个数，且满足式(25)；
[0096]
[0097] 步骤3.4：将T1、ES和P经过如式(26)所示的量化处理得到 和
[0098]
[0099] 将量化处理后的数据 和 代入式(24)，通过遍历的方法得到f(X)min对应的导引指令即为最优常值导引指令。
[0100] 本发明具有的有益效果是：
[0101] 本发明能够实现无人机自动避撞，完成避撞时间在线估计，姿态跟踪性能较好，能够在保证避撞安全性的条件下耗能最少，基于粒子群算法的避撞完成时间估计方法准确且求解过程简单。
附图说明
[0102] 图1为用于无人机自动避撞的最优常值导引指令求解方法的流程图。
[0103] 图2为避撞机与侵入机相对几何关系图。
[0104] 图3为避撞机机动轨迹长度曲线图。
[0105] 图4为最优常值导引下避撞机避撞仿真图。
具体实施方式
[0106] 下面结合附图以及具体实施方式对本发明作进一步详细说明：
[0107] 本发明提出了一种用于无人机自动避撞的最优常值导引指令求解方法，根据侵入机和避撞机的相对几何关系检测两无人机是否会发生碰撞。如果不发生碰撞，无人机继续正常飞行飞往目标点。如果检测到两无人机会发生碰撞，则计算导引指令可行域，根据粒子群算法估计避撞完成时间，建立目标评价函数，通过遍历的方法求解目标评价函数最小值对应的导引指令即最优常值导引指令。
[0108] 一种用于无人机自动避撞的最优常值导引指令求解方法，其流程图如图1所示，包括以下步骤：
[0109] 步骤1：常值导引指令可行域求解；
[0110] 在求解最优常值导引指令之前，首先要求解常值导引指令可行域即可行导引指令的上、下界，初始状态下避撞机处于正常飞行模式，沿预定的正常飞行轨迹飞往目标点，当避撞机与障碍(侵入机)的相对速度Vrel(t)在碰撞锥内，即视线角λ(t)与相对速度方位角ψrel(t)的偏差绝对值|ε(t)|小于碰撞锥的半顶角θ(t)，公式为：
[0111] |λ(t)-ψrel(t)|＝|ε(t)|＜θ(t) (1)；
[0112] 如果满足式(1)则两个无人机会发生碰撞。利用机载传感器获得此时避撞机和侵入机的飞行状态信息，飞行状态信息包括避撞机初始位置(x0,y0)、飞行速度V和航向角ψ(t)，侵入机的位置(xOB,yOB)、速度VOB和航向角ψOB，根据飞行状态信息得出两无人机的相对距离RT(t)为：
[0113]
[0114] 其中，RS为给定的常值安全距离。
[0115] 两无人机相对速度Vrel(t)为：
[0116] Vrel(t)＝Vcos(ψrel-ψ(t))+VOBcos(π+ψOB-ψrel(t)) (3)；
[0117] 相对速度方位角ψrel(t)可由式(4)获得。
[0118]
[0119] 视线角λ(t)为：
[0120]
[0121] 检测到两无人机将要发生碰撞时，如果不采取避撞机动，记两无人机在时间T0后发生碰撞。如果采取避撞机动，则两无人机相对速度Vrel(t)能在有限时间T1内碰撞锥边界重合。T1满足式(6)。
[0122] T1≤T0 (6)；
[0123] 导引指令的下界amin应使得不等式(2)、(6)等号成立。根据协调转弯公式：
[0124]
[0125] 其中，为无人机航向角变化率；φ(t)为避撞机的滚转角；a(t)为常值导引指令；g为重力加速度。由于避撞过程中只改变避撞机的速度方向，速度的大小不变，因此避撞机速度V为常数。根据式(7)可以得知避撞机的航向角变化率 也为常值，任意时刻避撞机航向角ψ(t)可由下式(8)求得。
[0126]
[0127] 将式(6)和(8)代入式(2)可求解导引指令的下界amin。
[0128] 导引指令的上界amax受无人机过载能力制约，导引指令上界取避撞机最大的滚转角φ(t)max。
[0129] 步骤2：基于粒子群算法的避撞时间估计；
[0130] 导引指令引导避撞机避撞的过程分为两个阶段：第一阶段使两无人机的相对速度Vrel(t)跟踪到碰撞锥的边界；第二阶段为避撞机追踪避撞点的过程，该段时间内的导引指令幅值为0，避撞机不再滚转。避撞机到达避撞点则避撞完成，以后避撞机进入飞向目标点的正常飞行模式。因此总的避撞时间T包括两部分。采用粒子群算法分别对两部分避撞时间进行求解。
[0131] 1.基于粒子群算法的避撞时间T1求解；
[0132] 1)粒子群初始化
[0133] 初始化一群粒子(方程组的解)，包括种群粒子个数、粒子随机位置和速度；由于求解避撞时间的模型为关于时间t的非线性方程组，因此粒子的维度为1。
[0134] 2)计算每个粒子的适应度(fitness)
[0135] 取适应度函数
[0136] fitness1(t)＝|ε(t)-θ(t)| (9)；
[0137] 其中，ε(t)为相对速度方位角ψrel(t)和视线角λ(t)的差值，fitness1(t)可联立式(1)、(3-5)求得。θ(t)为碰撞锥的半顶角，可由式(10)获得。
[0138]
[0139] 3)记录每个粒子最优位置
[0140] 对每个粒子，将当前的适应度fitness1(t)与其历史最好位置Pj的适应度fitness1(Pj)比较，若fitness1(t)＜fitness1(pj)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度。
[0141] 4)记录粒子群的最优位置
[0142] 对于每个粒子，将当前的适应度fitness1(t)与其全局经历的最好位置Pg的适应度fitness1(Pj)比较，若fitness1(t)＜fitness1(pg)，则将当前位置作为最优位置，将当前位置的适应度作为最优位置的适应度。
[0143] 5)对粒子的速度和位置进行更新
[0144] 速度vi更新公式如(11)所示
[0145] vi(t+1)＝wvi(t)+c1r1[pi,j-xi(t)]+c2r2[pg-xi(t)] (11)；
[0146] 其中，vi(t+1)和vi(t)分别为t+1时刻和t时刻第i个粒子的速度；pi,j为第i个粒子本身的最好位置；pg为整个粒子群最好位置；权重因子w采用惯性权重；ci和ri均为常数；
[0147] 权重因子w更新公式如(12)所示：
[0148]
[0149] 其中，wmax、wmin分别为权重因子w的最大值和最小值，均为常数；l为当前迭代步数，lmax为最大迭代步数；
[0150] 粒子位置xi更新公式如(13)所示：
[0151] xi(t+1)＝xi(t)+vi(t+1) (13)；
[0152] 其中，xi(t+1)和xi(t)分别为t+1时刻和t时刻第i个粒子的位置。
[0153] 6)若达到最大迭代次数或者适应度达到预定的标准，则求解结束。返回此时的最优个体即导引指令引导避撞机滚转的避撞时间T1。
[0154] 2.基于粒子群算法的避撞时间T2估计
[0155] 到达T1时刻之后，两无人机的相对速度已跟踪到碰撞锥的边界，然后避撞机维持当前的航向角继续追踪避撞点。避撞机在常值导引指令导引下沿着碰撞锥边界直至到达碰撞点这段时间T2同样利用粒子群算法求解。
[0156] 1)首先根据的估计过程获得初始条件。
[0157] 主要包括T1时刻的避撞机的位置(x(T1),y(T1))、避撞机航向角ψ(T1)、侵入机的位置(xOB(T1),yOB(T1))、侵入机航向角ψOB等。
[0158] 2)利用粒子群算法进行T2时间求解。参数变量仍然选择时间t，适应度函数更换为：
[0159]
[0160] 其中，xT1(t)和yT1(t)为T1时刻后避撞机的位置， 和 为T1时刻后侵入机的位置，可通过式(15)求解
[0161]
[0162] 重新执行对避撞时间T1的估计过程即可求得避撞时间T2。
[0163] 3)总的避撞完成时间T则可由式(16)求解：
[0164] T＝T1+T2 (16)；
[0165] 步骤3：建立最优常值导引指令的目标评价函数f(X)，通过遍历常值导引指令可行域的方法进行求解目标评价函数的最小值f(X)min。则目标函数最小值f(X)min对应的导引指令值即为最优常值导引指令。
[0166] 建立的最优指标函数f(X)主要包括三个指标：
[0167] 1)避撞机的滚转时间T1
[0168] 避撞机滚转时间T1作为避撞时间的一部分，可通过步骤2的粒子群算法进行估计。导引指令越大，两无人机的相对速度Vrel(t)收敛到碰撞锥的边界的速度越快即避撞机滚转时间T1越短。则避撞机滚转机动能耗越少，避撞机水平飞行时间T2越长，总的避撞过程能耗相对越少。
[0169] 2)两无人机最小间距RTmin与安全距离RS的差值ES
[0170] 图2中，两无人机最小间距RTmin与安全距离RS的差值ES体现了避撞机避撞完成的精准程度。根据姿态控制回路响应过程可近似估计最终相对速度Vrel(t)与碰撞锥边界的夹角ξ(T1+t0)，公式为：
[0171]
[0172] 其中，a(T1)为T1时刻的导引指令，φ(T1)为T1时刻的滚转角。t0为姿态控制回路的响应时间，可由式(18)求解。
[0173]
[0174] 根据余弦定理可直接计算ES，公式为：
[0175]
[0176] 根据式(17)，如果导引指令较大，那么最终相对速度Vrel(t)与碰撞锥边界的夹角越大，最终导致ES较大，避撞机的实际机动范围将越大，避撞过程能耗将越大。
[0177] 3)避撞过程的能耗P
[0178] 相同初始条件的避撞过程中，采用不同的导引指令，避撞机的舵偏机动大小不同，避撞机的机动范围也不同，因此完成避撞所需的能耗不同。采用单位里程耗油量与避撞过程的航迹长度S的乘积估计整个避撞过程的耗油量作为能耗指标P。避撞过程的避撞航迹长度如图3所示。避撞过程的轨迹长度包括两部分：第一部分为使两无人机的相对速度Vrel(t)跟踪到碰撞锥边界的避撞机航迹，即T1时间段的避撞机航迹。取0.1s为采样时间间隔，对避撞机航迹进行离散化，求取离散航迹点间距之和近似为T1时间段避撞机航迹的长度S1。公式为：
[0179]
[0180] 第二部分为避撞机维持T1时刻航向角水平飞行追踪避撞点的航迹S2，可由式(21)求得。
[0181]
[0182] 其中，x(T)和y(T)为避撞完成时的避撞机位置。x(T1)和y(T1)为T1时刻避撞机的位置。避撞机避撞航迹总长度S可由式(22)求得。
[0183] S＝S1+S2 (22)；
[0184] 避撞机的避撞过程的总能耗为：
[0185] P＝C*S (23)；
[0186] 其中，C是单位里程的耗油量，为常数。由于三个评价指标并不是相互独立，各指标间有较强的关联性，因此避撞过程性能评价指标函数f(X)由式(24)所示。
[0187]
[0188] 其中Xj为评价指标，ωj为给定的权重系数。m为评价指标的个数取为3。且满足式(25)。
[0189]
[0190] 和 是步骤2和式(18-23)获得的导引指令域的3个对应评价指标数据T1、ES和P经过量化处理得到的数据。处理方法如式(26)。
[0191]
[0192] 将量化处理后的数据 和 代入式(24)通过遍历的方法得到f(X)min对应的导引指令即为最优常值导引指令。基于最优常值导引指令的无人机自动避撞仿真如图4所示。
