1. 项目概述从赛题到实战的跨越去年国赛B题“无人机遂行编队飞行中的纯方位无源定位”一出来就在我们这个小圈子里炸开了锅。这题目听起来就带劲它把两个前沿且极具挑战性的领域——无人机集群协同和被动定位——给拧到了一起。简单来说题目设定是一个无人机编队在飞行其中只有少数几架比如一架或两架知道自己的精确位置我们称之为“参考机”或“领航机”其他大部分无人机“跟随机”啥定位设备都没有既没有GPS也没有惯导它们唯一能获取的信息就是通过机载传感器比如视觉摄像头、声学阵列或射频测向设备测量到的、相对于其他无人机的纯方位角Bearing-Only。题目要求就是让这些“瞎子”无人机仅凭这些角度信息推算出自己在编队坐标系乃至全局坐标系下的位置并完成队形保持或变换。这可不是纸上谈兵。在实际的无人机集群应用中比如军事上的隐蔽侦察、民用上的密集表演或协同作业让所有无人机都依赖GPS是危险且不现实的。GPS信号容易被干扰、欺骗在室内、峡谷或复杂电磁环境下也会失效。“纯方位无源定位”技术追求的正是一种不依赖外部信号、仅通过集群内部相互“观察”就能实现自定位和协同的“内功”。这道赛题可以说是直指当前多智能体协同领域的核心痛点之一。我花了大量时间研究这个题目从理论推导到仿真验证再到代码实现踩了不少坑也总结出一些行之有效的思路。今天我就把自己整理的参考代码核心框架、建模思路、求解策略以及那些“教科书上不会写”的实操细节分享出来。无论你是正在备战数模竞赛的学生还是对无人机集群定位感兴趣的工程师希望这篇“战后总结”能给你带来实实在在的启发和帮助。2. 核心思路拆解如何将方位角转化为位置坐标面对“纯方位无源定位”这个问题我们首先要建立一个清晰的数学模型把实际问题抽象成数学语言。这是解决任何数模问题的第一步也是最关键的一步。2.1 问题本质与坐标系建立问题的核心输入是“方位角”。什么是方位角在二维平面我们先从二维简化问题入手中假设无人机A观测无人机B这个方位角通常定义为从无人机A的某个参考方向比如机头朝向、北向逆时针旋转到视线AB连线所需的角度。在三维空间中则需要仰角和方位角两个参数。为了简化我们通常先建立两个坐标系全局坐标系世界坐标系一个固定的参考系用于描述所有无人机的绝对位置。通常题目会给出部分参考机在此坐标系下的坐标。机体坐标系局部坐标系以每架无人机自身为中心建立的坐标系。传感器测量的方位角信息是在这个坐标系下表达的。定位的本质就是利用已知的、在全局坐标系下表达的几何关系如参考机位置去求解未知的、在全局坐标系下的跟随机位置。而连接局部观测与全局几何的桥梁就是无人机自身的姿态朝向。这里就引出了第一个关键点姿态是否已知赛题有时会假设无人机姿态偏航角已知或可通过其他传感器如磁力计、光流估计有时则作为未知量。这直接决定了模型的复杂度。情况A姿态已知。这是相对简单的情况。我们可以直接将机体坐标系下测量的方位角通过无人机的偏航角旋转转换到全局坐标系下的方位角。此时问题简化为在全局坐标系下已知点A观测机位置可能未知、点B被观测机可能是参考机或另一架跟随机之间的连线方向方位角以及部分点的绝对坐标求所有点的坐标。这本质上是一个基于角度测量的网络定位问题类似于三角测量但更普遍。情况B姿态未知。这是更一般也更难的情况。此时方位角测量值混杂了目标相对位置和自身姿态两个因素未知数急剧增加。我们需要同时估计所有无人机的位置和姿态。这通常需要更多的观测约束比如每架无人机观测到多个其他无人机和更复杂的优化算法。2.2 主流建模方法非线性最小二乘优化无论姿态是否已知经过坐标转换后我们都能得到一组关于无人机位置和姿态的方程。这些方程通常是非线性的。例如对于姿态已知的情况从无人机i观测无人机j有arctan2((y_j - y_i), (x_j - x_i)) - ψ_i θ_ij k * π其中(x_i, y_i)和(x_j, y_j)是无人机i和j的全局坐标ψ_i是无人机i的已知偏航角θ_ij是机体坐标系下测量到的方位角arctan2是四象限反正切函数k是整数处理角度模糊性。这个方程显然是非线性的。我们很难直接求解。最有效、最通用的方法就是将其构建为一个非线性最小二乘优化问题。核心思想我们定义所有未知无人机的初始位置猜测值如果姿态未知还包括初始姿态猜测。然后根据这些猜测值我们可以计算出“预测的”方位角。优化器的目标就是调整这些猜测值使得所有“预测的方位角”与“实际测量的方位角”之间的误差平方和最小。用数学公式表达代价函数为min Σ Σ [ measured_angle(i,j) - predicted_angle(i,j; positions, attitudes) ]^2其中求和遍历所有有效的观测对(i, j)。注意这里有一个至关重要的细节——角度误差的处理。方位角是一个周期量0~360度或-180~180度直接相减可能会因为跨0度360度边界而产生巨大误差例如测量值359度预测值1度实际只差2度但直接相减得358度。因此在计算误差时必须使用角度差函数如angle_diff mod(measured - predicted π, 2π) - π确保误差落在[-π, π]的合理范围内。2.3 求解器选择与初始化策略定义了代价函数接下来就是选择优化算法并给它一个合适的起点。求解器选择对于这类中等规模的非线性最小二乘问题Levenberg-Marquardt算法是业界标准也是MATLABlsqnonlin、Pythonscipy.optimize.least_squares等函数库的默认或常用算法。它在梯度下降和高斯-牛顿法之间自适应切换既能快速收敛又对初始值有一定鲁棒性。如果你的问题规模特别大无人机数量上百可能需要考虑更高级的优化库如Ceres Solver, g2o但对于国赛规模scipy或MATLAB内置函数完全够用。初始值猜测非线性优化高度依赖初始值。一个糟糕的初始值可能导致优化陷入局部最优甚至发散。如何给出一个好的初始猜测利用参考机如果存在已知位置的参考机可以将所有未知无人机的位置初始化为参考机位置的平均值附近加上一个小的随机扰动。这假设编队是聚集的。粗略三角测量如果一架未知无人机能观测到至少两架已知位置的参考机可以利用方位角进行粗略的直线交汇得到一个初始位置估计。虽然由于误差存在交汇点可能不精确但作为优化起点通常足够好。随机初始化多起点当缺乏先验信息时可以在一个合理的空间范围内比如编队预期展开的区域随机生成多组初始值分别进行优化最后选择代价函数最小的那组结果作为最终解。这是一种简单有效的鲁棒策略。问题尺度与模糊性纯方位观测存在固有的模糊性。例如所有无人机的位置整体旋转、平移或缩放在无绝对距离信息时可能产生相同的方位角观测集。这就是所谓的尺度、旋转和平移模糊性。为了消除这些模糊性必须依赖“锚点”——也就是已知绝对位置的参考机。参考机提供了绝对的坐标基准从而固定了整个网络的尺度和朝向。在建模时参考机的位置通常作为固定参数不参与优化。3. 代码框架与核心实现解析理论清晰后我们来看代码如何落地。我将以Python为例结合scipy.optimize和numpy展示一个面向姿态已知情况的二维定位核心框架。三维或姿态未知的情况是类似的扩展。3.1 数据结构定义与测量数据模拟首先我们需要定义清晰的数据结构来表征问题。import numpy as np from scipy.optimize import least_squares import matplotlib.pyplot as plt class Drone: def __init__(self, drone_id, is_referenceFalse, xNone, yNone, yaw0.0): 无人机类 :param drone_id: 无人机ID :param is_reference: 是否为参考机位置已知 :param x, y: 全局坐标参考机已知跟随机未知则为None :param yaw: 偏航角弧度假设已知 self.id drone_id self.is_reference is_reference self.x x self.y y self.yaw yaw # 机体坐标系x轴相对于全局坐标系北向的夹角逆时针为正 # 模拟生成一个简单的编队数据5架无人机其中0号、4号为参考机 drones [] # 参考机0位于 (0, 0)朝向0度北 drones.append(Drone(0, is_referenceTrue, x0.0, y0.0, yaw0.0)) # 跟随机1-3位置未知假设其真实位置用于生成模拟观测数据 true_positions { 1: (10.0, 5.0), 2: (15.0, 0.0), 3: (10.0, -5.0), } for i in [1,2,3]: drones.append(Drone(i, is_referenceFalse, xNone, yNone, yawnp.deg2rad(0))) # 假设都朝北飞 # 参考机4位于 (20, 0)朝向0度 drones.append(Drone(4, is_referenceTrue, x20.0, y0.0, yaw0.0)) # 构建观测列表每个元素为 (观测者ID, 被观测者ID, 测量方位角弧度) # 这里模拟一个简单的观测拓扑每架无人机都能看到所有其他无人机完全图 observations [] for i in range(len(drones)): for j in range(len(drones)): if i ! j: obs_i drones[i] target_j_true_pos true_positions.get(j, (drones[j].x, drones[j].y)) # 计算真实的全局方位角 dx target_j_true_pos[0] - obs_i.x dy target_j_true_pos[1] - obs_i.y true_global_angle np.arctan2(dy, dx) # 相对于全局坐标系x轴东 # 转换到机体坐标系减去观测者的偏航角 measured_body_angle true_global_angle - obs_i.yaw # 归一化到 [-pi, pi] measured_body_angle np.mod(measured_body_angle np.pi, 2*np.pi) - np.pi # 添加高斯噪声模拟测量误差 noise np.random.normal(0, np.deg2rad(2)) # 2度标准差噪声 observations.append((i, j, measured_body_angle noise))这段代码定义了无人机类并模拟了一个包含5架无人机的编队及其观测数据。observations列表存储了所有“谁看了谁看到了什么角度”的信息并加入了高斯噪声以更贴近现实。3.2 优化问题构建残差计算函数这是整个定位算法的核心。我们需要编写一个函数它接收一组优化变量所有未知无人机的位置然后根据当前的位置猜测、已知的参考机位置和姿态计算出预测的方位角并与实际测量值比较返回残差向量。def residual_function(params, drones, observations): 计算非线性最小二乘的残差。 :param params: 一维数组包含所有未知无人机的位置 [x1, y1, x2, y2, ...] :param drones: 无人机对象列表 :param observations: 观测数据列表 :return: 残差向量所有观测的角度误差 # 1. 将优化变量params映射回无人机对象 param_idx 0 drone_positions {} # 临时存储所有无人机的最新位置包括参考机 for drone in drones: if drone.is_reference: drone_positions[drone.id] (drone.x, drone.y) else: drone_positions[drone.id] (params[param_idx], params[param_idx1]) param_idx 2 residuals [] for obs in observations: obs_id, target_id, measured_angle obs obs_drone drones[obs_id] # 观测者和目标者的当前位置 x_obs, y_obs drone_positions[obs_id] x_tar, y_tar drone_positions[target_id] # 2. 计算预测的全局方位角 dx x_tar - x_obs dy y_tar - y_obs predicted_global_angle np.arctan2(dy, dx) # 3. 转换到观测者的机体坐标系减去观测者偏航角 predicted_body_angle predicted_global_angle - obs_drone.yaw # 归一化到 [-pi, pi] predicted_body_angle np.mod(predicted_body_angle np.pi, 2*np.pi) - np.pi # 4. 计算角度残差注意处理周期 angle_error predicted_body_angle - measured_angle angle_error np.mod(angle_error np.pi, 2*np.pi) - np.pi # 关键确保误差在[-pi, pi] residuals.append(angle_error) return np.array(residuals)关键技巧angle_error的计算后再次进行mod(angle_error pi, 2pi) - pi操作是必须的。因为即使predicted_body_angle和measured_angle各自都在[-pi, pi]区间它们的直接差值仍可能超出这个范围例如预测值179度测量值-179度实际相差2度但直接相减得358度。这个操作将误差“折叠”回最短弧长表示。3.3 求解与结果可视化有了残差函数就可以调用优化器进行求解了。# 1. 准备初始猜测值将所有未知无人机的位置初始化为所有参考机位置的中心 ref_positions [(drone.x, drone.y) for drone in drones if drone.is_reference] ref_center np.mean(ref_positions, axis0) initial_guess [] for drone in drones: if not drone.is_reference: # 在参考机中心附近加一点随机扰动作为初始值 initial_guess.extend([ref_center[0] np.random.randn()*2, ref_center[1] np.random.randn()*2]) # 2. 定义优化变量的边界可选但建议设置 # 假设我们大致知道编队活动范围在x[-5,25], y[-10,10] bounds_low [] bounds_high [] for drone in drones: if not drone.is_reference: bounds_low.extend([-5, -10]) bounds_high.extend([25, 10]) # 3. 调用最小二乘优化器 result least_squares(residual_function, x0initial_guess, args(drones, observations), bounds(bounds_low, bounds_high) if bounds_low else (np.array([]), np.array([])), methodtrf, # 信赖域反射法适合有边界的问题 ftol1e-8, # 函数容忍度 xtol1e-8, # 参数变化容忍度 gtol1e-8, # 梯度容忍度 verbose2) # 输出详细过程 # 4. 提取优化结果 optimized_params result.x print(f优化成功: {result.success}) print(f代价函数终值: {result.cost}) print(f优化消息: {result.message}) # 5. 将结果写回无人机对象并可视化 param_idx 0 estimated_positions {} for drone in drones: if drone.is_reference: estimated_positions[drone.id] (drone.x, drone.y) print(f参考机 {drone.id}: ({drone.x:.2f}, {drone.y:.2f})) else: x_est, y_est optimized_params[param_idx], optimized_params[param_idx1] estimated_positions[drone.id] (x_est, y_est) x_true, y_true true_positions[drone.id] error np.sqrt((x_est-x_true)**2 (y_est-y_true)**2) print(f跟随机 {drone.id}: 估计位置 ({x_est:.2f}, {y_est:.2f}), 真实位置 ({x_true:.2f}, {y_true:.2f}), 误差 {error:.2f} m) param_idx 2 # 可视化 plt.figure(figsize(10,6)) for drone_id, pos in estimated_positions.items(): if drones[drone_id].is_reference: plt.plot(pos[0], pos[1], rs, markersize12, label参考机 if drone_id0 else ) # 红色方块 else: plt.plot(pos[0], pos[1], bo, markersize10, label估计位置 if drone_id1 else ) # 蓝色圆圈 # 画出真实位置对比 true_pos true_positions[drone_id] plt.plot(true_pos[0], true_pos[1], gx, markersize12, label真实位置 if drone_id1 else ) # 绿色叉叉 plt.plot([pos[0], true_pos[0]], [pos[1], true_pos[1]], k--, linewidth0.5) # 误差连线 # 画出观测连线稀疏化避免图太乱 for obs in observations[::len(observations)//20]: # 抽样显示部分观测 obs_id, target_id, _ obs pos_obs estimated_positions[obs_id] pos_tar estimated_positions[target_id] plt.plot([pos_obs[0], pos_tar[0]], [pos_obs[1], pos_tar[1]], gray, alpha0.1) plt.xlabel(X坐标 (m)) plt.ylabel(Y坐标 (m)) plt.title(无人机纯方位无源定位结果) plt.legend() plt.grid(True) plt.axis(equal) plt.show()这段代码完成了从初始化、优化到结果输出和可视化的完整流程。least_squares函数的verbose2参数可以输出迭代过程方便调试。可视化部分将估计位置、真实位置和观测关系清晰地展示出来误差连线直观地显示了定位精度。4. 从仿真到实战关键问题与进阶策略上面的框架解决了一个理想化的仿真问题。但在实际竞赛或工程应用中你会遇到更多棘手的情况。下面分享几个我踩过坑的要点和进阶策略。4.1 观测拓扑与可定位性分析不是随便怎么“看”都能定位成功的。观测拓扑谁能看到谁直接决定了问题是否“可解”可定位。例如如果一架跟随机只能看到一架参考机那么它只能知道自己位于从参考机出发的某条射线上无法确定具体位置。如果一架跟随机能同时看到两架非共线的参考机理论上可以通过两条射线的交点确定位置三角定位。在只有方位信息的情况下要唯一确定所有无人机的相对位置在消除了整体旋转和平移模糊后需要满足一定的图论条件通常要求对应的“方位测量图”是刚性或全局刚性的。实操建议构建强连通观测网在编队设计或算法假设中尽量保证每架无人机尤其是跟随机能观测到至少两架以上非共线的其他无人机且其中最好包含参考机。仿真验证可定位性在代码中可以尝试移除部分观测看看优化结果是否发散或误差剧增。也可以计算观测矩阵的雅可比行列式或条件数数值上判断问题的病态程度。利用历史信息在动态编队中可以利用前一时刻的位置估计作为当前时刻优化的初始值这能极大提高收敛速度和稳定性这就是滤波思想如扩展卡尔曼滤波EKF的雏形。4.2 测量噪声与野值处理仿真中我们添加了高斯噪声。现实中噪声可能非高斯并且存在野值——严重偏离真实值的错误测量。野值会严重破坏最小二乘优化的结果。应对策略鲁棒损失函数将最小二乘的L2范数误差平方和换成更鲁棒的损失函数如Huber损失或Cauchy损失。scipy.optimize.least_squares直接支持通过loss参数指定。result least_squares(residual_function, x0initial_guess, args(drones, observations), losssoft_l1, f_scale0.1) # 使用soft_l1损失抑制野值soft_l1损失对大的残差不那么敏感能一定程度上抑制野值的影响。随机采样一致性更彻底的方案是采用RANSAC算法。其基本思想是随机选取最小观测集例如一架无人机要定位随机选它的两个观测计算一个位置假设然后用这个假设去检验所有的观测统计“内点”符合该假设的观测数量。重复多次选择内点最多的那个假设并只用内点进行最终的精优化。这能有效剔除野值。先验信息融合如果无人机有粗略的运动模型如匀速模型可以将模型预测的位置与方位观测进行融合使用卡尔曼滤波族算法它们对野值也有一定的鲁棒性。4.3 三维空间与姿态未知的扩展将上述二维模型扩展到三维主要变化是观测值从1个方位角变为2个角度方位角azimuth和俯仰角elevation。如果姿态未知则每架无人机的状态变量从(x, y)变为(x, y, z, roll, pitch, yaw)或使用四元数表示姿态更利于优化。姿态参数会引入更强的非线性。残差计算中需要将目标在全局坐标系下的向量通过观测机的三维旋转矩阵由roll, pitch, yaw构成转换到机体坐标系再计算与测量方位/俯仰角的偏差。核心公式扩展 假设观测机姿态由旋转矩阵R_i从全局系到机体系表示目标在全局系下相对于观测机的向量为p_ij_global [x_j-x_i, y_j-y_i, z_j-z_i]^T。 则在机体系下的向量为p_ij_body R_i * p_ij_global。 预测的方位角az_pred arctan2(p_ij_body[1], p_ij_body[0])预测的俯仰角el_pred arcsin(p_ij_body[2] / ||p_ij_body||)然后与测量值az_meas, el_meas作差求残差。重要提示在三维姿态未知情况下问题变得非常复杂容易陷入局部最优。必须提供质量非常高的初始猜测尤其是姿态。可以考虑使用其他传感器如加速度计、磁力计进行初始姿态估计或者利用多帧观测通过运动恢复结构技术进行初始化。4.4 动态编队与滤波跟踪国赛题目往往是静态定位。但在实际应用中编队是运动的我们需要进行实时定位与跟踪。这时单纯的批处理优化如上文所述计算量太大延迟高。更常用的方法是结合运动模型的滤波算法。扩展卡尔曼滤波这是最经典的方案。将每架无人机的位置和姿态作为状态量建立通常为简单的匀速或匀加速运动模型作为状态预测方程。将方位角观测模型作为观测方程。由于观测方程是非线性的需要进行雅可比矩阵线性化EKF。EKF能在线运行计算效率高是工程实践中的主流选择。优化与滤波结合也可以采用“优化定位滤波平滑”的思路。例如用一个时间窗口内的多帧观测数据进行批量优化得到一个较高精度的位置估计然后将这个估计值作为观测量输入到一个卡尔曼滤波器中对无人机的运动状态位置、速度进行平滑和预测从而得到更稳定、延迟更低的输出。5. 常见问题排查与调试心得在实际编写和调试代码的过程中你肯定会遇到各种问题。下面是我总结的一些常见“坑”和解决思路。5.1 优化不收敛或结果离谱这是最常见的问题。症状result.success为False代价函数终值很大或者估计出的位置明显不合理如飞到无穷远。排查步骤检查残差函数这是重中之重。在优化开始前用初始猜测值x0手动调用一次residual_function打印出残差向量。看看残差是否巨大比如远大于π。如果是说明你的残差计算逻辑可能有误特别是角度误差处理那一步。检查初始值将初始猜测值x0设置得离真实解尽可能近。在仿真中你可以直接用真实位置加一点噪声作为初始值看看优化是否成功。如果这样能成功但用粗糙初始值失败说明问题对初始值敏感需要改进初始化策略如使用RANSAC或多起点优化。检查观测数据确认你的观测列表observations是否正确生成。是否存在自己观测自己的无效数据观测角度单位是弧度吗噪声水平是否设置得过于夸张检查参考机设置确保参考机is_referenceTrue的无人机其x, y属性是固定数值并且在残差函数中未被更改。参考机是定位的“锚”必须固定不动。缩放问题如果坐标数值非常大例如经纬度可能会导致优化器数值计算困难。考虑对坐标进行归一化处理减去均值除以尺度优化后再变换回来。尝试不同的求解方法和参数scipy.optimize.least_squares的method可以尝试lmLevenberg-Marquardt无边界时或dogbox。适当调大max_nfev最大函数评估次数或放宽ftol,xtol。5.2 定位存在系统性旋转或平移误差症状所有跟随机的估计位置整体上相对于真实位置发生了旋转或平移但它们之间的相对形状基本正确。原因这通常表明参考机提供的绝对基准信息不足或存在矛盾。例如如果只有一架参考机理论上只能消除平移模糊无法消除绕该参考机的旋转模糊。如果参考机的位置信息本身有误差会导致整个编队的估计发生系统性偏差。解决方案确保至少有两架非共线的参考机。这两架参考机提供了确定整个编队尺度和旋转所必需的基线。在仿真中检查你的参考机坐标是否正确输入。5.3 部分无人机定位误差远大于其他无人机症状编队中大部分无人机定位准确但某一两架误差特别大。原因通常是观测几何差导致的。例如该无人机只能观测到另外两架距离很近、几乎共线的无人机或者观测到的无人机自身位置估计就不准误差传递。分析工具计算并可视化每架无人机的定位精度几何稀释因子虽然严格来说是针对测距的但思想类似。可以通过计算优化问题信息矩阵近似为雅可比矩阵的转置乘以雅可比矩阵的逆其对角线元素对应各状态量位置的方差估计。数值越大说明该位置的可观性越差预期误差越大。对策优化编队观测拓扑。让误差大的无人机能观测到更多、几何分布更好的参考点其他无人机。5.4 代码运行速度慢原因残差函数被调用次数过多无人机数量或观测数量很大使用了低效的循环。优化技巧向量化计算这是Python性能优化的关键。避免在残差函数内部使用for循环遍历观测。可以将所有观测数据观测者索引、目标索引、测量角存储在numpy数组中利用numpy的广播和索引功能一次性计算所有预测角度和残差。稀疏雅可比矩阵如果你能提供残差函数关于优化变量的雅可比矩阵导数优化器会收敛得更快。对于这类问题雅可比矩阵是稀疏的每个残差只与少数几个无人机状态有关。使用least_squares的jaccs通过复步法有限差分近似或jac3-point可以自动处理但如果你能解析推导并实现稀疏雅可比计算速度会提升一个数量级。减少无人机/状态数量如果问题规模实在太大考虑是否能用子图定位、分层定位等简化策略。最后分享一个我自己的调试习惯始终从最简单、最确定性的案例开始。比如先做2架参考机1架跟随机的三角定位不加噪声验证整个数据流和优化流程是否正确。然后逐步增加无人机数量、加入噪声、引入姿态未知等复杂度。每增加一层复杂度都确保前一层的功能是正常的。这种渐进式的调试方法能帮你快速定位问题所在层避免在一开始就被复杂系统的各种交互bug搞得晕头转向。这个项目最吸引人的地方在于它完美地结合了优美的几何理论、实用的优化算法和充满挑战的工程实现当你看到算法成功地从一堆角度数据中“变”出无人机的精确位置时那种成就感是无与伦比的。