[发明专利]一种多无人机协同避障的规划方法

权利要求书

1.一种多无人机协同避障的规划方法，其特征在于包括以下步骤：

步骤一：设置任务场景参数和任务参数；

所述任务场景参数包括：任务环境场景大小、相同构型的无人机数量N_uav和栅格离散度resolution；

所述任务参数包括：协同避障规划的起点坐标start_pt、终点坐标end_pt以及无人机的可探测距离，所述无人机的可探测距离为sense_horizon；

步骤二：无人机通过无人机通信邻居表与相邻无人机进行同构信息交换；

所述同构信息包括感知信息和飞行状态信息；

所述感知信息包括无人机当前位置在世界坐标系中的坐标和自身持有的局部占据栅格地图；

所述飞行状态信息包括无人机的自身全局路径和预期轨迹；

所述局部占据栅格地图构建步骤为：

首先，利用栅格离散度resolution对任务环境场景大小进行离散化，得到局部占据栅格地图的大小，然后利用无人机在飞行过程中不断通过机载传感器获取自身感知范围的障碍物的位置和几何性质，并将障碍物的位置和几何性质进行凸化膨胀处理，得到障碍物栅格区域，最后，根据局部占据栅格地图的大小和障碍物栅格区域构建局部占据栅格地图；

步骤三：基于最大一致性理论和分布式通信策略设计分布式信息融合算法，然后利用分布式信息融合算法和当前无人机与其他无人机交换的感知信息对当前无人机自身持有的局部占据栅格地图进行迭代更新，并设置标识缓存池，当标识缓存池中的信息无更新时，停止迭代，此时，无人机持有的局部占据栅格地图即为最优全局占据栅格地图G；

步骤四：在协同避障规划的起点坐标start_pt和终点坐标end_pt之间搜索一条无障碍的时间最短全局路径；

步骤五：利用无障碍的时间最短全局路径在sense_horizon范围内选取局部目标点tag_pt，并对协同避障规划的起点坐标start_pt和局部目标点tag_pt之间的路径进行轨迹优化，得到一条由起点坐标start_pt通往局部目标点tag_pt的光滑可行预期轨迹；

步骤六：利用步骤三中得到的最优全局占据栅格地图G对步骤五中得到的光滑可行预期轨迹进行碰撞检测，碰撞检测具体步骤为：

设定前向检查时间t_head和紧急制动时间t_stop，然后计算碰撞概率p_collision，碰撞概率p_collision表示为：

其中，dv为新检测出障碍物的速度，dp为障碍物的当前位置，odomPt为无人机的当前位置，getOccupy为最优全局占据栅格地图G的占据概率获取函数，prePt为无人机在t时刻无人机的预期轨迹点；

若碰撞概率p_collision大于0.5，则判定光滑可行预期轨迹与新检测出的障碍物产生了碰撞冲突；

当碰撞冲突时机在前向检查时间t_head内时，将碰撞指标置为1；当碰撞冲突时机在制动时间t_stop内时，使无人机进入紧急悬停状态；

步骤七：当达到重规划的时机时，将协同避障规划的起点start_pt更新为无人机当前时刻超前0.5秒的预期轨迹点，重新执行步骤四、步骤五和步骤六；

所述重规划的时机为：

(1)局部目标点tag_pt与无人机当前位置odomPt的欧式距离达到设定阈值，并且无人机未到达终点；

(2)步骤五获得的碰撞指标值等于1；

(3)当前无人机与邻居无人机间的距离大于协同距离或者小于安全距离，所述当前无人机与邻居无人机间的距离通过当前无人机和其他无人机交换的飞行状态信息得到。

2.根据权利要求1所述的一种多无人机协同避障的规划方法，其特征在于所述步骤四中无障碍的时间最短全局路径通过以下步骤得到：

利用当前无人机自身局部占据栅格地图，以起点坐标start_pt为波源，模拟波在环境中的传播，在传播过程中对当前传播环境节点的邻节点进行评价，得到评价值，并选取评价值最小的邻节点为下一传播环境节点，当波扩展到终点坐标end_pt时结束传播过程，并在终点坐标end_pt处沿到达时间的梯度下降方向回溯传播路径，得到一条从终点坐标end_pt到起点坐标start_pt的最短时间的传播路径，即为无障碍的时间最短全局路径。