无人机集群自主编队控制与路径规划仿真技术【附仿真】

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导，毕业论文、期刊论文经验交流。
✅ 专业定制毕设、代码
✅如需沟通交流，可以私信，或者点击《获取方式》

（1）基于动态启发权重的改进A*全局路径与领航路径生成层：

针对传统A*算法在复杂环境搜索效率低和转折多的问题，设计了动态加权A*算法DW-AStar。估价函数f(n)=g(n)+w(n)*h(n)，其中w(n)根据节点周围障碍物密度动态调整：障碍物占比小于20%时w(n)=1.5以加速搜索，大于50%时w(n)=0.8以保证路径安全。同时引入影响函数，对历史扩展节点周围加惩罚值以避免重复探索。路径生成后，采用分段线性简化删除冗余节点：从起点开始，依次检查中间节点是否与起点连线无障碍，若可行则删除中间节点。在50×50栅格地图上DW-AStar较传统A*搜索时间缩短16.6%，转折点减少51.06%，路径长度缩短7.3%。该算法为领航者uav0生成全局路径，领航者沿此路径飞行，作为编队运动的参考轨迹。

（2）改进人工势场法的动态避障与队形保持约束融合：

传统人工势场法存在目标不可达和局部极小问题，提出双重修正的人工势场法DR-APF。引力势场函数引入调节因子η*(1-exp(-d/d_goal))，使得引力随距离减小而非线性降低避免目标点振荡；斥力势场在障碍物影响范围内增加与目标距离相关的修正项ξ*d_goal，解决目标旁有障碍物时无法到达的问题。为解决编队通过障碍物区域的队形保持问题，将编队虚拟结构引入势场：对于每个跟随者，在引力项中加入编队保持力F_formation=Kf*(p_desired - p_i)，在斥力项中增加无人机间避碰力F_coll=Kc*exp(-d_ij/r0)防止内部相撞。通过调节编队保持力权值Kf实现编队构型动态调整：当检测到障碍物走廊时，Kf衰减至0.2，编队由三角形转为直线通过；通过后恢复。MATLAB仿真验证能够通过宽3m的狭窄缝隙，且通过后队形恢复误差小于0.15m。

（3）领航-跟随法与混合路径规划融合的全自主编队系统与三维仿真：

将领航-跟随法、DW-AStar和DR-APF整合为分层混合控制架构。上层由领航者运行DW-AStar规划全局路径并发布参考轨迹；中层编队控制器计算每个跟随者的期望位置并输出DR-APF势场力；底层为PID姿态控制器将力转化为速度和航向角指令。在Gazebo三维仿真中使用6架四旋翼模型，设置包含高楼、树木和移动障碍的城市场景。通过ROS节点进行分布式通信。仿真显示，编队在3km飞行任务中，总飞行距离较纯APF方案减少9.7%，飞行时间减少17.8%，通过障碍物区域时队形最大畸变率仅18%，并且在遇到动态障碍物（气球）时可成功规避而维持队形。进一步在MATLAB的GUI中集成参数调节面板，方便调试Kf和η等参数，提升了编队设计效率。

import numpy as np import heapq import matplotlib.pyplot as plt # 动态加权A*算法 class DW_AStar: def __init__(self, grid): self.grid = grid def heuristic(self, a, b): return np.linalg.norm(np.array(a)-np.array(b)) def obstacle_density(self, node, radius=3): x,y = node; count=0; total=0 for i in range(-radius, radius+1): for j in range(-radius, radius+1): if 0<=x+i<self.grid.shape[0] and 0<=y+j<self.grid.shape[1]: total+=1 if self.grid[x+i,y+j]==1: count+=1 return count/total if total>0 else 0 def plan(self, start, goal): open_set = [(0, start)]; came_from={}; g_score={start:0}; f_score={start:self.heuristic(start,goal)} while open_set: _, current = heapq.heappop(open_set) if current == goal: path=[current]; while current in came_from: current = came_from[current]; path.append(current) return path[::-1] for dx,dy in [(-1,0),(1,0),(0,-1),(0,1)]: neighbor = (current[0]+dx, current[1]+dy) if 0<=neighbor[0]<self.grid.shape[0] and 0<=neighbor[1]<self.grid.shape[1] and self.grid[neighbor]==0: dens = self.obstacle_density(neighbor) w = 1.5 if dens<0.2 else (0.8 if dens>0.5 else 1.0) tentative_g = g_score[current] + 1 if neighbor not in g_score or tentative_g < g_score[neighbor]: g_score[neighbor]=tentative_g f_score[neighbor]=tentative_g + w*self.heuristic(neighbor,goal) heapq.heappush(open_set, (f_score[neighbor], neighbor)) came_from[neighbor]=current return None # DR-APF势场力计算 def dr_apf_force(pos, goal, obstacles, other_uavs, Kf): # 引力 d = np.linalg.norm(pos-goal) eta = 1.0; F_att = eta*(1-np.exp(-d/5.0)) * (goal-pos)/d # 斥力 F_rep = np.array([0.0,0.0,0.0]) for obs in obstacles: d_obs = np.linalg.norm(pos-obs['center']) if d_obs < obs['radius']: F_rep += 2.0*(1/d_obs - 1/obs['radius'])*(1/d_obs**2)*(pos-obs['center'])/d_obs # 编队保持力 F_form = Kf * (desired_pos - pos) # 无人机间避碰 F_coll = np.zeros(3) for other in other_uavs: dij = np.linalg.norm(pos-other) if dij < 1.5: F_coll += 0.5*np.exp(-dij/0.6) * (pos-other)/dij return F_att + F_rep + F_form + F_coll # 编队控制节点 def formation_control_loop(leader_path, followers): for t in range(len(leader_path)): leader_pos = leader_path[t] for i, f in enumerate(followers): desired = leader_pos + formation_offset[i] force = dr_apf_force(f['pos'], desired, obstacles, [f2['pos'] for f2 in followers if f2!=f], Kf=0.8) f['vel'] += force*dt f['pos'] += f['vel']*dt