1. 项目背景与核心挑战：当无人机“看不见”时如何定位
在无人机集群协同作业，特别是遂行编队飞行这个场景里，有一个经典且极具挑战性的问题：如果一架或多架无人机失去了GPS、北斗等有源定位信号，甚至机载的视觉、激光雷达等主动感知设备也因故障或干扰失效，它们该如何仅凭彼此间“听”到的方位信息，来确定自己的精确位置，从而维持编队队形、完成既定任务？这就是“纯方位无源定位”问题的现实背景。它听起来有点像我们在一个完全黑暗、没有地图的房间里，仅凭听到的几个同伴的声音方向，来推断自己所在的位置。
2022年的全国大学生数学建模竞赛B题，正是将这个极具工程价值和理论深度的前沿问题，抽象成了一个可供数学建模的赛题。题目要求参赛者针对无人机编队中部分无人机“失明”的情况，建立数学模型，仅利用它们接收到的来自编队中其他正常无人机发出的方位角信息（即方向，没有距离），来求解这些“失明”无人机的坐标。这不仅仅是解一道数学题，其背后对应的是无人机在复杂电磁环境、峡谷或室内等GNSS拒止条件下的生存与协同能力。我之所以对这个题目印象深刻，是因为它完美地将几何、优化和状态估计理论融合在了一起，并且有非常明确的工程落地场景。那些热搜词如“无人机编队”、“无源定位”、“定位模型”、“规划模型”，都精准地指向了这个问题的不同侧面。
简单来说，核心挑战在于：信息的不完备性。我们只知道角度（方位线），不知道距离。这就好比从一点看向另一点，你只知道那条视线的方向，但对方究竟在100米外还是1000米外，是未知的。单条方位线无法定位一个点，它定义了一条从观测点出发的射线。因此，必须依赖多个不同位置的观测者，或者同一观测者在不同时间点的移动，来获得多条方位线，它们的交点（理论上）就是目标的位置。在无人机编队中，我们拥有的正是多个“观测者”（正常无人机）对多个“目标”（失联无人机）的方位观测。问题的核心就转化为：如何从这些带有噪声的方位线观测数据中，鲁棒、高效地反演出最可能的目标位置坐标。
2. 问题拆解：从现实场景到数学模型框架
面对这样一个工程问题，直接上手写代码是行不通的。我们必须先进行严谨的问题拆解和数学抽象。整个问题可以分解为几个层次：
2.1 场景与假设定义
首先，我们需要明确物理场景和建模假设，这是所有模型的基石。
- 坐标系：通常建立二维平面直角坐标系（如果考虑高度，则是三维，但国赛题常简化在二维）。将整个编队飞行的空域投影到平面上。
- 无人机角色：
- 基准无人机（或导引机）：假设其位置精确已知（通过自身完好的GPS等系统）。它们是整个定位系统的“锚点”。
- 待定位无人机（失联机）：其位置未知，是需要求解的对象。
- 观测信息：已知基准无人机对待定位无人机的方位角测量值。方位角通常定义为从正北方向（或X轴正方向）顺时针旋转到观测者与目标连线的角度。
- 观测噪声：真实的传感器（如电子罗盘、视觉系统）测量存在误差。因此，我们得到的方位角数据是带有噪声的，通常假设为高斯白噪声。这意味着我们建立的模型必须具备一定的抗噪声能力。
- 问题目标：给定所有基准无人机的位置坐标
(x_i, y_i)，以及它们对待定位无人机j的方位角测量值θ_ij，求解待定位无人机j的最优位置估计(x_j, y_j)。
2.2 核心数学模型：方位交汇定位（Bearings-Only Localization）
最基本的数学模型源于三角测量原理。对于单个待定位无人机j，如果有一个基准无人机i观测到它，那么它们满足以下几何关系：
tan(θ_ij) = (y_j - y_i) / (x_j - x_i)或者更常用的是正弦/余弦形式：
sin(θ_ij) * (x_j - x_i) - cos(θ_ij) * (y_j - y_i) = 0这个方程描述了一条通过基准点(x_i, y_i)、方向角为θ_ij的直线。待定位点(x_j, y_j)理论上应该位于这条直线上。
当有多个（至少两个）基准无人机观测同一个目标时，我们就得到了多条这样的直线方程。在无噪声的理想情况下，这些直线应交于一点，该点即为目标位置。这就是方位交汇定位的直观理解。
2.3 从理想交汇到实际优化
然而，由于观测噪声的存在，多条方位线往往不会精确交于一点，而是形成一个小的“误差三角形”区域。因此，我们不能直接求解方程组（因为无解），而是需要寻找一个点，使得它到所有这些方位线的“距离”之和最小。这就将一个几何问题转化为了一个非线性最小二乘优化问题。
定义代价函数（Cost Function）为所有观测的方位角误差的平方和：
F(x_j, y_j) = Σ [ (测量的方位角 - 根据当前估计位置计算的理论方位角) ^ 2 ] = Σ [ θ_ij_measured - arctan2( (y_j - y_i), (x_j - x_i) ) ] ^ 2其中，arctan2是四象限反正切函数，能给出正确的角度值。我们的目标就是找到一组(x_j, y_j)，使得代价函数F的值最小。
注意：这里有一个关键的建模细节。直接使用角度差平方和作为代价函数，在角度值接近圆周跳变点（如359°和1°）时会出现不连续问题。更稳健的做法是处理角度的正弦和余弦值。我们可以将代价函数定义为基于向量投影的误差：
F(x_j, y_j) = Σ || ( [x_j - x_i, y_j - y_i]^T / d_ij ) × u_ij ||^2其中
d_ij是估计距离，u_ij = [sin(θ_ij), -cos(θ_ij)]是垂直于观测方向线的单位向量。这个形式的物理意义是目标位置到每条方位线的垂直距离的平方和，它避免了角度周期性问题，是工程上更常用的形式。
3. 求解策略与算法选型：从理论到代码实现
建立了优化模型后，接下来就是选择求解算法。这是一个典型的无约束非线性最小二乘问题。我们可以根据问题的规模（无人机数量）、对实时性的要求以及初始化质量，来选择不同的求解器。
3.1 梯度下降法家族：稳健的通用选择
对于这类问题，基于梯度的迭代优化方法是主流。它们的核心思想是：从一个初始猜测位置出发，沿着代价函数下降最快的方向（负梯度方向）逐步调整位置，直至收敛到局部极小值点。
- 最速下降法：最简单，但收敛速度慢，尤其在“山谷”地形中容易 zig-zag。
- 高斯-牛顿法（Gauss-Newton）：专门为最小二乘问题设计。它利用目标函数的特殊结构（误差项的平方和），用雅可比矩阵（J）近似代替海森矩阵（H），大大减少了计算量。其迭代公式为：
Δp = - (J^T * J)^(-1) * J^T * e(p)其中p = [x_j, y_j]是待估参数，e(p)是误差向量，J是误差向量对参数的雅可比矩阵。高斯-牛顿法在初始点靠近真值、且近似成立时，收敛速度非常快。 - 列文伯格-马夸尔特法（Levenberg-Marquardt, LM）：这可以说是解决非线性最小二乘问题的“瑞士军刀”。它综合了最速下降法和高斯-牛顿法的优点。当迭代点远离解时，LM算法更像最速下降法，保证收敛；当接近解时，它自动切换到高斯-牛顿法，加速收敛。LM算法通过引入一个阻尼因子 λ 来调节：
(J^T * J + λ * I) * Δp = - J^T * e(p)当 λ 很大时，方程近似为λ * I * Δp = - J^T * e(p)，即最速下降；当 λ 很小时，方程退化为高斯-牛顿法。LM算法具有非常好的鲁棒性，是很多数学库（如SciPy, MATLAB的lsqnonlin）的默认选择。
3.2 初始化的重要性：如何给算法一个“好起点”
非线性优化算法严重依赖于初始值。一个糟糕的初始猜测可能导致算法收敛到错误的局部极小点，甚至发散。在纯方位定位中，由于没有距离信息，初始值的选择需要一些技巧。
- 几何重心法：一种简单有效的方法是，将每条方位线向外延伸一段合理的估计距离（例如，根据编队通常的间距设定一个值，如50米），得到一个估计点。对所有基准无人机产生的估计点取几何平均，作为初始值。虽然粗糙，但通常能提供一个位于真实位置附近的起点。
- 最小二乘直接解（伪线性化）：对方位观测方程
tan(θ) = (y - y_i)/(x - x_i)进行变形，可以得到一个关于x和y的线性方程：x * sinθ - y * cosθ = x_i * sinθ - y_i * cosθ。对于每个观测，我们都能得到这样一个方程。将所有观测的方程联立，就构成了一个线性方程组A * [x, y]^T = b。我们可以用最小二乘法直接求解这个线性方程组，得到的结果可以作为非线性优化的一个优质初始值。这个方法被称为“伪线性估计（PLE）”，它计算速度快，但需要注意，由于对方程进行了非线性变换，其噪声统计特性发生了变化，在噪声较大时可能会有偏。不过，作为初始化步骤，它非常出色。
3.3 代码实现骨架（Python示例）
下面，我将结合上述思路，给出一个使用Python（SciPy库）实现的核心代码骨架。这里假设我们已经有了基准无人机位置列表anchors（形状为[n_anchors, 2]）和对应的方位角测量列表bearings（形状为[n_anchors]，单位为弧度）。
import numpy as np from scipy.optimize import least_squares def bearing_residuals(params, anchors, bearings): """ 计算代价函数的残差（误差向量）。 params: 待估参数 [x, y] anchors: 基准点数组，形状 (n, 2) bearings: 方位角测量值数组，形状 (n,)，单位弧度 返回：残差向量，形状 (n,) """ x, y = params dx = x - anchors[:, 0] dy = y - anchors[:, 1] # 计算理论方位角 pred_bearings = np.arctan2(dy, dx) # 处理角度周期性问题：确保误差在 [-pi, pi] 之间 residuals = np.unwrap(pred_bearings - bearings) # 另一种更稳定的残差定义：点到直线的垂直距离 # unit_vec_perp = np.column_stack([np.sin(bearings), -np.cos(bearings)]) # 垂直单位向量 # vec_to_target = np.column_stack([dx, dy]) # residuals = np.sum(vec_to_target * unit_vec_perp, axis=1) # 投影长度即距离 return residuals def pseudo_linear_estimator(anchors, bearings): """ 伪线性估计器，用于提供优化的初始值。 求解 A * [x, y]^T = b 的最小二乘解。 """ sin_t = np.sin(bearings) cos_t = np.cos(bearings) A = np.column_stack([sin_t, -cos_t]) # 系数矩阵 b = anchors[:, 0] * sin_t - anchors[:, 1] * cos_t # 常数项 # 最小二乘解 x_est, y_est = np.linalg.lstsq(A, b, rcond=None)[0] return np.array([x_est, y_est]) def locate_target(anchors, bearings, initial_guess=None): """ 主定位函数。 anchors: 基准点坐标，二维数组 bearings: 方位角测量，一维数组（弧度） initial_guess: 可选初始猜测 返回：优化后的位置 [x, y] """ if initial_guess is None: # 使用伪线性估计提供初始值 initial_guess = pseudo_linear_estimator(anchors, bearings) # 或者使用简单的几何方法：将方位线平移一段距离后求平均 # est_distance = 50.0 # 假设的初始距离 # est_points = anchors + est_distance * np.column_stack([np.cos(bearings), np.sin(bearings)]) # initial_guess = np.mean(est_points, axis=0) # 使用Levenberg-Marquardt算法求解非线性最小二乘问题 result = least_squares(bearing_residuals, initial_guess, args=(anchors, bearings), method='lm') if result.success: return result.x else: raise RuntimeError(f"Optimization failed: {result.message}") # ===== 模拟数据生成与测试 ===== if __name__ == "__main__": np.random.seed(42) # 1. 设置真实目标位置 true_target = np.array([30.0, 40.0]) # 2. 生成4个基准无人机位置（假设已知） anchors = np.array([[0, 0], [100, 0], [0, 100], [100, 100]]) # 3. 计算真实的方位角，并加入高斯噪声 true_vecs = true_target - anchors true_bearings = np.arctan2(true_vecs[:, 1], true_vecs[:, 0]) noise_std = np.deg2rad(2.0) # 假设方位角测量噪声标准差为2度 noisy_bearings = true_bearings + np.random.randn(len(anchors)) * noise_std # 4. 调用定位函数 estimated_target = locate_target(anchors, noisy_bearings) # 5. 输出结果 print(f"真实目标位置: {true_target}") print(f"估计目标位置: {estimated_target}") error = np.linalg.norm(estimated_target - true_target) print(f"定位误差: {error:.4f} 米")这段代码提供了一个完整的从数据生成、初始化、优化到结果评估的流程。least_squares函数是SciPy中强大的优化器，其method='lm'指定使用列文伯格-马夸尔特算法。在实际比赛中，你需要根据题目给出的具体数据格式，调整数据读取和预处理部分。
4. 模型进阶、误差分析与实战避坑指南
基础的定位模型搭建起来后，要让它真正实用、鲁棒，还需要考虑更多现实因素。这也是区分优秀模型和普通模型的关键。
4.1 观测几何与精度稀释（GDOP）
并不是任意分布的基准无人机都能提供好的定位精度。想象一下，如果所有基准无人机都挤在同一个方向，那么它们发出的方位线几乎平行，交汇区域会是一个又长又窄的带状，定位在垂直方向上的误差就会非常大。这种由于观测几何构型导致的定位精度下降，称为精度稀释（Geometric Dilution of Precision, GDOP）。
在纯方位定位中，GDOP与基准点相对于目标的张角有关。理想的情况是基准点均匀分布在目标周围（张角接近360度）。在建模时，我们可以通过Fisher信息矩阵（FIM）或克拉美-罗下界（CRLB）来理论分析给定几何构型下所能达到的最佳定位精度。这可以帮助我们在编队规划阶段，就有意识地优化基准无人机的布局，从源头上提升定位性能。例如，在解题时，如果题目允许调整部分无人机的位置，那么一个优化模型可以是如何调整这些“锚点”的位置，使得整体编队的定位误差的某种统计量（如最大值或均方根）最小化。
4.2 异常观测检测与鲁棒估计
我们的模型假设观测噪声是高斯分布。但现实中可能存在粗大误差（Outliers），例如某个无人机的罗盘突然受到强磁干扰，给出了一个完全错误的方位角。这种“野值”会严重扭曲最小二乘的结果，因为最小二乘对大的残差给予平方倍的权重，非常敏感。
为了提高模型的鲁棒性，我们需要引入鲁棒估计技术：
- RANSAC（随机采样一致性）：这是一种非常经典的方案。其基本思想是：随机从所有观测中选取最小子集（对于二维定位，最少需要2个观测）计算一个候选解，然后用这个解去测试其他观测点，统计符合该解的“内点”数量。重复这个过程多次，最终选择内点最多的那个候选解，并用所有内点重新进行精细的最小二乘估计。RANSAC能有效剔除局外点。
- 使用更鲁棒的损失函数：将最小二乘中的
L2范数（平方和）替换为对异常值不敏感的范数，例如L1范数（绝对值和），或Huber损失函数。Huber损失在误差较小时是二次的（像L2），在误差较大时是线性的（像L1），从而兼具对高斯噪声的效率和对抗异常值的鲁棒性。在SciPy的least_squares中，可以通过设置loss参数（如loss='huber'）来轻松实现。
4.3 多目标与数据关联问题
当有多架待定位无人机时，问题变得更加复杂。我们不仅需要估计每个目标的位置，还需要解决数据关联问题：即判断哪个方位角观测是属于哪个目标的。如果基准无人机只能测量方位角而无法区分目标ID，这就成了一个复杂的组合优化问题。在2022年国赛题中，通常假设观测数据是已经关联好的（即知道哪个方位角对应哪架无人机）。但如果是更一般的情况，可能需要引入诸如多假设跟踪（MHT）或基于聚类的方法来解决。
4.4 实战编程中的关键细节与避坑点
- 角度单位与周期处理：这是最容易出错的地方。数学库（如
np.arctan2,math.atan2）返回的角度单位是弧度。题目数据或传感器数据很可能是度。务必在计算前统一转换为弧度。另外，角度误差pred - meas要使用np.unwrap或手动加减2π来处理359°和1°这种跳变，否则代价函数会不连续，导致优化失败。我在代码中使用了np.unwrap，这是最安全的方式。 - 优化算法的收敛性与调参：
scipy.optimize.least_squares功能强大，但需要合理设置参数。ftol（函数值变化容忍度）、xtol（参数变化容忍度）和max_nfev（最大函数评估次数）是常用的调参项。如果优化不收敛，首先检查初始值是否太差，其次可以适当放宽容忍度或增加迭代次数。使用verbose=2参数可以打印迭代过程，帮助调试。 - 雅可比矩阵的提供：默认情况下，
least_squares会使用有限差分法数值计算雅可比矩阵。对于性能要求高的问题，我们可以手动提供解析的雅可比矩阵，这能显著提升优化速度和精度。对于我们的残差函数r_i = sin(θ_i)*(x - x_i) - cos(θ_i)*(y - y_i)，其对x和y的偏导数是：dr_i/dx = sin(θ_i),dr_i/dy = -cos(θ_i)。将这个雅可比矩阵通过jac参数传入，优化效率会更高。 - 结果验证与不确定性评估：优化得到点估计后，如何评估这个估计的可信度？我们可以计算估计值的协方差矩阵。对于最小二乘估计，在最优解附近，参数估计的协方差矩阵可以近似为
Cov = σ^2 * (J^T * J)^{-1}，其中σ^2是观测噪声方差的估计（可通过残差均方和估算），J是最优解处的雅可比矩阵。这个协方差矩阵的对角线元素就是x和y坐标的估计方差，其平方根即为标准差，可以理解为定位误差的“置信区间”。
5. 从定位到编队控制：模型的闭环与应用延伸
解决了单点、单时刻的定位问题，只是第一步。在无人机遂行编队飞行的动态场景中，我们需要的是连续、实时的位置估计，并以此为基础进行队形保持控制。这就引出了更高级的模型。
5.1 基于滤波的时序定位（跟踪）
静态定位模型处理的是“快照”数据。而在飞行中，无人机是连续运动的。我们可以利用运动模型（如匀速模型、匀加速模型）来预测无人机下一时刻的位置，然后将方位观测作为量测信息，通过滤波算法来更新和修正预测，实现跟踪。最经典的算法是扩展卡尔曼滤波（EKF）。
- 状态预测：根据上一时刻的位置和速度，预测当前时刻的位置。
- 观测更新：用当前时刻获得的多个方位角观测，来修正预测的位置。由于方位观测方程是非线性的，EKF需要对其进行线性化（求雅可比矩阵）。 EKF将纯方位定位从一个静态批处理优化问题，变成了一个动态的、递归的序贯估计问题，计算效率高，非常适合实时系统。这也是“无人机飞控”、“无人机控制算法”等热词背后涉及的核心技术之一。
5.2 编队队形重构与协同路径规划
当部分无人机失联被重新定位后，整个编队需要调整，以恢复到期望的几何队形（如三角形、菱形、一字形）。这涉及到编队控制问题。一种常见的思路是“领航-跟随”法：指定一架或几架无人机为领航者，其余跟随者根据与领航者的期望相对位置（Δx, Δy）以及自身估计出的绝对位置，计算出控制指令（速度、航向），驱使自己到达期望的相对位置。这里的控制律可以是简单的P（比例）控制，也可以是更复杂的模型预测控制（MPC）。
更进一步，如果整个编队需要在有障碍物的环境中飞行，那么结合了定位信息的协同路径规划就至关重要。这正对应了热词中的“无人机路径规划算法”、“无人机自主路径规划仿真”。我们可以将重新集结的编队视为一个整体，为其规划一条安全、高效的全局路径，同时内部各成员根据避障和队形保持要求进行局部调整。
5.3 仿真验证：从MATLAB/Python到更逼真环境
在数学建模竞赛中，用MATLAB或Python进行数值仿真验证模型是标准流程。但若要贴近工程实践，可以考虑在更高级的仿真环境中测试，例如：
- ROS + Gazebo：可以搭建一个包含无人机动力学模型、传感器噪声模型和物理引擎的仿真环境，用你写的定位算法作为ROS节点，订阅虚拟的方位传感器话题，发布估计的位置，再通过控制器去控制无人机，形成一个完整的闭环。这能极大地验证算法在更真实扰动下的鲁棒性。
- PX4/Gazebo仿真：直接使用PX4开源飞控的软件在环（SITL）仿真，配合Gazebo环境，可以测试算法与真实飞控软件的集成能力。
这些仿真平台的学习曲线较陡，但它们是连接理论模型与真实无人机系统的桥梁。对于有志于从事无人机、机器人相关研究的同学，尽早接触这些工具非常有价值。
回顾整个从问题理解、模型建立、算法实现到进阶应用的过程，纯方位无源定位就像是为无人机赋予了一种“盲人听声辨位”的协同感知能力。它的魅力在于，用看似简单的角度信息，通过精巧的数学和算法，构建出对空间位置的精确感知。在实际操作中，我最大的体会是：模型的鲁棒性往往比单纯的精度更重要。一个能处理异常数据、对初始值不敏感、在恶劣几何条件下仍能给出可用结果的算法，远比一个只在理想仿真中精度高但脆弱的算法有价值。因此，在实现基本定位功能后，务必投入精力到初始化策略、异常值处理和不确定性分析这些“非核心”但至关重要的环节上，这才是从“模型跑通”到“系统可靠”的关键一跃。
