【路径规划】基于matlab RBF优化Qlearning算法机器人避障路径规划【含Matlab源码 1219期】

一、RBF及Qlearning算法简介

1 RBF
1.1 什么是径向基函数
1985年，Powell提出了多变量插值的径向基函数（RBF）方法。径向基函数是一个取值仅仅依赖于离原点距离的实值函数，也就是Φ（x）=Φ(‖x‖),或者还可以是到任意一点c的距离，c点称为中心点，也就是Φ（x，c）=Φ(‖x-c‖)。任意一个满足Φ（x）=Φ(‖x‖)特性的函数Φ都叫做径向基函数，标准的一般使用欧氏距离（也叫做欧式径向基函数），尽管其他距离函数也是可以的。最常用的径向基函数是高斯核函数 ,形式为 k(||x-xc||)=exp{- ||x-xc||^2/(2*σ)2) } 其中x_c为核函数中心,σ为函数的宽度参数 , 控制了函数的径向作用范围。

1.2 RBF神经网络
RBF神将网络是一种三层神经网络，其包括输入层、隐层、输出层。从输入空间到隐层空间的变换是非线性的，而从隐层空间到输出层空间变换是线性的。流图如下：

RBF网络的基本思想是：用RBF作为隐单元的“基”构成隐含层空间，这样就可以将输入矢量直接映射到隐空间，而不需要通过权连接。当RBF的中心点确定以后，这种映射关系也就确定了。而隐含层空间到输出空间的映射是线性的，即网络的输出是隐单元输出的线性加权和，此处的权即为网络可调参数。其中，隐含层的作用是把向量从低维度的p映射到高维度的h，这样低维度线性不可分的情况到高维度就可以变得线性可分了，主要就是核函数的思想。这样，网络由输入到输出的映射是非线性的，而网络输出对可调参数而言却又是线性的。网络的权就可由线性方程组直接解出，从而大大加快学习速度并避免局部极小问题。
径向基神经网络的激活函数可表示为：

其中xp为第p个输入样本，ci为第i个中心点，h为隐含层的结点数，n是输出的样本数或分类数。径向基神经网络的结构可得到网络的输出为：

当然，采用最小二乘的损失函数表示：

3 RBF神经网络的学习问题

求解的参数有3个：基函数的中心、方差以及隐含层到输出层的权值。
(1）自组织选取中心学习方法：
第一步：无监督学习过程，求解隐含层基函数的中心与方差
第二步：有监督学习过程，求解隐含层到输出层之间的权值
首先，选取h个中心做k-means聚类，对于高斯核函数的径向基，方差由公式求解：
cmax为所选取中心点之间的最大距离。
隐含层至输出层之间的神经元的连接权值可以用最小二乘法直接计算得到，即对损失函数求解关于w的偏导数，使其等于0，可以化简得到计算公式为：
（2）直接计算法
隐含层神经元的中心是随机地在输入样本中选取，且中心固定。一旦中心固定下来，隐含层神经元的输出便是已知的，这样的神经网络的连接权就可以通过求解线性方程组来确定。适用于样本数据的分布具有明显代表性。
（3）有监督学习算法
通过训练样本集来获得满足监督要求的网络中心和其他权重参数，经历一个误差修正学习的过程，与BP网络的学习原理一样，同样采用梯度下降法。因此RBF同样可以被当作BP神经网络的一种。

1.4 RBF神经网络与BP神经网络之间的区别
1.4.1 局部逼近与全局逼近
BP神经网络的隐节点采用输入模式与权向量的内积作为激活函数的自变量，而激活函数采用Sigmoid函数。各调参数对BP网络的输出具有同等地位的影响，因此BP神经网络是对非线性映射的全局逼近。
RBF神经网络的隐节点采用输入模式与中心向量的距离（如欧式距离）作为函数的自变量，并使用径向基函数（如Gaussian函数）作为激活函数。神经元的输入离径向基函数中心越远，神经元的激活程度就越低（高斯函数）。RBF网络的输出与部分调参数有关，譬如，一个wij值只影响一个yi的输出，RBF神经网络因此具有“局部映射”特性。

所谓局部逼近是指目标函数的逼近仅仅根据查询点附近的数据。而事实上，对于径向基网络，通常使用的是高斯径向基函数，函数图象是两边衰减且径向对称的，当选取的中心与查询点（即输入数据）很接近的时候才对输入有真正的映射作用，若中心与查询点很远的时候，欧式距离太大的情况下，输出的结果趋于0，所以真正起作用的点还是与查询点很近的点，所以是局部逼近；而BP网络对目标函数的逼近跟所有数据都相关，而不仅仅来自查询点附近的数据。

1.4.2 中间层数的区别
BP神经网络可以有多个隐含层，但是RBF只有一个隐含层。

1.4.3 训练速度的区别
使用RBF的训练速度快，一方面是因为隐含层较少，另一方面，局部逼近可以简化计算量。对于一个输入x，只有部分神经元会有响应，其他的都近似为0，对应的w就不用调参了。

1.4.4 RBF网络是连续函数的最佳逼近，而BP网络不是。

2 QLearning简介
** 算法思想**
QLearning是强化学习算法中value-based的算法，Q即为Q（s,a）就是在某一时刻的 s 状态下(s∈S)，采取 动作a (a∈A)动作能够获得收益的期望，环境会根据agent的动作反馈相应的回报reward r，所以算法的主要思想就是将State与Action构建成一张Q-table来存储Q值，然后根据Q值来选取能够获得最大的收益的动作。

二、部分源代码

function varargout =PathPlanning(varargin)
% 移动机器人路径规划仿真平台接口：仿真平台提供了机器人工作环境的仿真界面，利用inf=load('inf'),sp=inf.StartPoint,
% EP=inf.EndPoint,WS=inf.env得到机器人工作环境的出发点、目标点位置及障碍物位置信息，工作空间边界及障碍物区域设置为1，自由空间
%设置为0。 
gui_Singleton = 1;
gui_State = struct('gui_Name',       mfilename, ...
                   'gui_Singleton',  gui_Singleton, ...
                   'gui_OpeningFcn', @Simulation_OpeningFcn, ...
                   'gui_OutputFcn',  @Simulation_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 GridSimulation is made visible.
function Simulation_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 GridSimulation (see VARARGIN)

% Choose default command line output for GridSimulation
handles.output = hObject;
% Update handles structure
guidata(hObject, handles);
% UIWAIT makes GridSimulation wait for user response (see UIRESUME)
% uiwait(handles.mainfig);
%cd D:\Simulation\EvolvingPath\path
cla
grid on
xlabel('X'); ylabel('Y');
%初始化，获取各对象句柄
handles.StartPoint=findobj('tag','StartPoint'); %获取“设置开始点”按钮句柄
handles.EndPoint=findobj('tag','EndPoint');     %获取“设置目标点”按钮句柄
handles.Obstacle=findobj('tag','Obstacle');     %获取“设置障碍物”按钮句柄
handles.Start=findobj('tag','Start');           %获取“开始运行”按钮句柄
handles.OldEnv=findobj('tag','OldEnv');           %获取“还原环境”按钮句柄
handles.MainAxes=findobj('tag','MainAxes');     %获取主坐标句柄
handles.MainFigure=findobj('tag','MainFigure'); %获取主窗口句柄
%初始化，设置各按钮显示状态
set(handles.StartPoint,'Enable','on')   %“设置开始点”按钮可用
set(handles.EndPoint,'Enable','off')    %“设置目标点”按钮禁用
set(handles.Obstacle,'Enable','off')    %“设置障碍物”按钮禁用
set(handles.Start,'Enable','off')       %“开始运行”按钮禁用
set(handles.OldEnv,'Enable','off')       %“还原环境”按钮可用
set(handles.MainFigure,'WindowButtonDownFcn','');   %
set(handles.MainFigure,'WindowButtonUpFcn','');     %
set(handles.MainAxes,'ButtonDownFcn','');           %
set(handles.MainAxes,'ButtonDownFcn','');           %
inf=load('inf');    %打开环境信息文件，inf.mat由save命令创建，存储了开始点、目标点、障碍物信息等
XLim=30;    %x轴最大取值
YLim=30;    %y轴最大取值
BreakTask=0;        %初始化终止任务变量
    for i=1:XLim  %将边界设置成障碍物
        for j=1:YLim
            if ((i==1)|(i==XLim)|(j==1)|(j==YLim))
                ws(i,j)=1;
            end
        end
    end
save('inf','ws','-append');
save('inf','BreakTask','-append');

% --- Outputs from this function are returned to the command line.
function varargout = Simulation_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;
% --- Executes on button press in StartPoint.
function StartPoint_Callback(hObject, eventdata, handles)
% hObject    handle to StartPoint (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.StartPoint,'Enable','off')
set(handles.EndPoint,'Enable','on')
set(handles.Obstacle,'Enable','off')
set(handles.Start,'Enable','off')
flag=0;
save('inf','flag','-append');
set(handles.MainFigure,'WindowButtonDownFcn','');
set(handles.MainFigure,'WindowButtonUpFcn','');
set(handles.MainAxes,'ButtonDownFcn','PathPlanning(''MainAxes_ButtonDownFcn'',gcbo,[],guidata(gcbo))');
% --- Executes on button press in EndPoint.
function EndPoint_Callback(hObject, eventdata, handles)
% hObject    handle to EndPoint (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
set(handles.StartPoint,'Enable','off')
set(handles.EndPoint,'Enable','off')
set(handles.Obstacle,'Enable','on')
set(handles.Start,'Enable','on')
flag=1;
save('inf','flag','-append');
%set(handles.MainFigure,'WindowButtonDownFcn','');
%set(handles.MainFigure,'WindowButtonUpFcn','');
set(handles.MainAxes,'ButtonDownFcn','PathPlanning(''MainAxes_ButtonDownFcn'',gcbo,[],guidata(gcbo))');
% --- Executes on mouse press over axes background.
function MainAxes_ButtonDownFcn(hObject, eventdata, handles)
% hObject    handle to MainAxes (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
inf=load('inf');
flag=inf.flag;
start_end=inf.start_end;
p=get(handles.MainAxes,'CurrentPoint');
hold on;
if(flag==0)
    p=round(p);
    start_end(1,1)=p(1,1);start_end(1,2)=p(1,2);   %记录起点信息，给inf.mat文件赋值
    StartPoint(1,1)=p(1,1);StartPoint(1,2)=p(1,2);       %为当前点赋值，当前点为起点的位置信息

    save('inf','StartPoint','-append');
    HRobot=plot(start_end(1,1),start_end(1,2),'pentagram');                %画开始点位置
    text(start_end(1,1)-.5,start_end(1,2)-.5,'起点');
    RobotDirection=inf.RobotDirection;%机器人方向应该是传递参数
    x=start_end(1,1);
    y=start_end(1,2);
    RobotPosX=x;
    RobotPosY=y;
   save('inf','RobotPosX','-append');
   save('inf','RobotPosY','-append');
else
    p=round(p);
    start_end(2,1)=p(1,1);start_end(2,2)=p(1,2);
    EndPoint(1,1)=p(1,1);EndPoint(1,2)=p(1,2);       %为当前点赋值，当前点为结束点的位置信息
    EndPoint=round(EndPoint);
    save('inf','EndPoint','-append');
    plot(start_end(2,1),start_end(2,2),'*','color','r')
    text(start_end(2,1)-.5,start_end(2,2)+.5,'目标点');
end
save('inf','start_end','-append');
set(handles.MainAxes,'ButtonDownFcn','');
set(handles.MainAxes,'ButtonDownFcn','');

% --- Executes on button press in Obstacle.
function Obstacle_Callback(hObject, eventdata, handles)
% hObject    handle to Obstacle (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
env=zeros(50);
save('inf','env','-append');
set(handles.StartPoint,'Enable','off')
set(handles.EndPoint,'Enable','off')
set(handles.Obstacle,'Enable','on')
set(handles.Start,'Enable','on')
set(handles.OldEnv,'Enable','on')       %“开始运行”按钮禁用
set(handles.MainFigure,'WindowButtonDownFcn','PathPlanning(''MainFigure_WindowButtonDownFcn'',gcbo,[],guidata(gcbo))');
%set(handles.MainFigure,'WindowButtonUpFcn','PathPlanning(''MainFigure_WindowButtonUpFcn'',gcbo,[],guidata(gcbo))');
function MainFigure_WindowButtonDownFcn(hObject, eventdata, handles)
% hObject    handle to MainFigure (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
    inf=load('inf'); 
    ws=inf.env;
    Pos=get(handles.MainAxes,'CurrentPoint');
    Pos=round(Pos);
    XPos=Pos(1,1);YPos=Pos(1,2);       %当前点坐标
    X=[XPos-.5,XPos-.5,XPos+.5,XPos+.5];
    Y=[YPos-.5,YPos+.5,YPos+.5,YPos-.5];
    fill(X,Y,[0 0 0])                   %画障碍物
    text(13-.2,12,'B','color',[1 1 1]);
    text(7-.2,8,'A','color',[1 1 1]);
  %  for i=XPos-1:XPos+1
   %     for j=YPos-1:YPos+1
  %          if((i>0)&(i<=XLim))           %防止出现环境矩阵元素下标为零
  %              if((j>0)&(j<=YLim))
                    ws(XPos,YPos)=1;
  %              end
  %          end
  %      end
  %end
   env=ws;
    save('inf','env','-append');

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


% Hint: get(hObject,'Value') returns toggle state of SensorChecked

function RobotVelocity_Callback(hObject, eventdata, handles)    %设置机器人运行速度
% hObject    handle to RobotVelocity (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 RobotVelocity as text
%        str2double(get(hObject,'String')) returns contents of RobotVelocity as a double   
 
function RobotRadius_Callback(hObject, eventdata, handles)      %设置机器人半径
% hObject    handle to RobotRadius (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 RobotRadius as text
%        str2double(get(hObject,'String')) returns contents of RobotRadius as a double
% --- Executes during object creation, after setting all properties.

function SensorMaxValue_Callback(hObject, eventdata, handles)       %设置传感器测量范围
% hObject    handle to SensorMaxValue (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 SensorMaxValue as text
%        str2double(get(hObject,'String')) returns contents of SensorMaxValue as a double    
    
function Handbook_Callback(hObject, eventdata, handles)                 %系统简介
% hObject    handle to Handbook (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
uiopen('系统简介.txt',1)

function ClearScreen_Callback(hObject, eventdata, handles)      %重新开始
% hObject    handle to ClearScreen (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)

set(handles.StartPoint,'Enable','on')   %“设置开始点”按钮可用
set(handles.EndPoint,'Enable','off')    %“设置目标点”按钮禁用
set(handles.Obstacle,'Enable','off')    %“设置障碍物”按钮禁用
set(handles.Start,'Enable','off')       %“开始运行”按钮禁用
cla
clear all
% --- Executes on button press in OldEnv.
function OldEnv_Callback(hObject, eventdata, handles)
% hObject    handle to OldEnv (see GCBO)
% eventdata  reserved - to be defined in a future version of MATLAB
% handles    structure with handles and user data (see GUIDATA)
XLim=30;    %x轴最大取值
YLim=30;    %y轴最大取值
inf=load('inf');
ws=inf.env;  %得到障碍物信息
SP=inf.StartPoint; %出发点位置
EP=inf.EndPoint;   %目标点位置
set(handles.StartPoint,'Enable','off')
set(handles.EndPoint,'Enable','off')
set(handles.Obstacle,'Enable','off')
set(handles.Start,'Enable','on')
HandleStart=line([SP(1,1) SP(1,1)],[SP(1,2) SP(1,2)]);  %设置开始点
text(SP(1,1)-.5,SP(1,2)-.5,'起点');
HandleTarget=line([EP(1,1) EP(1,1)],[EP(1,2) EP(1,2)]); %设置目标点
text(EP(1,1)-.5,EP(1,2)+.5,'目标点');
set(HandleStart,'marker','pentagram')
set(HandleTarget,'marker','*','color','r')
    for i=1:XLim  %将边界设置成障碍物
        for j=1:YLim
            if ((i==1)|(i==XLim)|(j==1)|(j==YLim))
                ws(i,j)=1;
            end
        end
    end
for i=2:XLim-1                          %还原障碍物信息
    for j=2:YLim-1
        if((ws(i,j)==1))
           X=[i-.5,i-.5,i+.5,i+.5];
            Y=[j-.5,j+.5,j+.5,j-.5];
         fill(X,Y,[0 0 0]);  %设置障碍物
        end
    end
end
           X=[1-.5,1-.5,1+.5,1+.5];
            Y=[6-.5,6+.5,6+.5,6-.5];
         fill(X,Y,[0 0 0]);  %设置障碍物
          X=[1-.5,1-.5,1+.5,1+.5];
            Y=[14-.5,14+.5,14+.5,14-.5];
         fill(X,Y,[0 0 0]);  %设置障碍物
                   X=[10-.5,10-.5,10+.5,10+.5];
            Y=[1-.5,1+.5,1+.5,1-.5];
         fill(X,Y,[0 0 0]);  %设置障碍物
text(13-.2,12,'B','color',[1 1 1]);
text(7-.2,8,'A','color',[1 1 1]);
X=[0,0,.5,.5];
Y=[0,YLim,YLim,0];
fill(X,Y,[0 0 0]);  %设置边界为障碍物
X=[XLim-.5,XLim-.5,XLim,XLim];
Y=[0,YLim,YLim,0];
fill(X,Y,[0 0 0]);  %设置边界为障碍物
X=[0,0,XLim,XLim];
Y=[YLim-.5,YLim,YLim,YLim-.5];
fill(X,Y,[0 0 0]);  %设置边界为障碍物
X=[0,0,XLim,XLim];
Y=[0,.5,.5,0];
fill(X,Y,[0 0 0]);  %设置边界为障碍物
%axis([0 20 0 20])
%机器人当前位置设置为开始位置
RobotPosX=SP(1,1);
RobotPosY=SP(1,2);
    save('inf','RobotPosX','-append');
    save('inf','RobotPosY','-append');

  

 

  
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
  
270
  
271
  
272
  
273
  
274
  
275
  
276
  
277
  
278
  
279
  
280
  
281
  
282
  
283
  
284
  
285
  
286
  
287
  
288
  
289
  
290
  
291
  
292
  
293
  
294
  
295
  
296
  
297
  
298
  
299
  
300
  
301
  
302
  
303
  
304
  
305
 

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

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

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

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