蚁群算法实现路径规划MATLAB实现_蚁群算法对机器人的路径规划csdn

ASA 路径规划—自己用

参考了若干论文

1. 问题描述：

      移动机器人路径规划是机器人学的一个重要研究领域。它要求机器人依据某个或某些优化原则(如最小能量消耗，最短行走路线，最短行走时间等)，在其工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。机器人路径规划问题可以建模为一个有约束的优化问题，都要完成路径规划、定位和避障等任务。

2. 算法理论

      蚁群算法（Ant Colony Algorithm，ACA），最初是由意大利学者Dorigo M. 博士于1991 年首次提出，其本质是一个复杂的智能系统，且具有较强的鲁棒性，优良的分布式计算机制等优点。该算法经过十多年的发展，已被广大的科学研究人员应用于各种问题的研究，如旅行商问题，二次规划问题，生产调度问题等。但是算法本身性能的评价等算法理论研究方面进展较慢。
      Dorigo 提出了精英蚁群模型（EAS），在这一模型中信息素更新按照得到当前最优解的蚂蚁所构造的解来进行，但这样的策略往往使进化变得缓慢，并不能取得较好的效果。次年Dorigo 博士给出改进模型（ACS），文中 改进了转移概率模型，并且应用了全局搜索与局部搜索策略，来得进行深度搜索。 Stützle 与Hoos给出了最大-最小蚂蚁系统（MAX-MINAS），所谓最大-最小即是为信息素设定上限与下限，设定上限避免搜索陷入局部最优，设定下限鼓励深度搜索。蚂蚁作为一个生物个体其自身的能力是十分有限的，比如蚂蚁个体是没有视觉的，蚂蚁自身体积又是那么渺小，但是由这些能力有限的蚂蚁组成的蚁群却可以做出超越个体蚂蚁能力的超常行为。蚂蚁没有视觉却可以寻觅食物，蚂蚁体积渺小而蚁群却可以搬运比它们个体大十倍甚至百倍的昆虫。这些都说明蚂蚁群体内部的某种机制使得它们具有了群体智能，可以做到蚂蚁个体无法实现的事情。经过生物学家的长时间观察发现，蚂蚁是通过分泌于空间中的信息素进行信息交流，进而实现群体行为的。
      下面简要介绍蚁群通过信息素的交流找到最短路径的简化实例。如图 2-1 所示，AE 之间有两条路ABCDE 与ABHDE，其中AB，DE，HD，HB 的长度为1，BC，CD 长度为0.5，并且，假设路上信息素浓度为0，且各个蚂蚁行进速度相同，单位时间所走的长度为1，每个单位时间内在走过路径上留下的信息素的量也相同。当t=0时，从A 点，E 点同时各有30 只蚂蚁从该点出发。当t=1，从A 点出发的蚂蚁走到B 点时，由于两条路BH 与BC 上的信息素浓度相同，所以蚂蚁以相同的概率选择BH 与BC，这样就有15 只蚂蚁选择走BH，有15 只蚂蚁选择走BC。同样的从E 点出发的蚂蚁走到D 点，分别有15 只蚂蚁选择DH 和DC。当t=2 时，选择BC 与DC的蚂蚁分别走过了BCD 和DCB，而选择BH 与DH 的蚂蚁都走到了H 点。所有的蚂蚁都在所走过的路上留下了相同浓度的信息素，那么路径BCD 上的信息素的浓度是路径BHD 上信息素浓度的两倍，这样若再次有蚂蚁选择走BC 和BH 时，或选择走DC 与DH 时，都会以较大的概率选择信息素浓度高的一边。这样的过程反复进行下去，最短的路径上走过的蚂蚁较多，留下的信息素也越多，蚁群这样就可以找到一条较短的路。这就是它们群体智能的体现。
      蚁群算法就是模拟蚂蚁觅食过程中可以找到最短的路的行为过程设计的一种仿生算法。在用蚁群算法求解组合优化问题时，首先要将组合优化问题表达成与信息素相关的规范形式，然后各个蚂蚁独立地根据局部的信息素进行决策构造解，并根据解的优劣更新周围的信息素，这样的过程反复的进行即可求出组合优化问题的优化解。
      归结蚁群算法有如下特点：
（1）分布式计算：各个蚂蚁独立地构造解，当有蚂蚁个体构造的解较差时，并不会影响整体的求解结果。这使得算法具有较强的适应性；
（2）自组织性：系统学中自组织性就是系统的组织指令是来自系统的内部。同样的蚁群算法中的各个蚂蚁的决策是根据系统内部信息素的分布进行的。这使得算法具有较强的鲁棒性；
（3）正反馈机制与负反馈机制结合：若某部分空间上分布的信息素越多，那么在这个空间上走过的蚂蚁也就越多；走过的蚂蚁越多，在那个空间上留下的信息素也就越多，这就是存在的正反馈机制。但蚁群算法中解的构造是通过计算转移概率实现的，也就是说构造解的时候可以接受退化解，这限制了正反馈机制，可以使得搜索范围扩大，这是蚁群算法中隐含的负反馈机制。

3. 求解步骤：

（1）输入由0和1组成的矩阵表示机器人需要寻找最优路径的地图的地图，其中0表示此处可以通过的，1表示此处为障碍物。
（2）输入初始的信息素矩阵，选择初始点和终止点并且设置各种参数。在此次计算中，
我们设置所有位置的初始信息素相等。
（3）选择从初始点下一步可以到达的节点，根据每个节点的信息素求出前往每个节点的
概率，并利用轮盘算法选取下一步的初始点。
（4）更新路径，以及路程长度。
（5）重复（3）（4）过程，直到蚂蚁到达终点或者无路可走。
（6）重复（3）（4）（5），直到某一代m只蚂蚁迭代结束。
（7）更新信息素矩阵，其中没有到达的蚂蚁不计算在内。
（8）重复（3）-（7），直至n代蚂蚁迭代结束。

4.效果图：

5. 代码：

%% 蚁群算法实现路径规划

%% ACA
function m_main()

%% 地图数据，1表示障碍物
G=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
    0 1 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 1 1 0 0 1 0 1 0 1 1 1 1 0 0 0 0 0 0;
    0 1 1 1 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 1 0 0 1 0 0 0 0 0 0;
    0 0 0 0 0 0 0 1 1 1 1 1 0 1 0 0 0 0 0 0;
    0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 1 1 1 0;
    0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 1 1 1 0;
    0 0 1 1 0 0 0 0 0 0 0 1 1 1 0 0 1 1 1 0;
    0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
    0 0 0 0 0 0 1 1 1 0 1 1 0 0 0 0 0 1 1 0;
    0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 0 0 1 1 0;
    0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0;
    0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;];

MM=size(G,1);      % G 地形图为01矩阵，如果为1表示障碍物
Tau=ones(MM*MM,MM*MM);% Tau 初始信息素矩阵（认为前面的觅食活动中有残留的信息素）
Tau=8.*Tau;
K=100;          % K 迭代次数（指蚂蚁出动多少波）
M=50;         % M 蚂蚁个数（每一波蚂蚁有多少个）
S=1 ;         % S 起始点（最短路径的起始点）
E=MM*MM;         % E 终止点（最短路径的目的点）
Alpha=1;    % Alpha 表征信息素重要程度的参数
Beta=7;     % Beta 表征启发式因子重要程度的参数
Rho=0.3 ;     % Rho 信息素蒸发系数
Q=1;          % Q 信息素增加强度系数
minkl=inf;
mink=0;
minl=0;
D=G2D(G);
N=size(D,1);%N表示问题的规模（象素个数）
a=1;%小方格象素的边长
Ex=a*(mod(E,MM)-0.5);%终止点横坐标
if Ex==-0.5
    Ex=MM-0.5;
end
Ey=a*(MM+0.5-ceil(E/MM));%终止点纵坐标
Eta=zeros(N);%启发式信息，取为至目标点的直线距离的倒数
%下面构造启发式信息矩阵
for i=1:N
    ix=a*(mod(i,MM)-0.5);
    if ix==-0.5
        ix=MM-0.5;
    end
    iy=a*(MM+0.5-ceil(i/MM));
    if i~=E
        Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;
    else
        Eta(i)=100;
    end
end
ROUTES=cell(K,M);%用细胞结构存储每一代的每一只蚂蚁的爬行路线
PL=zeros(K,M);%用矩阵存储每一代的每一只蚂蚁的爬行路线长度

%% 开始觅食，共K轮，每轮M只蚂蚁
for k=1:K 
    for m=1:M
        %% 第一步：状态初始化
        W=S;%当前节点初始化为起始点
        Path=S;%爬行路线初始化
        PLkm=0;%爬行路线长度初始化
        TABUkm=ones(N);%禁忌表初始化
        TABUkm(S)=0;%已经在初始点了，因此要排除
        DD=D;%邻接矩阵初始化
        %% 第二步：下一步可以前往的节点
        DW=DD(W,:);
        DW1=find(DW);
        for j=1:length(DW1)
            if TABUkm(DW1(j))==0
                DW(DW1(j))=0;
            end
        end
        LJD=find(DW);
        Len_LJD=length(LJD);%可选节点的个数
        %% 觅食停止条件：蚂蚁未遇到食物或者陷入死胡同
        while W~=E&&Len_LJD>=1
            %% 第三步：转轮赌法选择下一步怎么走
            PP=zeros(Len_LJD);
            for i=1:Len_LJD
                PP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta);
            end
            sumpp=sum(PP);
            PP=PP/sumpp;%建立概率分布
            Pcum(1)=PP(1);
            for i=2:Len_LJD
                Pcum(i)=Pcum(i-1)+PP(i);
            end
            Select=find(Pcum>=rand);
            to_visit=LJD(Select(1));
            %% 第四步：状态更新和记录
            Path=[Path,to_visit];%路径增加
            PLkm=PLkm+DD(W,to_visit);%路径长度增加
            W=to_visit;%蚂蚁移到下一个节点
            for kk=1:N
                if TABUkm(kk)==0
                    DD(W,kk)=0;
                    DD(kk,W)=0;
                end
            end
            TABUkm(W)=0;%已访问过的节点从禁忌表中删除
            DW=DD(W,:);
            DW1=find(DW);
            for j=1:length(DW1)
                if TABUkm(DW1(j))==0
                    DW(j)=0;
                end
            end
            LJD=find(DW);
            Len_LJD=length(LJD);%可选节点的个数
        end
        %% 第五步：记下每一代每一只蚂蚁的觅食路线和路线长度
        ROUTES{k,m}=Path;
        if Path(end)==E
            PL(k,m)=PLkm;
            if PLkm<minkl
                mink=k;minl=m;minkl=PLkm;
            end
        else
            PL(k,m)=0;
        end
    end
    %% 第六步：更新信息素
    Delta_Tau=zeros(N,N);%更新量初始化
    for m=1:M
        if PL(k,m)
            ROUT=ROUTES{k,m};
            TS=length(ROUT)-1;%跳数
            PL_km=PL(k,m);
            for s=1:TS
                x=ROUT(s);
                y=ROUT(s+1);
                Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;
                Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;
            end
        end
    end
    Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分
end

%% 绘图部分
plotif=1;%是否绘图的控制参数
if plotif==1
    %绘收敛曲线
    minPL=zeros(K);
    for i=1:K
        PLK=PL(i,:);
        Nonzero=find(PLK);
        PLKPLK=PLK(Nonzero);
        minPL(i)=min(PLKPLK);
    end
    figure(1)
    plot(minPL);
    hold on
    grid on
    title('收敛曲线（最小路径长度）');
    xlabel('迭代次数');
    ylabel('路径长度');
    
    %绘爬行图
    figure(2)
    axis([0,MM,0,MM])
    for i=1:MM
        for j=1:MM
            if G(i,j)==1
                x1=j-1;y1=MM-i;
                x2=j;y2=MM-i;
                x3=j;y3=MM-i+1;
                x4=j-1;y4=MM-i+1;
                fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);
                hold on
            else
                x1=j-1;y1=MM-i;
                x2=j;y2=MM-i;
                x3=j;y3=MM-i+1;
                x4=j-1;y4=MM-i+1;
                fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
                hold on
            end
        end
    end
    hold on
    ROUT=ROUTES{mink,minl};
    LENROUT=length(ROUT);
    Rx=ROUT;
    Ry=ROUT;
    for ii=1:LENROUT
        Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);
        if Rx(ii)==-0.5
            Rx(ii)=MM-0.5;
        end
        Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));
    end
    plot(Rx,Ry)
end
plotif2=0;%绘各代蚂蚁爬行图
if plotif2==1
    figure(3)
    axis([0,MM,0,MM])
    for i=1:MM
        for j=1:MM
            if G(i,j)==1
                x1=j-1;y1=MM-i;
                x2=j;y2=MM-i;
                x3=j;y3=MM-i+1;
                x4=j-1;y4=MM-i+1;
                fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);
                hold on
            else
                x1=j-1;y1=MM-i;
                x2=j;y2=MM-i;
                x3=j;y3=MM-i+1;
                x4=j-1;y4=MM-i+1;
                fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
                hold on
            end
        end
    end
    for k=1:K
        PLK=PL(k,:);
        minPLK=min(PLK);
        pos=find(PLK==minPLK);
        m=pos(1);
        ROUT=ROUTES{k,m};
        LENROUT=length(ROUT);
        Rx=ROUT;
        Ry=ROUT;
        for ii=1:LENROUT
            Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);
            if Rx(ii)==-0.5
                Rx(ii)=MM-0.5;
            end
            Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));
        end
        plot(Rx,Ry)
        hold on
    end
end

function D=G2D(G)
l=size(G,1);
D=zeros(l*l,l*l);
for i=1:l
    for j=1:l
        if G(i,j)==0
            for m=1:l
                for n=1:l
                    if G(m,n)==0
                        im=abs(i-m);jn=abs(j-n);
                        if im+jn==1||(im==1&&jn==1)
                            D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5;
                        end
                    end
                end
            end
        end
    end
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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269