多无人机避障仿真控制的轨迹规划方法、装置和仿真系统与流程

本发明属于无人机智能控制，具体涉及一种多无人机避障仿真控制的轨迹规划方法、装置和仿真系统。

背景技术：

1、现有集群多无人机针对内部邻机和外部障碍物的主要避障方法为速度障碍法和人工势场法。速度障碍法利用无人机与邻机和障碍物之间的相对速度在速度空间内构建三角区域，选择三角区域外的无人机可达速度即可实现集群避障。人工势场法将待控制无人机外部视为一个人造势场，该势场由引力场和斥力场组成，障碍物和邻机产生斥力，目标点产生引力，合理调整不同对象的权重可以使集群内部每台单机实现机间和外部避障。但是这两种方法只考虑了某一时刻集群的状态，而集群的状态是时刻变化的，所以不具备对无人机平稳性、动力学稳定性和控制连续性的约束，容易导致集群陷入局部最小值或失控状态。

2、同时，现有多无人机仿真平台多集中于matlab/simulink等可视化、模块化和移植性较差的系统中运行。并且只注重对算法本身的仿真验证，忽略了真实环境空间复杂度、传感器感知能力、数据通信能力以及飞控控制能力的制约。如果对多无人机避障算法进行验证而不考虑以上多种能力制约因素，那么所验证的避障算法在实际环境中进行多机验证时，必然失效。。

技术实现思路

1、鉴于上述的分析，本发明旨在公开了一种多无人机避障仿真控制的轨迹规划方法、装置和仿真系统，用于保证多无人机实现安全高效的避障效果，且同时保证多无人机运行时的稳定和平滑。

2、本发明公开了一种多无人机避障仿真控制的轨迹规划方法，包括：

3、步骤s1、根据接收的各仿真无人机输出的所处位置场景深度图，生成表征各仿真无人机可行状态空间的避障感知地图；

4、步骤s2、根据各仿真无人机的避障感知地图进行路径规划，生成每个仿真无人机的连续性期望轨迹；

5、步骤s3、根据各仿真机器人同时间段内的连续性期望轨迹进行无人机间碰撞判断；对判断的发生碰撞的两个仿真无人机，进行二次路径规划生成避免无人机碰撞的连续性期望轨迹；

6、步骤s4、将各仿真无人机的连续性期望轨迹分别输出到各无人机的飞行控制器进行飞行仿真。

7、进一步地，所述步骤s2中，包括：

8、1)对所述避障感知地图进行基于全局a星路径规划的一次规划生成离散路径；

9、2)以设定的路径距离进行下采样，生成采样后的路径点；

10、3)将下采样后的路径点作为贝塞尔曲线控制点，建立关于时间的参数化轨迹曲线方程；

11、4)根据参数化轨迹曲线方程生成每时刻的连续性期望轨迹。

12、进一步地，所述步骤s3中，包括：

13、1)在多无人机之间互相传递在同时间段内的轨迹曲线，对该时间段进行下采样，获取多个采样时刻；

14、2)针对所有无人机的连续性期望轨迹，在每个采样时刻计算各无人机的采样位置点；

15、3)判断相同采样时刻的任意两个无人机的位置点之间的距离是否处于设定安全距离内；是，则进入步骤4)的二次轨迹规划，否则结束步骤s3；

16、4)在二次轨迹规划中，在两个无人机各自的期望轨迹中确定出位置优化的两个贝塞尔曲线控制点；所述两个贝塞尔曲线控制点为最接近处于安全距离内的位置点的前后两个塞尔曲线控制点；更新所述两个塞尔曲线控制点，将两个塞尔曲线控制点之间的连线向远离对方期望轨迹的方向扩充，使相同时刻连线上的位置点距离超过安全距离；

17、5)根据更新后的控制点重新构建贝塞尔轨迹曲线，获得多无人机满足安全避障要求的轨迹。

18、进一步地，多个采样时刻构成的采样时刻集合为t＝{0,δt,2δt,3δt,…,nδt}；n为采样点数，δt为采样间隔；

19、第个k无人机n段贝塞尔曲线上采样位置点集合为：

20、

21、在m段贝塞尔曲线上，无人机i、j在同一时刻的两两采样位置点之间的距离表示为：

22、

23、在时，sf为安全距离；选取采样位置点所在m段贝塞尔曲线上的两个控制点{qm,qm+1}；

24、对控制点{qm,qm+1}沿采样位置点差值矢量dir分别移动距离distance；更新后的控制点得到更新后的控制点{qm,new,qm+1,new}；其中，

25、

26、

27、

28、

29、式中，λm、λm+1≥1为拉离系数；

30、根据更新后的控制点{q0,q1,…,qm,new,qm+1,new,…,qn}重新构建贝塞尔轨迹曲线，获得多无人机满足安全避障要求的连续性期望轨迹。

31、本发明还公开了一种多无人机避障控制的轨迹规划装置，包括：感知地图生成模块、一次轨迹规划模块、二次轨迹规划模块和轨迹输出模块；

32、所述感知地图生成模块，用于根据各仿真无人机的避障感知地图进行路径规划，生成每个仿真无人机的连续性期望轨迹；

33、所述一次轨迹规划模块，用于根据各仿真无人机的避障感知地图进行路径规划，生成每个仿真无人机的连续性期望轨迹；

34、所述二次轨迹规划模块，用于根据各仿真机器人同时间段内的连续性期望轨迹进行无人机间碰撞判断；对判断的发生碰撞的两个仿真无人机，进行二次路径规划生成避免无人机碰撞的连续性期望轨迹；

35、轨迹输出模块，用于将各仿真无人机的连续性期望轨迹分别输出到各无人机的飞行控制器进行飞行仿真。

36、进一步地，所述一次轨迹规划模块包括全局a星路径规划模块、路径采样模块、贝塞尔模块、轨迹生成模块；

37、全局a星路径规划模块，用于对所述避障感知地图进行基于全局a星路径规划的一次规划生成离散路径；

38、路径采样模块，用于以设定的路径距离进行下采样，生成采样后的路径点；

39、贝塞尔模块，用于将下采样后的路径点作为贝塞尔曲线控制点，建立关于时间的参数化轨迹曲线方程；

40、轨迹生成模块，用于根据参数化轨迹曲线方程生成每时刻的连续性期望轨迹。

41、进一步地，所述二次轨迹规划模块中包括时间采样模块、采样位置点计算模块、碰撞判断模块、规划模块和轨迹输出模块；

42、时间采样模块，用于多无人机之间互相传递在同时间段内的轨迹曲线，对该时间段进行采样，获取多个采样时刻；

43、采样位置点计算模块，用于针对所有无人机的连续性期望轨迹，在每个采样时刻计算各无人机的采样位置点；

44、碰撞判断模块，用于判断相同采样时刻的任意两个无人机的位置点之间的距离是否处于设定安全距离内；是，则将该个无人机的期望轨迹输入到规划模块；

45、二次规划模块，用于在两个无人机各自的期望轨迹中确定出位置优化的两个贝塞尔曲线控制点；所述两个贝塞尔曲线控制点为最接近处于安全距离内的位置点的前后两个塞尔曲线控制点；更新所述两个塞尔曲线控制点，将两个塞尔曲线控制点之间的连线向远离对方期望轨迹的方向扩充，使相同时刻连线上的位置点距离超过安全距离；

46、轨迹输出模块，用于根据更新后的控制点重新构建贝塞尔轨迹曲线，获得多无人机满足安全避障要求的连续性期望轨迹。

47、进一步地，时间采样模块中，多个采样时刻构成的采样时刻集合为t＝{0,δt,2δt,3δt,…,nδt}；n为采样点数，δt为采样间隔；

48、采样位置点计算模块中，第个k无人机n段贝塞尔曲线上采样位置点集合为：

49、

50、碰撞判断模块中，判断和安全距离sf；为在m段贝塞尔曲线上，无人机i、j在同一时刻的两两采样位置点之间的距离；

51、在时，二次规划模块中，选取采样位置点所在m段贝塞尔曲线上的两个控制点{qm,qm+1}；对控制点{qm,qm+1}沿采样位置点差值矢量dir分别移动距离distance；更新后的控制点得到更新后的控制点{qm,new,qm+1,new}；其中，

52、

53、

54、

55、

56、式中，λm、λm+1≥1为拉离系数；

57、轨迹输出模块中，根据更新后的控制点{q0,q1,…,qm,new,qm+1,new,…,qn}重新构建贝塞尔轨迹曲线，获得多无人机满足安全避障要求的连续性期望轨迹。

58、本发明还公开了一种多无人机飞行避障控制仿真系统，包括多个仿真无人机以及与仿真无人机一一对应的多个轨迹规划装置；

59、所述轨迹规划装置为如上所述的多无人机避障控制的轨迹规划装置；

60、所述仿真无人机分别向对应的所述轨迹规划装置输出的所处位置场景深度图，并接收所述轨迹规划装置输出的连续性期望轨迹进行飞行控制仿真；实现无人机的避障飞行。

61、进一步地，所述仿真无人机为四旋翼无人机，采用的飞行控制器为非线性仿真控制器。

62、本发明可实现以下有益效果之一：

63、本发明的多无人机避障仿真控制的轨迹规划方法、装置和仿真系统实现了多无人机的安全高效避障，且同时保证多无人机运行时的稳定和平滑。并且通过全局a星规划实现多无人机外部避障，通过下采样轨迹拉离机间碰撞点实现多无人机内部避障；

64、并且，本发明提供了一个可以实现避障感知、算法验证、轨迹跟踪响应的完整仿真平台，解决了多无人机智能控制算法验证中无法考虑其他模块因素影响的问题。