【路径规划】人工势场求解机器人路径动态规划matlab源码_人工势场算法的动态实现

原理

人工建立势场，将障碍物设置成斥力，目标设置成吸引力，进行力的矢量相加，最后算出合力的方向。\ \

引力场

常用的引力函数：\ \ 这里的ε是尺度因子.ρ(q,q_goal)表示物体当前状态与目标的距离。引力场有了，那么引力就是引力场对距离的导数(类比物理里面W=FX)：

斥力场

\ 公式(3)是传统的斥力场公式。公式中η是斥力尺度因子，ρ(q,q_obs)代表物体和障碍物之间的距离。ρ_0代表每个障碍物的影响半径。换言之，离开一定的距离，障碍物就对物体没有斥力影响。

斥力就是斥力场的梯度\ \ \ 总的场就是斥力场合引力场的叠加，也就是U=Uatt+Urep,总的力也是对对应的分力的叠加，如下图所示：

优点：

简单实用，有良好的实时性\ 结构简单，便于底层的实时控制，在实时壁障和平滑的轨迹控制方面得到广泛的应用

缺点：

1. 当目标点距离较远的话，引力将变得特别大，相对较小的斥力下，物体路径可能会碰到障碍物
2. 当目标点附近有障碍物时，斥力将非常大，引力相对较小，物体很难到达目标点
3. 在某个点，引力斥力刚好相等，方向相反，物体容易陷入局部最优解或震荡
4. 容易陷入局部最优解

改进：

1. 对于碰到障碍物的问题，可以通过修正引力函数来解决，避免距离过大导致引力过大。\ 和(1)式相比，(5)式增加了范围限定。d*_goal 给定了一个阈值限定了目标和物体之间的距离。对应的梯度也就是引力相应变成：\
2. 目标点附近有障碍物导致目标不可达的问题，引入一种新的斥力函数

\ 这里在原有斥力场的基础上，加上了目标和物体距离的影响,(n是正数，我看到有篇文献上n=2)。直观上来说，物体靠近目标时，虽然斥力场要增大，但是距离在减少，所以在一定程度上可以起到对斥力场的拖拽作用\ 相应斥力变成：\

所以可以看到这里引力分为两个部分，编程时要格外注意

1. 局部最优问题是一个人工势场法的一个大问题，这里可以通过加一个随机扰动，让物体跳出局部最优值。类似于梯度下降法局部最优值的解决方案。

```

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

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

% Last Modified by GUIDE v2.5 28-Nov-2013 20:49:25

% Begin initialization code - DO NOT EDIT guiSingleton = 1; guiState = struct('guiName', mfilename, ... 'guiSingleton', guiSingleton, ... 'guiOpeningFcn', @AFMOpeningFcn, ... 'guiOutputFcn', @AFMOutputFcn, ... 'guiLayoutFcn', [] , ... 'guiCallback', []); if nargin && ischar(varargin{1}) guiState.gui_Callback = str2func(varargin{1}); end

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

% --- Executes just before AFM is made visible. function AFM_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 AFM (see VARARGIN)

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

% Update handles structure guidata(hObject, handles);

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

% --- Outputs from this function are returned to the command line. function varargout = AFM_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 during object creation, after setting all properties. function axes4CreateFcn(hObject, eventdata, handles) Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 %n=3;%障碍个数 longth=1.2;%步长 J=2000;%循环迭代次数 %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 %[mObs,mObsR]=ObsGenerate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj axis([-20 120 -20 120]); axis equal; hold on; axis off; %set(gcf,'color','y') %fill([-10,110,110,-10],[-10 -10 110 110],'w') fill([-20,120,120,-20],[-20 -20 120 120],'y') %title ('人工势场路径规划'); text(-5,-5,' Start','FontSize',12); fill([95,120,120,95],[-20 -20 10 10],'w') text(100,5,'Notes:','FontSize',12) plot(101,-5,'sb','markerfacecolor','b'); text(101,-5,' Robot','FontSize',12); plot(101,-15,'om','markerfacecolor','m'); text(101,-15,' Ball','FontSize',12); plot(0,0,'bs') car=plot(0,0,'sb','markerfacecolor','b'); %carname=text(0,0,' ','FontSize',12); object=plot(0,100,'om','markerfacecolor','m'); %objectname=text(0,100,' Ball','FontSize',12); but=1; xobs=1; yobs=1;

% hObject handle to axes4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called

% Hint: place code in OpeningFcn to populate axes4 % --- Executes on button press in pushbutton1. function pushbutton1Callback(hObject, eventdata, handles) %main %%%%%%%%%%%%%%%%%%%%%初始化参数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 %n=3;%障碍个数 longth=1.2;%步长 %循环迭代次数 J=2000; %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 %[mObs,mObsR]=ObsGenerate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj axis([-20 120 -20 120]); axis equal; hold on; axis off; %set(gcf,'color','y') %fill([-10,110,110,-10],[-10 -10 110 110],'w') fill([-20,120,120,-20],[-20 -20 120 120],'y') %title ('人工势场路径规划'); text(-5,-5,' Start','FontSize',12); fill([95,120,120,95],[-20 -20 10 10],'w') text(100,5,'Notes:','FontSize',12) plot(101,-5,'sb','markerfacecolor','b'); text(101,-5,' Robot','FontSize',12); plot(101,-15,'om','markerfacecolor','m'); text(101,-15,' Ball','FontSize',12); plot(0,0,'bs') car=plot(0,0,'sb','markerfacecolor','b'); %carname=text(0,0,' ','FontSize',12); object=plot(0,100,'om','markerfacecolor','m'); %objectname=text(0,100,' Ball','FontSize',12); but=1; xobs=1; yobs=1; %pause(1); %选取障碍 %h=msgbox('单击鼠标左键选择障碍，单击右键完成选择','提示'); %uiwait(h,10); %if ishandle(h) == 1 % delete(h); %end numobs=1; while but == 1 [xobs,yobs,but] = ginput(1); if but==1 mObs(numobs,:)=[xobs yobs]; mObsR(numobs)=3; %mObs=[0 40;20 70;60 50]; Po=min(mObsR)/0.5; % for i=1:n Theta=0:pi/20:pi; xx =mObs(numobs,1)+cos(Theta)*mObsR(numobs); yy= mObs(numobs,2)+sin(Theta)*mObsR(numobs); fill(xx,yy,'w') Theta=pi:pi/20:2*pi; xx =mObs(numobs,1)+cos(Theta)*mObsR(numobs); yy= mObs(numobs,2)+sin(Theta)*mObsR(numobs); fill(xx,yy,'k') numobs=num_obs+1;

end
%plot(xx,yy,'LineWidth',2);

% xval=floor(xval); % yval=floor(yval); % MAP(xval,yval)=-1;%Put on the closed list as well % plot(xval+.5,yval+.5,'ro'); end%End of While loop numobs=numobs-1; %object=plot(0,100,'om'); %car=plot(Xo(1),Xo(2),'sb'); %carname=text(Xo(1),Xo(2),'Robot','FontSize',12); %objectname=text(0,100,'Ball','FontSize',12); %startFlag=pushbutton2Callback(hObject, eventdata, handles); uiwait; %***********初始化结束，开始主体循环************** for j=1:J%循环开始 % if j<200 x=j/1.5;y=100; % else % x=100;y=100; %end m_Target=[x,y]; Goal(j,1)=m_Target(1); Goal(j,2)=m_Target(2); Current(j,1)=Xj(1);%Goal保存走过的每个点的坐标。刚开始先将起点放进该向量 Current(j,2)=Xj(2); %调用计算角度模块 [angle_att,angle_rep]=compute_angle(Xj,m_Target,m_Obs,num_obs); %调用计算引力模块 [Fatt,Uatt(j)]=compute_Attract(Xj,m_Target,k,angle_att); %调用计算斥力模块 [Frep,Fatt_add,Urep(j)]=compute_repulsion(Xj,m_Target,m_Obs,m_ObsR,... m,angle_att,angle_rep,num_obs,Po); %计算合力和方向 [Position_angle(j)]=compute_Ftotal(Fatt,Frep,Fatt_add,num_obs); %计算车的下一步位置 Xnext(1)=Xj(1)+longth*cos(Position_angle(j)); Xnext(2)=Xj(2)+longth*sin(Position_angle(j)); %保存车的每一个位置在向量中 Xj=Xnext; % Draw(Xj,m_Obs,m_Target,n); %判断 % if (Is_Reach(Xj,m_Target,longth)==1)%是应该完全相等的时候算作到达， %还是只是接近就可以？现在按完全相等的时候编程。 % K=j;%记录迭代到多少次，到达目标。 % break; %记录此时的j值 % end%如果不符合if的条件，重新返回循环，继续执行。 if x>=100&&IsReach(Xj,mTarget,longth)==1 break; end
X=Current(j,1); Y=Current(j,2); A=Goal(j,1); B=Goal(j,2); delete(car);
car=plot(X,Y,'sb','markerfacecolor','b'); %v=plot(X,Y,'ro','linewidth',2); %set(v,'Color',[1,0,0]) %plot(Xo(1),Xo(2),'ms'); delete(object); object=plot(A,B,'om','markerfacecolor','m'); %delete(carname); %carname=text(X,Y,' Robot','FontSize',12); %delete(objectname); %objectname=text(A,B,' Ball','FontSize',12); pause(0.05)
end%大循环结束 %************************以下是图形显示部分************************ %画势场分析图 %Draw(Uatt,Urep); %画模型效果图 %DrawModel(Xo,Current,mObs,mObsR,mTarget,n);
%画总体势场分布图 %DrawPotential(Xo,mObs,m,mObsR,Po,mTarget,k,n,[0 100],[0 100]); clear; % --- Executes on button press in pushbutton2. function pushbutton2Callback(hObject, eventdata, handles) uiresume; % 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)

% --- Executes on button press in pushbutton3. function pushbutton3Callback(hObject, eventdata, handles) uiwait; % hObject handle to pushbutton3 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % --- Executes on button press in pushbutton4. function pushbutton4Callback(hObject, eventdata, handles) uiresume; % hObject handle to pushbutton4 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % --- Executes on button press in pushbutton5. function pushbutton5_Callback(hObject, eventdata, handles)

Xo=[0 0];%起点位置 k=1;%计算引力需要的增益系数 m=1;%计算斥力的增益系数 %n=3;%障碍个数 longth=1.2;%步长 J=2000;%循环迭代次数 %如果不能实现预期目标，可能也与初始的增益系数，Po设置的不合适有关。 K=0;%初始化 %[mObs,mObsR]=ObsGenerate([5 10],[20 80],[10 80],n); Xj=Xo;%j=1循环初始，将车的起始坐标赋给Xj axis([-20 120 -20 120]); axis equal; hold on; axis off; %set(gcf,'color','y') %fill([-10,110,110,-10],[-10 -10 110 110],'w') fill([-20,120,120,-20],[-20 -20 120 120],'y') %title ('人工势场路径规划'); text(-5,-5,' Start','FontSize',12); fill([95,120,120,95],[-20 -20 10 10],'w') text(100,5,'Notes:','FontSize',12) plot(101,-5,'sb','markerfacecolor','b'); text(101,-5,' Robot','FontSize',12); plot(101,-15,'om','markerfacecolor','m'); text(101,-15,' Ball','FontSize',12); plot(0,0,'bs') car=plot(0,0,'sb','markerfacecolor','b'); %carname=text(0,0,' ','FontSize',12); object=plot(0,100,'om','markerfacecolor','m'); %objectname=text(0,100,' Ball','FontSize',12); but=1; xobs=1; y_obs=1; % hObject handle to pushbutton5 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)

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

% -------------------------------------------------------------------- function help_Callback(hObject, eventdata, handles)

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

% -------------------------------------------------------------------- function state_Callback(hObject, eventdata, handles) aa=['1.进入仿真界面，单击 <绘制障碍> 功能键在工作窗口绘制障碍' ... char(10) '2.绘制障碍结束，单击右键完成绘制，十字光标消失表示完成' ... char(10) '3.点击 <演示> 功能键，将按预订算法自动演示' ... char(10) '4.点击 <暂停> 功能键，演示过程将暂停运行，点击 <继续> 功能键后，将从暂停位置继续演示' char(10)... '5.点击 <重置> 功能键，仿真界面将回到初始状态，重新等待操作']; msgbox(aa,'使用说明'); % hObject handle to state (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)

% -------------------------------------------------------------------- function about_Callback(hObject, eventdata, handles) msgbox(' 北航自动化','关于'); % hObject handle to about (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA)

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

% -------------------------------------------------------------------- function out_Callback(hObject, eventdata, handles) selection=questdlg(['是否退出演示窗口？'], ... ['退出'],'Yes','No','Yes');%当选择退出按钮时，得出一个问是否确定关闭的框 if strcmp(selection,'No') return;
else if strcmp(selection,'Yes') clc; %当选择关闭时，清空所有matla输入面上的所有错误信息，同时关闭图像窗口 clear all; delete(gcf); else return; end end

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