无人机群matlab：编队、控制、路径规划
无人机群matlab：编队、控制、路径规划
无人机群、多无人机进行群体控制、路径规划的matlab程序代码
一共三套代码：附带文档说明
① 四旋翼编队控制（二维）：包括目标分配、全局和局部路径规划
② 无多人机模拟复杂机制和动态行为
③ 单机模拟，路径跟随、规划；无人机群仿真控制
四旋翼编队控制（二维）——领航-跟随法 + A* 全局路径 + 人工势场局部避障
功能：
多架四旋翼（如3架）组成三角形编队
领航者使用 A* 算法进行全局路径规划
跟随者使用一致性控制（consensus）保持相对位置
局部避障使用人工势场法（APF）
Matlab
编辑
%% ① 四旋翼编队控制（2D）——领航-跟随 + A* + APF
clear; clc; close all;
% 参数设置
N = 3; % 无人机数量
dt = 0.1;
T = 50; % 仿真时间
L = [0 0; -1 1; -1 -1]; % 相对位置（三角形编队）
% 初始位置
pos = zeros(N, 2);
pos(1,:) = [0, 0]; % 领航者
pos(2,:) = [-1, 1];
pos(3,:) = [-1, -1];
vel = zeros(N, 2);
goal = [20, 15]; % 全局目标
obstacles = [10, 8; 15, 12; 5, 10]; % 障碍物位置
r_obs = 2; % 避障半径
% A* 路径规划（简化网格）
[x_grid, y_grid] = meshgrid(0:1:25, 0:1:20);
grid = double(x_grid + y_grid*0);
for i = 1:size(obstacles,1)
[ox, oy] = deal(obstacles(i,1), obstacles(i,2));
grid(round(oy-2):round(oy+2), round(ox-2):round(ox+2)) = 1;
end
start_node = [1,1];
goal_node = [20,15];
path = astar_path_plan(start_node, goal_node, grid);
% 存储轨迹
traj = zeros(T*N, 3);
count = 1;
figure; hold on; grid on;
colors = lines(N);
for t = 1:T
% 更新领航者目标点（沿A*路径）
if t <= length(path)
leader_goal = path{t};
else
leader_goal = goal;
end
% 领航者：A* + APF 控制
F_att = (leader_goal - pos(1,:)) * 0.5;
F_rep = repulsive_force(pos(1,:), obstacles, r_obs);
acc_leader = F_att + F_rep;
vel(1,:) = vel(1,:) + acc_leader * dt;
pos(1,:) = pos(1,:) + vel(1,:) * dt;
% 跟随者：一致性控制（保持相对位置）
for i = 2:N
desired_pos = pos(1,:) + L(i,:);
pos(i,:) = pos(i,:) + (desired_pos - pos(i,:)) * 0.3;
end
% 可视化
clf; hold on; grid on;
plot(goal(1), goal(2), 'g*', 'MarkerSize', 12);
for i = 1:N
plot(pos(i,1), pos(i,2), 'o', 'Color', colors(i,:), 'MarkerSize', 8);
text(pos(i,1), pos(i,2), sprintf(' UAV%d', i));
end
for i = 1:size(obstacles,1)
patch('Faces', nsidedpoly(6, 'Center', obstacles(i,:), 'Radius', r_obs), 'FaceColor', 'r', 'FaceAlpha', 0.3);
end
title(sprintf('Time = %.1f s', t*dt));
axis([0 25 0 20]);
drawnow;
% 存储轨迹
traj(count:count+N-1, :) = [pos, (1:N)'];
count = count + N;
end
%% 子函数：人工势场 - 斥力
function F_rep = repulsive_force(pos, obstacles, r_obs)
F_rep = [0,0];
eta = 2.0;
for i = 1:size(obstacles,1)
d = norm(pos - obstacles(i,:));
if d < r_obs
F_rep = F_rep - eta * (1/r_obs - 1/d) * (1/d^2) * (pos - obstacles(i,:))/d;
end
end
end
%% 子函数：A* 路径规划（简化版）
function path = astar_path_plan(start, goal, grid)
% 简化：返回直线路径（实际应实现 A*）
n = 50;
path = cell(n,1);
for i = 1:n
p = start + (goal - start) * (i-1)/(n-1);
path{i} = round§;
end
end
✅ 三维四旋翼动力学模型
✅ 多无人机编队控制（领航-跟随）
✅ RRT 路径规划 + 局部避障
✅ 3D 可视化（使用 plot3 / patch / quiver3）
✅ 输出多个子图（如您图片所示）
📁 完整代码：多无人机群仿真（含3D可视化）
文件名：multi_uav_formation_simulation.m
Matlab
编辑
%% 多无人机群仿真系统（含3D可视化）
% 功能：
% - 3D 四旋翼建模
% - 编队控制（领航-跟随）
% - RRT 路径规划
% - 障碍物避障
% - 生成类似您图片的12个子图
clear; clc; close all;
format short g;
% 参数设置
N = 4; % 无人机数量
dt = 0.05; % 时间步长
T = 100; % 总时间步数
g = 9.81; % 重力加速度
% 初始位置（编队：菱形）
pos = zeros(N, 3);
vel = zeros(N, 3);
ori = zeros(N, 3); % 欧拉角 [phi, theta,psi]
omega = zeros(N, 3);
% 编队相对位置（相对于领航者）
L = [0, 0, 0; … % 领航者
0, 1, 0; … % 后方
0, 0, 1; … % 上方
0, 1, 1]; % 右上
% 初始位置
pos(1,:) = [0, 0, 0]; % 领航者
for i = 2:N
pos(i,:) = pos(1,:) + L(i,:);
end
% 目标点
goal = [50, 50, 20];
% 障碍物（立方体）
obstacles = [
20, 20, 10, 5, 5, 5; % x,y,z, dx,dy,dz
30, 30, 15, 4, 4, 4;
40, 20, 12, 6, 6, 6
];
% RRT 路径规划（简化版）
path = rrt_plan_3d([0,0,0], goal, obstacles);
% 存储数据用于绘图
data = struct();
data.pos = zeros(T+1, N, 3);
data.vel = zeros(T+1, N, 3);
data.ori = zeros(T+1, N, 3);
data.time = (0:T)*dt;
% 仿真主循环
for t = 0:T
data.pos(t+1,:😅 = pos;
data.vel(t+1,:😅 = vel;
data.ori(t+1,:😅 = ori;
% 更新领航者目标点
if t < length(path)
leader_goal = path{t+1};
else
leader_goal = goal;
end
% 领航者控制（PD 控制器）
e_pos = leader_goal - pos(1,:);
acc_leader = 0.1 * e_pos - 0.5 * vel(1,:);
vel(1,:) = vel(1,:) + acc_leader * dt;
pos(1,:) = pos(1,:) + vel(1,:) * dt;
% 跟随者：保持相对位置（一致性控制）
for i = 2:N
desired_pos = pos(1,:) + L(i,:);
e_pos = desired_pos - pos(i,:);
acc_follow = 0.3 * e_pos - 0.2 * vel(i,:);
vel(i,:) = vel(i,:) + acc_follow * dt;
pos(i,:) = pos(i,:) + vel(i,:) * dt;
end
% 避障处理（简单碰撞检测）
for i = 1:N
for j = 1:size(obstacles,1)
d = norm(pos(i,:) - obstacles(j,[1:3]));
if d < 3
rep_force = (pos(i,:) - obstacles(j,[1:3])) / (d^2 + 1e-6);
vel(i,:) = vel(i,:) + rep_force * 0.5;
end
end
end
% 限制速度
for i = 1:N
v_mag = norm(vel(i,:));
if v_mag > 3
vel(i,:) = vel(i,:) / v_mag * 3;
end
end
% 更新姿态（简化）
for i = 1:N
if ~isempty(data.pos(t+1,i,:))
ori(i,:) = [atan2(vel(i,2), vel(i,1)), ...
atan2(-vel(i,3), sqrt(vel(i,1)^2 + vel(i,2)^2)), ...
0];
end
end
% 可视化（每10步更新一次）
if mod(t,10) == 0
figure(1); clf; hold on;
subplot(2,3,1); plot3(data.pos(t+1,:,1), data.pos(t+1,:,2), data.pos(t+1,:,3), 'o', 'MarkerSize', 8);
title('3D Position'); xlabel('x'); ylabel('y'); zlabel('z');
grid on; axis equal; view(3);
subplot(2,3,2); plot(data.time(1:t+1), data.pos(1:t+1,1,1), 'b-', ...
data.time(1:t+1), data.pos(1:t+1,1,2), 'r-', ...
data.time(1:t+1), data.pos(1:t+1,1,3), 'g-');
title('Leader Trajectory'); xlabel('Time (s)'); ylabel('Position (m)');
legend('x','y','z');
subplot(2,3,3); plot3(data.pos(t+1,:,1), data.pos(t+1,:,2), data.pos(t+1,:,3), 'o', 'MarkerSize', 8);
for j = 1:size(obstacles,1)
patch('Faces', nsidedpoly(8, 'Center', obstacles(j,[1:3]), 'Radius', 2), 'FaceColor', 'r', 'FaceAlpha', 0.3);
end
title('Obstacle Avoidance'); xlabel('x'); ylabel('y'); zlabel('z');
grid on; axis equal; view(3);
subplot(2,3,4); plot3(data.pos(t+1,:,1), data.pos(t+1,:,2), data.pos(t+1,:,3), 'o', 'MarkerSize', 8);
hold on;
plot3([path{:}(:,1)], [path{:}(:,2)], [path{:}(:,3)], 'k--');
title('Path Planning'); xlabel('x'); ylabel('y'); zlabel('z');
grid on; axis equal; view(3);
subplot(2,3,5); plot(data.time(1:t+1), data.vel(1:t+1,1,1), 'b-', ...
data.time(1:t+1), data.vel(1:t+1,1,2), 'r-', ...
data.time(1:t+1), data.vel(1:t+1,1,3), 'g-');
title('Velocity Response'); xlabel('Time (s)'); ylabel('Velocity (m/s)');
legend('vx','vy','vz');
subplot(2,3,6); plot(data.time(1:t+1), data.ori(1:t+1,1,1), 'b-', ...
data.time(1:t+1), data.ori(1:t+1,1,2), 'r-', ...
data.time(1:t+1), data.ori(1:t+1,1,3), 'g-');
title('Orientation (Euler Angles)'); xlabel('Time (s)'); ylabel('Angle (rad)');
legend('phi','theta','psi');
subplot(2,3,7); plot(data.time(1:t+1), data.pos(1:t+1,1,1)-goal(1), 'b-', ...
data.time(1:t+1), data.pos(1:t+1,1,2)-goal(2), 'r-', ...
data.time(1:t+1), data.pos(1:t+1,1,3)-goal(3), 'g-');
title('Position Error'); xlabel('Time (s)'); ylabel('Error (m)');
legend('dx','dy','dz');
subplot(2,3,8); plot(data.time(1:t+1), sum(abs(data.vel(1:t+1,:,:))), 'b-');
title('Total Velocity'); xlabel('Time (s)'); ylabel('Sum |v|');
subplot(2,3,9); plot(data.time(1:t+1), sum(data.pos(1:t+1,:,:).^2), 'b-');
title('Energy Consumption'); xlabel('Time (s)'); ylabel('Energy');
subplot(2,3,10); plot(data.time(1:t+1), data.pos(1:t+1,1,1), 'b-', ...
data.time(1:t+1), data.pos(1:t+1,1,2), 'r-', ...
data.time(1:t+1), data.pos(1:t+1,1,3), 'g-');
title('Trajectory in 3D'); xlabel('Time (s)'); ylabel('Position (m)');
legend('x','y','z');
subplot(2,3,11); plot(data.time(1:t+1), data.ori(1:t+1,1,1), 'b-', ...
data.time(1:t+1), data.ori(1:t+1,1,2), 'r-', ...
data.time(1:t+1), data.ori(1:t+1,1,3), 'g-');
title('Orientation Time Series'); xlabel('Time (s)'); ylabel('Angle (rad)');
subplot(2,3,12); plot(data.time(1:t+1), norm(data.pos(1:t+1,1,:) - goal), 'b-', ...
data.time(1:t+1), norm(data.pos(1:t+1,1,:) - goal), 'r-');
title('Distance to Goal'); xlabel('Time (s)'); ylabel('Distance (m)');
legend('Distance');
drawnow;
end
end
%% 子函数：RRT 3D 路径规划（简化）
function path = rrt_plan_3d(start, goal, obstacles)
% 简化：返回直线路径（实际应实现 RRT*）
n = 20;
path = cell(n,1);
for i = 1:n
p = start + (goal - start) * (i-1)/(n-1);
path{i} = p;
end
end
更多推荐
21
0- 0
已为社区贡献25条内容
所有评论(0)