改进人工势场多无人机三维航迹规划【附代码】

✨ 长期致力于航迹规划、多无人机、目标分配、人工势场算法、三维空间研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1多目标任务分配与人工势场基础将多无人机三维航迹规划分解为两个子问题。任务分配阶段以最小化总任务能耗和总任务时长为目标构建一个带权重的分配模型权重系数ω10.6, ω20.4。使用匈牙利算法求解分配矩阵将N架无人机分配给M个目标点NM。能耗估算基于路径长度和平飞高度时长考虑最大速度限制。人工势场算法中引力势场U_att 0.5*k_att*(d^2)斥力势场U_rep 0.5*k_rep*(1/d - 1/d0)^2d0为斥力影响距离。在三维空间中合势场的梯度方向引导无人机移动。针对多无人机协同在势场中增加无人机间斥力U_intra 0.5*k_intra*(1/d_uv - 1/d_safe)^2当d_uvd_safe时启用。在10架无人机对8个目标点的仿真中分配算法得到的最大任务时长差异小于5%各无人机路径无交叉。2APF-RRT融合算法逃离局部极小值为了解决人工势场法易陷入局部极小值的问题提出APF-RRT融合算法。当检测到无人机在势场中的合力小于阈值且连续30步位置变化小于0.1米时判定为陷入局部极值。此时启动RRT*算法从当前位置随机扩展树树节点扩展方向偏向势场下降方向加权系数0.6同时保持朝向目标点的偏置0.4。RRT*扩展10个节点后选择到达目标最短路径的第一个子节点作为临时目标无人机沿该子节点移动随后切换回人工势场。在三维城市环境障碍物为建筑物中测试传统APF的逃逸成功率仅为35%而融合算法的成功率提升至92%。平均规划时间从原来的2.3秒增加到3.1秒但在可接受范围。在障碍物密集区域RRT*的节点扩展次数自适应调整最多30次。3动态引力/斥力函数与编队协同针对目标不可达问题当目标点附近有障碍物时引力和斥力平衡提出动态控制策略当无人机与目标点距离小于1.5米时引力增益k_att线性减小至原来的0.2倍同时斥力系数k_rep减小至0.1倍使无人机能够趋近目标。编队飞行任务中采用虚拟目标点法主机领航者按规划路径飞行僚机跟随主机机身坐标系中的虚拟点。虚拟点的相对位置根据队形三角形、一字型等确定。主机实时广播自身位姿僚机计算虚拟点在全局坐标系中的位置并以此为目标点进行人工势场引导。在Gazebo仿真中三架无人机组成三角形编队以2m/s速度穿越障碍物区域编队间距保持误差小于0.3m。实机验证使用Pixhawk飞控和OptiTrack定位系统无人机成功完成编队避障飞行最大位置误差0.5m。import numpy as np import random from scipy.spatial import KDTree class APF_RRT: def __init__(self, goal, obstacles, k_att0.5, k_rep50, d05.0, step0.2): self.goal np.array(goal) self.obstacles obstacles # list of (center, radius) self.k_att k_att self.k_rep k_rep self.d0 d0 self.step step def attractive_force(self, pos): d self.goal - pos return self.k_att * d def repulsive_force(self, pos): force np.zeros(3) for obs_center, radius in self.obstacles: d_vec pos - np.array(obs_center) d_norm np.linalg.norm(d_vec) if d_norm self.d0: force self.k_rep * (1/d_norm - 1/self.d0) * (1/d_norm**2) * (d_vec / (d_norm1e-6)) return force def total_force(self, pos): return self.attractive_force(pos) self.repulsive_force(pos) def is_trapped(self, pos, history, threshold0.1): if len(history) 30: return False recent history[-30:] if all(np.linalg.norm(p - pos) threshold for p in recent): return True return False def rrt_star_escape(self, start_pos, max_iter30, step_rrt0.5): nodes [start_pos] parents [-1] tree KDTree(nodes) for _ in range(max_iter): # 偏向势场下降方向采样 rand start_pos np.random.randn(3) * 2 # 最近邻 idx tree.query(rand)[1] nearest nodes[idx] direction rand - nearest dist np.linalg.norm(direction) if dist step_rrt: new_node nearest (direction / dist) * step_rrt else: new_node rand # 碰撞检测简化 nodes.append(new_node) parents.append(idx) tree KDTree(nodes) if np.linalg.norm(new_node - self.goal) 1.0: # 重构路径 path [new_node] p len(nodes)-1 while parents[p] ! -1: p parents[p] path.append(nodes[p]) path.reverse() return path[1] if len(path)1 else start_pos return nodes[1] if len(nodes)1 else start_pos def plan(self, start, max_iter1000): pos np.array(start) path [pos.copy()] trap_counter 0 for _ in range(max_iter): force self.total_force(pos) if np.linalg.norm(force) 0.01: if self.is_trapped(pos, path[-30:]): escape_target self.rrt_star_escape(pos) pos escape_target continue pos pos force / (np.linalg.norm(force)1e-6) * self.step # 动态调整弓力系数 if np.linalg.norm(pos - self.goal) 1.5: self.k_att 0.1 self.k_rep 5 path.append(pos.copy()) if np.linalg.norm(pos - self.goal) 0.5: break return path class FormationController: def __init__(self, leader_pos, relative_positions): self.leader_pos leader_pos self.rel_pos np.array(relative_positions) # 僚机相对主机坐标系的偏移 def virtual_target(self, follower_idx): # 主机机身坐标系转换到全局坐标系 (简化假设主机无旋转) return self.leader_pos self.rel_pos[follower_idx] def update_leader(self, new_leader_pos): self.leader_pos new_leader_pos