多智能体最优一致性避障算法Matlab仿真源程序

【实例简介】多智能体最优一致性避障算法Matlab仿真源程序

【实例截图】

【文件目录】

Optimal Consensus Obstacle Avoidance

├── Optimal consensus algorithmintegrated with obstacle avoidance.pdf
└── 源程序1
    ├── circle.asv
    ├── circle.m
    ├── concensus_effort_input.asv
    ├── concensus_effort_input.m
    ├── consensus_test.mdl
    ├── draw.m
    └── oa_input.m

1 directory, 8 files

【核心代码】

%以下代码用于计算无人船在多个障碍物情况下，通过避障函数实现无碰撞

function Usum=oa_input(x)
R=[2.5 1];        %检测区域半径集合
r=[0.5 0.2];      %临界碰撞半径集合
ob=[7 7;-1 -1];  %障碍物位置信息集合
wc=4;                 %控制效果权重参数
u1=zeros(4,2);        %用于存放躲避各障碍物输出量
x1=[x(1) x(2)];       %第一个无人船位置信息
x2=[x(3) x(4)];       %第二个无人船位置信息
x3=[x(5) x(6)];       %第三个无人船位置信息
x4=[x(7) x(8)];       %第四个无人船位置信息
xx=[x1;x2;x3;x4];     %无人船状态集合
N=size(xx);           %无人船个数
M=size(ob);           %障碍物个数

%%以下循环用于计算无人船躲避障碍物所需输入，及代价函数的计算
for i=1:N(1)
    for j=1:M(1)
       d(i)=(xx(i,1)-ob(j,1))^2 (xx(i,2)-ob(j,2))^2;%障碍物与无人船之间的距离平方
       D(i)=sqrt(d(i));                             %障碍物与无人船之间的距离
       a(i,:)=xx(i,:)-ob(j,:);                      %障碍物与无人船的坐标向量
       K(i)=4*(R(j)^2-r(j)^2)*(R(j)^2-d(i))/(2*wc^2*(d(i)-r(j)^2)^3);%避障代价函数标量部分
      %K(i)=4*(1-(d(i)^2-R(j)^2)/(2*sqrt((d(i)-R(j)^2 0.05)^2)));
      %K(i)=exp(0.0001*((R(j)^2-d(i)^2)/(d(i)^2-r(j)^2))^2)*4*(R(j)^2-r(j)^2)*(R(j)^2-d(i))/(2*wc^2*(d(i)-r(j)^2)^3);
%%以下语句用于判断无人船是否需要避障
if d(i)>R(j)
    u(i,:)=[0 0];
else d(i)<=R(j) && d(i)>r(j)
    u(i,:)=K(i)*a(i,:);
%else 
    %disp('the muti-agent  will collide with obstacle inevitably');
end
 u1(i,:)=u1(i,:) u(i,:);%每个无人船在躲避单个障碍物时所需能量
     end
   u1(i,:)=u1(i,:) u(i,:);%每个无人船在躲避各个障碍物时，所需输入量
end

b=u1';                    %将输出u按行展开成列，反馈到系统
Usum=b(:);
end