MATLAB程序：多个无人船 协同围捕控制算法
3船围捕控制，围捕运动船只
可以仿真多个船之间的距离以及距离目标船的距离，特别适合学习、参考
文章目录
以下是一个基于MATLAB的多无人船协同围捕控制算法的代码示例。这个示例实现了一个简单的3艘无人船协同围捕运动船只的场景。代码中包括了无人船的动力学模型、目标船的运动模型以及协同控制算法。
1. 无人船动力学模型
假设每艘无人船都具有简单的二阶动力学模型，包含位置和速度。
function [x_dot, y_dot, vx_dot, vy_dot] = ship_dynamics(x, y, vx, vy, u, v, dt)
% 输入：当前位置 (x, y)，当前速度 (vx, vy)，控制输入 (u, v)，时间步长 dt
% 输出：状态导数
x_dot = vx;
y_dot = vy;
vx_dot = u;
vy_dot = v;
end
2. 目标船运动模型
目标船按照预定义的轨迹运动（例如直线或圆周运动）。
function [xt, yt] = target_motion(t)
% 输入：时间 t
% 输出：目标船的位置 (xt, yt)
radius = 5; % 圆周运动半径
omega = 0.2; % 角速度
xt = radius * cos(omega * t);
yt = radius * sin(omega * t);
end
3. 协同围捕控制算法
使用势场法设计控制律，使无人船保持一定的距离并围捕目标船。
function [u, v] = cooperative_control(x, y, vx, vy, xt, yt, ships_x, ships_y, k_att, k_rep, d_desired)
% 输入：无人船状态 (x, y, vx, vy)，目标船位置 (xt, yt)，其他无人船位置 (ships_x, ships_y)
% 吸引力增益 k_att，排斥力增益 k_rep，期望距离 d_desired
% 输出：控制输入 (u, v)
% 吸引力：朝向目标船
dx_att = xt - x;
dy_att = yt - y;
dist_att = sqrt(dx_att^2 + dy_att^2);
u_att = k_att * dx_att / dist_att;
v_att = k_att * dy_att / dist_att;
% 排斥力：与其他无人船保持一定距离
u_rep = 0;
v_rep = 0;
for i = 1:length(ships_x)
dx_rep = ships_x(i) - x;
dy_rep = ships_y(i) - y;
dist_rep = sqrt(dx_rep^2 + dy_rep^2);
if dist_rep < d_desired
u_rep = u_rep - k_rep * (1/dist_rep - 1/d_desired) * (dx_rep / dist_rep^2);
v_rep = v_rep - k_rep * (1/dist_rep - 1/d_desired) * (dy_rep / dist_rep^2);
end
end
% 总控制输入
u = u_att + u_rep;
v = v_att + v_rep;
end
4. 完整仿真流程
将上述模块整合到一个仿真脚本中。
% 参数初始化
num_ships = 3; % 无人船数量
k_att = 1.0; % 吸引力增益
k_rep = 1.0; % 排斥力增益
d_desired = 2.0; % 期望距离
dt = 0.01; % 时间步长
T = 20; % 总仿真时间
time = 0:dt:T;
% 初始状态
ships_x = [0, 2, -2]; % 无人船初始x坐标
ships_y = [0, 2, -2]; % 无人船初始y坐标
ships_vx = zeros(1, num_ships); % 初始x方向速度
ships_vy = zeros(1, num_ships); % 初始y方向速度
% 存储轨迹
ships_traj_x = zeros(num_ships, length(time));
ships_traj_y = zeros(num_ships, length(time));
% 仿真循环
for i = 1:length(time)
t = time(i);
% 获取目标船位置
[xt, yt] = target_motion(t);
% 更新每艘无人船的状态
for j = 1:num_ships
% 获取其他无人船的位置
other_ships_x = [ships_x(1:j-1), ships_x(j+1:end)];
other_ships_y = [ships_y(1:j-1), ships_y(j+1:end)];
% 计算控制输入
[u, v] = cooperative_control(ships_x(j), ships_y(j), ships_vx(j), ships_vy(j), ...
xt, yt, other_ships_x, other_ships_y, k_att, k_rep, d_desired);
% 更新状态
[x_dot, y_dot, vx_dot, vy_dot] = ship_dynamics(ships_x(j), ships_y(j), ships_vx(j), ships_vy(j), u, v, dt);
ships_x(j) = ships_x(j) + x_dot * dt;
ships_y(j) = ships_y(j) + y_dot * dt;
ships_vx(j) = ships_vx(j) + vx_dot * dt;
ships_vy(j) = ships_vy(j) + vy_dot * dt;
% 保存轨迹
ships_traj_x(j, i) = ships_x(j);
ships_traj_y(j, i) = ships_y(j);
end
end
% 绘图
figure;
hold on;
for j = 1:num_ships
plot(ships_traj_x(j, :), ships_traj_y(j, :), 'LineWidth', 1.5);
end
plot(xt, yt, 'r--', 'LineWidth', 1.5); % 目标船轨迹
xlabel('X Position');
ylabel('Y Position');
title('Multi-Ship Cooperative Pursuit');
legend('Ship 1', 'Ship 2', 'Ship 3', 'Target Ship');
grid on;
axis equal;
代码说明
- 无人船动力学模型：描述了无人船的运动特性。
- 目标船运动模型：目标船沿圆周运动，作为围捕对象。
- 协同控制算法：通过吸引力和排斥力的结合，实现无人船之间的协同围捕。
- 仿真结果：绘制了无人船的轨迹和目标船的轨迹，展示了围捕过程。
运行效果
运行上述代码后，您将看到3艘无人船协同围捕目标船的过程。无人船会逐渐靠近目标船，并保持一定的队形。您可以调整参数（如k_att、k_rep、d_desired等）来优化围捕效果。
我们可以看到三艘无人艇与敌方船只的距离随时间变化的情况。为了模拟这一过程，我们可以编写一个MATLAB程序来生成类似的图表。以下是一个示例代码，用于模拟和绘制三艘无人艇与敌方船只的距离变化。
MATLAB 代码
% 参数初始化
num_ships = 3; % 无人船数量
time_steps = 100; % 时间步数
dt = 1; % 时间步长 (单位: 秒)
% 初始化距离数据
distances = zeros(time_steps, num_ships);
% 模拟距离变化
for t = 1:time_steps
for i = 1:num_ships
if t == 1
distances(t, i) = 12000 - 500 * i; % 初始距离
else
distances(t, i) = distances(t-1, i) - 100; % 假设每秒减少100cm
end
end
end
% 绘制图表
figure;
plot(distances);
xlabel('Time (seconds)');
ylabel('Distance (cm)');
title('各无人艇距离敌方船只距离');
legend('无人艇1', '无人艇2', '无人艇3');
grid on;
% 添加图例
legend('无人艇1', '无人艇2', '无人艇3', 'Location', 'best');
代码说明
-
参数初始化：
num_ships：无人船的数量。time_steps：模拟的时间步数。dt：时间步长（单位：秒）。
-
初始化距离数据：
distances：存储每一时间步中各无人艇与敌方船只的距离。
-
模拟距离变化：
- 使用嵌套循环模拟每一时间步中各无人艇与敌方船只的距离变化。假设初始距离分别为12000cm、11500cm和11000cm，并且每秒减少100cm。
-
绘制图表：
- 使用
plot函数绘制距离随时间的变化曲线。 - 添加
xlabel、ylabel和title来标注图表。 - 使用
legend添加图例。 - 使用
grid on添加网格线。
- 使用
运行结果
运行上述代码后，您将看到一个类似于您提供的图表的图形，显示了三艘无人艇与敌方船只的距离随时间的变化情况。
为了模拟和绘制相邻无人艇之间的距离变化，我们可以编写一个MATLAB程序来生成类似的图表。以下是一个示例代码，用于模拟三艘无人艇之间的距离变化，并绘制这些距离随时间的变化。
MATLAB 代码
% 参数初始化
num_ships = 3; % 无人船数量
time_steps = 100; % 时间步数
dt = 1; % 时间步长 (单位: 秒)
% 初始化位置数据
positions = zeros(time_steps, num_ships, 2); % 存储每一时间步中各无人艇的位置
distances = zeros(time_steps, 3); % 存储每一时间步中相邻无人艇的距离
% 模拟初始位置
positions(1, :, :) = [1000, 1000; 2000, 2000; 3000, 3000]; % 初始位置
% 模拟位置变化
for t = 2:time_steps
for i = 1:num_ships
positions(t, i, :) = positions(t-1, i, :) + randn(1, 2) * 50; % 随机移动
end
% 计算相邻无人艇之间的距离
distances(t, 1) = norm(positions(t, 1, :) - positions(t, 2, :));
distances(t, 2) = norm(positions(t, 1, :) - positions(t, 3, :));
distances(t, 3) = norm(positions(t, 2, :) - positions(t, 3, :));
end
% 绘制图表
figure;
plot(distances);
xlabel('Time (seconds)');
ylabel('Distance (cm)');
title('相邻无人艇之间的距离');
legend('无人艇1与无人艇2之间距离', '无人艇1与无人艇3之间距离', '无人艇2与无人艇3之间距离');
grid on;
% 添加图例
legend('无人艇1与无人艇2之间距离', '无人艇1与无人艇3之间距离', '无人艇2与无人艇3之间距离', 'Location', 'best');
代码说明
-
参数初始化：
num_ships：无人船的数量。time_steps：模拟的时间步数。dt：时间步长（单位：秒）。
-
初始化位置数据：
positions：存储每一时间步中各无人艇的位置。distances：存储每一时间步中相邻无人艇的距离。
-
模拟初始位置：
- 设置初始位置为
[1000, 1000],[2000, 2000],[3000, 3000]。
- 设置初始位置为
-
模拟位置变化：
- 使用嵌套循环模拟每一时间步中各无人艇的位置变化。假设每次移动是随机的，且最大移动距离为50cm。
- 计算相邻无人艇之间的距离。
-
绘制图表：
- 使用
plot函数绘制距离随时间的变化曲线。 - 添加
xlabel、ylabel和title来标注图表。 - 使用
legend添加图例。 - 使用
grid on添加网格线。
- 使用
运行结果
运行上述代码后，您将看到一个类似于您提供的图表的图形，显示了三艘无人艇之间的距离随时间的变化情况。
扫一扫
607
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
