【路径规划】基于matlab麻雀算法机器人栅格地图最短路径规划【含Matlab源码 1582期】

一、简介

路径规划是实现移动机器人自主导航的关键技术，是指在有障碍物的环境中，按照一定的评价标准（如距离、时间、能耗等），寻找到一条从起始点到目标点的无碰撞路径，这里选取最短距离路径规划的评价标准，即最短路径规划问题。

1．路径规划数学模型的建立
将移动机器人周围环境用一组数据进行抽象表达，建立二维或三维的环境模型，得到移动机器人能够理解分析的环境数据，是机器人路径规划的基本前提。我这里用的是栅格法，其原理是将周围环境看成一个二维平面，将平面分成一个个等面积大小的具有二值信息的栅格，每个栅格中存储着周围环境信息量，下图我给出了一个栅格法地图，方便大家更好的理解栅格地图。这里设计的栅格地图为一个20×20的地形矩阵，黑色的地方表示有障碍，白色的地方表示没有障碍。

图1　栅格法地图
在用栅格法建立环境模型时，为了将环境信息转换成移动机器人可以识别的数据，一般采用序号法标记环境地图信息，即将栅格地图中一个个栅格从序号１依次累加直到标记到最后一个栅格。如图2所示。


图3　八叉树搜索策略
那么，怎么判断一个栅格点是否为另一个栅格点的相邻栅格点呢，另外，又怎么判断是否为有障碍栅格呢。这就需建立矩阵D，记录每个栅格点至其相邻栅格点的代价值。本例中栅格地图有20×20个栅格点，则D的大小为400×400，其中列是起点栅格，行是局部终点栅格，各栅格点至其各相邻无障碍栅格点的代价值非零，而有障碍栅格及非相邻栅格设为0。

二、部分源代码

clc
clear
close all
tic
%% 地图
G=[0 0 0 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 0 0 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 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 0 0 0 0 0 0 0 0;
   0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 0 1 1 1 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 1 1 0; 
   0 0 1 1 0 0 0 0 0 0 1 1 0 0 1 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;];
for i=1:20/2
    for j=1:20
        m=G(i,j);
        n=G(21-i,j);
        G(i,j)=n;
        G(21-i,j)=m;
    end
end
%% 
S = [1 1];   
E = [20 20];  
G0 = G;
G = G0(S(1):E(1),S(2):E(2)); 
[Xmax,dimensions] = size(G);        
dimensions = dimensions - 2;             

%% 参数设置
max_gen = 200;    % 最大迭代次数
num_polution = 50;         % 种群数量
soft_value = 0.8;        %安全值
recover_Percent = 0.3;  %%发现者比例
scout_Percent = 0.2;  %%侦查者比例
recover_Num = round( num_polution * recover_Percent );    % %发现者
scout_Num = round(num_polution * scout_Percent);      %%侦查者数量
X_min = 1;  

%% 初始化
X = zeros(num_polution,dimensions);
for i = 1:num_polution
    for j = 1:dimensions
       column = G(:,j+1);      % 地图的一列
       id = find(column == 0); % 该列自由栅格的位置
       X(i,j) =  id(randi(length(id))); % 随机选择一个自由栅格
       id = [];
    end 
    fit( i ) = fitness(X( i, : ),G);%%%行向量
end
fit_person_best = fit;   % 个体最优适应度
person_best = X;      % 个体最优位置
[fit_global_best, best_person_Index] = min( fit );        % 全局最优适应度
global_best = X(best_person_Index, : );    % 全局最优位置
[fit_max,B]=max(fit);
worse_person= X(B,:);  
%%
for gene = 1:max_gen
    gene
    [ans1,sort_Index] = sort(fit);   %适应值度从小到大排序
    [fit_max,B] = max(fit);
    worse_person = X(B,:);           %找出适应度最差的个体
    [~,Index] = sort(fit_person_best);
    r2 = rand(1);
    if r2 < soft_value

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        for i = 1:recover_Num                                                  
            r1 = rand(1);  
            X(Index(i),:) = person_best(Index(i),:)*exp(-(i)/(r1*max_gen));         %%%%%发现者广泛进行搜索操作
            X(Index(i),:) = Bounds(X(Index(i),:), X_min,Xmax);                      %%%%输出整数路径
            fit(Index(i)) = fitness(X(Index(i),:),G);
        end
    else
            X(Index(i),:) = person_best(Index(i),:) + randn(1)*ones(1,dimensions);  %%%%%%%飞往其他地方觅食
            X(Index(i),:) = Bounds(X(Index(i),:),X_min,Xmax);    
            fit(Index(i)) = fitness(X(Index(i),:),G);
    end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%发现者%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    [fit_MMin,best_best] = min(fit);      
    best_best_bt = X(best_best,:);     
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    for i = (recover_Num+1):num_polution                   
        A = floor(rand(1,dimensions)*2)*2-1;                %%%%%%%%-1到1的随机数
        if(i>(num_polution/2))
            X(Index(i),:) = randn(1)*exp((worse_person-person_best(Index(i),:))/(i)^2);%%%%%%%适应度较低的个体没有获得食物，将飞往其他地方随机范围
        else
            X(Index(i),:) = best_best_bt+(abs((person_best(Index(i),:)-best_best_bt)))*(A'*(A*A')^(-1))*ones(1,dimensions); %%%%%%%中等适应度值的个体更新位置
        end
        X(Index(i),:) = Bounds(X(Index(i),:),X_min,Xmax);
        fit(Index(i)) = fitness(X(Index(i),:),G);
    end
    p_1 = randperm(numel(sort_Index));
    p_2 = sort_Index(p_1(1:scout_Num));
    for j = 1:length(p_2)     
        if(fit_person_best(Index(p_2(j)))>(fit_global_best))
            X(Index(p_2(j)),:) = global_best+(randn(1,dimensions)).*(abs((person_best(Index( p_2(j)),:) -global_best)));%%%%%%%%意识到危险的麻雀向种群适应度最高的个体靠拢
        else
            X(Index(p_2(j)),:) = person_best(Index(p_2(j)),:)+(2*rand(1)-1)*(person_best(Index(p_2(j)),:)-worse_person)/( fit_person_best(Index(p_2(j)))-fit_max+1e-50);
        end
        X(Index(j),:) = Bounds(X(Index(j),:), X_min, Xmax);
        fit(Index(j)) = fitness(X(Index(j),:),G);
    end
     for i=1:num_polution
         X(i,:) = Bounds(X(i,:),X_min,Xmax);
     end
    % 更新个体最优值和全局最优值
    for i = 1:num_polution
     
        end
        if(fit_person_best(i) < fit_global_best)
            fit_global_best = fit_person_best(i);
            global_best = person_best(i,:);
        end
    end
    global_best = LocalSearch(global_best,Xmax,G);%%%%%把全局最优解进行局部搜索，提高全局最优解适应度值
    fit_global_best = fitness(global_best,G);
    final_goal(gene,1)=fit_global_best;
end
function fx = fitness(x,G)
    S = [1 1];       % 新地图的起点
    E = size(G);     % 新地图的终点
    route = [S(1) x E(1)];
    dim = length(route);
    nB = 0;        % 粒子路径是否经过障碍的数目
    route=round(route);
    for j = 2 : dim-1
       if G(route(j),j) == 1
           nB = nB + 1;
       end
    end
    if nB == 0     % 
        path=generateContinuousRoute(route,G); % 中点邻域搜索
%         path=shortenRoute(path);
        path=GenerateSmoothPath(path,G);
        path=GenerateSmoothPath(path,G);
        fx = 0;
        for i = 1:size(path,1)-1
            fx = fx + sqrt((path(i+1,1)-path(i,1))^2 + (path(i+1,2)-path(i,2))^2);
        end
    else
    fx = E(1)*E(2) * nB;
    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
 

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

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

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