Multi-robot cooperative formation obstacle avoidance path planning based on MATLAB Artificial potential Field Algorithm

Function [output_args] = formation_avoidance3(input_args) %% 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 relative leader position delta_x=[2 - - 6 2 - - 6 0]; % relative interval error delta_y=[4 4 4 - 4 - 0]; % Pilot and self no error fol_num=4;        
    N=5;             % 4follower and 1 leader
    countmax=2000;
    dt=0.1;
    gama=3;
    di=0.02; Center of gravity offset unit m K0=0; %%% line K1 and K2 are set0.2About k3 =0The %%% circle does not consider K2=0K1, K3, let's say0.2About K1 =1; Position deviation line speed adjustment K2=1; Position deviation angular velocity adjustment K3=0.01; %% Orientation deviation angular velocity adjustment %% % Communication topology:14 -Behavior follower 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]; %% % Communication topology:14 -Behavior follower last behavior leader % A=[0 0 0 0 1;     % a(ij)
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 0];
    A=[0 1 1 1 3;     % a(ij)
       0 0 0 0 3;
       0 0 0 1 3;
       0 0 1 0 3;
       0 0 0 0 0];
   
    linear_v(:,1) = [0;0;0;0;1];
    angular_w(:,1) = [0;0;0;0;1];
    k=0; % Maximum speed m/s], maximum rotation speed [RAD /s], Acceleration [M /ss], rotation acceleration [RAD /ss]] Kinematic=[1.5,toRadian(60.0),0.5,toRadian(50.0)]; %% Movement limit error_temp(1:fol_num,1:4) =0; In_degree =sum(A,2); %% start loop going clockwise circle % figure;for count=1:countmax
        k=k+1;
        linear_v(N,k+1)=linear_v(N,k); % The pilot speed does not change angular_w,k+1)=angular_w(N,k);       
%         if count==500
%             linear_v(N,k+1) =0.5; % Navigator speed unchanged % angular_w(N,k+1) =0.5;
%         end
%         if count==1000
%             linear_v(N,k+1) =0.5; % Navigator speed unchanged % angular_w(N,k+1) =0;
%         end
        
        for i=1:fol_num

            sum_delta_x=0;
            sum_delta_y=0;
            for j=1:N %% Consider the influence of neighbors on itif 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);

            linear_v(i,k+1) =cos(pose_th(i,k))*sum_delta_x+sin(pose_th(i,k))*sum_delta_y;
            angular_w(i,k+1) = (...sin(pose_th(i,k))*sum_delta_x+cos(pose_th(i,k))*sum_delta_y)/di;
            u_old=[linear_v(i,k);angular_w(i,k)];
            u=[linear_v(i,k+1); angular_w(i,k+1)]; %%% Add speed limit u= LIMIT (u_old,u,Kinematic); old_position=[pose_x(i,k);pose_y(i,k);pose_th(i,k)]; new_position=motion(old_position,u,dt); pose_x(i,k+1)=new_position(1)-di*cos(new_position(3));
            pose_y(i,k+1)=new_position(2)-di*sin(new_position(3));
            pose_th(i,k+1)=new_position(3); End % % update pilot old_position = [pose_x (N, k); pose_y (N, k); pose_th (N, k)]; u=[linear_v(N,k+1); angular_w(N,k+1)];
        new_position=motion(old_position,u,dt);
        pose_x(N,k+1)=new_position(1);
        pose_y(N,k+1)=new_position(2);
        pose_th(N,k+1)=new_position(3);

        tt_x(1:4,k)=pose_x(5,k);
        error_x(:,k)=tt_x(1:4,k)-pose_x(1:4,k)+(delta_x(1:4))'; tt_y(1:4,k)=pose_y(5,k); error_y(:,k)=tt_y(1:4,k)-pose_y(1:4,k)+(delta_y(1:4))';
        function interpoint( x1,y1,x2,y2,x3,y3,colo,lstyle)%UNTITLED2 displays a summary of this function. % Displays a detailed descriptionif (nargin==6)
    colo='k';
    lstyle=':';
end
syms k b m n x y;
ifSolx =x1; solx=x1; solx=x1; soly=y3; Elseif (y1==y2)%x1x20
    solx=x3;
    soly=y1;
else 
    solk=(y2-y1)/(x2-x1);
    solb=y2-solk*x2;
    solk1=- 1/solk;
    solb1=y3-solk1*x3;
    solx=(solb1-solb)/(solk-solk1);
    soly=solk*solx+solb;
%     [solx,soly] = solve(solk1*x-y+solb1==0,solk*x-y+solb==0,x,y);
end
line([x1,solx],[y1,soly],'color',colo,'linestyle',lstyle);
line([x3,solx],[y3,soly],'color',colo,'linestyle',lstyle);
end
Copy the code

Version: 2014 a