A list,
Artificial potential field method is a common method for local path planning. This method assumes that the robot moves under a kind of virtual force field.
As shown in the figure, the robot is moving in a two-dimensional environment. The figure indicates the relative positions of the robot, the obstacle and the target.
This figure clearly illustrates the function of artificial potential field method. The initial point of the object is on a high “mountain”, and the target point to be reached is under the “mountain foot”, which forms a potential field. Under the guidance of this potential, the object avoids obstacles and reaches the target point.
The artificial potential field includes the repulsive field in the gravitational field, in which the target point exerts gravity on the object and guides the object towards it (this point is somewhat similar to the heuristic function H in the A* algorithm). Obstacles act as repulsive forces to prevent objects from colliding with them. The net force on the body at each point along its path is equal to the sum of all the repulsive and gravitational forces at that point. The key here is how to construct gravitational and repulsive fields. Let’s discuss it separately:
The gravitational field:
Commonly used gravity functions:
ρ(q,q_goal) represents the distance between the current state of the object and the target. With the gravitational field, gravity is the derivative of the gravitational field with respect to distance (W=FX in physics analogy) :
[δx,δy], this symbol is partial derivative, not quite correct, sorry.
Fig. Gravitational field model
Repulsive force field:
Formula (3) is a traditional repulsive field formula, and it is not clear how it is derived. In the formula, η is the repulsive scale factor, ρ (q,q_obs) represents the distance between the object and the obstacle. ρ_0 represents the radius of influence of each obstacle. In other words, away from a certain distance, the obstacle has no repulsive effect on the object.
Repulsion is the gradient of the repulsion field
Fig repulsive field model
The total field is the superposition of the gravitational field in the repulsive field, that is, U=U_att+U_rep. The total force is also the superposition of the corresponding components, as shown in the figure below:
Second, existing problems
(a) When the object is further from the target point, the attraction will become extremely strong, and the relatively small repulsion force may even be negligible, so that the object may encounter obstacles in its path
(b) When there is an obstacle near the target point, the repulsive force will be very large and the attraction will be relatively small, making it difficult for the object to reach the target point
(c) At a certain point, the gravitational and repulsive forces are just equal in magnitude and the direction is opposite, then the object is easy to fall into the local optimal solution or oscillation
Various improved versions of the artificial potential field method
(a) The problem that obstacles may be encountered can be solved by modifying the gravity function to avoid excessive gravity due to the distance from the target point
Compared with Eq. (1), eq. (5) adds scope limitation. D *_goal is given a threshold that limits the distance between the target and the object. The corresponding gradient, the gravitational force, becomes:
(b) The target is unreachable due to obstacles near the target point, and a new repulsive function is introduced
Here, on the basis of the original repulsive field, the influence of the distance between the target and the object is added (n is a positive number, I see that n=2 in some literature). Intuitively speaking, when the object approaches the target, although the repulsive force field increases, the distance decreases, so it can play a role of dragging the repulsive force field to a certain extent
The corresponding repulsion becomes:
So you can see that there are two parts to gravity, and you have to be careful when you program it
(c) The local optimum problem is a big problem of the artificial potential field method, where the object can jump out of the local optimum value by adding a random disturbance. A solution similar to the local optimal value of gradient descent.
Ii. Source code
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 anew AFM or raises the
% existing singleton*. Starting from the left, property value pairs are
% applied to the GUI before AFM_OpeningFcn gets called. An
% unrecognized property name or invalid value makes property application
% stop. All inputs are passed to AFM_OpeningFcn 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
gui_Singleton = 1;
gui_State = struct('gui_Name', mfilename, ...
'gui_Singleton', gui_Singleton, ...
'gui_OpeningFcn', @AFM_OpeningFcn, ...
'gui_OutputFcn', @AFM_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 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 axes4_CreateFcn(hObject, eventdata, handles)
Xo= [0 0]; % starting position k=1; % Calculate the gain coefficient required by gravity m=1; % Calculate the gain coefficient of repulsion %n=3; % Number of obstacles longth=1.2; J = % step length2000; % Number of loop iterations % If the desired goal is not achieved, it may also be due to the initial gain coefficient, Po setting is not appropriate. K=0; [m_Obs,m_ObsR]=Obs_Generate([5 10], [20 80], [10 80],n); Xj=Xo; %j=1At the beginning of the cycle, assign the starting coordinates of the vehicle to 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 ('Artificial potential field path Planning');
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');
%car_name=text(0.0.' '.'FontSize'.12);
object=plot(0.100.'om'.'markerfacecolor'.'m');
%object_name=text(0.100.' Ball'.'FontSize'.12);
but=1;
x_obs=1;
y_obs=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 pushbutton1_Callback(hObject, eventdata, handles)The main % % % % % % % % % % % % % % % % % % % % % % initialization parameter % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % % 5 Xo= [0 0]; % starting position k=1; % Calculate the gain coefficient required by gravity m=1; % Calculate the gain coefficient of repulsion %n=3; % Number of obstacles longth=1.2; % step size % number of iterations J=2000; % Failure to achieve the desired goal may also be due to improper initial gain coefficient, Po setting. K=0; [m_Obs,m_ObsR]=Obs_Generate([5 10], [20 80], [10 80],n); Xj=Xo; %j=1At the beginning of the cycle, assign the starting coordinates of the vehicle to 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 ('Artificial potential field path Planning');
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');
%car_name=text(0.0.' '.'FontSize'.12);
object=plot(0.100.'om'.'markerfacecolor'.'m');
%object_name=text(0.100.' Ball'.'FontSize'.12);
but=1;
x_obs=1;
y_obs=1;
%pause(1); % selection obstacle %h=msgbox('Click the left mouse button to select obstacles, right-click to complete selection'.'tip');
%uiwait(h,10);
%if ishandle(h) = =1
% delete(h);
%end
num_obs=1;
while but == 1
[x_obs,y_obs,but] = ginput(1);
if but==1
m_Obs(num_obs,:)=[x_obs y_obs];
m_ObsR(num_obs)=3;
%m_Obs=[0 40;20 70;60 50];
Po=min(m_ObsR)/0.5;
% for i=1:n
Theta=0:pi/20:pi;
xx =m_Obs(num_obs,1) +cos(Theta)*m_ObsR(num_obs);
yy= m_Obs(num_obs,2) +sin(Theta)*m_ObsR(num_obs);
fill(xx,yy,'w')
Theta=pi:pi/20:2*pi;
xx =m_Obs(num_obs,1) +cos(Theta)*m_ObsR(num_obs);
yy= m_Obs(num_obs,2) +sin(Theta)*m_ObsR(num_obs);
fill(xx,yy,'k')
num_obs=num_obs+1;
end
%plot(xx,yy,'LineWidth'.2);
% xval=floor(xval);
% yval=floor(yval);
% MAP(xval,yval)=- 1; %Put on the closedlist as well
% plot(xval+. 5,yval+. 5.'ro');
end%End of While loop
num_obs=num_obs- 1;
%object=plot(0.100.'om');
%car=plot(Xo(1),Xo(2),'sb');
%car_name=text(Xo(1),Xo(2),'Robot'.'FontSize'.12);
%object_name=text(0.100.'Ball'.'FontSize'.12); %startFlag=pushbutton2_Callback(hObject, eventdata, handles); uiwait; %*************** end of initialization, start main loop ******************for j=1:J% Start of loop %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 saves the coordinates of each point you walk through. So let's just start by putting the starting point in the vector Current(j,2)=Xj(2); % Angle_ATT, ANGLE_rep]=compute_angle(Xj,m_Target,m_Obs,num_obs); % Call gravity module [Fatt,Uatt(j)]=compute_Attract(Xj,m_Target,k, ANGLE_ATT);Copy the code3. Operation results
Fourth, note
Version: 2014 a
Complete code or ghostwrite plus 1564658423