[Path planning] Robot dynamic obstacle avoidance path planning based on matlab A_star algorithm [including Matlab source code 1033]

[Path planning] Robot dynamic obstacle avoidance path planning based on matlab A_star algorithm [including Matlab source code 1033]

1. Introduction

A algorithm
A algorithm is a typical heuristic search algorithm, based on the Dijkstra algorithm. It is widely used in game maps and the real world to find the shortest path between two points. The main thing of the A algorithm is to maintain a heuristic evaluation function, as shown in equation (1).
f(n)=g(n)+h(n)(1)
Among them, f(n) is the corresponding heuristic function when the algorithm searches for each node. It consists of two parts. The first part g(n) is the actual travel cost from the starting node to the current node, and the second part h(n) is the estimated value of the travel cost from the current node to the end point. Each time the algorithm expands, the node with the smallest value of f(n) is selected as the next node on the optimal path.
In practical applications, if the shortest distance is taken as the optimization goal, h(n) is often taken as the Euclidean Distance or Manhattan Distance from the current point to the end point. If h(n)=0, it means that no information about the current node and destination is used, and the A
algorithm degenerates to the non-heuristic Dijkstra algorithm. The algorithm search space becomes larger and the search time becomes longer.
The steps of the A* algorithm are as follows. The algorithm maintains two sets: P list and Q list. The P table stores those nodes that have been searched but have not been added to the optimal path tree; the Q table maintains those nodes that have been added to the optimal path tree.
(1) Set the P and Q tables to be empty, add the starting point S to the P list, set its g value to 0, the parent node to be empty, and set the g value of other nodes in the road network to infinity.
(2) If the P list is empty, the algorithm fails. Otherwise, select the node with the smallest f value in the P list, record it as BT, and add it to the Q list. Judge whether BT is the end point T, if yes, go to step (3); otherwise, find each adjacent node NT of BT according to the topological attributes of the road network and traffic rules, and perform the following steps:

Calculate the heuristic value of
NT f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
where cost(BT, NT) It is the cost of passing BT to NT.
If NT is in the P table, and the g value calculated by formula (3) is smaller than the original g value of NT, then the g value of NT is updated to the result of formula (3), and the parent node of NT is set to BT.
If NT is in the Q table, and the g value calculated by equation (3) is smaller than the original g value of NT, then the g value of NT is updated to the result of equation (3), and the parent node of NT is set to BT, and Move NT to the P list.
If NT is neither in the P list nor in the Q list, set the parent node of NT to BT and move NT to the P list.
Go to step (2) to continue execution.
(3) Backtrack from the end point T, find the parent nodes in turn, and add them to the optimized path until the starting point S, and then the optimized path can be obtained.

2. the source code

function varargout = Astar(varargin) % ASTAR MATLAB code for Astar.fig % ASTAR, by itself, creates a new ASTAR or raises the existing % singleton*. % % H = ASTAR returns the handle to a new ASTAR or the handle to % the existing singleton*. % % ASTAR( 'CALLBACK' ,hObject,eventData,handles,...) calls the local % function named CALLBACK in ASTAR.M with the given input arguments. % % ASTAR( 'Property' , 'Value' ,...) creates a new ASTAR or raises the % existing singleton*. Starting from the left, property value pairs are % applied to the GUI before Astar_OpeningFcn gets called. An % unrecognized property name or invalid value makes property application % stop. All inputs are passed to Astar_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 Astar Last Modified by GUIDE V2% .5 30 -OCT -2020 10 : 58 : 46 is % Begin initialization code-DO NOT EDIT gui_Singleton = 1 ; gui_State = struct( 'gui_Name' , mfilename, ... 'gui_Singleton' , gui_Singleton, ... 'gui_OpeningFcn' , @Astar_OpeningFcn, ... 'gui_OutputFcn' , @Astar_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 Astar is made visible. function Astar_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 Astar (see VARARGIN) % Choose default command line output for Astar handles.output = hObject; % Update handles structure guidata (hObject, handles) ; % UIWAIT makes Astar wait for user response (see UIRESUME) % uiwait ( handles.figure1 ) ; % --- Outputs from this function are returned to the command line. function varargout = Astar_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 axes1_CreateFcn (hObject, eventdata , handles) %%%%%%%%%%%%%%%%%%%%%%%%%%Initialization parameters%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%% Xo =[ 0 0 ];% starting position longth = 1 ;% step length J = 2000 ;% number of loop iterations Xj=Xo;%j= 1 Initial cycle, assign the starting coordinates of the car to Xj MAX_X = 100 ; MAX_Y = 100 ; %This array stores the coordinates of the map and the %Objects in each coordinate MAP = 2 *(ones(MAX_X,MAX_Y)); % Obtain Obstacle, Target and Robot Position % Initialize the MAP with input values % Obstacle = -1 , Target = 0 , Robot = 1 , Space = 2 j = 0 ; x_val = 1 ; y_val = 1 ; axis([ -20 120 -20 120 ]) axis equal; hold on; axis off; % set (gcf, 'color' , 'y' ) %title ( 'A*Algorithm path planning' ); fill([ -20 , 120 , 120 , -20 ],[ -20 -20 120 120 ], 'y' ) 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( 1 , 1 , 'bs' ) car=plot( 1 , 1 , '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 ; text( -4 , -4 , 'Start' , 'FontSize' , 12 ); n = 0 ;%Number of Obstacles % BEGIN Interactive Obstacle, Target, Start Location selection xTarget = 0 ;%X Coordinate of the Target yTarget = 100 ;%Y Coordinate of the Target m_Target=[xTarget,yTarget]; %object=plot( 1 , 100 , 'om' , 'markerfacecolor' , 'm' ); %object_name=text( 1 , 100 , 'Ball' , 'FontSize' , 12 ); %Select the starting position of the trolley xval = 1 ; yval = 1 ; xStart=xval; yStart=yval; MAP(xval,yval) = 1 ; %car=plot( 1 , 1 , 'bs' , 'markerfacecolor' , 'b' ); %car_name=text( 1 , 1 , '' , 'FontSize' , 12 ); % hObject handle to axes1 (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 axes1 % --- Executes on button press in pushbutton1. function pushbutton1_Callback (hObject, eventdata, handles) %main Xo =[ 0 0 ];% starting position longth = 1 ;% step length J = 2000 ;% number of loop iterations Xj=Xo;%j= 1 Initial cycle, assign the starting coordinates of the car to Xj %DEFINE THE 2 -D MAP ARRAY MAX_X = 100 ; MAX_Y = 100 ; %MAX_VAL = 100 ; %This array stores the coordinates of the map and the %Objects in each coordinate MAP = 2 *(ones(MAX_X,MAX_Y)); % Obtain Obstacle, Target and Robot Position % Initialize the MAP with input values % Obstacle = -1 , Target = 0 , Robot = 1 , Space = 2 j = 0 ; x_val = 1 ; y_val = 1 ; axis([ -20 120 -20 120 ]) %grid on; %hold on; axis equal; hold on; axis off; % set (gcf, 'color' , 'y' ) %title ( 'A*Algorithm path planning' ); fill([ -20 , 120 , 120 , -20 ],[ -20 -20 120 120 ], 'y' ) 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( 1 , 1 , 'bs' ) car=plot( 1 , 1 , '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 ; text( -4 , -4 , 'Start' , 'FontSize' , 12 ); n = 0 ;%Number of Obstacles % BEGIN Interactive Obstacle, Target, Start Location selection xTarget = 0 ;%X Coordinate of the Target yTarget = 100 ;%Y Coordinate of the Target m_Target=[xTarget,yTarget]; %object=plot( 1 , 100 , 'om' , 'markerfacecolor' , 'm' ); %object_name=text( 1 , 100 , 'Ball' , 'FontSize' , 12 ); %Select the starting position of the trolley xval = 1 ; yval = 1 ; xStart=xval; yStart=yval; MAP(xval,yval)= 1 ; while but == 1 [xval,yval,but] = ginput( 1 ); if but== 1 xval = floor (xval); yval = floor (yval); MAP(xval,yval)= -1 ;%Put on the closed list as well %plot(xval,yval, 'ro' , 'markerfacecolor' , 'r' , 'MarkerSize' , 10 ); Theta = 0 :pi/20 :pi; xx = xval+ cos (Theta)* 3 ; yy = yval+ sin (Theta)* 3 ; fill(xx,yy, 'w' ) Theta=pi:pi/20 : 2 *pi; xx = xval+ cos (Theta)* 3 ; yy = yval+ sin (Theta)* 3 ; fill(xx,yy, 'k' ) end end%End of While loop Copy code

3. running results

4. remarks

Version: 2014a

Complete code or write on behalf of 1564658423