无人机集群纯方位无源定位：从几何原理到非线性最小二乘实战

1. 项目概述：从“保姆思路”到实战定位

看到“2022国赛B题保姆思路及代码”这个标题，很多参加过数学建模竞赛或者对无人机技术感兴趣的朋友，尤其是学生和刚入行的工程师，眼睛都会一亮。这背后反映的是一个非常普遍且迫切的需求：在面对一个综合性、跨学科的复杂工程问题时，如何从零开始，一步步拆解问题、建立模型、编写代码，最终得到一个能跑通、有说服力的解决方案。这个项目标题直指2022年全国大学生数学建模竞赛B题的核心——无人机遂行编队飞行中的纯方位无源定位。

简单来说，这就是一个典型的“仅测向”定位问题。想象一下，你带领一个无人机编队在空中执行任务，比如进行灯光秀表演或者协同侦察。编队中只有少数几架无人机（我们称之为“发射机”或“参考机”）装备了精确的GPS或北斗定位系统，知道自己在哪里。而其他大部分无人机（“接收机”或“待定位机”）为了降低成本、减轻重量或应对GPS拒止环境，没有定位模块。但是，这些待定位的无人机能够通过机载的无线电或声学阵列，测量到已知位置参考机发出的信号的方向（即方位角，有时还包括俯仰角）。我们的任务就是，仅利用这些方向测量信息，反推出所有待定位无人机在空间中的精确位置。

这听起来像是一个几何谜题，但背后涉及的是信号处理、优化理论、线性代数和几何学的深度交叉。题目中的“保姆思路”意味着我们需要一个手把手、极度详细、避免任何思维跳跃的解题指南。而“代码”则要求思路必须能落地，转化为可执行的程序（通常是MATLAB或Python）。对于参赛学生而言，这直接关系到能否在三天内完成论文；对于工程师，这则是实现低成本、高鲁棒性无人机集群定位的关键技术路径。接下来，我将以一个过来人和实践者的角度，彻底拆解这个问题，不仅告诉你每一步怎么做，更深入剖析为什么这么做，以及在实际操作中会遇到哪些坑，如何避开它们。

2. 核心问题拆解：什么是“纯方位无源定位”？

在深入算法之前，我们必须像解构一台精密仪器一样，把这个学术名词拆解成我们能够理解和操作的零部件。

2.1 关键概念解析

“无源”：这是相对于“有源”定位而言的。有源定位，像GPS，接收机需要接收卫星发射的、带有精确时间戳的信号，通过计算信号传播时间（乘以光速）来得到距离，再通过多个距离球面交汇来定位。而无源定位中，待定位的接收机本身不发射任何信号，它只是一个被动的“倾听者”和“观察者”，通过接收来自已知位置发射机的信号来推断自身位置。这带来了隐蔽性好、不易被干扰的优点，但也增加了定位的难度，因为丢失了“距离”这一关键维度信息。

“纯方位”：这是定位信息的类型。我们能够获取的，仅仅是信号到来的方向线（Line of Bearing, LOB）。在二维平面（假设无人机在同一高度飞行），这是一个角度（例如，与正北方向的夹角，即方位角Azimuth）。在三维空间，这通常是一个方向矢量，由方位角和俯仰角共同决定。我们没有信号到达时间（TOA）、到达时间差（TDOA）或接收信号强度（RSSI）这些可能隐含距离信息的数据。这就好比你在茫茫大海上，只看到远处两座灯塔的灯光方向，却不知道离它们各自有多远，你需要据此画出自己的位置。

“编队飞行”：这个场景引入了关键的约束和先验信息。无人机不是随机散布的，它们需要保持一个特定的几何队形（如菱形、一字形等）。这意味着待定位无人机之间的相对位置关系是已知或部分已知的。例如，我们知道2号机应该在1号机正东方向10米处。这个队形约束是解决纯方位定位模糊性的“救命稻草”。没有这个约束，仅凭少数几个方向线，定位问题可能有无穷多解或者解极其不稳定。

2.2 问题数学模型抽象

让我们用数学语言来精确描述。假设有M架位置已知的参考无人机（信标），其坐标为p_i = [x_i, y_i, z_i]^T,i=1,2,...,M。有N架待定位的无人机，其真实坐标为q_j = [x_j, y_j, z_j]^T,j=1,2,...,N。

对于第j架待定位无人机，观测到第i架参考无人机的方向单位矢量（从q_j指向p_i）为：u_{ij} = (p_i - q_j) / ||p_i - q_j||我们实际测量到的是带有噪声的观测值\tilde{u}_{ij}，噪声通常假设为加性高斯白噪声，作用于角度测量。

观测方程：\tilde{θ}_{ij} = arctan2( (y_i - y_j), (x_i - x_j) ) + w_{ij}（二维情况），其中w_{ij}是测量噪声。

队形约束：对于编队中的任意两架待定位无人机j和k，它们的相对位置应满足q_k - q_j ≈ d_{kj}，其中d_{kj}是编队预设的相对位置向量。

定位目标：在给定噪声观测{\tilde{θ}_{ij}}和编队约束{d_{kj}}的条件下，估计出所有待定位无人机的位置{q_j}。

这个问题的核心挑战在于：观测方程关于待估参数q_j是非线性的（因为arctan2函数），且仅凭方向观测本身无法确定尺度（距离）。编队约束的引入，相当于提供了额外的“尺子”和“连接件”，将多个非线性问题耦合在一起，使得联合求解成为可能。

注意：在实际竞赛或工程中，题目往往会提供简化条件，例如所有无人机在同一高度飞行，从而将问题降维到二维平面，这能极大简化模型和计算。我们接下来的讨论也主要基于二维场景。

3. 保姆级求解思路：从理论到模型的构建路径

面对这样一个复杂问题，新手最容易犯的错误就是一头扎进细节，或者试图找到一个“万能公式”直接套用。正确的做法是建立清晰的求解路线图。下面这个“三步走”策略，是我经过多次实践总结出的高效路径。

3.1 第一步：几何与最小二乘框架建立

最直观的想法是利用几何关系。对于一架待定位无人机，如果它能同时看到两个已知位置的参考点，那么它的位置就应该在这两个方向线的交点上。由于存在测量噪声，这两条线通常不会精确相交，会形成一个“误差三角形”。

数学模型：对于单个待定位点q，接收到来自参考点p_i的方向观测，其方向线可以表示为：(p_i - q) × u_i ≈ 0（在二维中，叉积为零表示向量共线）。由于噪声存在，我们引入残差：r_i = |(p_i - q)^T * n_i|，其中n_i是垂直于观测方向单位矢量\tilde{u}_i的法向量。我们的目标是找到q，使得所有残差的平方和最小：min_q Σ_i r_i^2。

这就是一个非线性最小二乘问题。对于只有两三个参考点的情况，我们可以尝试解析求解（如利用三角正弦定理），但通常更普适的方法是采用迭代数值优化算法，例如高斯-牛顿法或列文伯格-马夸尔特算法。

为什么选择非线性最小二乘？因为它直接对应了“测量误差最小”这一最自然的优化目标，具有清晰的统计意义（在测量噪声为高斯分布时，其解是最大似然估计）。LM算法尤其适合，因为它能自适应地在梯度下降和高斯-牛顿法之间切换，对初始值鲁棒性较好，是解决这类问题的标准工具。

3.2 第二步：引入编队约束——从独立定位到协同定位

如果每架无人机独立进行上述定位，由于观测信息少（可能只看到2-3个参考点），解可能不唯一（模糊）或对噪声极其敏感。编队约束是我们的“王牌”。

如何引入约束？有两种主流思路：

1. 紧耦合优化：将所有待定位无人机的位置{q_j}一起作为优化变量，构建一个全局的目标函数。

   min_{q_1, ..., q_N} Σ_j Σ_i || (p_i - q_j) / ||p_i - q_j|| - \tilde{u}_{ij} ||^2 + λ * Σ_{(j,k)∈E} || (q_k - q_j) - d_{kj} ||^2

   其中，第一项是所有方位观测的误差，第二项就是编队约束误差（惩罚偏离预设相对位置的量），λ是一个权重系数，E表示编队中具有已知相对关系的无人机对集合。这是一个更大规模的非线性最小二乘问题。

2. 松耦合分步法：先利用部分几何关系较强的观测（例如，某架无人机能同时看到多个参考点），初步估计出少数几架“锚点”无人机的位置。然后，以这些初步定位的无人机作为新的“已知参考点”，结合编队约束，像搭积木一样，逐步推算出整个编队中其他无人机的位置。这种方法逻辑清晰，易于实现和调试，在竞赛时间紧张时往往是更稳妥的选择。

实操心得： 在国赛这种时间有限的场景下，我强烈推荐松耦合分步法。它的优势在于：

• 模块化：可以将问题分解为多个子问题逐个击破，降低编程和调试复杂度。
• 容错性高：如果某一步估计出现较大误差，不会像紧耦合方法那样污染整个全局解。
• 直观：每一步都有明确的几何意义，方便写进论文进行解释。 紧耦合方法理论上更优，但实现复杂，对初始值敏感，且求解耗时可能更长。

3.3 第三步：处理模糊性与提升鲁棒性

纯方位定位天生存在模糊性。最经典的例子是，在二维平面上，对于一个待定位点和两个参考点，如果三点接近共圆（即待定位点位于由两个参考点和其观测方向所确定的圆的圆周上），那么定位误差会急剧放大，甚至出现两个对称的镜像解。这被称为“基线-目标”几何构型恶劣。

应对策略：

1. 增加观测多样性：题目通常会设计让无人机编队进行机动（如转弯、变换队形），从而产生多个时刻的观测数据。充分利用时间序列数据是关键。我们可以将不同时刻的观测联合起来进行优化，相当于增加了虚拟的参考点数量，能有效抑制模糊性。
2. 利用队形先验：编队约束本身就是一个强大的去模糊工具。镜像解通常会导致编队形状发生翻转或扭曲，违反已知的队形几何关系，从而在优化过程中被排除。
3. 鲁棒损失函数：标准的平方和损失对大的离群噪声（野值）非常敏感。可以考虑使用Huber损失、Cauchy损失等鲁棒损失函数代替平方损失，当残差较大时，给予线性惩罚而非平方惩罚，避免个别坏数据带偏整个估计。

4. 代码实现详解：以MATLAB/Python为例

思路清晰后，代码就是将思路机械化的过程。这里我提供一个基于松耦合分步法和非线性最小二乘的MATLAB实现框架，并附上关键步骤的Python对照。我们假设一个简化场景：二维平面，9架无人机呈3x3方格编队，其中四角的4架为已知位置的参考机，需要定位中间5架。

4.1 数据准备与问题初始化

% 假设已知数据 % ref_pos: 4x2矩阵，4架参考机的 [x, y] 坐标 % bearing_measurements: 一个5x4的元胞数组，bearing_measurements{j}{i} 表示第j架待定位机对第i架参考机的方位角测量值（弧度） % formation_template: 5x2矩阵，5架待定位机在编队坐标系（以编队中心为原点）中的相对坐标 % 示例数据生成（实际应从题目文件读取） ref_pos = [0, 0; 100, 0; 0, 100; 100, 100]; % 四角参考机 formation_template = [-10, -10; 0, -10; 10, -10; -10, 10; 10, 10]; % 中间5架机的相对位置 % 为每架待定位机生成带噪声的方位角观测（模拟过程） num_target = size(formation_template, 1); num_ref = size(ref_pos, 1); bearing_meas = cell(num_target, num_ref); true_target_pos = formation_template + 50; % 假设编队中心在(50,50)，计算真实位置用于生成模拟观测 for j = 1:num_target for i = 1:num_ref true_vec = ref_pos(i,:) - true_target_pos(j,:); true_bearing = atan2(true_vec(2), true_vec(1)); % 真实方位角 noise = 0.05 * randn; % 加入高斯噪声，标准差0.05弧度（约2.87度） bearing_meas{j, i} = true_bearing + noise; end end

注意：在真实解题中，bearing_measurements和ref_pos应从赛题提供的附件数据文件中读取。务必仔细检查数据格式（单位是度还是弧度？坐标系原点在哪？）。读取数据是第一步，也是最容易出错的一步。

4.2 核心定位函数：非线性最小二乘求解

我们首先实现一个函数，用于求解单架无人机在给定多个方位观测下的位置。

function [estimated_pos, residual] = locate_one_target(ref_positions, bearing_measurements, initial_guess) % ref_positions: N x 2, 已知参考点坐标 % bearing_measurements: 1 x N 向量，对每个参考点的方位角观测（弧度） % initial_guess: 1 x 2, 优化初始值（至关重要） % 定义目标函数：残差平方和 cost_func = @(x) sum( (atan2(ref_positions(:,2) - x(2), ref_positions(:,1) - x(1)) - bearing_measurements').^2 ); % 使用fminunc或lsqnonlin进行优化。lsqnonlin更直接，因为它专为最小二乘设计。 % 这里使用lsqnonlin，它允许我们直接返回残差向量 residual_func = @(x) atan2(ref_positions(:,2) - x(2), ref_positions(:,1) - x(1)) - bearing_measurements'; options = optimoptions('lsqnonlin', 'Display', 'off', 'Algorithm', 'levenberg-marquardt'); [estimated_pos, ~, residual] = lsqnonlin(residual_func, initial_guess, [], [], options); end

Python (SciPy) 对照实现：

import numpy as np from scipy.optimize import least_squares def locate_one_target(ref_positions, bearing_measurements, initial_guess): """ ref_positions: np.array of shape (N, 2) bearing_measurements: np.array of shape (N,) initial_guess: np.array of shape (2,) """ def residual_func(x): # 计算预测方位角与观测的差 pred_bearings = np.arctan2(ref_positions[:, 1] - x[1], ref_positions[:, 0] - x[0]) return pred_bearings - bearing_measurements result = least_squares(residual_func, initial_guess, method='lm', verbose=0) estimated_pos = result.x residual = result.cost # 残差平方和的一半 return estimated_pos, residual

4.3 分步定位策略实施

现在，我们实施松耦合策略。假设我们决定先定位5架待定位机中几何条件最好的那一架（例如，能观测到所有4个参考点的）。

% 1. 选择并定位“锚点”目标机 % 假设我们选择第一架待定位机作为锚点（索引j=1） bearing_for_target1 = cell2mat(bearing_meas(1, :)); % 获取其所有观测 initial_guess = [50, 50]; % 一个粗略的初始猜测（例如，编队中心） [anchor_pos, anchor_residual] = locate_one_target(ref_pos, bearing_for_target1, initial_guess); fprintf('锚点机估计位置: (%.2f, %.2f), 残差: %.4f\n', anchor_pos, anchor_residual); % 2. 将已定位的锚点机加入参考集 extended_ref_pos = [ref_pos; anchor_pos]; % 参考机数量变为5 % 3. 定位剩余目标机，利用编队约束提供更好的初始值 estimated_positions = zeros(num_target, 2); estimated_positions(1, :) = anchor_pos; % 保存锚点机结果 for j = 2:num_target % 获取该目标机的观测（可能只对部分原参考机有观测，这里假设全部） current_bearing = cell2mat(bearing_meas(j, :)); % **关键：利用编队约束计算初始猜测** % 已知锚点机在编队中的相对位置是 formation_template(1,:) % 当前目标机在编队中的相对位置是 formation_template(j,:) % 它们的相对偏移是 delta = formation_template(j,:) - formation_template(1,:) % 那么，当前目标机的初始猜测位置可以是：anchor_pos + delta delta = formation_template(j, :) - formation_template(1, :); initial_guess_j = anchor_pos + delta; % 使用扩展后的参考集进行定位 [est_pos, res] = locate_one_target(extended_ref_pos, current_bearing, initial_guess_j); estimated_positions(j, :) = est_pos; fprintf('目标机%d估计位置: (%.2f, %.2f)\n', j, est_pos); end % 4. （可选）全局微调 % 将所有估计位置和编队约束一起，进行一次全局优化（紧耦合），以平差误差。 % 此步可作为精度提升的选项，如果时间允许且模型稳定，可以实施。

实操心得：初始猜测的艺术非线性优化算法的结果严重依赖初始值。一个糟糕的初始值可能导致算法收敛到局部极小值甚至发散。利用编队约束来生成初始猜测，是本方法成功的关键。它确保了我们的迭代起点离真实解非常近，大大提高了收敛速度和成功率。在实际比赛中，一定要在论文中强调这一技巧，并分析其有效性。

4.4 结果可视化与误差分析

代码跑通了不算完，科学地呈现结果和评估性能同样重要。

% 绘制结果 figure; hold on; grid on; axis equal; % 绘制参考机 plot(ref_pos(:,1), ref_pos(:,2), 'ks', 'MarkerSize', 10, 'MarkerFaceColor', 'b'); text(ref_pos(:,1), ref_pos(:,2), num2str((1:size(ref_pos,1))'), 'VerticalAlignment', 'bottom'); % 绘制真实位置（模拟中我们知道，实际比赛不知道） plot(true_target_pos(:,1), true_target_pos(:,2), 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % 绘制估计位置 plot(estimated_positions(:,1), estimated_positions(:,2), 'r^', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % 连接估计位置，显示估计的队形 for j = 1:num_target for k = j+1:num_target % 这里可以根据编队连接关系（如相邻关系）来画线，简化起见，画所有连线 plot([estimated_positions(j,1), estimated_positions(k,1)], ... [estimated_positions(j,2), estimated_positions(k,2)], 'r:'); end end legend('参考机', '真实位置（模拟用）', '估计位置'); xlabel('X坐标 (米)'); ylabel('Y坐标 (米)'); title('无人机纯方位无源定位结果'); % 计算并显示定位误差 position_errors = sqrt(sum((estimated_positions - true_target_pos).^2, 2)); fprintf('\n===== 定位误差分析 =====\n'); fprintf('无人机编号 | X误差(米) | Y误差(米) | 总误差(米)\n'); for j = 1:num_target fprintf('%4d | %8.3f | %8.3f | %8.3f\n', ... j, estimated_positions(j,1)-true_target_pos(j,1), ... estimated_positions(j,2)-true_target_pos(j,2), position_errors(j)); end fprintf('平均定位误差: %.3f 米\n', mean(position_errors));

5. 常见问题、调试技巧与进阶思考

即使按照上述流程，在实际操作中你依然会遇到各种问题。下面是我总结的“避坑指南”和进阶建议。

5.1 典型问题排查清单

问题现象	可能原因	排查与解决思路
算法不收敛，残差巨大	1. 初始猜测值离真实解太远。 2. 观测数据存在严重野值（错误）。 3. 参考点与目标点几何构型恶劣（接近共圆）。	1.输出迭代过程：查看优化器每次迭代的目标函数值，如果几乎不变或震荡，说明初始值不好。尝试使用不同的初始猜测策略，如利用多个观测线的粗略交点。 2.数据可视化：将参考点和观测方向线画出来，检查是否有明显不合逻辑的观测（如方向线完全偏离）。 3.敏感性分析：轻微扰动观测数据，看解的变化是否剧烈。如果剧烈，说明问题本身病态，需考虑增加观测或利用时间序列数据。
定位结果存在系统性偏移	1. 坐标系不统一。例如，观测角度是相对于机头方向，但算法假设是全局北东坐标系。 2. 编队约束中的相对位置d_{kj}定义有误（符号或尺度）。	1.仔细审题：确认题目中所有角度（方位角）的定义（从北顺时针，还是从东逆时针？）。在代码中做统一的转换。 2.验证约束：用一两个简单特例（如两架机一前一后）手动计算一下，看你的编队约束公式是否给出正确的关系。
部分无人机定位精度极差，其他良好	1. 该无人机观测到的参考点数量少或几何分布差。 2. 该无人机的观测噪声特别大（传感器故障模拟）。	1.观测分析：统计每架无人机能“看到”的参考机数量和质量。对于观测条件差的节点，其定位应更多依赖于通过编队约束从邻居节点传递过来的信息。在松耦合策略中，应优先定位观测条件好的节点。 2.鲁棒优化：考虑在目标函数中引入加权，给噪声大的观测更低的权重，或使用Huber损失函数。
MATLAB的lsqnonlin报错或陷入无限循环	1. 目标函数或残差函数返回了NaN或Inf。 2. 算法参数设置不当。	1.添加边界条件：使用lsqnonlin的lb和ub参数，将搜索范围限制在合理的物理区域内（如任务区域）。 2.调试函数：在残差函数内部设置断点或打印语句，检查当输入x为某些值时，atan2的分母是否为零（即目标点与参考点重合，这通常不会发生，但初始猜测可能导致）。 3.调整选项：尝试减小StepTolerance或FunctionTolerance，或者换用trust-region-reflective算法试试。

5.2 模型稳健性提升技巧

1. 时间序列融合：如果题目提供了多个时刻的观测数据，千万不要每个时刻独立处理然后取平均。正确做法是将所有时刻的观测方程全部纳入同一个优化问题。此时，待估参数变成了每个无人机在每个时刻的位置q_j(t)。目标函数变为所有时刻、所有观测的残差平方和，再加上相邻时刻之间的运动平滑约束（如果无人机匀速运动，可假设q_j(t+1) ≈ q_j(t) + v_j * Δt）。这能极大提高定位精度和稳定性。
2. 自适应权重：在全局优化中，不同观测的可靠性不同。可以根据信噪比估计、几何稀释精度因子或者残差大小，动态调整每个观测误差项在目标函数中的权重。残差大的观测，权重降低。
3. 利用高度信息：如果是三维问题，且有一部分高度信息（如气压计测量的相对高度），可以将其作为一个软约束或额外的观测方程加入优化，能有效降低模糊性。

5.3 从竞赛到工程的思考

竞赛模型是高度简化的。要将此技术应用于真实工程，还需考虑：

• 异步时钟与时钟漂移：真实系统中，测量时刻可能存在微小偏差，需要进行时间同步或估计时钟差。
• 非高斯噪声：实际测量噪声可能服从更复杂的分布，需要更精细的误差建模。
• 通信拓扑与分布式计算：在大型编队中，集中式处理所有数据可能不现实。需要研究分布式定位算法，每架无人机仅与邻居通信，协同估计全局位置。
• 传感器标定：测向天线或阵列的安装偏差需要事先标定，否则会引入系统性误差。

这个“保姆级”项目，从理解“纯方位无源定位”这个拗口的概念开始，到建立几何与优化模型，再到手把手实现代码和调试，最后思考其局限与扩展，完成了一次完整的从理论到实践的闭环。它锻炼的不仅仅是编程和数学能力，更是将一个复杂现实问题抽象、分解、求解和验证的系统工程思维能力。无论你是为了备战数模国赛，还是真正想踏入多智能体协同感知的领域，希望这份超详细的拆解能成为你手中一张可靠的“导航图”。