✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、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