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

Posted 2021-06-05 博主QQ2449341593

 

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

完整代码添加QQ1575304183