import os import csv from pathlib import Path from dataclasses import dataclass import numpy as np import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt from matplotlib.patches import Circle from concurrent.futures import ProcessPoolExecutor, as_completed ROOT = Path(__file__).resolve().parent FIGDIR = ROOT / "figures_generated_by_code" FIGDIR.mkdir(exist_ok=True) # ========================= # Scenario definition # ========================= q = np.array([ [-1.2, -0.8, 0.0], [1.2, -0.8, 0.0], [-1.2, 0.8, 0.0], [1.2, 0.8, 0.0], [-0.75, -0.45, 0.65], [0.75, -0.45, 0.65], [-0.75, 0.45, 0.65], [0.75, 0.45, 0.65] ], dtype=float) obs = np.array([ [3.20, 0.55, 1.25, 0.48], [4.80, -0.70, 1.50, 0.52], [6.55, 0.20, 1.80, 0.58], [8.10, -0.95, 1.42, 0.55], [10.00, 0.85, 1.95, 0.54], [12.15, -0.40, 1.62, 0.62], [14.30, 0.68, 2.08, 0.55], [16.15, -0.78, 1.78, 0.58] ], dtype=float) plt.rcParams.update({ 'font.size': 8, 'font.family': 'DejaVu Sans', 'axes.titlesize': 8, 'axes.labelsize': 8, 'xtick.labelsize': 7, 'ytick.labelsize': 7, 'axes.linewidth': 0.8, 'mathtext.default': 'regular', }) COL_W = 84 / 25.4 DPI = 600 @dataclass class Metrics: finalRMS: float maxRMS: float minClearance: float minPair: float meanEnergy: float @dataclass class Result: time: np.ndarray P: np.ndarray U: np.ndarray e: np.ndarray clearance: np.ndarray pair: np.ndarray Pd: np.ndarray metrics: Metrics _cache = {} def savefig(fig, path): fig.savefig(path, dpi=DPI, facecolor='white', bbox_inches='tight', pad_inches=0.02) plt.close(fig) def stylize(ax): ax.tick_params(direction='out', width=0.8, length=3) for s in ax.spines.values(): s.set_linewidth(0.8) def sphere_surface(ax, center, r, face='#8ed0f6', edge='#3f96c4', alpha=0.10): u = np.linspace(0, 2 * np.pi, 28) v = np.linspace(0, np.pi, 18) x = r * np.outer(np.cos(u), np.sin(v)) + center[0] y = r * np.outer(np.sin(u), np.sin(v)) + center[1] z = r * np.outer(np.ones_like(u), np.cos(v)) + center[2] ax.plot_surface(x, y, z, color=face, edgecolor=edge, linewidth=0.25, alpha=alpha, shade=False) def formation_edges(): return [(0, 1), (1, 3), (3, 2), (2, 0), (4, 5), (5, 7), (7, 6), (6, 4), (0, 4), (1, 5), (2, 6), (3, 7)] def affine_reference_series(time, variant='full'): M = len(time) N = q.shape[0] c = np.stack([0.42 * time, 0.32 * np.sin(0.16 * time), 1.45 + 0.24 * np.sin(0.11 * time)], axis=1) dx = c[:, 0, None] - obs[None, :, 0] infl = np.exp(-(dx ** 2) / (2 * 1.25 ** 2)) sgn = -np.sign(obs[:, 1]) sgn[sgn == 0] = 1 lateral = 0.58 * (infl * sgn[None, :]).sum(axis=1) lift = 0.33 * infl.sum(axis=1) sy = 1.0 + 0.18 * infl.sum(axis=1) sz = 1.0 + 0.12 * infl.sum(axis=1) if variant in ('fixed', 'no_avoidance'): lateral[:] = 0.0 lift[:] = 0.0 sy[:] = 1.0 sz[:] = 1.0 if variant == 'no_scale': sy[:] = 1.0 sz[:] = 1.0 c = c + np.stack([np.zeros(M), lateral, lift], axis=1) theta = 0.08 * np.sin(0.13 * time) sx = 1.0 + 0.08 * np.sin(0.09 * time) P = np.zeros((M, N, 3)) for m in range(M): ct = np.cos(theta[m]) st = np.sin(theta[m]) A = np.array([ [ct * sx[m], -st * sy[m], 0.0], [st * sx[m], ct * sy[m], 0.0], [0.0, 0.0, sz[m]], ]) P[m] = c[m] + q @ A.T V = np.zeros_like(P) Aacc = np.zeros_like(P) dt = time[1] - time[0] V[1:] = np.diff(P, axis=0) / dt V[0] = V[1] Aacc[1:] = np.diff(V, axis=0) / dt Aacc[0] = Aacc[1] return P, V, Aacc def get_desired(dt, T, variant): key = (dt, T, variant) if key not in _cache: time = np.arange(0.0, T + 1e-12, dt) _cache[key] = (time,) + affine_reference_series(time, variant) return _cache[key] def simulate_case(param, variant='gwo', dt=0.06, T=42.0): kp, kv, ko, kc, rhoO, rhoC = param useObs = variant not in ('affine_only', 'no_avoidance') useCol = variant not in ('no_barrier', 'no_avoidance') refVariant = 'full' if variant == 'fixed_apf': refVariant = 'fixed' if variant == 'no_scale': refVariant = 'no_scale' if variant == 'no_avoidance': refVariant = 'fixed' time, Pd, Vd, Ad = get_desired(dt, T, refVariant) N = q.shape[0] np.random.seed(10) p = Pd[0] + 0.25 * np.random.randn(N, 3) v = np.zeros((N, 3)) safetyObs = 0.18 safetyPair = 0.45 umax = 4.8 P = np.zeros((len(time), N, 3)) U = np.zeros_like(P) e = np.zeros(len(time)) clear = np.zeros(len(time)) pair = np.zeros(len(time)) energy = 0.0 maxErr = 0.0 minClear = 1e9 minPair = 1e9 for m in range(len(time)): pdes = Pd[m] vdes = Vd[m] ades = Ad[m] u = ades + kp * (pdes - p) + kv * (vdes - v) if useObs: vec = p[:, None, :] - obs[None, :, :3] d = np.sqrt(np.sum(vec**2, axis=2)) + 1e-9 boundary = obs[None, :, 3] + safetyObs influence = boundary + rhoO mask = d < influence eta = np.maximum(0.0, 1.0 / (d - boundary + 0.05) - 1.0 / (rhoO + 0.05)) mag = ko * eta / (d - boundary + 0.08) ** 2 mag *= mask u += (mag[:, :, None] * (vec / d[:, :, None])).sum(axis=1) if useCol: vec = p[:, None, :] - p[None, :, :] d = np.sqrt(np.sum(vec**2, axis=2)) + 1e-9 mask = (d < rhoC) & (~np.eye(N, dtype=bool)) coeff = kc * (1.0 / (d - safetyPair + 0.06) - 1.0 / (rhoC - safetyPair + 0.06)) / (d - safetyPair + 0.06) ** 2 coeff *= mask u += (coeff[:, :, None] * (vec / d[:, :, None])).sum(axis=1) norms = np.sqrt(np.sum(u**2, axis=1)) u *= np.minimum(1.0, umax / (norms + 1e-9))[:, None] v = 0.996 * (v + dt * u) p = p + dt * v err = np.sqrt(np.mean(np.sum((p - pdes) ** 2, axis=1))) maxErr = max(maxErr, err) cmin = (np.sqrt(np.sum((p[:, None, :] - obs[None, :, :3])**2, axis=2)) - obs[None, :, 3] - safetyObs).min() minClear = min(minClear, cmin) dp = np.sqrt(np.sum((p[:, None, :] - p[None, :, :])**2, axis=2)) dp[np.eye(N, dtype=bool)] = np.inf dmin = dp.min() minPair = min(minPair, dmin) energy += np.mean(np.sum(u ** 2, axis=1)) * dt P[m] = p U[m] = u e[m] = err clear[m] = cmin pair[m] = dmin metrics = Metrics(float(err), float(maxErr), float(minClear), float(minPair), float(energy / T)) return Result(time, P, U, e, clear, pair, Pd, metrics) def objective(param): res = simulate_case(param, variant='gwo', dt=0.06, T=42.0) m = res.metrics penalty = 0.0 if m.minClearance < 0.0: penalty += 200.0 * abs(m.minClearance) else: penalty += 0.03 / max(m.minClearance, 0.02) if m.minPair < 0.45: penalty += 200.0 * (0.45 - m.minPair) else: penalty += 0.03 / max(m.minPair - 0.44, 0.02) fit = 1.00 * m.finalRMS + 0.45 * m.maxRMS + 0.10 * m.meanEnergy + penalty return float(fit), res def gwo_optimize(n_wolves=12, n_iter=50, seed=7): rng = np.random.default_rng(seed) lb = np.array([1.2, 1.2, 0.10, 0.05, 0.80, 0.70]) ub = np.array([3.2, 3.2, 1.50, 0.80, 1.80, 1.60]) X = lb + (ub - lb) * rng.random((n_wolves, lb.size)) fitness = np.zeros(n_wolves) for i in range(n_wolves): fitness[i], _ = objective(X[i]) order = np.argsort(fitness) X = X[order] fitness = fitness[order] alpha, beta, delta = X[0].copy(), X[1].copy(), X[2].copy() conv = [float(fitness[0])] for t in range(n_iter): a = 2.0 - 2.0 * (t / max(1, n_iter - 1)) for i in range(n_wolves): r1 = rng.random(lb.size); r2 = rng.random(lb.size) A1 = 2 * a * r1 - a; C1 = 2 * r2 D_alpha = np.abs(C1 * alpha - X[i]) X1 = alpha - A1 * D_alpha r1 = rng.random(lb.size); r2 = rng.random(lb.size) A2 = 2 * a * r1 - a; C2 = 2 * r2 D_beta = np.abs(C2 * beta - X[i]) X2 = beta - A2 * D_beta r1 = rng.random(lb.size); r2 = rng.random(lb.size) A3 = 2 * a * r1 - a; C3 = 2 * r2 D_delta = np.abs(C3 * delta - X[i]) X3 = delta - A3 * D_delta X[i] = np.clip((X1 + X2 + X3) / 3.0, lb, ub) fitness[i], _ = objective(X[i]) order = np.argsort(fitness) X = X[order] fitness = fitness[order] alpha, beta, delta = X[0].copy(), X[1].copy(), X[2].copy() conv.append(float(fitness[0])) best_fit, best_res = objective(alpha) return alpha, np.array(conv), best_fit, best_res def run_single_experiment(r, n_wolves, n_iter): seed = 7 + r param, conv, best_fit, res = gwo_optimize(n_wolves=n_wolves, n_iter=n_iter, seed=seed) return r, param, conv, best_fit, res # ========================= # Plotting functions # ========================= def fig1a(res, path): # 绝对恢复你的初版代码：保持 figsize 完全不变以锁定字体大小比例 fig = plt.figure(figsize=(COL_W, 2.10)) # 核心修补：略微减小 add_axes 的上下膨胀幅度（从原本极端的 1.56 缩回 1.26），从而减小白边，但不改变横向拉伸 ax = fig.add_axes([-0.04, -0.13, 1.08, 1.26], projection='3d') for i in range(res.P.shape[1]): ax.plot(res.P[:, i, 0], res.P[:, i, 1], res.P[:, i, 2], linewidth=0.95) for k in range(obs.shape[0]): sphere_surface(ax, obs[k, :3], obs[k, 3], alpha=0.10) ax.set_xlim(-0.2, 18.3); ax.set_ylim(-2.2, 2.2); ax.set_zlim(0.8, 3.3) ax.set_xticks([0, 6, 12, 18]); ax.set_yticks([-2, 0, 2]); ax.set_zticks([1, 2, 3]) ax.set_xlabel('x (m)', labelpad=-1); ax.set_ylabel('y (m)', labelpad=0); ax.set_zlabel('z (m)', labelpad=-1) # 完全恢复原始坐标轴文本样式 ax.tick_params(axis='x', pad=0, labelsize=5); ax.tick_params(axis='y', pad=0, labelsize=5); ax.zaxis.set_tick_params(pad=0, labelsize=5) ax.set_box_aspect((18.5, 4.4, 2.8)); ax.view_init(elev=15, azim=-64) # 图名紧凑：将 y 值大幅下调（从原先的 0.86 下调到 0.78），强行让图名贴脸 ax.set_title('(a) 3-D trajectories', pad=0, fontsize=7.2, y=0.78) savefig(fig, path) def fig1b(res, path): fig, ax = plt.subplots(figsize=(COL_W, 2.10), constrained_layout=True) for i in range(res.P.shape[1]): ax.plot(res.P[:, i, 0], res.P[:, i, 1], linewidth=1.0) for k in range(obs.shape[0]): ax.add_patch(Circle((obs[k, 0], obs[k, 1]), obs[k, 3], fill=False, edgecolor='steelblue', linewidth=0.9, alpha=0.8)) ax.set_aspect('equal', adjustable='box'); ax.set_xlabel('x (m)'); ax.set_ylabel('y (m)') ax.grid(True, linewidth=0.3, alpha=0.4); stylize(ax); ax.set_title('(b) Top view', pad=2) savefig(fig, path) def fig1c(res, path): fig, ax = plt.subplots(figsize=(COL_W, 2.10), constrained_layout=True) for i in range(res.P.shape[1]): ax.plot(res.P[:, i, 0], res.P[:, i, 2], linewidth=1.0) for k in range(obs.shape[0]): ax.add_patch(Circle((obs[k, 0], obs[k, 2]), obs[k, 3], fill=False, edgecolor='lightsteelblue', linewidth=1.0, alpha=0.95)) ax.set_xlim(-0.2, 18.3); ax.set_ylim(0.7, 3.3); ax.set_aspect('equal', adjustable='box') ax.set_xlabel('x (m)'); ax.set_ylabel('z (m)') ax.grid(True, linewidth=0.3, alpha=0.4); stylize(ax); ax.set_title('(c) Side view', pad=2) savefig(fig, path) def fig1d(res, path): snap_times = [0, 8, 16, 24, 32, 40] edge_pairs = formation_edges() node_colors = list(plt.cm.tab10.colors[:8]) edge_palette = list(plt.cm.tab20.colors[:len(edge_pairs)]) # 绝对恢复你的初版代码 fig = plt.figure(figsize=(COL_W, 2.10)) # 同 fig1a，稍微收缩垂直膨胀以挤走空白 ax = fig.add_axes([-0.04, -0.13, 1.08, 1.26], projection='3d') for cx, cy, cz, r in obs: sphere_surface(ax, (cx, cy, cz), r, alpha=0.10) label_offsets = {0: (-0.20, -0.22, 0.12), 8: (0.10, 0.18, 0.28), 16: (0.03, 0.14, 0.26), 24: (0.02, 0.16, 0.30), 32: (0.03, 0.16, 0.25), 40: (0.03, 0.18, 0.32)} for t in snap_times: idx = int(np.argmin(np.abs(res.time - t))) P = res.P[idx] for col, (i, j) in zip(edge_palette, edge_pairs): ax.plot([P[i, 0], P[j, 0]], [P[i, 1], P[j, 1]], [P[i, 2], P[j, 2]], color=col, linewidth=0.90) for i, c in enumerate(node_colors): ax.scatter(P[i, 0], P[i, 1], P[i, 2], color=c, s=5, edgecolors='k', linewidths=0.22) ctr = P.mean(axis=0) dx, dy, dz = label_offsets[t] ax.text(ctr[0] + dx, ctr[1] + dy, min(3.05, ctr[2] + dz), f'{t} s', fontsize=5.0, ha='center', va='bottom', bbox=dict(facecolor='white', edgecolor='none', alpha=0.68, pad=0.1)) ax.set_xlim(-0.2, 18.0); ax.set_ylim(-2.2, 2.2); ax.set_zlim(0.0, 3.2) ax.set_xticks([0, 6, 12, 18]); ax.set_yticks([-2, 0, 2]); ax.set_zticks([0, 1.5, 3]) ax.set_xlabel('x (m)', labelpad=-1); ax.set_ylabel('y (m)', labelpad=1); ax.set_zlabel('z (m)', labelpad=-1) # 完全恢复原始样式 ax.tick_params(axis='x', pad=0, labelsize=5); ax.tick_params(axis='y', pad=0, labelsize=5); ax.zaxis.set_tick_params(pad=0, labelsize=5) ax.set_box_aspect((18, 4.5, 2.9)); ax.view_init(elev=16, azim=-61) # 图名紧凑：将 y 值大幅下调（从原先的 0.86 下调到 0.78） ax.set_title('(d) Formation evolution', pad=0, fontsize=7.2, y=0.78) savefig(fig, path) def history_plot(x, y, ylabel, title, path, hline=None): fig, ax = plt.subplots(figsize=(COL_W, 2.15), constrained_layout=True) ax.plot(x, y, linewidth=1.15) if hline is not None: ax.axhline(hline, linestyle='--', linewidth=0.9, color='tab:blue') ax.set_xlabel('Time (s)'); ax.set_ylabel(ylabel); ax.set_title(title, pad=2) ax.grid(True, linewidth=0.3, alpha=0.4); stylize(ax) savefig(fig, path) def barplot(labels, values, ylabel, title, path, h=2.35): fig, ax = plt.subplots(figsize=(COL_W, h), constrained_layout=True) x = np.arange(len(labels)) ax.bar(x, values, width=0.72) ax.set_xticks(x) ax.set_xticklabels(labels, rotation=35, ha='right') ax.set_ylabel(ylabel) ax.set_title(title, pad=2) ax.grid(True, axis='y', linewidth=0.3, alpha=0.4) stylize(ax) savefig(fig, path) def fig4a(conv, path): fig, ax = plt.subplots(figsize=(COL_W, 2.15), constrained_layout=True) ax.plot(np.arange(len(conv)), conv, '-o', markersize=2.6, linewidth=1.0) ax.set_xlabel('GWO iteration') ax.set_ylabel('Average best fitness') ax.set_title('(a) Average best-so-far fitness', pad=2) ax.grid(True, linewidth=0.3, alpha=0.4) stylize(ax) savefig(fig, path) def top_view_method(res, title, path): fig, ax = plt.subplots(figsize=(COL_W, 2.15), constrained_layout=True) for i in range(res.P.shape[1]): ax.plot(res.P[:, i, 0], res.P[:, i, 1], linewidth=1.0) for k in range(obs.shape[0]): ax.add_patch(Circle((obs[k, 0], obs[k, 1]), obs[k, 3], fill=False, edgecolor='steelblue', linewidth=0.8, alpha=0.7)) ax.set_aspect('equal', adjustable='box') ax.set_xlabel('x (m)') ax.set_ylabel('y (m)') ax.set_title(title, pad=2) ax.grid(True, linewidth=0.3, alpha=0.4) stylize(ax) savefig(fig, path) def generate_all(): n_runs = 30 n_iter = 50 print(f'Running GWO optimization ({n_runs} independent runs, {n_iter} iterations each)...') all_conv = np.zeros((n_runs, n_iter + 1)) best_overall_fit = np.inf best_param = None best_gwo_res = None max_workers = os.cpu_count() or 4 print(f'-> Accelerating computation using {max_workers} CPU cores in parallel. Please wait...') with ProcessPoolExecutor(max_workers=max_workers) as executor: futures = [executor.submit(run_single_experiment, r, 12, n_iter) for r in range(n_runs)] for future in as_completed(futures): r, param, conv, best_fit, res = future.result() all_conv[r] = conv print(f'Run {r+1:02d}/{n_runs} done: best fitness = {best_fit:.6f}') if best_fit < best_overall_fit: best_overall_fit = best_fit best_param = param best_gwo_res = res mean_conv = np.mean(all_conv, axis=0) print('\nOptimization finished!') print('Best overall parameter vector:', best_param) print('Best overall fitness across all runs:', best_overall_fit) methods = ['GWO-OAAF', 'OAAF-BA', 'Fixed-APF', 'Affine-only', 'No-scale', 'No-inter-UAV', 'No-avoidance'] params = [ best_param, np.array([2.15, 2.30, 0.70, 0.22, 1.45, 1.10]), np.array([2.00, 2.10, 0.95, 0.22, 1.45, 1.10]), np.array([2.15, 2.30, 0.00, 0.22, 1.45, 1.10]), np.array([2.15, 2.30, 0.70, 0.22, 1.45, 1.10]), np.array([2.15, 2.30, 0.70, 0.00, 1.45, 1.10]), np.array([2.00, 2.10, 0.00, 0.00, 1.45, 1.10]), ] variants = ['gwo', 'oaaf', 'fixed_apf', 'affine_only', 'no_scale', 'no_barrier', 'no_avoidance'] results = [best_gwo_res] + [simulate_case(params[i], variants[i], 0.06, 42.0) for i in range(1, len(methods))] # Fig. 1: separate panels fig1a(best_gwo_res, FIGDIR / 'Fig1a_3D_trajectories_python.png') fig1b(best_gwo_res, FIGDIR / 'Fig1b_top_view_python.png') fig1c(best_gwo_res, FIGDIR / 'Fig1c_side_view_python.png') fig1d(best_gwo_res, FIGDIR / 'Fig1d_formation_evolution_python.png') # Fig. 2: separate time histories history_plot(best_gwo_res.time, best_gwo_res.e, 'Error (m)', '(a) RMS formation error', FIGDIR / 'Fig2a_RMS_error_python.png') history_plot(best_gwo_res.time, best_gwo_res.clearance, 'Clearance (m)', '(b) Minimum obstacle clearance', FIGDIR / 'Fig2b_clearance_python.png', hline=0.0) history_plot(best_gwo_res.time, best_gwo_res.pair, 'Distance (m)', '(c) Minimum inter-UAV distance', FIGDIR / 'Fig2c_interUAV_python.png', hline=0.45) mean_acc = np.mean(np.sqrt(np.sum(best_gwo_res.U**2, axis=2)), axis=1) history_plot(best_gwo_res.time, mean_acc, 'm/s²', '(d) Mean control acceleration', FIGDIR / 'Fig2d_control_python.png') # Fig. 3: separate comparison plots barplot(methods, [r.metrics.finalRMS for r in results], 'Final RMS error (m)', '(a) Final RMS error', FIGDIR / 'Fig3a_finalRMS_python.png') barplot(methods, [r.metrics.maxRMS for r in results], 'Maximum RMS error (m)', '(b) Maximum RMS error', FIGDIR / 'Fig3b_maxRMS_python.png') barplot(methods, [r.metrics.minClearance for r in results], 'Minimum obstacle clearance (m)', '(c) Minimum obstacle clearance', FIGDIR / 'Fig3c_clearance_python.png') barplot(methods, [r.metrics.minPair for r in results], 'Minimum inter-UAV distance (m)', '(d) Minimum inter-UAV distance', FIGDIR / 'Fig3d_interUAV_python.png') # Fig. 4: convergence and optimized parameters fig4a(mean_conv, FIGDIR / 'Fig4a_GWO_convergence_python.png') param_labels_math = [r'$k_p$', r'$k_v$', r'$k_o$', r'$k_c$', r'$\rho_o$', r'$\rho_c$'] barplot(param_labels_math, best_param, 'Value', '(b) Optimized parameters', FIGDIR / 'Fig4b_optimized_parameters_python.png', h=2.2) # Fig. 5: separate top views for ablation/benchmark methods titles = ['(a) GWO-OAAF', '(b) OAAF-BA', '(c) Fixed-APF', '(d) Affine-only'] for key, idx, title in zip(['a', 'b', 'c', 'd'], [0, 1, 2, 3], titles): top_view_method(results[idx], title, FIGDIR / f'Fig5{key}_topview_python.png') # Save numerical outputs with open(ROOT / 'comparison_metrics_GWO_8_obstacles_python.csv', 'w', newline='', encoding='utf-8') as f: w = csv.writer(f) w.writerow(['Method', 'Final RMS error (m)', 'Maximum RMS error (m)', 'Minimum obstacle clearance (m)', 'Minimum inter-UAV distance (m)', 'Mean control energy']) for name, res in zip(methods, results): m = res.metrics w.writerow([name, m.finalRMS, m.maxRMS, m.minClearance, m.minPair, m.meanEnergy]) with open(ROOT / 'gwo_50iter_results_python.csv', 'w', newline='', encoding='utf-8') as f: w = csv.writer(f) w.writerow(['Parameter', 'Value']) for n, v in zip(['k_p', 'k_v', 'k_o', 'k_c', 'rho_o', 'rho_c'], best_param): w.writerow([n, float(v)]) w.writerow(['Best overall fitness', float(best_overall_fit)]) w.writerow(['Iterations', 50]) w.writerow(['Independent runs', 30]) print('Done. All figures were saved separately to:', FIGDIR) if __name__ == '__main__': generate_all()