✨ 长期致力于航迹规划、多无人机、目标分配、人工势场算法、三维空间研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）多目标任务分配与人工势场基础：
将多无人机三维航迹规划分解为两个子问题。任务分配阶段，以最小化总任务能耗和总任务时长为目标，构建一个带权重的分配模型，权重系数ω1=0.6, ω2=0.4。使用匈牙利算法求解分配矩阵，将N架无人机分配给M个目标点（N>=M）。能耗估算基于路径长度和平飞高度，时长考虑最大速度限制。人工势场算法中，引力势场U_att = 0.5*k_att*(d^2)，斥力势场U_rep = 0.5*k_rep*(1/d - 1/d0)^2，d0为斥力影响距离。在三维空间中，合势场的梯度方向引导无人机移动。针对多无人机协同，在势场中增加无人机间斥力U_intra = 0.5*k_intra*(1/d_uv - 1/d_safe)^2，当d_uv<d_safe时启用。在10架无人机对8个目标点的仿真中，分配算法得到的最大任务时长差异小于5%，各无人机路径无交叉。
（2）APF-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_att=0.5, k_rep=50, d0=5.0, step=0.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_norm+1e-6)) return force def total_force(self, pos): return self.attractive_force(pos) + self.repulsive_force(pos) def is_trapped(self, pos, history, threshold=0.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_iter=30, step_rrt=0.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_iter=1000): 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
