✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、程序设计科研仿真。
🍎完整代码获取 定制创新 论文复现点击：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：做科研，博学之、审问之、慎思之、明辨之、笃行之，是为：博学慎思，明辨笃行。
🔥 内容介绍
一、引言
在当今复杂多变的环境监测、应急救援以及军事侦察等领域，多无人机协同执行搜索与动态目标跟踪任务展现出显著优势。然而，要实现高效的协同作业并非易事，需要解决路径规划、目标状态估计等一系列关键问题。A*（A - star）算法和快速扩展随机树（RRT）在路径规划方面各有优势，而扩展卡尔曼滤波器（EKF）和无迹卡尔曼滤波器（UKF）则在动态目标状态估计中发挥重要作用。将 A*、RRT 与 EKF、UKF 相结合，有望为多无人机协同搜索与动态目标跟踪提供有效的解决方案。
二、多无人机协同搜索与跟踪任务分析
（一）任务场景
多无人机协同执行任务的场景通常设定在一个具有复杂地形和潜在障碍物的区域内，例如山区、城市建筑群等。目标是一个动态移动的对象，其运动轨迹和速度具有不确定性。多无人机需要相互协作，在尽可能短的时间内搜索到目标，并持续跟踪其移动。
（二）面临的挑战
- 路径规划难题
：无人机在搜索和跟踪过程中，必须规划出无碰撞且高效的飞行路径。复杂的环境使得路径规划空间庞大，传统方法难以快速找到最优或近似最优路径。同时，多无人机之间还需避免相互碰撞，这增加了路径规划的复杂性。
- 动态目标状态估计挑战
：动态目标的运动状态（位置、速度、加速度等）不断变化，且可能受到噪声干扰。准确估计目标状态是实现有效跟踪的基础，但由于目标运动的不确定性和测量噪声的存在，如何实时、精确地估计目标状态成为一大挑战。
三、A * 与 RRT 路径规划算法
（一）A * 算法原理
A算法是一种基于启发式搜索的路径规划算法。它通过维护一个开放列表（存放待扩展节点）和一个封闭列表（存放已扩展节点）来搜索路径。对于每个节点，A算法计算其评估函数 f(n)=g(n)+h(n)，其中 g(n) 是从起点到节点 n 的实际代价，h(n) 是从节点 n 到目标点的估计代价（启发函数）。A * 算法每次从开放列表中选择 f(n) 值最小的节点进行扩展，直到找到目标节点或开放列表为空。其优点是在静态环境中能找到理论上的最优路径，但计算量较大，尤其是在复杂环境下。
（二）RRT 算法原理
RRT 算法是一种基于采样的随机搜索算法，适用于高维空间和复杂环境下的路径规划。它从起点开始，通过在搜索空间中随机采样点，逐步扩展生成一棵树。每次扩展时，选择树中距离采样点最近的节点，向采样点移动一定步长，形成新的节点并加入树中。当新节点到达目标区域或满足一定条件时，搜索结束，通过回溯树得到路径。RRT 算法的优点是能够快速探索复杂环境，找到可行路径，但路径不一定是最优的。
（三）结合方式
在多无人机协同任务中，可以根据任务阶段和环境特点灵活结合 A和 RRT 算法。在搜索阶段，由于环境未知信息较多，优先使用 RRT 算法快速探索环境，找到可行路径。当无人机接近目标区域，环境信息相对清晰时，切换到 A算法，利用其寻找最优路径的特性，使无人机更高效地接近目标。
四、EKF 与 UKF 目标状态估计算法
（一）EKF 算法原理
EKF 是一种用于非线性系统状态估计的常用方法。它基于泰勒级数展开对非线性系统进行线性化近似，将非线性的状态转移方程和观测方程在当前估计状态附近进行一阶泰勒展开，得到线性化的状态转移矩阵和观测矩阵。然后，利用卡尔曼滤波器的框架进行状态估计，通过预测和更新两个步骤，不断修正状态估计值。然而，EKF 的线性化近似可能引入较大误差，特别是当系统非线性较强时。
（二）UKF 算法原理
UKF 则采用了一种基于 Sigma 点的采样策略来处理非线性问题。它通过一组精心选择的 Sigma 点来近似系统状态的概率分布，这些 Sigma 点能够更准确地捕捉非线性系统的统计特性。UKF 同样包括预测和更新步骤，在预测步骤中，将 Sigma 点通过非线性状态转移方程得到预测的 Sigma 点，进而计算预测状态和协方差；在更新步骤中，利用观测值对预测状态进行修正。相比 EKF，UKF 在处理非线性系统时通常具有更高的精度。
（三）应用场景选择
对于非线性程度较低的动态目标运动模型，EKF 由于其计算相对简单，可以满足实时性要求且具有一定的估计精度。而对于非线性较强的目标运动，如高度机动的目标，UKF 能够更准确地估计目标状态，但计算量相对较大。在实际多无人机协同跟踪任务中，可以根据目标的运动特性动态选择 EKF 或 UKF 进行状态估计。
五、基于 A*、RRT 结合 EKF、UKF 的多无人机协同方法
（一）系统架构
- 信息共享模块
：多无人机之间通过无线通信网络构建信息共享模块，实时交换自身位置、速度、已搜索区域信息以及对目标状态的估计等数据。这确保每架无人机都能获取全局信息，为协同决策提供基础。
- 路径规划模块
：该模块集成 A和 RRT 算法。在搜索初期，无人机根据 RRT 算法规划路径，快速探索环境。随着对目标位置的逐渐确定，切换到 A算法优化路径。同时，考虑多无人机之间的避碰约束，确保路径规划的安全性和协同性。
- 目标状态估计模块
：根据目标运动的非线性程度，动态选择 EKF 或 UKF 算法。无人机通过自身携带的传感器（如雷达、摄像头等）获取目标的观测数据，输入到相应的滤波器中进行状态估计。估计结果不仅用于跟踪目标，还反馈给路径规划模块，引导无人机调整飞行路径。
（二）协同策略
- 搜索阶段
：多无人机按照 RRT 算法规划的路径分散搜索。每架无人机在搜索过程中，将自身搜索到的信息（如发现目标的迹象、新的障碍物信息等）实时共享给其他无人机。当某架无人机发现目标后，立即将目标位置信息广播给所有无人机，协同任务进入跟踪阶段。
- 跟踪阶段
：根据目标运动的非线性特性，选择合适的 EKF 或 UKF 算法进行目标状态估计。无人机依据估计的目标状态，通过 A * 算法规划最优跟踪路径。在跟踪过程中，多无人机保持一定的相对位置关系，既保证对目标的有效跟踪，又避免相互碰撞。同时，不断更新目标状态估计，实时调整跟踪路径。
⛳️ 运行结果
📣 部分代码
function plotMissionSummary(cfg, scenario, results)
%PLOTMISSIONSUMMARY Create a polished post-mission analysis figure.
W = cfg.worldSize(1);
H = cfg.worldSize(2);
obstacles = scenario.obstacles;
fig = figure('Name', 'Mission Summary', 'Color', 'w', 'Position', [100 100 1450 820]);
tiledlayout(fig, 2, 2, 'Padding', 'compact', 'TileSpacing', 'compact');
% 1) Final coverage map
nexttile(1);
imagesc([0 W], [0 H], results.finalCoverageMap');
set(gca, 'YDir', 'normal');
hold on;
axis equal tight;
xlim([0 W]); ylim([0 H]);
box on; grid on;
colormap(parula);
colorbar;
title('Final Coverage Heatmap', 'FontWeight', 'bold');
xlabel('X'); ylabel('Y');
for j = 1:size(obstacles, 1)
c = obstacles(j, 1:2);
r = obstacles(j, 3);
rectangle('Position', [c(1)-r, c(2)-r, 2*r, 2*r], 'Curvature', [1 1], ...
'FaceColor', [0.12 0.12 0.12], 'EdgeColor', 'k', 'LineWidth', 1.2);
end
scatter(results.agentPos(:,1,end), results.agentPos(:,2,end), 95, [0 0.45 1], 'filled');
scatter(results.targetPos(:,1,end), results.targetPos(:,2,end), 110, 'r', 'filled', 'd');
% 2) Coverage curve
nexttile(2);
plot(results.time, 100 * results.coverageRatio, 'LineWidth', 2.2);
grid on; box on;
ylim([0 100]);
xlabel('Time (s)'); ylabel('Coverage (%)');
title('Coverage Growth', 'FontWeight', 'bold');
% 3) Tracking quality
nexttile(3);
plot(results.time, results.meanTrackError, 'LineWidth', 2.2);
grid on; box on;
xlabel('Time (s)'); ylabel('Mean tracking error');
title('Kalman Tracking Error', 'FontWeight', 'bold');
% 4) Observation / tracking raster
nexttile(4);
obsMap = double(results.targetObserved);
imagesc(results.time, 1:cfg.numTargets, obsMap);
set(gca, 'YDir', 'normal');
colormap(gca, gray);
colorbar;
caxis([0 1]);
box on;
xlabel('Time (s)'); ylabel('Target index');
title('Observation Timeline (1 = observed)', 'FontWeight', 'bold');
sgtitle({ ...
'MATLAB Project: Multi-UAV Cooperative Search and Dynamic Target Tracking', ...
sprintf('Final coverage = %.1f%% | Tracked targets = %d/%d | Final mean error = %.2f', ...
100 * results.coverageRatio(end), results.numTrackedTargets(end), cfg.numTargets, lastValid(results.meanTrackError))}, ...
'FontWeight', 'bold');
end
function v = lastValid(x)
idx = find(~isnan(x), 1, 'last');
if isempty(idx)
v = NaN;
else
v = x(idx);
end
end
