【路径规划】基于遗传算法实现机器人栅格地图路径规划

1遗传算法

• 遗传算法（Genetic Algorithm，GA）是一种进化算法，其基本原理是仿效生物界中的“物竞天择、适者生存”的演化法 则，它最初由美国Michigan大学的J. Holland教授于1967年提出。 • 遗传算法是从代表问题可能潜在的解集的一个种群（population）开始的，而一个种群则由经过基因（gene）编码的一 定数目的个体(individual)组成。因此，第一步需要实现从表现型到基因型的映射即编码工作。初代种群产生之后，按照 适者生存和优胜劣汰的原理，逐代（generation）演化产生出越来越好的近似解，在每一代，根据问题域中个体的适应度 （fitness）大小选择个体，并借助于自然遗传学的遗传算子（genetic operators）进行组合交叉和变异，产生出代表新 的解集的种群。这个过程将导致种群像自然进化一样，后生代种群比前代更加适应于环境，末代种群中的最优个体经过解 码（decoding），可以作为问题近似最优解。

• 遗传算法有三个基本操作：选择（Selection）、交叉（Crossover）和变异（Mutation）。 • （1）选择。选择的目的是为了从当前群体中选出优良的个体，使它们有机会作为父代为下一代繁衍子孙。根据各个个体的 适应度值，按照一定的规则或方法从上一代群体中选择出一些优良的个体遗传到下一代种群中。选择的依据是适应性强的 个体为下一代贡献一个或多个后代的概率大。 • （2）交叉。通过交叉操作可以得到新一代个体，新个体组合了父辈个体的特性。将群体中的各个个体随机搭配成对，对每 一个个体，以交叉概率交换它们之间的部分染色体。 • （3）变异。对种群中的每一个个体，以变异概率改变某一个或多个基因座上的基因值为其他的等位基因。同生物界中一样， 变异发生的概率很低，变异为新个体的产生提供了机会。

遗传算法的基本步骤：

1)编码：GA在进行搜索之前先将解空间的解数据表示成遗传空间的基因型串结构数据， 这些串结构数据的丌同组合便构成了丌同的点。 2)初始群体的生成：随机产生N个初始串结构数据，每个串结构数据称为一个个体，N个 个体构成了一个群体。GA以这N个串结构数据作为初始点开始进化。 3)适应度评估：适应度表明个体或解的优劣性。丌同的问题，适应性函数的定义方式也丌 同。

4)选择：选择的目的是为了从当前群体中选出优良的个体，使它们有机会作为父代为下一 代繁殖子孙。遗传算法通过选择过程体现这一思想，进行选择的原则是适应性强的个体为 下一代贡献一个或多个后代的概率大。选择体现了达尔文的适者生存原则。 5)交叉：交叉操作是遗传算法中最主要的遗传操作。通过交叉操作可以得到新一代个体， 新个体组合了其父辈个体的特性。交叉体现了信息交换的思想。 6)变异：变异首先在群体中随机选择一个个体，对于选中的个体以一定的概率随机地改变 串结构数据中某个串的值。同生物界一样， GA中变异发生的概率很低，通常取值很小。

2 栅格地图介绍

栅格地图有两种表示方法，直角坐标系法和序号法，序号法比直角坐标法节省内存

室内环境栅格法建模步骤

1.栅格粒大小的选取

栅格的大小是个关键因素，栅格选的小，环境分辨率较大，环境信息存储量大，决策速度慢。

栅格选的大，环境分辨率较小，环境信息存储量小，决策速度快，但在密集障碍物环境中发现路径的能力较弱。

2.障碍物栅格确定

当机器人新进入一个环境时，它是不知道室内障碍物信息的，这就需要机器人能够遍历整个环境，检测障碍物的位置，并根据障碍物位置找到对应栅格地图中的序号值，并对相应的栅格值进行修改。自由栅格为不包含障碍物的栅格赋值为0，障碍物栅格为包含障碍物的栅格赋值为1.

3.未知环境的栅格地图的建立

通常把终点设置为一个不能到达的点，比如（-1，-1），同时机器人在寻路过程中遵循“下右上左”的原则，即机器人先向下行走，当机器人前方遇到障碍物时，机器人转向右走，遵循这样的规则，机器人最终可以搜索出所有的可行路径，并且机器人最终将返回起始点。

备注：在栅格地图上，有这么一条原则，障碍物的大小永远等于n个栅格的大小，不会出现半个栅格这样的情况。

3 代码

function varargout = ditu(varargin)
% DITU MATLAB code for ditu.fig
%      DITU, by itself, creates a new DITU or raises the existing
%      singleton*.
%
%      H = DITU returns the handle to a new DITU or the handle to
%      the existing singleton*.
%
%      DITU('CALLBACK',hObject,eventData,handles,...) calls the local
%      function named CALLBACK in DITU.M with the given input arguments.
%
%      DITU('Property','Value',...) creates a new DITU or raises the
%      existing singleton*.  Starting from the left, property value pairs are
%      applied to the GUI before ditu_OpeningFcn gets called.  An
%      unrecognized property name or invalid value makes property application
%      stop.  All inputs are passed to ditu_OpeningFcn via varargin.
%
%      *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one
%      instance to run (singleton)".
%
% See also: GUIDE, GUIDATA, GUIHANDLES

% Edit the above text to modify the response to help ditu

% Last Modified by GUIDE v2.5 12-May-2020 21:55:41

% Begin initialization code - DO NOT EDIT
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @ditu_OpeningFcn, ...
                   'gui_OutputFcn',  @ditu_OutputFcn, ...
                   'gui_LayoutFcn',  [] , ...
                   'gui_Callback',   []);
if nargin && ischar(varargin{1})
    gui_State.gui_Callback = str2func(varargin{1});
end

if nargout
    [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
else
    gui_mainfcn(gui_State, varargin{:});
end
% End initialization code - DO NOT EDIT


% --- Executes just before ditu is made visible.
function ditu_OpeningFcn(hObject, eventdata, handles, varargin)
% This function has no output args, see OutputFcn.
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
% varargin   command line arguments to ditu (see VARARGIN)

% Choose default command line output for ditu
handles.output = hObject;

% Update handles structure
guidata(hObject, handles);

% UIWAIT makes ditu wait for user response (see UIRESUME)
% uiwait(handles.figure1);


% --- Outputs from this function are returned to the command line.
function varargout = ditu_OutputFcn(hObject, eventdata, handles) 
% varargout  cell array for returning output args (see VARARGOUT);
% hObject    handle to figure
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Get default command line output from handles structure
varargout{1} = handles.output;



function edit1_Callback(hObject, eventdata, handles)
% hObject    handle to edit1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit1 as text
%        str2double(get(hObject,'String')) returns contents of edit1 as a double


% --- Executes during object creation, after setting all properties.
function edit1_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit2_Callback(hObject, eventdata, handles)
% hObject    handle to edit2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit2 as text
%        str2double(get(hObject,'String')) returns contents of edit2 as a double


% --- Executes during object creation, after setting all properties.
function edit2_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit2 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit3_Callback(hObject, eventdata, handles)
% hObject    handle to edit3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit3 as text
%        str2double(get(hObject,'String')) returns contents of edit3 as a double


% --- Executes during object creation, after setting all properties.
function edit3_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit3 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit4_Callback(hObject, eventdata, handles)
% hObject    handle to edit4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit4 as text
%        str2double(get(hObject,'String')) returns contents of edit4 as a double


% --- Executes during object creation, after setting all properties.
function edit4_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit4 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit5_Callback(hObject, eventdata, handles)
% hObject    handle to edit5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit5 as text
%        str2double(get(hObject,'String')) returns contents of edit5 as a double


% --- Executes during object creation, after setting all properties.
function edit5_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit5 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end



function edit6_Callback(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit6 as text
%        str2double(get(hObject,'String')) returns contents of edit6 as a double


% --- Executes during object creation, after setting all properties.
function edit6_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit6 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end


% --- Executes on button press in pushbutton1.
function pushbutton1_Callback(hObject, eventdata, handles)
% hObject    handle to pushbutton1 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

kaishi=str2num(get(handles.edit1,'String'));
jieshu =str2num(get(handles.edit2,'String'));    % 终止序号
zhongqun = str2num(get(handles.edit3,'String'));      % 种群数量
diedaicishu = str2num(get(handles.edit4,'String'));  % 最大进化代数
jiaochaP = str2num(get(handles.edit5,'String'));      % 交叉概率
bainyiP = str2num(get(handles.edit6,'String'));      % 变异概率
z = 1;
zuixin_pop = {}; % 元包类型路径
% 输入数据,即栅格地图
num=str2double(get(handles.edit7,'String'));
switch num
    case 1
load ditu1.mat
case 2
load ditu2.mat
case 3
load ditu3.mat
case 4
load ditu4.mat
end
[y, x] = size(G);
% 起点所在列（从左到右编号1.2.3...）
kaishi_x = mod(kaishi, x) + 1;
% 起点所在行（从上到下编号行1.2.3...）
kaishi_y = fix(kaishi / x) + 1;
% 终点所在列、行
jieshu_x = mod(jieshu, x) + 1;
jieshu_y = fix(jieshu / x) + 1;

% 种群初始化step1，必经节点,从起始点所在行开始往上，在每行中挑选一个自由栅格，构成必经节点
pass_num = jieshu_y - kaishi_y + 1;
pop = zeros(zhongqun, pass_num);
min_value=1000;
for i = 1 : zhongqun
    pop(i, 1) = kaishi;
    j = 1;
    % 除去起点和终点
    for yxunhuan = kaishi_y+1 : jieshu_y-1
        j = j + 1;
        % 每一行的可行点
        kaxingjie = [];
        for xxunhuan = 1 : x
            % 栅格序号
            dd=rand(1);
            no = (xxunhuan - 1) + (yxunhuan - 1) * x;
            if G(yxunhuan, xxunhuan) == 0
                % 把点加入kaxingjie矩阵中
                kaxingjie = [kaxingjie no];
            end
        end
        kaxingjie_num = length(kaxingjie);
        % 产生随机整数
        index = randi(kaxingjie_num);
        % 为每一行加一个可行点
        pop(i, j) = kaxingjie(index);
    end
    pop(i, end) = jieshu;
       % 种群初始化step2将上述必经节点联结成无间断路径
    pop_jie = generate_continuous_path(pop(i, :), G, x);
    if ~isempty(pop_jie)
        zuixin_pop(z, 1) = {pop_jie};
        z = z + 1;
    end
end

% 计算初始化种群的适应度
% 计算路径长度
path_value = cal_path_value(zuixin_pop, x);

% 画每次迭代平均路径长度和最优路径长度图
figure(1)
plot(1:diedaicishu,  mean_path_value, 'r')
hold on;
title('优化曲线图');
xlabel('迭代次数');
ylabel('路径长度');
plot(1:diedaicishu, min_path_value, 'b')
legend('平均路径长度', '最优路径长度');
min_path_value(1, end)
% 在地图上画路径
[~, min_index] = max(fit_value);
min_path = zuixin_pop{min_index, 1};
axes(handles.axes1)

DrawMap(G);
[~, min_path_num] = size(min_path);
for i = 1:min_path_num
    % 路径点所在列（从左到右编号1.2.3...）
    x_min_path(1, i) = mod(min_path(1, i), x) + 1;
    % 路径点所在行（从上到下编号行1.2.3...）
    y_min_path(1, i) = fix(min_path(1, i) / x) + 1;
end
hold on;
plot(x_min_path, y_min_path, 'r')
hold on;
title(['遗传算法机器人运动轨迹']);
xlabel('坐标x');
ylabel('坐标y');
hold off



function edit7_Callback(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

% Hints: get(hObject,'String') returns contents of edit7 as text
%        str2double(get(hObject,'String')) returns contents of edit7 as a double


% --- Executes during object creation, after setting all properties.
function edit7_CreateFcn(hObject, eventdata, handles)
% hObject    handle to edit7 (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    empty - handles not created until after all CreateFcns called

% Hint: edit controls usually have a white background on Windows.
%       See ISPC and COMPUTER.
if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
    set(hObject,'BackgroundColor','white');
end