一种时间约束同时到达多无人机路径规划算法的制作方法

本发明一种时间约束同时到达多无人定翼机路径规划算法涉及的是一种无人机机群路径规划的方法，能够实现无人机机群的同时刻到达、相继时间间隔到达的在时间有具体要求的协同飞行。
背景技术：
：时间约束同时到达多无人定翼机路径规划算法会有多架无人机在同时刻到达目标点的时间要求，针对同时到达的问题通常采用路径规划、速度协同控制的策略来完成。只采用路径规划，通常设计等长度路径，无人机每个点的速度确定，在发生掉速、速度控制偏差大时无法实现时间上的协同；对于速度协同控制，缺陷在于速度调节存在上下限制，所以对于多无人机协同飞行，目前尚无较好的解决方法，有必要提供一种新的路径规划方法来满足实际的多无人机机群协同飞行的需求。技术实现要素：本发明所要解决的技术问题是，在固定的硬件平台下所设计的多无人机集结协同飞行速度控制算法能够满足系统实时性的要求，在误差允许范围内能够实现多机集结并同时刻到达的要求。为了解决以上技术问题，本发明提供一种约束同时到达多无人机路径规划算法，其方案如下：在考虑无人机速度、加减速限制，在飞行时间要求下，规划三维参考路径长度，以此为基础在可行路径长度范围内通过半径搜索来确定三维路径信息；再通过飞行时间计算当前速度给定和速度调节值来进行速度实时控制，从而合理的实现多架无人机协同飞行时间要求。本发明一种时间约束同时到达多无人定翼机路径规划算法的具体路径规划算法如下：（1）根据当前速度、终点速度、加减速度、飞行时间计算参考路径长度s参考。（2）根据当前速度、终点速度、加减速度、飞行时间计算不同情况下的飞行路径长度最大范围。（3）以参考路径长度s参考为基准，以无人机起点、终点位姿进行三维路径规划，通过对路径规划中半径进行分段处理，建立半径搜索算法，通过dubins半径搜索建立合理的三维飞行路径；若三维飞行路径不能规划成功，返回步骤（2），重新进行规划。（4）通过比较步骤（2）中不同阶段的路径长度和规划的dubins路径长度，确定速度规划的阶段形式。（5）按照步骤（4）中得到的速度规划性段形式，实时计算当前理论速度给定和剩余飞行距离。（6）若未能准时到达预定地点，根据无人机当前位置信息，按照步骤（3）规划的三维路径计算实际当前剩余飞行距离。（7）按照公式：实时计算当前速度控制值，进行无人机速度控制，从而实现无人机在约束时间内同时到达终点。步骤（3）中所述的重新进行规划，所需的重新规划需要的延时时间为1-20秒，优选为5秒。附图说明图1为本发明一种时间约束同时到达多无人定翼机路径规划算法中计算参考路径长度的示意图；图2为本发明一种时间约束同时到达多无人定翼机路径规划算法中模式1的示意图；图3为本发明一种时间约束同时到达多无人定翼机路径规划算法中模式2的示意图；图4为本发明一种时间约束同时到达多无人定翼机路径规划算法中模式3的示意图；图5为本发明一种时间约束同时到达多无人定翼机路径规划算法中模式4的示意图；图6为本发明一种时间约束同时到达多无人定翼机路径规划算法中模式6的示意图；图7为本发明一种时间约束同时到达多无人定翼机路径规划算法的步骤（5）中，当s>s3时的示意图；图8为本发明一种时间约束同时到达多无人定翼机路径规划算法的工作流程示意图；图9为本发明一种时间约束同时到达多无人定翼机路径规划算法的实施例1的路径规划示的平面示意图；图10为本发明一种时间约束同时到达多无人定翼机路径规划算法的实施例2的路径规划示的三维立体状态示意图。具体实施方式参照附图1-10，本发明一种时间约束同时到达多无人定翼机路径规划算法的具体路径规划算法如下：（1）参照附图1，根据当前速度、无人机加速度、终点速度、协同飞行时间，计算参考路径长度。根据终点速度、当前速度的关系计算参考路径长度：（2）以参考路径长度为基准，通过dubins路径规划中的半径搜索规划三维路径并计算路径长度s，由于半径搜索的阶段不连续性可能会导致路径长度s≥s参考。（3）计算指定路径长度通过半径搜索来实现；根据dubins路径特点，对路径长度和半径的关系进行分段处理，通过半径搜索实现规划指定路径长度的dubins路径。（4）参照附图2-3，根据当前速度v起始、无人机加速度a、终点速度v终点、协同飞行时间t、最大速度v最大来计算无人机飞行路径不同情况阶段下的最大范围。当无人机的v起始＜v终点，分为两种模式；模式1，无人机的v起始小于v终点，v最大大于v终点，对应路径s1；模式2，无人机的v起始小于v终点，v最大大于v终点，v最大能够从t1持续至t2时间段，对应路径s2；计算模式1、模式2所对应的路径长度s1、s2。参照附图4-6，当无人机的v起始＞v终点，并分为三种模式；模式3，无人机的v最大＞v起始＞v终点，并且v最大能够从t1持续至t2时间段，对应路径s3；模式4，无人机的v最大＞v起始＞v终点，对应路径s4；模式5，无人机的v最大＝v起始＞v终点，对应路径s5；计算模式3、模式4、模式5所对应的路径长度s3、s4、s5。（5）通过比较dubins半径搜索所规划出的路径长度s与s1、s2，或与s3、s4、s5的关系，确定规划的路径长度位于哪种情况阶段，从而确定速度随时间的变化情况，从而根据规划出的路径长度s求解出对应的时间节点。例如当s>s3时，速度规划情况参照附图7：求解出t1、t2、t3从而得到速度规划的情况。（6）通过协同飞行实时时间，按照步骤（4）得到的速度规划来实时计算当前理论速度给定v给定和理论飞行剩余路径长度s理论剩余。（7）根据无人机当前位置信息，按照步骤（2）规划出的三维路径计算实际剩余路径长度s实际剩余。（8）当前理论速度给定v给定、理论飞行剩余路径长度s理论剩余、实际剩余路径长度s实际剩余计算速度控制值v控制：其中，kp为可调节参数，通过实时调节速度控制值从而达到同时刻到达的要求。实施例1二维平面内，5架不同位置的飞机，起点/终点速度、起点/终点航向、转弯半径、飞行时间受限。初始仿真参数如下表所示。表1多无人机初始仿真参数飞行时间起点速度终点速度起点x起点y最小转弯半径初始航向终点x终点y最小转弯半径终点航向飞机190014016010010050009050000700005000140飞机2900140160200001500005000050100702005000140飞机3900140160600002000005000050000701005000140飞机49001401601000002000005000050200702005000140飞机59001401601200000500027050000702005000140最大飞行速度270m/s；最大加减速能力0.5m/s2。仿真结果参照附图9所示（5菱形为起飞点，圆形为目标点）。实施例2三维空间内，3架不同位置的飞机，起点/终点速度、起点/终点航向、转弯半径、飞行时间受限。初始仿真参数如下所示。表2多无人机初始仿真参数起点x起点y起点z最小转弯半径起点航向起点速度飞机1262226843147540009140飞机2149766948713400087160飞机3124162371599354000241200终点x终点y终点z最小转弯半径终点航向终点速度飞机1121593014997764000230180飞机246841346117244000204180飞机3277262620116774000252180最大飞行速度270m/s；最大加减速能力0.5m/s2；最大爬升率3m/s；飞行时间15分钟。仿真结果如下图所示（3架飞机同时到达不同任务点，菱形为起飞点，星形为三维空间不同目标点）。三维空间路径规划仿真结果如图10所示。当前第1页12