技术领域
[0001] 本
发明涉及无人机技术领域，尤其涉及一种多无人机协调避碰导引规划方法。
背景技术
[0002] 以无人机为代表的新一代
无人飞行器(Unmanned Aerial Vehicle,UAV)要替代有人驾驶飞机在具有非结构化环境中执行各类任务，也开始具备一定的自主飞行能
力。由于多机共同执行任务会产生很强大的功能，也有很广阔的应用前景，已经得到人们越来越多的关注。在对多无人机之间的避碰导引飞行中，目前一般将其它无人飞行器视为移动的障碍物来进行避碰，没有采用协调策略，这样可能会使编队内多个无人飞行器同时进行机动，导致编队内所有飞行器都偏离期望
位置太远或发生震荡，影响到达时间，从而不能有效发挥多机协同的效能。
[0003] 这个问题如果不能得到很好的解决，就不能很好的控制导引规划的收敛性能和抗扰
动能力，也难以实现多机的协调控制，因此提出多无人机在复杂动态环境下的避碰协调策略。
[0004] 在遭遇到非预见的威胁或者任务变更时，自主式UAV必须在线自主规划新的航迹。由于UAV平台是高速实时运动的，平台的自主控制系统必须实时操作和计算，因此，很多耗时、计算量大的离线方法和一些智能优化
算法将很难直接适用与UAV的航迹规划。
[0005] 有的文献基于随机搜索的规划方法，比如快速搜索随机树法、
遗传算法、稀疏A*算法等，通过将目标区域进行划分，找出能避碰的航迹点，UAV沿着航迹点飞行至目标点，使其满足UAV最小
转弯半径等的需求。但上述随机搜索方法比较耗时，且需根据航迹点对航迹进行平滑。
[0006] 一些学者提出应用曲线来直接规划整个航迹，目前，Pythagorean Hodograph(PH)曲线被广泛应用于飞行器的在线航迹规划中，其具有
曲率连续、能克服
传感器的不确定性及航迹上的点能精确可知的优点，PH曲线生成可基于Bézier样条曲线，因此其还拥有Bézier曲线规划的优点。其中五次PH曲线是最低阶含有拐点的PH曲线。拐点使航迹有更好的柔性，进而能更好的规划航迹。发明内容
[0007] 为了解决目前多无人机之间避碰导引飞行中由于没有采用有效的协调策略而导致的多机协同效能不能有效发挥的问题，本发明的发明目的在于提供一种多无人飞行器协调避碰导引规划方法，本发明基于Pythagorean Hodograph(PH)曲线航迹的全局规划与基于相互速度避碰原则的Legendre伪谱法局部规划相结合的航迹规划方法，可用于处理多无人飞行器及其它自主运动的多运动体避碰的情况。
[0008] 本发明的发明目的通过以下技术方案实现：
[0009] 一种多无人飞行器协调避碰导引规划方法，包含以下步骤：
[0010] 1)根据各UAV规划的起始点、结束点及运动速度方向，按照PH曲线的生成方法，结合各UAV飞行时的动态性能约束，给出各UAV的PH曲线航迹作为全局规划航迹；
[0011] 2)根据各PH曲线航迹来判断各UAV之间是否会发生碰撞及碰撞点，求出碰撞点位置后，根据UAV速度大小，计算碰撞时间；
[0012] 3)若存在碰撞，则对UAV进行速度大小和方向的改变，对单个UAV可能与多个UAV发生碰撞的情况，采用速度障碍方法计算出各UAV的相对避碰区域作局部避碰规划；
[0013] 4)以相对避碰区域的开始点作为局部规划航迹的起点，采用伪谱法生成局部规划航迹，将UAV引入局部规划航迹；
[0014] 5)判断局部规划航迹是否已到达局部避碰规划的终点，若是，则UVA转入全局规划航迹。
[0015] 依据上述特征，所述步骤1)中动态性能约束包含以下约束条件：
[0016] 条件1：UAV飞行路径上的z坐标不能超过最大爬升高度；
[0017] 条件2：UAV飞行路径上的速度方向与
水平面的夹
角不能超过最大爬升角；
[0018] 条件3：UAV飞行路径上的曲率绝对值不能大于最大曲率；
[0019] 条件4：UAV飞行路径上的挠率绝对值不能大于最大挠率。
[0020] 依据上述特征，所述步骤3)中在作局部避碰规划时包含以下步骤：
[0021] 步骤3.1)、给出各UAV与其它UAV的所有平均避碰区域，根据各平均避碰区域给出避碰交集区域，UAV的新速度方向在此区域中选择；
[0022] 步骤3.2)、根据各UAV的优先级对避碰的权重系数进行调整，优先级较高的UAV承担较少的避碰任务，所述权重系数根据各预测避碰点处的可能碰撞时间、偏离原航线的程度、偏离目标点的距离进行加权。
[0023] 依据上述特征，所述步骤4)包含以下步骤：
[0024] 步骤4.1)以偏离原航迹最小为性能指标，将包含UAV的运动学微分方程、路径约束、速度大小及方向改变的状态变量和控制变量在一系列LGL点上离散化，并以离散点为
节点构造全局Lagrange插值多项式来逼近状态变量和控制变量；
[0025] 步骤4.2)、通过对插值多项式求导近似状态变量对时间的导数，将微分动态约束转化为代数约束，性能指标中的
积分项由Gauss积分计算，将连续时间最优控制问题转化为具有一系列代数约束的非线性规划问题；
[0026] 步骤4.3)、通过相应的非线性规划求解器得到数值最优解；
[0027] 步骤4.4)、以预估碰撞点为中心、以UAV间的安全飞行距离为2δ，其中δ为方差，设置符合高斯分布的
碰撞检测点，将其作为局部避碰路径规划约束，加入到伪谱法求解避碰航迹中。
[0028] 本发明提出的方法考虑了多UAV之间的相对运动趋势，加入了对速度预测后的动态避碰可飞行航迹规划，相比于基于避碰点位置信息的避碰来说，提高了避碰效率。对多UAV的速度相互协调避碰，增加了UAV的智能自主能力及多UAV的协同能力。基于伪谱法的局部路径生成能较好的满足各种避碰约束及路径偏离最小等约束，提高了求解速度，同时对高斯避碰测试点的引入保证了最终系统的性能。
附图说明
[0029] 图1为基于相对速度信息的单个UAV避碰示意图；
[0030] 图2为多UAV相互协调避碰示意图；
[0031] 图3为基于速度信息的多UAV速度调节避碰示意图；
[0032] 图4为基于相对避碰区域的多UAV避碰示意图；
[0033] 图5为两架无人飞行器二维平面避碰飞行；
[0034] 图6为两架无人飞行器三维空间的避碰飞行；
[0035] 图7为本发明的流程示意图。
[0036] 具体实现方式
[0037] 下面结合本发明
实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实例仅仅是本发明一部分实施例，而不是全部的实例。基于本发明的实例，本领域普通技术人员在没有做出创造性劳动前提下获得的所有其他实例，都属于本发明的保护范围。
[0038] 如图7所示，本发明一种多无人飞行器协调避碰导引规划方法的具体实施步骤如下：
[0039] 一、PH曲线航迹生成
[0040] 设已知各UAV的起始位置ps(xs,ys,zs)和角度(θs,φs)即起始点的
位姿信息poses(xs,ys,zs,θs,φs)，及终止点的位置pf(xf,yf,zf)和角度(θf,φf)即终止点的位姿信息posef(xf,yf,zf,θf,φf)，θ为爬升角，φ为水平航向角。采用PH曲线进行UAV全局航迹规划，设全局航迹为r(t)＝[x(t),y(t),z(t)]，其曲率为κ，挠率为τ，全局航迹规划公式可表示为：
[0041]
[0042] 全局航迹r(t)规划时，若满足如下约束，则UAV航迹为可飞行航迹。
[0043] (条件1)最大爬升高度H约束：UAV飞行路径上的z坐标不能超过H，即z≤H。
[0044] (条件2)最大爬升角θ约束：UAV飞行路径上的速度方向与水平面的夹角θ不能超过最大约束值θmax，即：|θ(t)|≤θmax，
[0045] 其中 θmax为UAV飞行的最大爬升角约束。
[0046] (条件3)最大曲率κmax约束：UAV飞行路径上的曲率κ绝对值不能大于κmax(κmax＞0)，即-κmax≤κ≤κmax，
[0047] 其中，
[0048] (条件4)最大挠率τmax约束：UAV飞行路径上的挠率τ绝对值不能大于τmax(τmax＞0)，即-τmax≤τ≤τmax，
[0049] 其中，
[0050] 采用五次PH曲线进行三维路径规划。选择合适的多项式σ(t)，使其满足σ2(t)2 2 2
＝x'(t)+y'(t)+z'(t)，则可通过对多项式σ(t)的积分算出路径长度s(t)，而σ(t)本身即为速度 曲线的路径长度为：
[0051]
[0052] 式中，均匀变化的曲线参数t可以得到沿曲线均匀变化的路径长度。
[0053] 全局航迹规划可根据UAV的曲率、挠率、爬升角、爬升高度等性能约束生成可飞行的曲率连续的路径，航迹便于
跟踪；曲线上各点的位置可精确求解，便于多UAV航迹的碰撞估计。
[0054] 二、多UAV碰撞判断。
[0055] 根据多UAV的PH曲线航迹，计算各UAV可能的碰撞点，并根据UAV速度范围，找出碰撞时间范围。
[0056] 三、多UAV避碰规划。
[0057] 在速度空间中，针对N个UAV，找出可能与第i个UAV发生碰撞的m个UAV(m＜N)，计算出第i个UAV与每个UAV可能发生碰撞的时间 取避碰导引时间
[0058] 速度避障原理如图1所示，UAVA的速度VA∈VOB→A(VB)，UAVB的速度VB∈VOA→B(VA)，由速度障碍原理可得出，UAVA和UAVB若按当前速度飞行会发生碰撞。设二者相对速度VAB经过ΔV改变后可以避免发生碰撞，如图2所示，且此时速度改变最小。按照和谐避碰原则，若UAVA和UAVB均承担避碰的一半，即 则比只靠单个UAV完成避碰的效率要高。
[0059] 在多UAV系统中，各UAV可能需要和多个UAV避碰，如果均采用平均避障的思想，则可能会与其它UAV发生碰撞，不能达到所有避碰的要求。为此，给出各UAV与其它UAV的所有平均避碰区域，如图2中阴影部分所示。如假设UAVA与多个UAV会发生碰撞，此时，根据各避碰区域给出避碰交集区域(如图4所示)，UAVA的新速度方向在此区域中选择。
[0060] 在UAVA与其它UAV避碰时，根据各UAV的优先级可对避碰的权重进行调整，如UAVA、UAVB避碰时，假设UAVA的优先级较高，其可承担较少的避碰任务，因此设其避碰权重小于0.5。在此，权重系数可根据各预测避碰点处的可能碰撞时间、偏离原航线的程度、偏离目标点的距离等进行加权。
[0061] 四、基于伪谱法的避碰航迹生成
[0062] 应用Legendre伪谱法进行局部避碰路径生成。以偏离原航迹最小为性能指标，将UAV的运动学微分方程、路径约束、速度大小及方向改变等状态变量和控制变量在一系列Legendre-Gauss-Lobatto(LGL)点上离散化，并以离散点为节点构造全局Lagrange插值多项式来逼近状态变量和控制变量。通过对插值多项式求导近似状态变量对时间的导数，将微分动态约束转化为代数约束，性能指标中的积分项由Gauss积分计算。经上述变化，最终可将连续时间最优控制问题转化为具有一系列代数约束的非线性规划问题。随后可通过相应的非线性规划求解器得到数值最优解。与直接考虑微分动态方程求解相比，伪谱法可大大减少计算时间，有利于局部在线实时避碰规划。
[0063] 以预估碰撞点为中心μ、以UAV间的安全飞行距离为2δ，其中δ为方差，设置符合高斯分布的碰撞检测点，将其作为局部避碰路径规划约束，加入到伪谱法求解避碰航迹中。
[0064] 五、全局在线航迹生成
[0065] 将全局航迹规划与局部避碰路径规划相结合，在较短时间内可以找到一条最优航迹。在全局航迹规划后，可得到一组理想的航迹，将其存放在
数据库中。局部避碰规划时，飞行器自主飞行，当预估出与其它UAV的碰撞点后，则启动局部避碰模
块，完成避碰。在规划周期内，对理想路径进行修正，并检测是否运动到所设定的局部目标点，若是，则回到原来的理想航迹。
[0066] 六、多UAV飞行避碰仿真验证
[0067] 图5、图6分别给出了二维及三维避碰仿真结果。从仿真结果可以看出，本发明所提出的速度避碰方法可有效实现多UAV的协调避碰。
