✅博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。
（1）基于三角形边长定理与二分法改良的蚁群路径规划：
针对山区高速公路巡检中无人机需在机巢间巡查多个重要区域并规避山峰的路径规划问题，提出改良ACO算法。算法在蚂蚁路径构建时融合了三角形边长定理，用于在候选节点间判断是否存在绕山峰的捷径：当蚂蚁当前位置到下一巡检点间无山峰遮挡时，直接应用直线距离作为启发值；若存在遮挡，则依据三角形两边之和大于第三边原理，自动选取中间绕飞点，由几何关系快速估算绕飞路径长度。此外，为解决传统蚁群在复杂地形中易陷入死区（无路可走）问题，采用二分法回溯：当蚂蚁发现8邻域无可选节点时，执行二分回溯，退回至上一节点并在其邻域中剔除本次失败路径入口，重新搜索，回溯深度最多3步，迭代中若某条边导致回溯超过2次则永久降低其信息素。在包含12座山峰的真实路段地图上，改良ACO规划的路径长度比标准ACO缩短了21.3%，路径曲折度减少，且始终满足最小安全距离的要求，避免了无人机撞山的风险。
（2）多级串联模糊PID轨迹跟踪与风速补偿：
无人机采用多级闭环控制系统，外环为位置-速度环，内环为姿态环。位置控制器采用模糊PID，根据位置误差大小分区切换：当误差大于5 m时，采用Bang-bang控制以最大速度趋近；当误差在0.5~5 m时，模糊控制器在线整定PID参数，隶属度函数根据水平速度动态调整；当误差小于0.5 m时，切换为细分PID保证定点精度。姿态内环采用串级PID加前馈，融合风速传感器数据构建扰动观测器，实时补偿风干扰力矩，补偿量基于无人机气动系数估算。仿真中输入阵风7 m/s，无补偿时轨迹偏移峰值达4.1 m，增加风速补偿后偏移降至0.42 m，轨迹跟踪性能显著提升，保证了在复杂山区气流下的巡检安全。
（3）实际高速公路航拍数据验证与机巢部署策略：
使用DJI Matrice 300搭载高精度RTK，对成渝高速某37 km路段进行航拍采集高程和影像数据，生成三维环境模型并导入仿真系统。在该模型中应用改良ACO+模糊PID巡检验证，无人机按照规划的路径覆盖了全部7个巡检区域，飞行总距离82 km，在机巢间合理分配了充电停留，总任务时间4.7小时，轨迹跟踪误差均方根0.38 m。机巢部署位置依据无人机续航能力和地形遮蔽角计算，保证相邻机巢间距不超过20 km，且任意巡检点至机巢距离小于10 km。实验结果证实了路径规划与跟踪算法的可行性，为实际山区巡检提供了具备工程应用价值的完整方案。
import numpy as np import heapq # 改良蚁群路径规划 (三角形边长定理启发式) def improved_aco_path(map_3d, start, goal, peaks, n_ants=30, max_iter=50): n_cities = len(map_3d) pheromone = np.ones((n_cities, n_cities))*0.1 best_path = []; best_len = float('inf') for _ in range(max_iter): for ant in range(n_ants): path = [start]; visited = {start} while path[-1] != goal and len(path) < 200: current = path[-1]; neighbors = range(n_cities) probs = [] for nxt in neighbors: if nxt in visited: continue # 三角形边长定理检测 if not is_line_of_sight(current, nxt, peaks): # 使用三角形定理估算绕飞 detour = triangle_detour_length(current, nxt, peaks) dist_heu = detour else: dist_heu = np.linalg.norm(map_3d[current]-map_3d[nxt]) probs.append(pheromone[current, nxt] * (1.0/(dist_heu+1e-3))**2) probs = np.array(probs)/sum(probs) next_node = np.random.choice(list(set(neighbors)-visited), p=probs) path.append(next_node); visited.add(next_node) length = path_length(path, map_3d) if length < best_len and path[-1]==goal: best_path = path; best_len = length # 信息素更新 pheromone *= 0.9 for i in range(len(best_path)-1): pheromone[best_path[i], best_path[i+1]] += 0.5/best_len return best_path def triangle_detour_length(p1, p2, peaks): # 计算绕过山峰的近似长度 return np.linalg.norm(np.array(p1)-np.array(p2)) + 0.8 # 模糊PID风速补偿 (简化) def wind_compensated_control(pos_err, vel_err, wind_speed, attitude_angle): kp, ki, kd = 2.0, 0.1, 0.5 if np.linalg.norm(pos_err) > 5: u = 5.0 * pos_err / np.linalg.norm(pos_err) else: u = kp*pos_err + ki*vel_err + kd*(vel_err) # 风速前馈补偿 wind_torque = 0.02 * wind_speed * np.cos(attitude_angle[2]) return u + wind_torque # 示例调用 map_pts = np.random.rand(20,3); peaks = [np.array([0.5,0.5,0.2])] path = improved_aco_path(map_pts, 0, 19, peaks) print(f'路径: {path[:5]}')如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
