1 简介
本文提出一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法.通过引入与障碍物移动方向垂直的辅助牵引加速度信息,可消除局部优化解的影响.同时,为了实现无人机机群快速编队和避障,本文将有限时间一致性算法与人工势场方法相结合,提出一种编队及其避障控制方法.
2 部分代码
clc
clear all
close all
%%%%一阶算法仿真 非完整型约束
%%% 设置好重心偏移量和gama，权重，基本可行
%% 初始化 位置pose、速度V、加速度控制量control
init_f=[-3 -6 0; %%%[x y th]
-5 6 0;
2 4 pi/4;
5 -3 -pi/4;
3 0 pi/2];
pose_x=init_f(:,1);
pose_y=init_f(:,2);
pose_th=init_f(:,3);
%% follower相对leader的位置
delta_x=[-2 -6 -2 -6 0]; % 相对间隔误差
delta_y=[4 4 -4 -4 0]; %领航者与自己无误差
fol_num=4;
N=5; % 4follower and 1 leader
countmax=2000;
dt=0.1;
gama=3;
di=0.02; %%%重心偏移 单位m
K0=0;
%%% 直线K1 K2都设在0.2左右 k3=0
%%% 圆不考虑K2=0 K1，K3设为0.2左右
K1=1;%%%位置偏差线速度调节
K2=1;%%位置偏差角速度调节
K3=0.01;%%朝向偏差角速度调节
% %% 通信拓扑图:1-4行为follower 最后一行为leader
A=[0 1 1 1 1; % a(ij)
0 0 0 0 1;
0 0 0 1 1;
0 0 1 0 1;
0 0 0 0 0];
% %% 通信拓扑图:1-4行为follower 最后一行为leader
linear_v(:,1)=[0;0;0;0;1];
angular_w(:,1)=[0;0;0;0;1];
k=0;
% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss]]
Kinematic=[1.5,toRadian(60.0),0.5,toRadian(50.0)];%% 运动限制
error_temp(1:fol_num,1:4)=0;%%行为follower个数、列为两个时刻的x,y偏差
in_degree=sum(A,2);
%% 开始循环 走顺时针圆周
% figure;
for count=1:countmax
k=k+1;
linear_v(N,k+1)=linear_v(N,k);%领航者速度不变
angular_w(N,k+1)=angular_w(N,k);
% if count==500
% linear_v(N,k+1)=0.5;%领航者速度不变
% angular_w(N,k+1)=0.5;
% end
% if count==1000
% linear_v(N,k+1)=0.5;%领航者速度不变
% angular_w(N,k+1)=0;
% end
for i=1:fol_num
sum_delta_x=0;
sum_delta_y=0;
for j=1:N %%考虑邻居对它的影响
if k==1
temp_x=0;
temp_y=0;
else
temp_x=(pose_x(j,k)-pose_x(j,k-1))/dt;
temp_y=(pose_y(j,k)-pose_y(j,k-1))/dt;
end
sum_delta_x=sum_delta_x+A(i,j)*(temp_x+gama*((pose_x(j,k)-pose_x(i,k))-(delta_x(j)-delta_x(i))));
sum_delta_y=sum_delta_y+A(i,j)*(temp_y+gama*((pose_y(j,k)-pose_y(i,k))-(delta_y(j)-delta_y(i))));
end
sum_delta_x=sum_delta_x/in_degree(i);
sum_delta_y=sum_delta_y/in_degree(i);
%% 画误差图
color='mgbkr'; %%%定义颜色标记
figure % 生成三维平面图 连续
for i=1:N
plot(pose_x(i,:),pose_y(i,:),color(1,i),'LineWidth',2);
hold on
end
for i=1:N-1
plot(pose_x(i,1),pose_y(i,1),'bp','color',color(1,i),'LineWidth',1);
hold on
end
plot(pose_x(N,1),pose_y(N,1),'*','color',color(1,N),'LineWidth',1);
hold on
for i=1:N-1
plot(pose_x(i,countmax),pose_y(i,countmax),'m^','color',color(1,i),'LineWidth',2);
hold on
end
plot(pose_x(N,countmax),pose_y(N,countmax),'o','color',color(1,N),'LineWidth',2);
hold on;
grid on;
xlabel('x');
ylabel('y');
legend('跟随者1','跟随者2','跟随者3','跟随者4','领航者');
xlabel('x(m)');
ylabel('y(m)');
title('基于一致性的二阶编队算法');
%% 画误差图
cx=0:0.1:countmax/10;
figure % 生成三维平面图 连续
error=sqrt(error_x.^2+error_y.^2);
for i=1:4
plot(cx(1:countmax-1),error(i,1:countmax-1),color(1,i));
hold on;
end
legend('跟随者1','跟随者2','跟随者3','跟随者4');
xlabel('时间(s)');
ylabel('位置误差');
title('各跟随者与领航者的误差曲线');
3 仿真结果
4 参考文献
[1]李奕铭. 基于人工势场法的移动机器人避障研究[D]. 合肥工业大学, 2013.
[2]刘云平等. "一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法.", CN111290429A. 2020.
部分理论引用网络文献，若有侵权联系博主删除。
5 MATLAB代码与数据下载地址
见博客主页
