【路径规划】基于matlab人工势场算法多机器人协同编队避障路径规划【含Matlab源码 115期】

一、简介

基于matlab人工势场算法多机器人协同编队避障路径规划

二、源代码

function [ output_args ] = formation_avoidance3( input_args )
    %% 初始化 位置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
%     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;
    % 最高速度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);

            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)];
            %%%加入速度限制
            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
        %% 更新领航者
        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 此处显示有关此函数的摘要
%   此处显示详细说明
if (nargin==6)
    colo='k';
    lstyle=':';
end
syms k b m n x y;
if(x1==x2)%x1x2直线斜率不存在
    solx=x1;
    soly=y3;
elseif(y1==y2)%x1x2直线斜率为0
    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

三、运行结果

四、备注

版本：2014a