一种基于多无人机协同的3D路径规划方法
本发明涉及无人机智能导航领域,特别涉及一种基于多无人机协同的3d路径规划方法。
背景技术:
1、随着无人机技术的发展,多无人机集群作业因效率高、覆盖范围广的优势,逐渐替代单无人机应用于复杂场景。路径规划作为多无人机作业的核心技术,需解决“环境感知-路径搜索-协同避冲突”三大关键问题,但现有技术存在以下局限性:
2、1、3d环境感知精度不足:传统路径规划方法(如、)依赖人工定义的启发函数,仅能处理简单2d或低维度3d环境,无法捕捉复杂3d空间(如多层建筑物、立体障碍物)的全局特征;现有深度学习方法多基于2d卷积神经网络,将3d环境降维为2d处理,丢失垂直方向(z轴)的障碍物信息,导致可行区域预测偏差,路径易与3d环境中的障碍物碰撞。
3、2、多无人机协同效率低:现有多无人机路径规划多采用“单无人机规划+后验冲突调解”模式,缺乏统一的全局环境感知基础,各无人机独立规划路径后再调整冲突,导致总规划时间长、路径冗余;部分协同方法依赖集中式控制,但未结合深度学习优化环境感知,难以适应动态或复杂障碍物场景。
4、3、路径搜索与优化性能差:传统rrt算法采用均匀随机采样,在3d环境中采样盲目性高,路径搜索时间长(尤其障碍物密集场景);现有改进rrt算法未结合3d全局特征,采样效率提升有限,且路径优化仅依赖局部节点调整,无法实现全局路径长度最小化。
5、因此,亟需一种能精准感知3d环境、高效协同多无人机、快速生成优化路径的方法,解决现有技术在复杂3d场景下的适应性与效率问题。
技术实现思路
1、为了解决上述技术问题,本发明提供一种环境感知精度高、路径效率高的基于多无人机协同的3d路径规划方法。
2、本发明解决上述技术问题的技术方案是:一种基于多无人机协同的3d路径规划方法,包括以下步骤:
3、步骤1:收集路径规划的三维环境数据并处理成体素信息,构建城市三维建筑体素模型,城市三维建筑体素模型包含空间坐标体系和约束规则库;
4、步骤2:接受多无人机的路径规划请求,路径规划请求包括无人机的起点坐标、无人机的终点坐标以及目标优化参数;
5、步骤3:利用已经预训练的三维视觉变换模型3dvit对城市三维建筑体素模型进行分析,预测出可能存在有效路径的区域;
6、步骤4:采用改进型路径搜索算法对构建的城市三维建筑体素模型进行路径求解;
7、步骤5:通过优化路径的动态规划算法对路径进行优化;
8、步骤6:使用图形处理单元gpu进行并行加速,以加速三维视觉变换模型的训练和推理过程。
9、上述基于多无人机协同的3d路径规划方法,所述步骤1中,三维环境数据为多源异构数据,包括激光雷达点云数据和航拍图像数据;
10、激光雷达点云数据表示为三维点集:
11、;
12、其中,为三维点集;为中第1个点云数据点;为中第个点云数据点,为点云数据点的数量;,分别为在预设空间直角坐标系中沿x轴、y轴和z轴的坐标值;
13、航拍图像数据用二维矩阵表示:
14、;
15、其中,表示航拍图像的高;表示航拍图像的宽;3为rgb三通道;为整数集合;为航拍图像的像素值矩阵,中各元素的取值范围为内的整数;
16、处理成体素信息的方式为:将三维环境数据转换为体素网格,体素网格为一个大小为的三维矩阵,表示体素网格在每个维度方向上的体素划分数目;定义单个体素的物理边长为,即体素在三维空间各方向上的跨度均为米;体素网格中的元素表示空间位置索引处的体素属性,体素网格中的元素值为1时表征体素为障碍物,体素网格中的元素值为0时表征体素为可通行区域。
17、上述基于多无人机协同的3d路径规划方法,所述步骤1中,约束规则库中包括高度限制规则和障碍物碰撞阈值;
18、高度限制规则为:获取与规划路径点相对应的体素高度索引并根据体素尺寸将体素高度索引换算为相对于起飞平面的实际飞行高度,实际飞行高度不超出与当前地理区域类型相关联的预设高度阈值;其中,地理区域类型至少根据城市建成区密度划分为核心城区、郊区和空旷区域,核心城区、郊区和空旷区域对应的高度阈值分别为100米、200米和300米;
19、障碍物碰撞阈值为:定义无人机与静态障碍物之间的最小安全距离,。
20、上述基于多无人机协同的3d路径规划方法,所述步骤3中,三维视觉变换模型3dvit包括依次连接的3d卷积嵌入层、序列展平层、位置嵌入层、transformer编码器和输出层;
21、3d卷积嵌入层接收体素网格对应的五维输入张量,所述五维输入张量的维度依次表示批次维度、通道维度、深度维度、高度维度和宽度维度;经3d卷积嵌入层进行特征提取后,得到形状为[b, 64, , , ]的五维特征张量,其中,b表示批次大小,64表示特征通道数,分别表示五维特征张量在深度方向、高度方向和宽度方向上的尺寸;
22、序列展平层将所述五维特征张量的空间维度进行展平,得到形状为[b, , 64]的特征张量,其中,,表示展平后的空间位置数量;
23、在位置嵌入层中设置可学习的位置嵌入矩阵,所述位置嵌入矩阵的形状为,其中,1表示位置嵌入矩阵在批次维度上的维数,64表示每个空间位置对应的特征维度;将位置嵌入矩阵沿批次维度扩展至与序列展平层输出的特征张量相匹配,并与序列展平层输出的特征张量对应元素相加,以向各空间位置对应的特征中注入空间位置信息;
24、transformer编码器堆叠6层相同的编码器模块,每层编码器模块含8头自注意力机制与隐藏层维度512的前馈神经网络,采用gelu激活函数;每层编码器模块后接残差连接和层归一化;transformer编码器输出序列特征;
25、输出层将transformer编码器输出的序列特征恢复为空间排列,得到形状为[b,64,,,]的五维空间特征张量,并采用1×1×1卷积核将每个空间位置的特征维度从64映射至1,输出形状为 [b,1,,,]的单通道特征图,随后采用三线性插值将单通道特征图恢复至与输入体素网格相同的空间分辨率,得到形状为[b,1,,,]的单通道预测图,每个体素位置的值表示对应体素的可通行概率。
26、上述基于多无人机协同的3d路径规划方法,所述步骤3中,将体素网格输入三维视觉变换模型3dvit,3dvit输出每个体素的可通行概率,,的区域标记为潜力路径区域,潜力路径区域作为后续路径搜索的优先采样区域。
27、上述基于多无人机协同的3d路径规划方法,所述步骤4中,改进型路径搜索算法基于快速随机探索树算法优化后再进行路径求解,具体过程为:
28、步骤41,算法初始化:输入体素网格、无人机的起点坐标、无人机的终点坐标及3dvit,构建搜索树,搜索树的初始节点为起点坐标;
29、步骤42,潜力路径区域预测:将体素网格转换为张量,输入3dvit并关闭梯度计算,3dvit输出单通道预测图,单通道预测图中显示每个体素的可通行概率,将单通道预测图中可通行概率大于或等于0.7的体素确定为潜力路径体素,并将潜力路径体素对应的坐标索引集合确定为潜力路径区域;
30、步骤43,非均匀采样生成随机采样点;
31、首先,按以下概率分布生成采样点:
32、80%概率从潜力路径区域采样:从步骤42确定的潜力路径区域中随机选取1个体素网格的坐标索引作为采样点;
33、10%概率从终点邻域采样:用表示体素网格在对应坐标轴方向上的体素数量;在以无人机的终点坐标为中心,沿x轴、y轴和z轴方向分别向两侧扩展5个体素的邻域范围内生成随机坐标,通过截断函数限制在体素网格边界内,随机坐标计算式为:
34、;
35、;
36、;
37、其中,为截断函数,截断函数用于将生成的坐标限制在体素网格的有效索引范围内;为随机数生成函数,随机数生成函数用于在预设终点邻域范围内生成随机坐标;、、分别为随机坐标在预设空间直角坐标系中沿x轴、y轴和z轴的坐标值;、、分别为无人机的终点坐标在预设空间直角坐标系中沿x轴、y轴和z轴的坐标值;
38、10%概率全局随机采样:在体素网格全范围内随机生成三维坐标,三维坐标计算式为:
39、;
40、其中,、、分别为在全局所有体素网格中随机采样生成的采样点的x轴、y轴和z轴的坐标;
41、将按概率分布生成的采样点记为随机采样点,,其中,、和分别为随机采样点在预设空间直角坐标系中沿x轴、y轴和z轴的坐标值;
42、步骤44,遍历搜索树中的所有节点,将搜索树中的第个候选节点记为,,计算与随机采样点之间的欧氏距离:
43、;
44、其中,、、分别为搜索树中的第个候选节点的x轴、y轴和z轴坐标;
45、选取搜索树中与随机采样点欧氏距离最小的候选节点作为最近节点,最近节点记为,若最近节点与随机采样点重合则跳过本次迭代;
46、步骤45,生成新节点:计算最近节点到随机采样点的第一单位方向向量;
47、;
48、其中,、、分别为最近节点的x轴、y轴、z轴坐标,沿第一单位方向向量移动预设扩展步长生成新节点,预设扩展步长记为,新节点为,,新节点的坐标计算式为:
49、;
50、;
51、;
52、其中,、、分别为新节点的x轴、y轴、z轴坐标;、、分别表示第一单位方向向量的x轴、y轴、z轴方向上的分量;
53、对新节点的各坐标进行边界约束,使新节点位于体素网格的有效索引范围内;
54、步骤46,碰撞检测验证:设置无人机与静态障碍物之间的最小安全距离,并基于无人机与静态障碍物之间的最小安全距离检查最近节点与新节点之间的路径是否与静态障碍物发生碰撞;对通过碰撞检测的新节点,在其预设邻域范围内选择累计路径代价最小且路径无碰撞的节点作为父节点,并根据经新节点连接后的累计路径代价对邻域节点的父节点进行更新,以实现搜索树的重连优化;
55、步骤47:将通过碰撞检测的新节点加入搜索树;当新节点与无人机的终点坐标之间的欧氏距离小于或等于预设终点距离阈值,且新节点与无人机的终点坐标之间的直线路径通过碰撞检测时,将无人机的终点坐标加入搜索树;从无人机的终点坐标开始按照搜索树中的父节点关系依次反向追溯至无人机的起点坐标,并按照从无人机的起点坐标至无人机的终点坐标的顺序重新排列路径节点,得到待优化路径。
56、上述基于多无人机协同的3d路径规划方法,所述步骤46中,碰撞检测验证的过程为:
57、首先,计算最近节点指向新节点的第二单位方向向量,并记为,第二单位方向向量的计算公式为:
58、;
59、其中,分别表示第二单位方向向量在x轴、y轴和z轴方向上的分量;
60、计算最近节点与新节点之间的欧氏距离并向下取整,以确定碰撞检测的采样数量:
61、;
62、其中,为碰撞检测的采样数量;表示向下取整运算;
63、然后,沿第二单位方向向量依次生成个中间检测点,第个中间检测点坐标为:
64、;
65、其中,为中间检测点的索引,;
66、接着,以第个中间检测点为中心遍历第个中间检测点的三维邻域,将相对于中间检测点在x轴、y轴和z轴方向上的局部坐标偏移量分别记为 ,并按照预设安全距离确定待检测的三维邻域范围;,且满足邻域约束:,其中,为无人机与静态障碍物之间的最小安全距离;
67、若三维邻域范围内存在体素值为1的障碍物体素,则判定最近节点与新节点之间的路径发生碰撞;若三维邻域范围内不存在障碍物体素,则判定路径无碰撞。
68、上述基于多无人机协同的3d路径规划方法,所述步骤4中,快速随机探索树算法的规划终止条件、路径长度计算为:
69、迭代次数约束:设置最大迭代次数,达到最大迭代次数未生成待优化路径则规划失败;
70、时间约束:设置规划超时时间为240秒,超时判定公式为:
71、;
72、其中,为当前时刻,表示改进型路径规划算法的初始化时刻,若超时仍未生成待优化路径,则判定路径规划失败;
73、路径长度计算:设待优化路径包含若干个依次连接的路径节点,则待优化路径总长度为各相邻路径节点之间欧氏距离之和;
74、路径有效性约束:重构路径后需经动态规划优化,且优化后的路径通过碰撞检测验证,方可作为有效路径输出。
75、上述基于多无人机协同的3d路径规划方法,所述步骤5的具体过程为:
76、步骤51,路径初始化:将步骤4获得的待优化路径表示为节点序列:
77、
78、其中,为节点序列;为第1个节点;为第个节点;为待优化路径包含的节点数量;
79、步骤52,动态规划数组初始化:
80、构建距离数组,;表示从到的当前已知最短路径长度;,表示第1个节点到自身的距离为0,中其余元素初始化为无穷大;
81、构建前驱节点数组,,表示在当前已知最短路径上的前驱节点;并将前驱节点数组中的各元素初始化为空值,空值表示对应前驱节点的最优前驱节点尚未确定;
82、步骤53,节点对距离计算与更新:
83、对于待优化路径的第个节点,,遍历位于之前的第个节点,,若第个节点与第个节点之间的直线路径通过步骤46所述的碰撞检测验证,则计算与之间的欧氏距离:
84、
85、其中,表示与之间的欧氏距离;分别表示的x轴、y轴、z轴坐标;分别表示的x轴、y轴、z轴坐标;
86、采用距离数组存储从到各节点的当前已知最短路径长度,表示从到的当前已知最短路径长度,表示从到的当前已知最短路径长度;
87、若满足:,则更新:,同时记录:;即当经到达的累计路径长度小于当前记录的从到的当前已知最短路径长度,将确定为的当前最优前驱节点,并更新对应的当前已知最短路径长度;
88、步骤54,优化路径重构:
89、从开始,根据前驱节点索引依次反向追溯至,得到反向节点序列;将反向节点序列按照从起点至终点的顺序重新排列,得到优化路径;
90、步骤55,路径有效性验证:遍历优化路径中的相邻节点对,并按照步骤46的碰撞检测验证各相邻节点之间的路径是否无碰撞;当所有相邻节点之间的路径均通过碰撞检测时,输出有效路径。
91、本发明的有益效果在于:
92、1、3d环境感知精度显著提升:本发明将3dvit引入多无人机路径规划,通过3d卷积与全局注意力提取三维环境特征,在32×32×32体素网格上的体素级验证准确率达到98.92%,可为后续非均匀采样提供潜力路径区域预测。
93、2、路径规划质量提高:基于3dvit预测结果引导非均匀采样,并结合动态规划进行路径优化,3架无人机初始路径总长度由110.39个网格单元降至96.376个网格单元,缩短12.70%;相比传统rrt算法,路径总长度降低38.75%。
94、3、多无人机协同性能优化:采用“优先级规划+临时障碍区域标记”的协同机制,在3架无人机场景下,排除共同终点邻域后空间路径冲突率为0%。
95、4、计算效率提高:利用gpu加速3dvit推理,单次推理速度约为cpu的7.77倍,3架无人机批量推理相较gpu串行推理加速约2.29倍。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com