✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、背景
（一）水下多智能体编队的应用需求
在海洋探索、水下监测、海洋资源开发等领域，水下多智能体编队技术具有重要应用价值。例如，在海洋科考中，多艘水下机器人编队协作，可同时从不同位置对海洋环境参数进行测量，提高数据采集的全面性和准确性；在水下安防监控中，多智能体编队能够实现对特定海域的全方位巡逻，及时发现潜在威胁。通过合理的编队和轨迹跟踪控制，水下多智能体系统能够高效、协同地完成复杂任务。
（二）轨迹跟踪面临的挑战
- 复杂的水下环境：水下环境复杂多变，存在水流、水压、水温等多种干扰因素。水流的不确定性会对水下智能体的运动产生显著影响，使其实际运动轨迹偏离预期轨迹。例如，强水流可能导致智能体漂移，增加轨迹跟踪的难度。
- 传感器噪声：用于获取水下智能体位置、速度等状态信息的传感器，如声学定位系统、惯性测量单元等，会受到噪声的干扰。噪声会使测量数据存在误差，若不进行有效处理，这些误差会累积，严重影响轨迹跟踪的精度。
- 多智能体间的协同问题：多个水下智能体需要协同工作，保持特定的编队形式并跟踪预定轨迹。这要求各智能体之间能够准确地交换信息，协调彼此的运动。然而，水下通信存在信号衰减、延迟等问题，给多智能体间的协同带来挑战。
（三）无迹卡尔曼滤波的优势
无迹卡尔曼滤波（Unscented Kalman Filter，UKF）是一种非线性滤波算法，相较于传统的扩展卡尔曼滤波（EKF），它在处理非线性系统时具有更高的精度和稳定性。UKF 通过确定性采样策略，利用一组 Sigma 点来近似系统状态的概率分布，能够更准确地捕捉非线性系统的特性。在水下多智能体编队轨迹跟踪中，智能体的运动模型往往是非线性的，UKF 能够有效处理这种非线性问题，同时对传感器噪声具有较好的鲁棒性，因此适用于水下多智能体编队的轨迹跟踪。
二、原理
（一）水下多智能体运动模型
（三）基于 UKF 的水下多智能体编队轨迹跟踪实现
- 状态估计：对于每个水下智能体，利用 UKF 对其状态进行估计。将智能体的运动模型作为系统模型，传感器测量值作为观测值。通过 UKF 的时间更新和测量更新步骤，不断估计智能体的位置、速度等状态信息，以补偿传感器噪声和环境干扰带来的误差。
- 编队控制：根据 UKF 估计得到的各智能体状态，结合多智能体编队模型，设计编队控制算法。例如，可以采用基于行为的控制方法，根据智能体之间的相对位置误差和预定轨迹，计算每个智能体的控制输入 u，使智能体朝着保持编队形式和跟踪轨迹的方向运动。同时，各智能体之间通过水下通信进行信息交互，以实现协同控制。
- 实时调整：在实际运行过程中，随着水下环境的变化和传感器测量的更新，UKF 不断对智能体状态进行重新估计，编队控制算法根据新的状态估计实时调整智能体的运动，确保多智能体编队始终保持稳定，并准确跟踪预定轨迹。
通过基于无迹卡尔曼滤波的方法，能够有效处理水下多智能体编队轨迹跟踪中面临的非线性系统和噪声干扰问题，实现高精度的轨迹跟踪和稳定的编队控制，为水下多智能体系统在各种海洋任务中的应用提供有力支持。
⛳️ 运行结果
📣 部分代码
figure;
hold on;
grid on;
xlim([0 5500]); % 只显示X轴1000到5500的区域
ylim([1000 12000]); % 手动设置Y轴，覆盖整个飞行高度
pbaspect([1 1 1]); % 保持x/y/z比例为1:1:1
axis square; % 保持绘图区是正方形
grid on;
xlabel('X (m)');
ylabel('Y (m)');
zlabel('Z (m)');
title('UAV Flight Trajectories (Step-by-step)');
view(2)
% 获取轨迹长度
T = size(RedUAV{1}.state, 2);
% 初始化 plot 句柄
red_lines = gobjects(length(RedUAV), 1);
blue_lines = gobjects(length(BlueUAV), 1);
for i = 1:length(RedUAV)
red_lines(i) = plot3(NaN, NaN, NaN, 'r-', 'LineWidth', 2);
end
for i = 1:length(BlueUAV)
blue_lines(i) = plot3(NaN, NaN, NaN, 'b--', 'LineWidth', 2);
end
legend({'Red UAVs', 'Blue UAVs'}, 'Location', 'best');
% 动画循环
for t = 1:T
for i = 1:length(RedUAV)
traj = RedUAV{i}.state(1:3, 1:t);
set(red_lines(i), 'XData', traj(1,:), 'YData', traj(2,:), 'ZData', traj(3,:));
end
for i = 1:length(BlueUAV)
traj = BlueUAV{i}.state(1:3, 1:t);
set(blue_lines(i), 'XData', traj(1,:), 'YData', traj(2,:), 'ZData', traj(3,:));
end
pause(0.05); % 控制播放速度，越大越慢
end
% 绘制最终起止点
for i = 1:length(RedUAV)
traj = RedUAV{i}.state(1:3, :);
scatter3(traj(1,1), traj(2,1), traj(3,1), 60, 'g', 'filled');
scatter3(traj(1,end), traj(2,end), traj(3,end), 60, 'r', 'filled');
end
for i = 1:length(BlueUAV)
traj = BlueUAV{i}.state(1:3, :);
scatter3(traj(1,1), traj(2,1), traj(3,1), 60, 'g', 'filled');
scatter3(traj(1,end), traj(2,end), traj(3,end), 60, 'r', 'filled');
end
🔗 参考文献
[1]刘斌.粒子滤波理论及其在水下目标跟踪问题中的应用[J].声学所博硕士学位论文, 2009.
