%%-------------------------------------------------------------------------
% 作者: 赵敏琨
% 学号: 2018302068
% 时间: 2021年12月
%%-------------------------------------------------------------------------
clc
clear
close all
%% 初始化
% 两机状态变量
x1 = [1000 1000 20]; % x, y, v
x2 = [0 0 20];
X(:, :, 1) = [x1' x2'];
dis = norm([1000 1000]); % 两机距离
btheta = 45/180*pi; % 两机角度
v_r = 180 / 3.6; % 初始期望参考速度
% 航迹状态量
xw1 = [90/180*pi 0]; % theta, w
xw2 = xw1;
W(:, :, 1) = [xw1' xw2'];
% 两机控制变量
a1 = [0 0]; a2 = [0 0]; % alon, alat
U(:, :, 1) = [a1' a2'];
rx = 1400:300:8600;
ry = tand(90-75) * (rx-1000) + 1000;
rp = [rx' ry']; % 领航者L1参考点
rp_f = [1000 1000]; % 跟随者L1参考点初始化
drp_f = 600; % 跟随者L1导航点间隔
jj = 1;
dt = 0.01; % 采样时间
kw = 2; % 角速度控制比例系数
kv = 0.3; % 速度控制比例系数
kp = 0.03; % 位置控制比例系数
t = 0:dt:150;
%% 实时解算
figure
hold on
n = length(t);
plot(rp(:,1), rp(:,2), 'kx', 'MarkerSize', 6, 'LineWidth', 1)
for k = 2:n
l1d_l = norm(X(1:2, 1, k-1)-rp(jj, :)'); % 领航者L1距离
dtheta_l = atan((X(2, 1, k-1)-rp(jj, 2)) / (X(1, 1, k-1)-rp(jj, 1))); % 目标点与领航者间的方向角
eta_l = dtheta_l - W(1, 1, k-1);
if (rp(jj,1)-X(1, 1, k-1)) < 0
dtheta_l = dtheta_l + pi;
elseif (rp(jj,2)-X(2, 1, k-1)) < 0
dtheta_l = dtheta_l + pi*2;
end
% 计算跟随者的L1参考点
rx_f = X(1, 1, k-1) - 500*cosd(30)*cos(W(1, 1, k-1)) - 500*sind(30)*cosd(60);
ry_f = X(2, 1, k-1) - 500*cosd(30)*sin(W(1, 1, k-1)) + 500*sind(30)*sind(60);
if mod(k+1, drp_f) == 0
rp_f = [rp_f; rx_f ry_f];
end
l1d_f = norm(X(1:2, 2, k-1)-rp_f(end, :)'); % 跟随者L1距离
dtheta_f = atan((X(2, 2, k-1)-rp_f(end, 2)) / (X(1, 2, k-1)-rp_f(end, 1))); % 目标点与跟随者间的方向角
eta_f = dtheta_f - W(1, 2, k-1);
if (rp_f(end,1)-X(1, 2, k-1)) < 0
dtheta_f = dtheta_f + pi;
elseif (rp_f(end,2)-X(2, 2, k-1)) < 0
dtheta_f = dtheta_f + pi*2;
end
e_V = X(3, 1, k-1) - v_r;
U(1, 1, k) = -kv * e_V; % 纵向加速度
U(2, 1, k) = 2 * X(3, 1, k-1)^2 / l1d_l * sin(eta_l); % 横向加速度
W(2, 1, k) = kw * U(2, 1, k) / X(3, 1, k-1); % 更新角速度
W(1, 1, k) = W(1, 1, k-1) + W(2, 1, k) * dt; % 更新航向角
e_P = (X(1, 2, k-1) - rp_f(end, 1))/cos(W(1, 2, k-1));
e_D = dis(k-1, 1)*cos(btheta(k-1, 1)) - 500*cosd(30);
% 距离指领航者yoz平面与跟随者的距离
if abs(e_D) > 100 % 距离>100m则轨迹跟踪,反馈速度控制
e_V = X(3, 2, k-1) - (X(3, 1, k-1) + 40/3.6);
U(1, 2, k) = -kv * e_V; % 纵向加速度
U(2, 2, k) = 2 * X(3, 2, k-1)^2 / l1d_f * sin(eta_f); % 横向加速度
W(2, 2, k) = kw * U(2, 2, k) / X(3, 2, k-1); % 更新角速度
W(1, 2, k) = W(1, 2, k-1) + W(2, 2, k) * dt; % 更新航向角
else % 否则速度减小,反馈速度与距离控制
e_V = X(3, 2, k-1) - X(3, 1, k-1);
U(1, 2, k) = -kv * e_V + kp * e_D; % 纵向加速度
U(2, 2, k) = U(2, 2, k); % 横向加速度
W(2, 2, k) = W(2, 2, k); % 更新角速度
W(1, 2, k) = W(1, 2, k-1) + W(2, 2, k) * dt; % 更新航向角
end
for m = 1:2
Phi_dp = [0 0 dt*cos(W(1, m, k))
0 0 dt*sin(W(1, m, k))
0 0 0 ];
Phi = eye(3) + Phi_dp;
Gam(:, :, m) = [0 0; 0 0; dt 0];
% 状态方程
% [x [1 0 dt*cos(theta) [x [0 0 [alon
% y = 0 1 dt*sin(theta) * y + 0 0 * alat]
% v](k) 0 0 1 ] v](k-1) dt 0]
X(:, m, k) = Phi * X(:, m, k-1) + Gam(:, :, m) * U(:, m, k-1);
end
x1(k, :) = X(:, 1, k)'; x2(k, :) = X(:, 2, k)';
a1(k, :) = U(:, 1, k)'; a2(k, :) = U(:, 2, k)';
xw1(k, :) = W(:, 1, k)'; xw2(k, :) = W(:, 2, k)';
dis(k, 1) = norm(x1(k, :)-x2(k, :));
btheta(k, 1) = -atan2(x1(k,2)-x2(k,2),x1(k,1)-x2(k,1)) + W(1, 1, k);
% 实时画图
% plot(X(1, 1, k), X(2, 1, k), 'r.')
% plot(X(1, 2, k), X(2, 2, k), 'b.')
% if mod(k+1, drp_f) == 0
% plot(rp_f(end, 1), rp_f(end, 2), 'gx', 'LineWidth', 1)
% end
% pause(0.001)
if l1d_l < 1 % 更新目标点
if jj < length(rp)
jj = jj + 1;
else
% break % 到达最后目标点后退出
end
end
end
% xlabel('x/m','fontsize',12); ylabel('y/m','fontsize',12);
% legend({'领航者L1导航点', '领航者1', '跟随者2', '跟随者L1导航点'},'Location','best');
%% 根据记录数据画图
plot(x1(:, 1), x1(:, 2), 'r-', 'LineWidth', 1)
plot(x2(:, 1), x2(:, 2), 'b-', 'LineWidth', 1)
plot(rp_f(:, 1), rp_f(:, 2), 'gx', 'LineWidth', 1)
grid on
xlabel('x/m','fontsize',12); ylabel('y/m','fontsize',12);
title('轨迹曲线')
legend({'领航者L1导航点', '领航者1', '跟随者2', '跟随者L1导航点'},'Location','best')
figure
hold on
plot(t, rad2deg(xw1(:, 1)), 'LineWidth', 1)
plot(t, rad2deg(xw1(:, 2)), 'LineWidth', 1)
grid on
xlabel('t/s','fontsize',12); ylabel('\theta/deg \omega/deg/s','fontsize',12);
title('领航者航向角/角速度曲线')
legend({'航向角', '角速度'},'Location','best')
figure
subplot(2,2,1)
hold on
plot(t, x1(:, 3), 'LineWidth', 1)
plot(t, x2(:, 3), 'LineWidth', 1)
grid on
xlabel('t/s','fontsize',12); ylabel('v/m/s','fontsize',12);
title('速度曲线')
legend({'领航者1', '跟随者2'},'Location','best')
subplot(2,2,2)
hold on
plot(t, rad2deg(xw2(:, 1)), 'LineWidth', 1)
plot(t, rad2deg(xw2(:, 2)), 'LineWidth', 1)
grid on
xlabel('t/s','fontsize',12); ylabel('\theta/deg \omega/deg/s','fontsize',12);
title('跟随者航向角/角速度曲线')
legend({'航向角', '角速度'},'Location','best')
subplot(2,2,3)
plot(t, dis, 'LineWidth', 1)
grid on
xlabel('t/s','fontsize',12); ylabel('d/m','fontsize',12);
title('领航者跟随者距离曲线')
subplot(2,2,4)
plot(t, rad2deg(btheta), 'LineWidth', 1)
grid on
xlabel('t/s','fontsize',12); ylabel('btheta/deg','fontsize',12);
title('领航者跟随者角度曲线')
没有合适的资源?快使用搜索试试~ 我知道了~
多无人机协同控制技术的MATLAB代码.zip
共25个文件
m:20个
png:4个
mat:1个
1.该资源内容由用户上传,如若侵权请联系客服进行举报
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
2.虚拟产品一经售出概不退款(资源遇到问题,请及时私信上传者)
版权申诉
5星 · 超过95%的资源 2 下载量 132 浏览量
2023-04-05
13:21:58
上传
评论 3
收藏 78KB ZIP 举报
温馨提示
1.版本:matlab2014/2019a,内含运行结果,不会运行可私信 2.领域:智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真,更多内容可点击博主头像 3.内容:标题所示,对于介绍可点击主页搜索博客 4.适合人群:本科,硕士等教研学习使用 5.博客介绍:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可si信
资源推荐
资源详情
资源评论
收起资源包目录
多无人机协同控制技术的MATLAB代码.zip (25个子文件)
多无人机协同控制技术的MATLAB代码
领航固定翼撒L1点.png 22KB
eg_task3.m 1KB
L1切换策略.png 10KB
Homework_2_3.m 3KB
eg_task4.m 4KB
eg_task2.m 6KB
eg_task1.m 5KB
Homework_2_3_v.m 2KB
旋翼四机.png 25KB
anglemat.m 496B
Homework_2_1.m 4KB
my_VTH
picture1.png 1KB
caculate_abs.m 144B
get_map.m 338B
caculate_beta.m 408B
plotHistogram.m 766B
projected_dir.m 2KB
obstacle.mat 6KB
VFH.m 4KB
primary_dir.m 2KB
plotBinHistogram.m 624B
distancemat.m 383B
Homework_2_2.m 5KB
saturate.m 145B
Homework_2_4.m 6KB
共 25 条
- 1
资源评论
- dasixingge2024-07-09资源有一定的参考价值,与资源描述一致,很实用,能够借鉴的部分挺多的,值得下载。
- Akina.SoriNa2024-01-02发现一个超赞的资源,赶紧学习起来,大家一起进步,支持!
天天Matlab科研工作室
- 粉丝: 4w+
- 资源: 1万+
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功