简介：一套基于Matlab 2019b的多无人机协同目标定位仿真实现，面向无人机导航与目标探测方向的初学者、科研人员，代码已经实际运行验证，用户只需替换相关数据即可直接应用，特别适合快速上手。压缩包共46个文件，大小约3.7MB，以29个Matlab程序文件为主体，同时包含4个PDF文档、4个Python辅助脚本、7个zbak备份文件、1个zip备份包和1个md说明文档，覆盖算法主控、RSSI距离转换、偏航角确定、目标估算等关键子模块。目前已有30人浏览/学习，适合用于课程设计、毕业设计或科研预研。完整的main.m主程序与可独立解析的辅助函数，可以帮助理解RSSI测距数据处理、编队缩减、局部平移与目标定位的完整流程；另附完整报告、海报与测试脚本，可对照验证算法效果。资源来自网络分享，仅限学习交流，请勿商用。

1. 多无人机协同目标定位仿真：为什么单机测向永远不够用

单架无人机测向定位的精度极限取决于基线长度，而基线被机身尺寸锁死。多无人机协同的本质，是用空间分布的多个观测节点合成大孔径基线，把测向误差对定位精度的放大效应压下去。这个工程用 Matlab 实现了从航迹生成、协同测向到融合定位的完整闭环，核心算法是加权最小二乘与扩展卡尔曼滤波的对比实现，附带 GDOP 几何精度因子分析模块，能直观看到机队构型对定位结果的影响。适合做无人机协同导航、无源定位方向课题的学生，以及需要快速验证定位算法的工程师。用这套代码，输入几架无人机的航点坐标，就能得到目标位置的估计轨迹和误差曲线，省掉从零搭框架的时间。

2. 仿真架构与定位模型选择：先搞懂 GDOP 再写代码

2.1 测向交叉定位的基本原理与误差传递链

多无人机协同定位最常用的体制是测向交叉定位，也叫视线交汇定位。每架无人机测量目标相对于自身的方位角与俯仰角，多条视线在空间交汇，交点就是目标位置估计。这个原理不复杂，但误差传递链条值得拆开看：角度测量误差经过距离放大后，变成位置误差，放大倍数近似等于观测距离与基线长度之比。基线越长、观测距离越短，定位精度越高。

数学上，假设第 i 架无人机在位置 ( p_i = [x_i, y_i, z_i]^T ) 测得目标方向角 ( \theta_i )（方位角）和 ( \phi_i )（俯仰角），目标真实位置为 ( p_t = [x_t, y_t, z_t]^T )，则测向方程可以写成：

[ \theta_i = \arctan\frac{y_t - y_i}{x_t - x_i} + n_{\theta_i} ]

[ \phi_i = \arctan\frac{z_t - z_i}{\sqrt{(x_t - x_i)^2 + (y_t - y_i)^2}} + n_{\phi_i} ]

其中 ( n_{\theta_i} ) 和 ( n_{\phi_i} ) 是角度测量噪声，通常建模为零均值高斯分布。这个方程组是非线性的，求解方法直接决定了仿真的复杂度和精度。仿真里我建议两种方法都写出来对比：一种是对非线性方程做线性化后用最小二乘迭代求解，另一种是直接用扩展卡尔曼滤波做序贯估计。

GDOP 是这个场景里绕不开的概念。它把测角误差标准差与定位误差标准差联系起来：定位误差 = GDOP × 测角误差（弧度）× 观测距离。GDOP 值越小，说明当前机队几何构型对定位越有利。三架无人机与目标形成近似正三角形时 GDOP 最小，如果目标落在无人机连线的延长线上，GDOP 会急剧恶化。仿真前期先用 GDOP 模块选好航线构型，后面跑定位算法才有意义。

2.2 仿真工程的目录结构与核心模块职责

这个 Matlab 工程我按功能拆成了五个模块，每个模块对应一个文件夹，从顶层脚本到底层函数分层清晰：

主控脚本是整个工程的入口，所有参数都集中在文件头部。我习惯把参数区用%%分块注释隔开，这样换场景时只需要改参数区，不用动算法代码。无人机的数量、位置、测角精度、采样周期这些都在参数区定义，跑一次仿真观察结果，再回头调参数，效率比散落各处改要高得多。

3. 航线构型设计：用 GDOP 分布图避开几何退化

3.1 三机与四机编队的 GDOP 对比分析

编队构型的选择直接决定定位效果，这不是玄学，而是有明确的数学依据。GDOP 矩阵由视线方向矩阵的伪逆计算而来，视线方向越分散，矩阵条件数越小，GDOP 值越低。

三机编队时，当三架飞机与目标的视线方向夹角分别为 120° 时，GDOP 达到最小值。用等边三角形布站能获得最佳几何构型。四机编队理论上更好，但提升幅度取决于第四架飞机提供的视线方向是否与前三维线性独立。如果第四架飞机的位置只是前三架的线性组合，GDOP 几乎不变，白费一架飞机。

我把 GDOP 分析做成了一张分布图，目标在区域内移动时能看到精度变化。具体做法是在目标可能出现的区域铺一个网格，对每个网格点计算一次 GDOP 值，然后把结果画成等高线图或者伪彩图。这张图能直接用来看当前机队构型的覆盖范围，也能反过来指导航线设计——让无人机尽量长时间维持对目标区域的低 GDOP 朝向。

3.2 圆形航迹与直线航迹的构型演化仿真

仿真里我设计了两种典型航迹：直线穿越和圆形绕飞。直线航迹简单，但构型随时间变化大，特别是当某架无人机飞到目标与另一架无人机连线的延长线上时，GDOP 会瞬间恶化。圆形航迹的好处是构型周期性变化，GDOP 值在周期内有规律波动，能保证每过一段时间就回到较好的几何构型。

跑仿真的时候，重点看一个指标——GDOP 的时间演化曲线。这条曲线能精确告诉你哪段时间定位可信、哪段时间结果不可信。自动调度算法可以先设定一个 GDOP 阈值，超过阈值时无人机自动调整航线，这在工程上就是最朴素的协同航迹规划。

绘制 GDOP 分布图的脚本我直接贴出来：

% calc_gdop_map.m - 计算目标区域GDOP分布并绘制伪彩图 % 输入: uav_pos 无人机位置矩阵 [N,3], area 目标区域范围 % 输出: gdop_map 区域网格上的GDOP值 function gdop_map = calc_gdop_map(uav_pos, area, grid_size) % 生成目标区域网格 x = linspace(area(1), area(2), grid_size); y = linspace(area(3), area(4), grid_size); [X, Y] = meshgrid(x, y); gdop_map = zeros(size(X)); for i = 1:numel(X) target = [X(i), Y(i), 0]; % 假设目标在地面, z=0 gdop_map(i) = calc_gdop_single(target, uav_pos); end % 绘制伪彩图 figure('Name', 'GDOP Distribution'); imagesc(x, y, gdop_map); colorbar; xlabel('X (m)'); ylabel('Y (m)'); title('GDOP Distribution over Target Area'); axis xy; end

这段代码的核心是生成目标区域网格，对每个网格点调用calc_gdop_single计算单点 GDOP，最后用imagesc可视化。网格密度grid_size越大，图越精细，但计算时间线性增长。我一般取 50，区域范围 2000 米乘 2000 米时，计算量在几千个点，Matlab 秒级完成。如果后面要跑蒙特卡洛仿真，建议把网格降到 30，能省不少时间。

calc_gdop_single的工作是计算视线方向矩阵的 Gram 矩阵，再对 Gram 矩阵求逆取迹开方。视线方向越长，Gram 矩阵越接近奇异，对应 GDOP 值越大，这是所有定位精度的根源。

4. 定位解算实现：加权最小二乘与扩展卡尔曼滤波

4.1 加权最小二乘定位算法推导与 Matlab 实现

加权最小二乘定位的基本思路是：把非线性测向方程在某个初值附近线性化，然后利用残差的加权平方和最小化来迭代求解目标位置。

测向方程的雅可比矩阵构造如下：对每个无人机，视线方向单位向量与目标位置偏导构成矩阵的一行。加权矩阵取测角噪声方差的倒数，测角精度高的无人机权重更大。迭代公式就是标准的 Gauss-Newton 形式：

[ \Delta p = (J^T W J)^{-1} J^T W r ]

其中r是当前估计值下的测向残差向量。

代码实现如下：

% ls_locator.m - 加权最小二乘迭代定位解算 % 输入: uav_pos 无人机位置 [N,3], angles 测角数据 [N,2], sigma 角度噪声标准差 % 输出: est_pos 目标位置估计 [1,3], 迭代次数 function [est_pos, iter_cnt] = ls_locator(uav_pos, angles, sigma, init_pos) N = size(uav_pos, 1); % 加权矩阵: 每个测量噪声方差的倒数 W = diag(repmat(1 ./ sigma.^2, 1, 2)); % 方位角+俯仰角共2N个测量 est_pos = init_pos; % 初始猜测, 一般用质心或前一次估计 max_iter = 50; threshold = 1e-6; % 迭代收敛阈值 for iter = 1:max_iter % 计算当前估计下的视线向量和残差 r = zeros(2*N, 1); J = zeros(2*N, 3); for i = 1:N delta = est_pos - uav_pos(i, :); dist = norm(delta); % 方位角残差（注意角度环绕） obs_azi = angles(i, 1); calc_azi = atan2(delta(2), delta(1)); r(2*i-1) = angdiff(calc_azi, obs_azi); % 俯仰角残差 obs_ele = angles(i, 2); calc_ele = asin(delta(3) / dist); r(2*i) = angdiff(calc_ele, obs_ele); % 雅可比矩阵行 J(2*i-1, :) = [-delta(2)/dist^2, delta(1)/dist^2, 0]; J(2*i, :) = [-delta(1)*delta(3)/(dist^2*sqrt(delta(1)^2+delta(2)^2)), ... -delta(2)*delta(3)/(dist^2*sqrt(delta(1)^2+delta(2)^2)), ... sqrt(delta(1)^2+delta(2)^2)/dist^2]; end % 加权最小二乘更新 delta_p = (J' * W * J) \ (J' * W * r); est_pos = est_pos + delta_p'; if norm(delta_p) < threshold iter_cnt = iter; return; end end iter_cnt = max_iter; end

这段代码里最容易翻车的是方位角的残差计算。直接把atan2结果相减会遇到角度环绕问题——比如真实方位角是 179°，观测是 -179°，直接相减得到 -358°，但实际上误差只有 2°。angdiff函数把结果映射到 [-π, π] 区间，工程上必须要处理。第二个坑是雅可比矩阵的行列式在目标与无人机连线垂直时退化，这对应 GDOP 急剧恶化的情况。

4.2 EKF 扩展卡尔曼滤波的滤波方程与时序更新

加权最小二乘是批处理方式，把所有观测拿到后一次性解算。如果目标在运动而无人机也在运动，更好的做法是扩展卡尔曼滤波，把时间更新和量测更新结合起来，利用目标运动的先验模型做递推估计。

EKF 的状态向量取目标的位置和速度，共 6 维。时间更新用匀速运动模型（CV 模型）做状态转移，量测更新和上一节的最小二乘共享同一个量测方程与雅可比矩阵，所以两套算法的代码可以复用——量测函数写一次，最小二乘和 EKF 都能调用。

EKF 好处是实时性，每个采样周期更新一次估计，输出一条连续的目标轨迹。坏处是对初值敏感，初值差太远会发散。我对初值的处理方式是：仿真前 3 帧用加权最小二乘解算作为 EKF 的初始状态，后续帧用 EKF 递推，效果比纯 EKF 从零开始稳定得多。

4.3 两种算法的定位精度与收敛速度对比

仿真结果一般呈现这样的规律：在 GDOP 良好的构型下，加权最小二乘和 EKF 的稳态精度接近，都逼近 CRLB（克拉美-罗下界）。但在目标机动或者构型剧烈变化时，EKF 的估计曲线更平滑，因为它有运动模型的约束，不会被单帧异常观测带偏。而最小二乘每帧独立解算，噪声大的时候会出现点迹跳变。

收敛速度方面，最小二乘两到三次迭代就能收敛，而 EKF 需要十到二十帧才能从初值收敛到稳态。如果目标运动速度较快，EKF 会有滞后，这时候可以把过程噪声调大一些，代价是稳态误差变大，这是一个折中。

5. 避坑与排查：仿真系统最常见的四类翻车点

5.1 角度定义不一致导致定位结果整体偏移

现象：所有无人机定位结果都偏向某个方向，但偏差不是随机噪声，而是一致性偏移。

原因：部分航迹生成代码用的方位角定义是atan2(y, x)从 X 轴正方向起算，而测向代码用的是从 Y 轴正方向起算的导航惯例定义，两者差 90°。单个无人机看不出来，多机融合时误差被系统性地叠加。

解决：在代码开头定义一个函数calc_bearing_deg，注释写明坐标约定，所有模块统一调用。跑仿真前先用一组已知位置的数据做开环测试，输入真实角度（不加噪声），如果定位结果和真实值不一致，首先检查角度定义。

5.2 无人机高度为零导致雅可比矩阵奇异

现象：提示Matrix is singular或者Warning: Matrix is close to singular，定位结果出现 NaN。

原因：仿真里所有无人机和目标都在同一高度（z=0），这导致俯仰角恒为 0，雅可比矩阵中与 z 相关的列全为零，矩阵不可逆。

解决：把无人机的巡航高度设为 100 到 500 米，目标设定在地面（z=0）。这样俯仰角有了有效的变化范围，雅可比矩阵非奇异。如果确实需要全平面仿真，改用伪逆pinv替代求逆，但会损失 z 方向的估计精度，目标高度估计会发散。

5.3 EKF 过程噪声设置不当导致轨迹发散

现象：前几帧估计正常，几十帧后估计值漂移，最终飞出合理范围。

原因：匀速运动模型的过程噪声 Q 矩阵设得太小，模型认为目标速度完全恒定。当目标有轻微机动或无人机剧烈转弯时，模型失配，滤波器不相信观测数据，状态被错误的外推带走。

解决：Q 矩阵的对角元素根据目标最大加速度来设。比如目标最大加速度 2 m/s²，Q 中速度项设(2 * dt)^2量级。调试技巧是观察新息序列（innovation sequence），如果新息均值不趋近于零，说明模型失配，需要调大 Q。

5.4 蒙特卡洛仿真随机种子未设置导致结果不可复现

现象：同一组参数，两次运行得到的 RMSE 曲线不同，无法判断是算法改进还是随机噪声造成的波动。

原因：每次运行的随机数序列不同，蒙特卡洛结果自然不同。

解决：在run_main.m开头加rng(2024)固定随机种子。改参数后对比结果时，固定种子保证可复现性。正式出结果时，用多个种子（比如 1 到 20）各跑一遍，取 RMSE 均值并输出方差，这样可信度更高。

6. 链路级验证技巧：从单帧解算到全程蒙特卡洛统计

仿真做完不是终点，验证仿真结果可信才是重点。我习惯按三步走：第一步，关闭噪声，把角度量测设为真实值，定位结果必须等于真实位置（误差在数值精度范围内），这验证代码逻辑正确。第二步，只加很小的噪声（比如 0.1°），定位误差应该在几米量级，验证算法对噪声的响应合理。第三步，按设计值设置噪声（通常 1° 到 5°），做 100 次蒙特卡洛仿真，统计 RMSE。

蒙特卡洛仿真的框架代码：

% run_monte_carlo.m - 多次蒙特卡洛仿真统计RMSE clear; clc; rng(42); % 固定全局随机种子 N_MC = 100; % 蒙特卡洛次数 rmse_list = zeros(N_MC, 1); for mc = 1:N_MC % 每次仿真内部重置随机状态, 保证各次独立 % 注意: 不要在这里再设rng(mc), 会破坏独立性 run_script = 'run_main.m'; % 调用主仿真脚本 run(run_script); rmse_list(mc) = calc_total_rmse(err_pos_history); end % 输出统计结果 disp(['RMSE 均值: ', num2str(mean(rmse_list)), ' m']); disp(['RMSE 标准差: ', num2str(std(rmse_list)), ' m']);

蒙特卡洛次数取 100 次还是 1000 次取决于时间预算。100 次对精度评估足够给出趋势，1000 次的置信区间更窄但耗时十倍。先用 100 次快速筛选参数，锁定参数后再跑 1000 次出最终数据。

最后一个习惯：每次跑完仿真，把生成的 GDOP 图、RMSE 曲线和参数配置保存到带时间戳的文件夹里。因为改参数跑一圈很容易，但回头想找回某次实验的配置就难了。从那以后，我每次跑仿真都强制走一遍「固定种子 → 单帧校验 → 蒙特卡洛统计 → 存档」这个流程，虽然前期多花几分钟，后面找数据时省下的时间远超投入。希望帮到你。

本文还有配套的精品资源，点击获取