CN115857372A - 分布式多无人机集群协同空间搜索调度的控制仿真系统 - Google Patents

Abstract

本发明涉及一种分布式多无人机集群协同空间搜索调度的控制仿真系统，包括：协同搜索调度单元用于基于各仿真无人机的动态场景深度图进行共享概率占据地图的更新，搜索出用于区分地图未知和已知区域的边界集合以及边界集合中每条边界对应的最佳观测点位置；进行观测点调度为每个仿真无人机分配一个最佳观测点位置作为仿真无人机的下一个目标点；仿真无人机用于接收对应的目标点位置进行轨迹规划生成连续性期望轨迹；并根据所述连续性期望轨迹在虚拟仿真环境中进行仿真飞行，验证分布式集群运行的平稳和安全性。本发明提供了一个可以实现环境感知、算法验证、机动飞行控制的完整仿真平台。

Description

分布式多无人机集群协同空间搜索调度的控制仿真系统

技术领域

本发明属于无人机智能控制技术领域，具体涉及一种分布式多无人机集群协同空间搜索调度的控制仿真系统。

背景技术

现有多无人机搜索的主要方法为编队法，该方法利用多无人机组成一定形状的队形，以扩大无人机的感知范围，从而使整个编队可以快速完成区域的搜索任务。但是这种方法不适用于复杂空间的协同搜索，因为复杂空间中往往存在许多障碍物，在这种情况下，维护队形和躲避障碍为相互矛盾的要求，要同时满足这两者需要寻找合理的参数设置，需要经过漫长的调试。同时，编队的优势在于扩大感知范围，由于复杂空间结构繁多，对感知范围扩大有限，不能充分发挥编队的优势，从而可能导致该方法在这种环境下运行较为缓慢，无法实现快速协同遍历空间的需求。

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

发明内容

鉴于上述的分析，本发明旨在公开了一种分布式多无人机集群协同空间搜索调度的控制仿真系统，解决多无人机在室内复杂环境下的协同空间搜索调度的仿真验证问题。

本发明公开了一种分布式多无人机集群协同空间搜索调度的控制仿真系统，其特征在于，包括：协同搜索调度单元和与之连接的多个仿真无人机；

所述协同搜索调度单元，用于根据接收的各仿真无人机在进行仿真飞行时逐帧输出的动态场景深度图进行协同空间搜索调度；在协同空间搜索调度中，基于各仿真无人机的动态场景深度图进行共享概率占据地图的更新，搜索出用于区分地图未知和已知区域的边界集合以及边界集合中每条边界对应的最佳观测点位置；进行观测点调度为每个仿真无人机分配一个最佳观测点位置作为仿真无人机的下一个目标点；

所述仿真无人机，用于接收对应的目标点位置进行轨迹规划生成连续性期望轨迹；并根据所述连续性期望轨迹在虚拟仿真环境中进行仿真飞行，验证分布式集群运行的平稳和安全性。

进一步地，所述协同搜索调度单元包括共享地图处理子单元、边界搜索划分子单元和集群调度分配子单元；

共享地图处理子单元，用于根据多个无人机输出的各自所处位置的场景深度图，进行地图共享，生成共享概率占据地图；

边界搜索划分子单元，用于根据共享概率占据地图中的地图信息，进行边界搜索得到边界集合和与边界结合对应的边界最佳视点集合；

集群调度分配子单元，用于根据边界最佳观测点集合中观测点的数量和无人机的数量，以及无人机当前位置到观测点的代价进行运筹优化分配，为每个无人机分配到下一个目标点。

进一步地，所述共享地图处理子单元包括遮掩判定模块、掩码操作模块、共享概率更新模块和共享地图生成模块；

所述遮掩判定模块，用于根据各仿真无人机自所处位置的场景深度图进行多机相互遮掩判定，判定出存在遮掩的无人机；

所述掩码操作模块，用于对存在遮掩的无人机拍摄的深度信息进行掩码操作得到处理，得到处理后的深度图；

所述共享概率占据地图模块，用于基于处理后的深度信息将深度图转换为障碍点云，并根据所有无人机同时观测的障碍点云生成共享概率占据地图；

所述共享概率占据地图由一个动态数组构成，数组内每个数据单元内储存着由多无人机共同获得观测概率，以及由观测概率确定的地图元素所对应的状态；并根据逐帧更新的障碍点云，进行观测概率和观测状态的更新。

进一步地，边界搜索划分子单元包括边界生成模块和最佳观测点生成模块；

所述边界生成模块，用于搜索共享概率占据地图中各数据单元当前状态找出用于区分地图未知和已知区域的边界集合；

共享概率占据地图中处于未知状态与自由状态的交接处的数据单元属于边界上的点，其自身状态为自由状态，其邻近单元状态为未知状态；

最佳观测点生成模块，用于搜索边界集合中的每一条边界的观测效率最大化的观测点位置；

在搜索过程中，通过搜索边界的均值点、边界两端相距最远的两个空间点位置、边界中心法线和根据由边界两端相距最远的两个空间点位置计算出的中心点位置和深度相机的感知视野FOV确定出最佳观测点位置。

进一步地，所述集群调度分配子单元，包括优化问题模型、观测点与无人机数量判定模块和模型最优解寻求模块；

所述优化问题模型，用于将M个观测点VP_m∈{VP₁,…,VP_M}分配给N个无人机UAV_n∈{UAV₁,…,UAV_N}，一个无人机只分配一个观测点，并且分配后所有无人机以最小代价到达各自的观测点；

观测点与无人机数量判定模块，用于判定观测点数量M和无人机数量N的大小；

模型最优解寻求模块，用于根据观测点与无人机数量判定模块的判定结果，对优化问题模型进行求解，为每个无人机分配一个观测点作为下一个目标点；

具体的，当M＜N时，最优解寻求：

x_mn≤y_n,

/>

x_mn∈{0,1},

y_n∈{0,1},

当M>N时，最优解寻求：

x_mn≤y_n,

x_mn∈{0,1},

y_n∈{0,1},

y_n∈{0,1}表示无人机UAV_n是否参与搜索；x_mn∈{0,1}表示将观测点VP_m分配给无人机UAV_n表示为x_mn∈{0,1}；其中，无人机UAV_n到观测点VP_m的代价表示为d_mn，所有无人机到达各自观测点的总代价表示为

通过分配优化使总代价Cost达到最小。

进一步地，无人机UAV_n到观测点VP_m的代价为由距离代价、速度代价和速度变化代价组成的总观测代价d_mn。

进一步地，所述仿真无人机，为仿真的四旋翼无人机，包括轨迹规划模块、第一类仿真传感器、第二类仿真传感器、非线性仿真控制器以及四个仿真电机；

所述轨迹规划模块，用于根据分配的目标点进行轨迹规划，得到考虑避障和避免无人机之间碰撞的连续性期望轨迹，输出到非线性仿真控制器；

所述第一类仿真传感器中包括仿真的深度相机；用于从仿真无人机所处虚拟环境的场景信息中获得包括动态场景深度图在内的前端传感数据，输出到协同搜索调度单元；

所述第二类仿真传感器中包括仿真的IMU、GPS模块、气压计和磁力计在内的位姿测量传感器，用于从仿真无人机所处虚拟环境的自然环境模拟数据中，感知仿真无人机的位姿信息；

所述非线性仿真控制器；用于根据期望轨迹和第二类仿真传感器感知的仿真无人机位姿信息，进行非线性控制，输出与仿真电机对应的电机转速；

所述仿真电机，用于根据非线性仿真控制器输出的电机转速模拟出无人机使用无刷直流电机在螺旋桨旋转过程中产生的推力、空气阻力、旋转力矩和空气阻力矩；以得到在四个仿真电机作用下，仿真的四旋翼无人机在仿真环境中所受的合力及合力矩。

进一步地，所述轨迹规划模块包括一次轨迹规划模块、二次轨迹规划模块和轨迹输出模块；

所述一次轨迹规划模块，用于根据获得的下一个目标点位置进行路径规划，生成每个仿真无人机的连续性期望轨迹；

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

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

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

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

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

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

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

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

进一步地，还包括虚拟仿真环境单元；所述虚拟仿真环境单元与仿真无人机进行数据通信；将仿真的虚拟场景信息输出到仿真无人机的第一类仿真传感器中，使第一类仿真传感器能够感知场景信息得到包括动态场景在内的前端传感数据；根据仿真电机输出四旋翼无人机在仿真环境中所受的合力及合力矩，仿真得到的包括模拟力场、大气场和磁场在内的自然环境模拟数据输出到仿真无人机的第二类仿真传感器中，使第二类仿真传感器能够根据自然环境模拟数据感知无人机的位姿信息。

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

本发明的仿真系统克服了无人机在智能控制领域中，缺乏一种快速、完整、简洁明了的复杂空间多无人机的协同空间搜索调度以及仿真验证环境的问题；为该领域的研究人员提供了一个可以实现环境感知、算法验证、机动飞行控制的完整仿真平台，解决了无人机协同空间搜索调度中无法考虑其他模块因素影响的问题。

本发明的协同空间搜索调度解决了分布式集群系统相互感知独立的问题，提高了系统对环境一致性的感知能力，并且有效避免了机间无效感知的干扰。并且还通过一种运筹优化的方法为分布式集群的任务进行分配，解决了分布式集群运行缺少规划的问题，在全局范围使整个系统任务的耗时和能量消耗达到最低。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为发明实施例中的分布式多无人机集群协同空间搜索调度的控制仿真系统原理框图；

图2为发明实施例中的协同搜索调度单元原理框图；

图3为发明实施例中的共享地图处理子单元原理框图；

图4为发明实施例中的边界搜索划分子单元原理框图；

图5为发明实施例中的集群调度分配子单元原理框图；

图6为发明实施例中的所述仿真无人机组成连接原理框图；

图7为发明实施例中的轨迹规划模块原理框图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本发明实施例公开一种分布式多无人机集群协同空间搜索调度的控制仿真系统，如图1所示，包括：协同搜索调度单元和与之连接的多个仿真无人机，以及虚拟仿真环境单元；

所述协同搜索调度单元，用于根据接收的各仿真无人机在进行仿真飞行时逐帧输出的动态场景深度图进行协同空间搜索调度；在协同空间搜索调度中，基于各仿真无人机的动态场景深度图进行共享概率占据地图的更新，搜索出用于区分地图未知和已知区域的边界集合以及边界集合中每条边界对应的最佳观测点位置；进行观测点调度为每个仿真无人机分配一个最佳观测点位置作为仿真无人机的下一个目标点；

所述仿真无人机，用于接收对应的目标点位置进行轨迹规划生成连续性期望轨迹；并根据所述连续性期望轨迹在虚拟仿真环境中进行仿真飞行，验证分布式集群运行的平稳和安全性；

所述虚拟仿真环境单元与仿真无人机进行数据通信；将仿真的虚拟场景信息输出到仿真无人机的仿真深度相机中，使仿真深度相机能够感知场景信息得到包括所处位置场景深度图的前端传感数据；将仿真的模拟力场、大气场、磁场等自然环境模拟数据输出到仿真无人机中的仿真的IMU、GPS模块、气压计和磁力计中，使仿真的IMU、GPS模块、气压计和磁力计能够根据自然环境模拟数据感知无人机的位姿信息。

其中，协同搜索调度单元中，多无人机空间探索需要无人机之间共享感知到的场景信息，以此生成和更新共享地图，由于无人机的深度相机可以相互感知，因此在共享地图时，每个无人机需要对视野内的邻机进行掩码处理。

具体的，如图2所示，协同搜索调度单元包括：共享地图处理子单元、边界搜索划分子单元和集群调度分配子单元；

共享地图处理子单元，用于根据多个无人机输出的各自所处位置的场景深度图，进行地图共享，生成共享概率占据地图；

边界搜索划分子单元，用于根据共享概率占据地图中的地图信息，进行边界搜索得到边界集合和与边界结合对应的边界最佳视点集合；

集群调度分配子单元，用于根据边界最佳观测点集合中观测点的数量和无人机的数量，以及无人机当前位置到观测点的代价进行运筹优化分配，为每个无人机分配到下一个目标点。

如图3所示，所述共享地图处理子单元包括遮掩判定模块、掩码操作模块、共享概率更新模块和共享地图生成模块；

所述遮掩判定模块，用于根据各仿真无人机自所处位置的场景深度图进行多机相互遮掩判定，判定出存在遮掩的无人机；

在进行存在遮掩的无人机的判断中包括两次判定；其中，

一次判定；在获取每个无人机更新的深度图与全局位置时，判断任意两个无人机之间的距离，若两无人机的距离大于深度相机感知距离，则判定该两无人机之间不存在相互感知；反之，进入二次判定；

二次判定；根据两无人机的相对位置矢量和机载深度相机的视角矢量判定其中一个无人机是否在另一个无人机的视野范围内。

具体的，二次判定中对于UAV_i是否存在于UAV_j视野范围内，通过以下公式判定：

Result＝minf(sign(dir_ij·dir_r)),r∈0,1,2,3；

其中，

dir_r为UAV_j的视角矢量；dir_ij为无人机UAV_i和UAV_j的相对位置矢量；sign为数学符号函数；

分别为无人机UAV_i、UAV_j的全局位置数据；

UAV_j的视角矢量dir_r为：

其中，

为无人机UAV_i相机坐标系到全局坐标系下的旋转矩阵，angle_top、angle_bottom、angle_left和angle_right分别为无人机UAV_i相机上视视野角、下视视野角、左视视野角、右视视野角。

如果Result为0，表示UAV_i不在UAV_j视野内，反之则表示出现在视野内，并且影响到了深度相机的估计。

所述掩码操作模块，用于对存在遮掩的无人机拍摄的深度信息进行掩码操作得到处理，得到处理后的深度图；

掩码操作过程中，将UAV_i的空间位置转换到UAV_j的相机坐标系内：

其中，

为全局坐标系到UAV_j相机坐标系的变换矩阵，/>

为UAV_i在UAV_j相机坐标系下的坐标，/>

为无人机深度相机的内参数矩阵；

从而获得UAV_i的投影像素坐标[u_ij,v_ij,1]^T，对其在深度图上的投影形状简化为圆形，该圆半径为无人机UAV_i轴距L_i在像素平面上的等效投影长r_i，表示为：

在像素平面内，满足(u-u_ij)²+(v-v_ij)²≤r_i的像素点处深度置为0，从而完成掩码操作。

所述共享概率占据地图模块，用于基于处理后的深度信息将深度图转换为障碍点云，并根据所有无人机同时观测的障碍点云生成共享概率占据地图；

所述共享概率占据地图由一个动态数组构成，数组内每个数据单元内储存着由多无人机共同获得观测概率，以及由观测概率确定的地图元素所对应的状态；并根据逐帧更新的障碍点云，进行观测概率和观测状态的更新。

具体的，在转换障碍点云过程中，

根据深度相机的内、外参数据，将处理后的深度信息将深度图内的二维像素坐标对应的深度信息转换为全局坐标系下的三维障碍空间点云；

其中，根据相机的内参数据，将像素平面映射到相机坐标系为：

其中，u、v为图像的二维像素坐标；X、Y、Z为相机坐标系坐标；

为内参数矩阵，f_x、f_y、c_x、c_y为相机的内部参数；

根据相机的外参数据，将相机坐标系下坐标转移到全局坐标系下为：

其中，X_w、Y_w、Z_w为全局坐标系下三维坐标；R为相机外参的旋转矩阵，t为相机外参的平移向量。

所述共享概率占据地图由一个动态数组构成，数组内每个数据单元内储存着由多无人机共同获得观测概率，以及由观测概率确定的地图元素所对应的状态；并根据逐帧更新的障碍点云，进行观测概率和观测状态的更新。

具体的，在进行共享概率占据地图初始化时，对每个数据单元赋予初始值，表示所有的数据单元对于的地图元素的初始状态为未知状态；

当更新的三维障碍点云中，地图元素的当前观测状态为占据时，共享概率占据地图对应的数据单元中的更新的观测概率S_new＝S_old+loUAV；

其中，S_old为前一次观测概率；loUAV为当前次观测为被占据的状态数值增量；

p_hit为预先设定的观测到一次被占据状态的概率，0.5<p_hit<1。

对于多无人机的共享地图，该地图单元在无人机UAV_i中表现为占据，但在无人机UAV_j可能为自由，在其他无人机中可能又处于另外一种状态，所以共享地图中地图单元的状态由所有无人机共同决定。

所以共享概率占据地图中更新的当前观测概率由所有无人机共同决定：

其中，S_old,share为前一次观测概率；loUAV_i为无人机UAV_i当前次观测为被占据的状态数值增量；N为无人机的数量。

通过更新，当共享概率占据地图的数据单元的当观测概率S_new,share达到一定阈值时，则储存该值的地图单元被标记为占据状态，低于该阈值时，标记为自由状态，等于初始化值时，标记为未知状态。

所述阈值可根据实际情况进行预先设定。

如图4所示，所述边界搜索划分子单元包括边界生成模块和最佳观测点生成模块；

所述边界生成模块，用于搜索共享概率占据地图中各数据单元当前状态找出用于区分地图未知和已知区域的边界集合；

共享概率占据地图中处于未知状态与自由状态的交接处的数据单元属于边界上的点，其自身状态为自由状态，其邻近单元状态为未知状态；

最佳观测点生成模块，用于搜索边界集合中的每一条边界的观测效率最大化的观测点位置；

在搜索过程中，通过搜索边界的均值点、边界两端相距最远的两个空间点位置、边界中心法线和根据由边界两端相距最远的两个空间点位置计算出的中心点位置和深度相机的感知视野FOV确定出最佳观测点位置。

其中，在所述边界生成模块中，将共享概率占据地图中处于未知状态与自由状态的交接处的数据单元归属于边界，其自身状态为自由状态，其邻近单元状态为未知状态。具体可表示为：

F＝{cell_0,free,…,cell_l,free,…,cell_L,free}；

Neighbor(cell_l,free)∈Unknown；

其中，Unknown代表未知状态的集合。

地图上存在多个边界{F₀,…,F_m,…,F_M}构成边界集合，无人机对集合中的边界依次搜索以达到遍历空间的目的。

其中，在最佳观测点生成模块中，最佳观测点的搜索过程中，通过搜索边界的均值点、边界两端相距最远的两个空间点位置、边界中心法线，并根据由边界两端相距最远的两个空间点位置计算出的中心点位置和深度相机的感知视野FOV确定出最佳观测点位置。

具体的，对边界m观测点位置的搜索过程，具体包括：

1)获取所述边界m的均值点；

2)从边界均值点开始进行遍历搜索得到边界两端相距最远的两个空间点位置；

cell_max,m＝lenght_max·dir_PCA+cell_mean,m

cell_min,m＝lenght_min·dir_PCA+cell_mean,m

其中，lenght_max为搜索的边界上点与均值点的最大距离，lenght_min为搜索的边界上点与均值点的最小距离，dir_PCA为边界内所有空间点的主成分方向。

将通过搜索在边界中得到满足条件的目标点cell_max,m和cell_min,m，确定为边界两端相距最远空间点。

3)根据边界两端相距最远的两个空间点位置确定出边界中心法线；

中心法线方向表示为：

z_w为全局坐标系下的z轴。

4)根据由边界两端相距最远的两个空间点位置计算出的中心点位置和深度相机的感知视野FOV确定出最佳观测点位置。

由边界两端空间点计算的中心点位置为：

其中，s_m表示为在中心法线上最佳观测点VP距离中心点Mid的距离：

因此，最佳观测点VP可以表示为：

VP_m＝Mid_m+s_m·dir_normal,m；

各条边界的最佳观测点构成边界最佳视点集合。

具体的，如图5所示，所述集群调度分配子单元，包括优化问题模型、观测点与无人机数量判定模块和模型最优解寻求模块；

所述优化问题模型，用于将M个观测点VP_m∈{VP₁,…,VP_M}分配给N个无人机UAV_n∈{UAV₁,…,UAV_N}，一个无人机只分配一个观测点，并且分配后所有无人机以最小代价到达各自的观测点；建立如下分配优化数学模型：

无人机UAV_n是否参与搜索表示为y_n∈{0,1}。是否将观测点VP_m分配给无人机UAV_n表示为x_mn∈{0,1}。其中，无人机UAV_n到观测点VP_m的代价表示为d_mn，所有无人机到达各自观测点的总代价表示为

通过模型最优解寻求模块的分配优化使总代价Cost达到最小。

观测点与无人机数量判定模块，用于判定观测点数量M和无人机数量N的大小；

模型最优解寻求模块，用于根据观测点与无人机数量判定模块的判定结果，对优化问题模型进行求解，从而为每个无人机分配一个观测点作为下一个目标点；

具体的，当M＜N时，最优解寻求：

x_mn≤y_n,

x_mn∈{0,1},

y_n∈{0,1},

当M>N时，最优解寻求：

x_mn≤y_n,

x_mn∈{0,1},

y_n∈{0,1},

优选的，无人机UAV_n到观测点VP_m的代价为由距离代价、速度代价和速度变化代价组成的总观测代价d_mn。

具体的，总观测代价包括：

1)距离代价；考虑无人机UAV_n当前位置到观测点VP_m位置可达距离，该距离通过全局A星规划出无人机当前位置到达该点的路径长度获得：

cost_{reach_dist}＝lenght(A^*path)；

2)速度代价；考虑无人机UAV_n路径末端速度与观测点VP_m位置所处中心法线重合度，该值为全局A星规划出的路径末端的路径点矢量方向和边界中心法线向量的夹角：

cost_{vel_normal}＝cos^-1(dir_normal·dir_{path_end})；

3)速度变化代价；考虑无人机UAV_n的当前速度到达观测点VP_m位置的变化量，该值为无人机当前速度矢量方向和边界中心法线向量夹角：

cost_{vel_change}＝cos^-1(dir_{vel_current}·dir_{path_end})；

结合以上三个观测代价得到的观测点的总观测代价为：

d_mn＝cost_sum＝cost_{reach_dist}+cost_{vel_normal}+cost_{vel_change}。

通过上述分配优化数学模型，可为每个参与搜索的无人机找出合适观测点作为下一目标点。

具体的，如图6所示，所述仿真无人机为仿真的四旋翼无人机，包括轨迹规划模块、第一类仿真传感器、第二类仿真传感器、非线性仿真控制器以及四个仿真电机；

所述轨迹规划模块，用于根据分配的目标点进行轨迹规划，得到考虑避障和避免无人机之间碰撞的连续性期望轨迹，输出到非线性仿真控制器；

所述第一类仿真传感器中包括仿真的深度相机；用于从仿真无人机所处虚拟环境的场景信息中获得包括动态场景深度图在内的前端传感数据，输出到协同搜索调度单元；

所述第二类仿真传感器中包括仿真的IMU、GPS模块、气压计和磁力计在内的位姿测量传感器，用于从仿真无人机所处虚拟环境的自然环境模拟数据中，感知仿真无人机的位姿信息；

所述非线性仿真控制器；用于根据期望轨迹和第二类仿真传感器感知的仿真无人机位姿信息，进行非线性控制，输出与仿真电机对应的电机转速；

所述仿真电机，用于根据非线性仿真控制器输出的电机转速模拟出无人机使用无刷直流电机在螺旋桨旋转过程中产生的推力、空气阻力、旋转力矩和空气阻力矩；以得到在四个仿真电机作用下，仿真的四旋翼无人机在仿真环境中所受的合力及合力矩。

具体的，如图7所示，所述轨迹规划模块包括一次轨迹规划模块、二次轨迹规划模块和轨迹输出模块；

所述一次轨迹规划模块，用于根据获得的下一个目标点位置进行路径规划，生成每个仿真无人机的连续性期望轨迹；

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

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

其中，在一次轨迹规划模块中，进行轨迹规划的过程包括：

1)进行基于全局A星路径规划的一次规划生成离散路径；

利用全局A星算法寻找当前位置到目标位置在当前共享概率占据地图中的离散安全路径，路径的最小节点为地图的最小体积单元。在A星算法中，地图内每个节点的总代价被表示为距离起点的代价和距离终点的代价之和：

f(n)＝g(n)+h(n)；

从起点开始，A星算法依次寻找总代价最小的相邻节点，直到寻找到终点。节点的总代价储存在优先级队列结构中，保证算法可以快速提取总代价最小的节点。从而最终得到一条最小单位为地图分辨率的安全路径，该路径由连续相邻的地图单位体素大小的路径点表示。

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

对全局A星在计算的离散路径，以合适路径距离进行下采样，将连续相邻的路径体素点通过合适的距离间隔采样，形成更加分散的路径点数据。

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

建立关于时间的参数化轨迹曲线方程式：

其中，w_i表示第i项权重，

表示组合数。

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

利用参数化贝塞尔曲线将安全离散路径表示为关于时间的连续性期望轨迹，可以获取每时刻的无人机期望状态。

具体的，在二次轨迹规划模块中，进行轨迹规划的过程包括：

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

由多个采样时刻构成的采样时刻集合为T＝{0,Δt,2Δt,3Δt,…,nΔt}；n为采样点数，Δt为采样间隔；

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

经过采样后，第个k无人机n段贝塞尔曲线上采样位置点集合为：

/>

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

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

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

在

时，s_f为安全距离；进行二次轨迹规划中，选取采样位置点所在m段贝塞尔曲线上的两个控制点{Q_m,Q_m+1}；

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

式中，λ_m、λ_m+1≥1为拉离系数。

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

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

将各无人机的连续性期望轨迹分别输出到各无人机的飞行控制器进行飞行控制。

具体的，所述非线性仿真控制器包括轨迹解算器、位置环控制器、非线性角度转换器、非线性姿态映射器和混合控制器；

所述轨迹解算器，用于将输入的无人机期望轨迹仿真数据转化为当前时刻的系统期望状态量；

所述位置环控制器，用于根据系统期望状态量进行位置环PID控制，输出位速控制总误差；

所述非线性角度转换器，用于对位速控制总误差进行非线性SE(3)空间角度转换，得到空间期望旋转矩阵；

所述非线性姿态映射器，用于将当前的空间期望旋转矩阵和测量旋转矩阵进行非线性姿态SO(3)空间映射，输出姿态控制误差；

所述混合控制器，用于根据姿态控制误差和位速控制总误差，进行电机转速控制输出电机转速到仿真电机。

具体的，所述轨迹解算器的输入信号无人机期望轨迹为关于时间t的多项式方程：

其中，

表示无人机在时间t应该达到的位置，C_3×5表示三维多项式的5阶系数矩阵，[t⁴,t³,t²,t,1]^T表示关于时间t的高阶自变量。

具体的，所述轨迹解算器中将望轨迹转化为当前时刻t_k的系统状态量：

其中，P_des表示时刻t_k的期望位置

V_des表示时刻t_k的期望速度/>

A_des表示时刻t_k的期望加速度/>

分别为在时刻t_k处，对期望轨迹分别取0阶导、1阶导、2阶导：

P_des＝F⁽⁰⁾(t_k)

V_des＝F⁽¹⁾(t_k)

A_des＝F⁽²⁾(t_k)

同时，考虑到行进路线上可能会出现障碍物，轨迹解算器将期望偏航角姿态ψ_des调整为沿轨迹朝前：

[Δx,Δy]^T＝F⁽⁰⁾(t_k)-F⁽⁰⁾(t_k-1)

由此，轨迹解算器将期望轨迹解算为输入飞控的期望状态量

具体的，在所述位置环控制器中，轨迹解算器输出的期望状态量

传入位置环PID转换为位速控制总误差A_input。

e_P＝P_des-P_now

e_V＝V_des-V_now

A_input＝K_Pe_P+K_Ve_V+K_Vi∫e_V+A_des+g

其中，e_P、e_V表示期望位置和期望速度的误差值，g表示重力加速度，A_input表示位速控制总误差；K_P表示位置误差的比例增益，K_V表示速度误差的比例增益，K_Vi表示速度误差的积分增益，P_now表示仿真无人机当前时刻t_k的位置，V_now表示仿真无人机当前时刻t_k的速度。

具体的，在所述非线性角度转换器中，对输入的位速控制总误差A_input进行非线性SE(3)空间角度转换，得到空间期望旋转矩阵R_des。空间期望旋转矩阵R_des通过[x_B,des,y_B,des,z_B,des]表示，其中，

z_B,des表示加速度PID值的向量方向：

y_B,des表示z_B,des与向量[cosψ_des,sinψ_des,0]^T组成平面的法线向量：

x_B,des表示y_B,des与z_B,des组成平面的法线向量：

x_B,des＝y_B,des×z_B,des

由此，获得期望旋转矩阵R_des。

具体的，非线性姿态映射器中将空间期望旋转矩阵R_des和测量的旋转矩阵R_B进行非线性姿态SO(3)空间映射，输出姿态控制误差[e_p,e_q,e_r]^T；

测量的旋转矩阵R_B＝[x_B y_B z_B]由测量的机体坐标系上的正交三轴方向x_B,y_B,z_B进行表征。

将期望旋转矩阵R_des和当前旋转矩阵R_B通过vee映射到SO(3)空间，姿态角误差e_R表示为：

上标“∨”代表SO(3)的vee映射；R_B＝[x_B y_B z_B]由机体坐标系上的正交三轴方向x_B,y_B,z_B进行表征；

本实施例中，通过对姿态角误差e_R的进行PID控制，输出姿态控制误差[e_p,e_q,e_r]^T：

[e_p,e_q,e_r]^T＝K_Re_R+K_Ri∫e_R

K_R表示姿态角误差的比例增益，K_Ri表示姿态角误差的积分增益。

优选的，所述仿真无人机当前时刻t_k的位置P_now、速度V_now以及测量的旋转矩阵R_B＝[x_B y_B z_B]为仿真无人机中包括的仿真IMU、GPS模块、气压计和磁力计在内的位姿测量传感器通过感知虚拟环境中包括模拟力场、大气场和磁场在内的自然环境模拟数据测量获得。

此外，所述仿真无人机当前时刻t_k的位置P_now、速度V_now以及测量的旋转矩阵R_B＝[x_B y_B z_B]也可以是当前虚拟物理引擎得到仿真无人机的位姿数据。

具体的，所述混合控制器中，根据非线性姿映射器输出的姿态控制误差[e_p,e_q,e_r]^T和位置环控制器输出的位速控制总误差A_input，进行电机转速控制输出电机转速ω到仿真电机。

其中，第i个仿真电机产生的推力满足

产生的力矩满足/>

k_F为推力系数；k_M为力矩系数；

对于仿真系统内的无人机有：

得到的仿真四旋翼无人机的四个电机转速为：

其中L为四旋翼无人机各个电机到无人机质心的距离长度；m为无人机质量。

将四个电机转速输入到仿真电机中使仿真电机模拟出无人机使用无刷直流电机在螺旋桨旋转过程中产生的推力F_i、空气阻力

旋转力矩M_i、空气阻力矩/>

其中，k_D为空气阻力系数，μ_D为空气阻力矩系数；

表示螺旋桨表面气流速度，

dir_turn表示仿真电机的正反转方向，z_B为螺旋桨平面垂直法线向量；

在四个仿真电机作用下，四旋翼无人机所受合力及合力矩表示为：

其中，m为无人机质量，g为重力加速度。

具体的，所述虚拟仿真环境单元与各仿真无人机进行数据通信；将仿真的虚拟场景信息输出到仿真无人机的第一类仿真传感器中，使第一类仿真传感器能够感知场景信息得到包括动态场景在内的前端传感数据；根据仿真电机输出四旋翼无人机在仿真环境中所受的合力及合力矩，仿真得到的包括模拟力场、大气场和磁场在内的自然环境模拟数据输出到仿真无人机的第二类仿真传感器中，使第二类仿真传感器能够根据自然环境模拟数据感知无人机的位姿信息。

优选的，在虚拟仿真环境单元中利用Gazebo仿真平台和虚拟物理引擎搭建仿真场景环境。

可选的，仿真场景环境搭建过程中，主要步骤包括：

1)仿真障碍物建模；

通过在Gazebo仿真平台中建模，构建不同形状和大小的障碍物，模拟树林、建筑、室内等多种场景中的典型结构。为验证多无人机避障能力在不同环境下的适应性，构建不同密度的障碍物场景，为避障提供多种测试环境。

2)虚拟物理引擎配置；

虚拟物理引擎模拟无人机在自然环境中的受力，对其施加重力、空气动力、阻力等外力，通过动力学模型解算，在每步迭代中更新其当前运动学状态。同时，物理引擎对飞控内部传感器模块如IMU、气压计、磁力计施加模拟力场、大气场、磁场等，为飞控提供自然环境模拟数据。

3)模拟传感器接口编写；

在仿真环境和虚拟物理引擎下，在仿真无人机中构建无人机模型。

利用仿真无人机中的电机动力学模型：单个电机所产生的推力满足

产生的力矩满足/>

模拟四旋翼无人机电机产生的空气动力与力矩，编写无人机电机转速输入接口。

4)数据通信接口编写。

编写数据通信接口，将无人机模型内传感器和电机的数据与轨迹跟踪飞控进行联通；在机器人操作系统ROS的基础上，设置飞控与仿真系统的UDP端口，将仿真传感器IMU、GPS、磁力计和气压计的数据接入飞控，通过话题机制将飞控输出电机转速传入模拟电机中，生成模拟空气动力。

综上所述，本实施例的仿真系统克服了无人机在智能控制领域中，缺乏一种快速、完整、简洁明了的复杂空间多无人机的协同空间搜索调度以及仿真验证环境的问题；为该领域的研究人员提供了一个可以实现环境感知、算法验证、机动飞行控制的完整仿真平台，解决了无人机协同空间搜索调度中无法考虑其他模块因素影响的问题。

本实施例的协同空间搜索调度解决了分布式集群系统相互感知独立的问题，提高了系统对环境一致性的感知能力，并且有效避免了机间无效感知的干扰。并且还通过一种运筹优化的方法为分布式集群的任务进行分配，解决了分布式集群运行缺少规划的问题，在全局范围使整个系统任务的耗时和能量消耗达到最低。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种分布式多无人机集群协同空间搜索调度的控制仿真系统，其特征在于，包括：协同搜索调度单元和与之连接的多个仿真无人机；

所述协同搜索调度单元，用于根据接收的各仿真无人机在进行仿真飞行时逐帧输出的动态场景深度图进行协同空间搜索调度；在协同空间搜索调度中，基于各仿真无人机的动态场景深度图进行共享概率占据地图的更新，搜索出用于区分地图未知和已知区域的边界集合以及边界集合中每条边界对应的最佳观测点位置；进行观测点调度为每个仿真无人机分配一个最佳观测点位置作为仿真无人机的下一个目标点；

所述仿真无人机，用于接收对应的目标点位置进行轨迹规划生成连续性期望轨迹；并根据所述连续性期望轨迹在虚拟仿真环境中进行仿真飞行，验证分布式集群运行的平稳和安全性。

2.根据权利要求1所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，所述协同搜索调度单元包括共享地图处理子单元、边界搜索划分子单元和集群调度分配子单元；

共享地图处理子单元，用于根据多个无人机输出的各自所处位置的场景深度图，进行地图共享，生成共享概率占据地图；

边界搜索划分子单元，用于根据共享概率占据地图中的地图信息，进行边界搜索得到边界集合和与边界结合对应的边界最佳视点集合；

集群调度分配子单元，用于根据边界最佳观测点集合中观测点的数量和无人机的数量，以及无人机当前位置到观测点的代价进行运筹优化分配，为每个无人机分配到下一个目标点。

3.根据权利要求2所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，

所述共享地图处理子单元包括遮掩判定模块、掩码操作模块、共享概率更新模块和共享地图生成模块；

所述遮掩判定模块，用于根据各仿真无人机自所处位置的场景深度图进行多机相互遮掩判定，判定出存在遮掩的无人机；

所述掩码操作模块，用于对存在遮掩的无人机拍摄的深度信息进行掩码操作得到处理，得到处理后的深度图；

所述共享概率占据地图模块，用于基于处理后的深度信息将深度图转换为障碍点云，并根据所有无人机同时观测的障碍点云生成共享概率占据地图；

所述共享概率占据地图由一个动态数组构成，数组内每个数据单元内储存着由多无人机共同获得观测概率，以及由观测概率确定的地图元素所对应的状态；并根据逐帧更新的障碍点云，进行观测概率和观测状态的更新。

4.根据权利要求2所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，边界搜索划分子单元包括边界生成模块和最佳观测点生成模块；

所述边界生成模块，用于搜索共享概率占据地图中各数据单元当前状态找出用于区分地图未知和已知区域的边界集合；

共享概率占据地图中处于未知状态与自由状态的交接处的数据单元属于边界上的点，其自身状态为自由状态，其邻近单元状态为未知状态；

最佳观测点生成模块，用于搜索边界集合中的每一条边界的观测效率最大化的观测点位置；

在搜索过程中，通过搜索边界的均值点、边界两端相距最远的两个空间点位置、边界中心法线和根据由边界两端相距最远的两个空间点位置计算出的中心点位置和深度相机的感知视野FOV确定出最佳观测点位置。

5.根据权利要求2所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，所述集群调度分配子单元，包括优化问题模型、观测点与无人机数量判定模块和模型最优解寻求模块；

所述优化问题模型，用于将M个观测点VP_m∈{VP₁,…,VP_M}分配给N个无人机UAV_n∈{UAV₁,…,UAV_N}，一个无人机只分配一个观测点，并且分配后所有无人机以最小代价到达各自的观测点；

观测点与无人机数量判定模块，用于判定观测点数量M和无人机数量N的大小；

模型最优解寻求模块，用于根据观测点与无人机数量判定模块的判定结果，对优化问题模型进行求解，为每个无人机分配一个观测点作为下一个目标点；

具体的，当M＜N时，最优解寻求：

当M>N时，最优解寻求：

y_n∈{0,1}表示无人机UAV_n是否参与搜索；x_mn∈{0,1}表示将观测点VP_m分配给无人机UAV_n表示为x_mn∈{0,1}；其中，无人机UAV_n到观测点VP_m的代价表示为d_mn，所有无人机到达各自观测点的总代价表示为

通过分配优化使总代价Cost达到最小。

6.根据权利要求2所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，无人机UAV_n到观测点VP_m的代价为由距离代价、速度代价和速度变化代价组成的总观测代价d_mn。

7.根据权利要求1所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，所述仿真无人机，为仿真的四旋翼无人机，包括轨迹规划模块、第一类仿真传感器、第二类仿真传感器、非线性仿真控制器以及四个仿真电机；

所述轨迹规划模块，用于根据分配的目标点进行轨迹规划，得到考虑避障和避免无人机之间碰撞的连续性期望轨迹，输出到非线性仿真控制器；

所述第一类仿真传感器中包括仿真的深度相机；用于从仿真无人机所处虚拟环境的场景信息中获得包括动态场景深度图在内的前端传感数据，输出到协同搜索调度单元；

所述第二类仿真传感器中包括仿真的IMU、GPS模块、气压计和磁力计在内的位姿测量传感器，用于从仿真无人机所处虚拟环境的自然环境模拟数据中，感知仿真无人机的位姿信息；

所述非线性仿真控制器；用于根据期望轨迹和第二类仿真传感器感知的仿真无人机位姿信息，进行非线性控制，输出与仿真电机对应的电机转速；

所述仿真电机，用于根据非线性仿真控制器输出的电机转速模拟出无人机使用无刷直流电机在螺旋桨旋转过程中产生的推力、空气阻力、旋转力矩和空气阻力矩；以得到在四个仿真电机作用下，仿真的四旋翼无人机在仿真环境中所受的合力及合力矩。

8.根据权利要求7所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，所述轨迹规划模块包括一次轨迹规划模块、二次轨迹规划模块和轨迹输出模块；

所述一次轨迹规划模块，用于根据获得的下一个目标点位置进行路径规划，生成每个仿真无人机的连续性期望轨迹；

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

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

9.根据权利要求8所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，述二次轨迹规划模块中包括时间采样模块、采样位置点计算模块、碰撞判断模块、规划模块和轨迹输出模块；

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

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

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

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

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

10.根据权利要求1-9任一项所述的分布式多无人机协同空间搜索调度的控制仿真系统，其特征在于，还包括虚拟仿真环境单元；所述虚拟仿真环境单元与仿真无人机进行数据通信；将仿真的虚拟场景信息输出到仿真无人机的第一类仿真传感器中，使第一类仿真传感器能够感知场景信息得到包括动态场景在内的前端传感数据；根据仿真电机输出四旋翼无人机在仿真环境中所受的合力及合力矩，仿真得到的包括模拟力场、大气场和磁场在内的自然环境模拟数据输出到仿真无人机的第二类仿真传感器中，使第二类仿真传感器能够根据自然环境模拟数据感知无人机的位姿信息。

Images