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

一、简介

基于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



  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
 

三、运行结果

四、备注

版本：2014a

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/112553824