1. 项目背景与核心挑战当无人机编队“看不见”GPS在无人机集群协同作业比如农业植保、物流配送或者区域侦察时我们通常假设每架无人机都清楚自己的精确位置。这依赖于GPS、北斗这类全球卫星导航系统。但设想一个场景在强电磁干扰、复杂城市峡谷或者有意进行电子对抗的环境下GPS信号变得不可靠甚至完全失效。此时整个编队就变成了“睁眼瞎”传统的定位和队形保持方法瞬间失灵。这就是“纯方位无源定位”技术要解决的痛点。它不依赖外部信号发射源如GPS卫星仅通过编队内部无人机之间相互“观察”对方的方向方位角来推算彼此的相对位置从而维持编队队形。这就像一群在浓雾中飞行的鸟虽然看不见远处的参照物但通过感知邻近同伴的方位和距离这里仅感知方位也能保持队形不散。2022年高教社杯全国大学生数学建模竞赛的B题正是将这个极具现实意义和理论深度的挑战抛给了参赛者。题目要求无人机编队在纯方位信息条件下完成从初始随机位置到特定编队如锥形的调整。其核心难点在于信息极度匮乏每架无人机只能获得对若干邻居无人机的方位角测量值没有距离信息也没有全局坐标系。耦合性强任何一架无人机的位置估计误差都会通过观测网络传递给它的邻居进而影响整个编队的定位精度。非线性与模糊性仅凭方位角确定位置是一个非线性问题并且存在镜像模糊例如一个点关于观测连线的对称点也可能满足相同的方位角约束。因此解题的关键在于构建一个能够利用稀疏、带有噪声的方位观测信息协同估计出所有无人机相对位置的数学模型和算法。下面我将结合常见的解决思路和代码实现细节深入拆解这个问题。2. 核心思路解析从方位角到相对位置估计纯方位无源定位的核心是协同定位。我们无法孤立地确定单架无人机的位置必须将整个编队视为一个整体利用所有无人机之间的相互观测来联合求解。2.1 问题建模图论与约束优化首先我们将无人机编队抽象为一个无向图G(V, E)。顶点 V代表每一架无人机。边 E代表一对无人机之间存在方位观测。即如果无人机i能“看到”无人机j那么图中就存在边(i, j)。题目通常会给出观测矩阵或邻接关系。假设我们有N架无人机。对于存在观测的边(i, j)无人机i测量得到无人机j相对于自身机体坐标系或某个参考方向的方位角θ_ij例如以正东方向为0度逆时针旋转为正。这个测量值通常包含高斯白噪声θ_ij_measured θ_ij_true noise。我们的目标是估计所有无人机在一个公共二维坐标系下的位置p_i [x_i, y_i]^T(i1,...,N)。由于是相对定位我们需要固定至少两架无人机的位置或固定一个位置和一个方向来消除整个系统的平移、旋转和缩放自由度即锚点。通常我们会指定0号无人机位于原点(0,0)1号无人机位于(d, 0)其中d是0号到1号无人机的真实距离如果未知可设为一个单位距离。对于每一对观测(i, j)其真实的方位角θ_ij_true与估计位置之间的关系为θ_ij_true atan2(y_j - y_i, x_j - x_i)其中atan2是四象限反正切函数。2.2 主流解法非线性最小二乘最直观的建模方式是将定位问题转化为一个非线性最小二乘优化问题。我们寻找一组位置估计{p_i}使得所有方位角测量值与由估计位置计算出的理论方位角之间的差异平方和最小。构建代价函数F(P) Σ_{(i,j) in E} [ angle(p_j - p_i) - θ_ij_measured ]^2其中P是所有无人机位置向量堆叠而成的大向量angle(v)表示向量v的方位角。我们需要求解P* argmin_P F(P)这是一个典型的无约束非线性优化问题因为我们已经通过设定锚点固定了自由度。可以使用高斯-牛顿法、Levenberg-Marquardt算法等迭代优化算法来求解。在Python中scipy.optimize库的least_squares或minimize函数非常适合解决此类问题。为什么选择非线性最小二乘因为它直接对物理测量模型进行建模原理清晰能较好地处理噪声。只要初始值给得不太差并且观测网络连通性足够好图是刚性的通常能收敛到较好的解。缺点是计算量随无人机数量增加而增大且可能陷入局部最优。2.3 另一种思路半正定规划松弛当噪声较大或观测非常稀疏时非线性最小二乘可能不稳定。另一种更鲁棒的方法是半正定规划松弛。其核心思想是我们不直接估计位置p_i而是估计位置向量之间的内积p_i^T p_j。定义矩阵P [p_1, p_2, ..., p_N]那么格拉姆矩阵G P^T P是一个半正定矩阵其元素G_ij p_i^T p_j。方位角约束θ_ij可以转化为关于p_i和p_j的线性约束在忽略噪声的理想情况下方向向量垂直。通过一系列数学变换可以将定位问题转化为寻找一个满足一系列线性约束的半正定矩阵G的问题然后通过特征值分解从G中恢复出位置P。SDP方法的优势与劣势优势将非凸问题转化为凸问题保证能找到全局最优解对于松弛后的问题对初始值不敏感鲁棒性更强。劣势建模和实现相对复杂计算量也可能很大且恢复出的位置可能存在整体翻转镜像解需要后处理来纠正。在实际竞赛中考虑到时间和实现复杂度非线性最小二乘法是更主流和实用的选择。下面的参考代码也将基于这种方法。3. 参考代码实现与逐行解析以下是一个基于Python和scipy.optimize的简化版参考代码实现。我们假设一个包含5架无人机的编队目标是形成一个正五边形。观测关系是每个无人机都能看到其最近的两个邻居环形观测。import numpy as np import matplotlib.pyplot as plt from scipy.optimize import least_squares import networkx as nx # 3.1 参数设置与数据生成 np.random.seed(42) # 确保可重复性 num_uavs 5 true_positions np.zeros((num_uavs, 2)) # 生成真实位置一个半径为10的正五边形 for i in range(num_uavs): angle 2 * np.pi * i / num_uavs true_positions[i] [10 * np.cos(angle), 10 * np.sin(angle)] # 定义观测图每个节点连接其前后两个邻居环形 adjacency_matrix np.zeros((num_uavs, num_uavs), dtypebool) for i in range(num_uavs): adjacency_matrix[i, (i-1) % num_uavs] True adjacency_matrix[i, (i1) % num_uavs] True # 无向图对称化 adjacency_matrix adjacency_matrix | adjacency_matrix.T # 根据真实位置计算带噪声的方位角测量值 def calculate_bearing(from_pos, to_pos): 计算从from_pos到to_pos的真实方位角弧度范围[-pi, pi] delta to_pos - from_pos return np.arctan2(delta[1], delta[0]) measurements {} std_dev_noise np.deg2rad(5) # 方位角测量噪声标准差设为5度 for i in range(num_uavs): for j in range(num_uavs): if adjacency_matrix[i, j] and i ! j: true_bearing calculate_bearing(true_positions[i], true_positions[j]) noisy_bearing true_bearing np.random.normal(0, std_dev_noise) # 将角度归一化到[-pi, pi] noisy_bearing np.arctan2(np.sin(noisy_bearing), np.cos(noisy_bearing)) measurements[(i, j)] noisy_bearing print(f“生成了 {len(measurements)} 条方位角观测数据含噪声。”) # 3.2 优化问题定义残差计算函数 def residual_function(positions_flat, num_uavs, measurements, anchor_indices, anchor_positions): 计算非线性最小二乘的残差。 positions_flat: 一维数组形状为 (2*num_uavs,)按 [x0,y0,x1,y1,...] 排列 返回所有观测残差组成的一维数组 positions positions_flat.reshape((num_uavs, 2)) residuals [] # 强制锚点位置固定将其残差设为零或通过不包含它们来实现这里采用设置残差为零的方式 # 更常见的做法是在优化变量中排除锚点这里为了代码清晰采用惩罚项思想。 # 实际上我们在优化变量中包含了锚点但通过巨大的权重使其位置不变。 for idx, pos in zip(anchor_indices, anchor_positions): # 添加锚点位置约束的残差权重很大 residuals.append(100.0 * (positions[idx] - pos).ravel()) # 计算方位角观测残差 for (i, j), theta_measured in measurements.items(): delta positions[j] - positions[i] theta_estimated np.arctan2(delta[1], delta[0]) # 计算角度差注意处理圆周跳变 angle_diff theta_estimated - theta_measured angle_diff np.arctan2(np.sin(angle_diff), np.cos(angle_diff)) # 归一化到[-pi, pi] residuals.append(angle_diff) return np.concatenate(residuals) # 3.3 设定锚点并初始化 # 固定0号无人机在原点1号无人机在X轴正方向距离根据真实位置设定或假设为单位距离 anchor_indices [0, 1] # 使用真实位置作为锚点坐标模拟已知两个精确位置 anchor_positions [true_positions[0], true_positions[1]] # 初始化猜测位置在真实位置附近添加随机扰动 initial_guess true_positions np.random.normal(0, 3.0, (num_uavs, 2)) # 标准差为3的噪声 initial_guess[anchor_indices] anchor_positions # 确保锚点初始值正确 initial_guess_flat initial_guess.ravel() # 3.4 调用优化器求解 print(“开始优化求解...”) result least_squares( funresidual_function, x0initial_guess_flat, args(num_uavs, measurements, anchor_indices, anchor_positions), methodlm, # 使用Levenberg-Marquardt算法 verbose1 # 显示优化过程信息 ) print(“优化完成”) # 3.5 结果提取与评估 estimated_positions_flat result.x estimated_positions estimated_positions_flat.reshape((num_uavs, 2)) # 计算估计误差由于是相对定位可能存在整体旋转需用Procrustes分析或对比相对结构 # 这里简单计算与真实位置在去除刚体变换平移、旋转、缩放后的误差 from scipy.spatial import procrustes true_mat, est_mat, disparity procrustes(true_positions, estimated_positions) print(f“Procrustes差异均方根误差: {disparity:.6f}”) # 3.6 可视化结果 fig, axes plt.subplots(1, 2, figsize(12, 5)) # 子图1真实位置与观测关系 ax1 axes[0] ax1.scatter(true_positions[:, 0], true_positions[:, 1], cblue, s100, label真实位置, zorder5) for i in range(num_uavs): ax1.text(true_positions[i, 0]0.3, true_positions[i, 1]0.3, f‘U{i}’, fontsize12) # 绘制观测边 for i in range(num_uavs): for j in range(i1, num_uavs): if adjacency_matrix[i, j]: ax1.plot([true_positions[i, 0], true_positions[j, 0]], [true_positions[i, 1], true_positions[j, 1]], gray, linestyle--, alpha0.7) ax1.set_aspect(equal, adjustablebox) ax1.grid(True, alpha0.3) ax1.set_title(‘真实编队队形与观测拓扑’) ax1.set_xlabel(‘X’) ax1.set_ylabel(‘Y’) ax1.legend() # 子图2估计位置与真实位置对比 ax2 axes[1] ax2.scatter(true_positions[:, 0], true_positions[:, 1], cblue, s100, label真实位置’, zorder5) ax2.scatter(estimated_positions[:, 0], estimated_positions[:, 1], cred, s100, markers, label估计位置’, zorder5) for i in range(num_uavs): ax2.text(true_positions[i, 0]0.3, true_positions[i, 1]0.3, f‘T{i}’, fontsize12, colorblue) ax2.text(estimated_positions[i, 0]0.3, estimated_positions[i, 1]0.3, f‘E{i}’, fontsize12, colorred) # 连接每个无人机对应的真实与估计点显示误差 for i in range(num_uavs): ax2.plot([true_positions[i, 0], estimated_positions[i, 0]], [true_positions[i, 1], estimated_positions[i, 1]], k:, alpha0.5) ax2.set_aspect(equal, adjustablebox) ax2.grid(True, alpha0.3) ax2.set_title(‘纯方位定位结果对比’) ax2.set_xlabel(‘X’) ax2.set_ylabel(‘Y’) ax2.legend() plt.tight_layout() plt.show() # 打印最终估计位置 print(“\n估计位置坐标”) for i, pos in enumerate(estimated_positions): print(f“无人机 {i}: ({pos[0]:.4f}, {pos[1]:.4f})”)代码关键点解析残差函数的设计(residual_function)这是优化的核心。它同时包含了两种残差方位角观测残差对于每条观测边(i, j)计算估计位置得出的方位角与测量方位角之差。这里使用了np.arctan2(sin(diff), cos(diff))来确保角度差被包裹在[-π, π]区间内避免了360°跳变带来的不连续问题这对于优化器的稳定收敛至关重要。锚点约束残差我们将锚点位置也作为残差项加入并赋予一个很大的权重这里是100。这相当于一个软约束告诉优化器“这些位置必须非常接近给定值”。另一种更严谨的做法是在优化变量中直接排除锚点坐标只优化其他无人机的位置这样自由度更少。这里采用软约束是为了代码逻辑的统一性。优化方法选择scipy.optimize.least_squares的methodlm指定了Levenberg-Marquardt算法。它是一种非常高效且鲁棒的非线性最小二乘求解器能自动在梯度下降和高斯-牛顿法之间切换特别适合中等规模参数的问题。结果评估直接比较估计位置和真实位置的坐标没有意义因为相对定位存在整体旋转和平移的自由度。我们使用Procrustes分析来评估形状差异。它通过找到最优的平移、旋转和缩放变换将一个点集与另一个点集对齐然后计算残差。这个disparity值越小说明估计的相对队形与真实队形越接近。初始化的重要性代码中使用在真实位置加噪声的方式生成初始猜测。在实际竞赛或应用中如果没有任何先验可以用随机初始化或者使用更复杂的图嵌入算法如多维缩放MDS的变体来生成一个粗略的初始布局。一个好的初始值能显著提高收敛速度和成功率。4. 从定位到队形调整控制律设计解决了定位问题我们只完成了第一步。题目最终要求的是队形调整即控制无人机从初始位置飞到目标队形位置。假设我们通过上述方法或每一时刻都运行一次得到了所有无人机在当前时刻的相对位置估计p_i_estimated同时也知道它们在目标队形中的期望相对位置p_i_desired。一个简单有效的控制策略是基于一致性的比例控制器。每架无人机根据其邻居的位置误差来调整自己的速度v_i k_p * Σ_{j in N(i)} [ (p_j_desired - p_i_desired) - (p_j_estimated - p_i_estimated) ]其中v_i是无人机i的控制速度向量。k_p是比例增益系数。N(i)是无人机i的邻居集合即能观测到它的无人机集合。(p_j_desired - p_i_desired)是期望的编队中j相对于i的向量。(p_j_estimated - p_i_estimated)是当前估计的编队中j相对于i的向量。这个控制律的直观理解是每架无人机都试图让自己与邻居之间的相对位置向量逼近期望的相对位置向量。它只依赖于局部信息邻居的相对位置实现了分布式控制。所有无人机共同作用最终会使整个编队收敛到期望的几何形状。在实际仿真中的实现步骤初始化设定目标队形坐标p_desired初始化无人机真实位置p_true通常随机或给定。感知与定位循环 a. 根据当前真实位置p_true模拟方位角测量加入噪声。 b. 调用上述定位算法利用带噪声的方位角测量值和锚点信息估计出当前所有无人机的相对位置p_estimated。控制与运动循环 a. 根据p_estimated和p_desired按照上述控制律计算每架无人机的控制速度v_i。 b. 更新无人机的真实位置运动模型通常简化为积分p_true_new p_true v_i * dt其中dt是控制周期。重复步骤2-3直到所有无人机的位置误差||p_estimated - p_desired||小于某个阈值或达到最大迭代次数。注意这里的定位和控制是解耦的。更高级的做法是考虑协同定位与控制将定位不确定性纳入控制律设计或者使用滤波器如卡尔曼滤波器对位置状态进行持续估计和更新。5. 实战中的关键细节与避坑指南在实际编码和调试过程中以下几个细节往往决定了成败5.1 观测拓扑与“可定位性”不是随便一个观测图都能实现精确定位。图必须满足刚性条件。简单来说就是观测边的数量和分布必须能够唯一确定所有节点的相对位置排除整体平移、旋转。对于二维空间一个连通图至少需要2N - 3条边才有可能成为刚性图N为节点数。在竞赛中通常题目给出的观测图是连通的并且边数足够。但自己设计仿真时如果观测太稀疏例如每个无人机只观测一个邻居系统可能不可定位优化结果会发散或误差极大。建议在代码开始时检查图的连通性并使用networkx库的图刚性相关算法进行简单分析。5.2 角度归一化与残差平滑这是算法实现中最容易出错的地方。方位角是周期性的359°和1°之间只差2°但直接相减会得到-358°这会给优化器带来巨大的、不连续的梯度。# 错误的做法 residual theta_estimated - theta_measured # 正确的做法如代码所示 angle_diff theta_estimated - theta_measured residual np.arctan2(np.sin(angle_diff), np.cos(angle_diff))这行代码利用sin和cos的周期性将角度差映射回[-π, π]区间保证了残差函数的平滑性。5.3 锚点的选择与处理锚点的选择至关重要。至少需要两个非共线的锚点来固定整个坐标系。选择距离较远、观测质量较好的无人机作为锚点通常更稳定。在代码实现中如前所述有两种处理方式变量消元法在优化变量中只包含非锚点无人机的位置。这需要重写残差函数计算更复杂但优化变量更少问题更紧凑。强约束惩罚法如参考代码所示将所有无人机位置都作为优化变量但对锚点位置添加一个权重很大的残差项weight * (p_estimated - p_anchor)。权重需要足够大如1e6以确保锚点位置几乎不变。这种方法代码简单但可能因数值问题导致条件数变差。我个人的经验是对于中小规模问题惩罚法更易于实现和调试对于大规模问题消元法在数值稳定性上更有优势。5.4 优化器调参与初始值scipy.optimize.least_squares有很多参数可以调整ftol,xtol,gtol迭代停止的容差。通常使用默认值即可如果收敛太慢或精度不够可以适当调小。max_nfev最大函数评估次数。如果问题复杂可能需要增加。verbose2可以输出更详细的迭代信息便于调试。如果优化失败不收敛或结果明显错误首先检查初始值。可以尝试多次随机初始化选择代价函数最小的结果。或者先用一个简单的线性方法如基于方位角线的交点或图布局算法如networkx.spring_layout生成一个粗略的初始布局。5.5 噪声模型与鲁棒性测试真实的方位角测量噪声不一定是高斯白噪声。可能存在野值Outliers或系统性偏差。在建模时可以考虑使用更鲁棒的损失函数例如Huber损失或Cauchy损失来代替简单的平方损失。在least_squares中可以通过设置loss参数来实现例如losssoft_l1。在最终测试时务必在不同噪声水平如1°,5°,10°标准差下运行算法评估其定位误差与控制效果以检验系统的鲁棒性。6. 性能优化与扩展思路对于大规模无人机集群例如100架以上上述集中式优化将所有变量放在一起优化的计算开销会变得很大。可以考虑以下扩展方向分布式定位算法每架无人机只估计自己和邻居的位置通过多次迭代与邻居交换信息最终达成全局一致。分布式梯度下降或交替方向乘子法是常见选择。这更符合实际无人机集群分布式、自主协同的特点。引入运动模型与滤波将定位问题建模为状态估计问题。使用扩展卡尔曼滤波器或粒子滤波器融合历史方位观测信息和无人机自身的运动模型如惯性测量单元数据进行连续跟踪。这能有效平滑噪声提高定位精度和鲁棒性。融合其他弱信息在纯方位之外如果还能获得一些其他信息哪怕很弱也能极大提升性能。例如无人机间的相对距离估计通过信号强度或通信延迟精度很低、无人机的粗略速度方向等。这些信息可以作为额外的约束加入优化框架。考虑通信延迟与丢包在实际系统中观测信息的传递存在延迟和可能丢失。算法需要具备异步和容错能力。这通常需要更复杂的基于时间戳的融合算法或一致性协议。这个项目从理论到实践完整地串联了图论、优化理论、估计理论和控制理论。通过动手实现这个代码框架并尝试解决其中的各种坑你不仅能应对类似的数模竞赛题更能深入理解多智能体协同感知与控制的精髓。在实际操作中不妨从5架无人机的小规模编队开始逐步增加数量、引入更复杂的观测拓扑和噪声观察算法的表现这会让你对系统性能的边界有更深刻的认识。