机器人路径规划 无需积分下载

function varargout = pathPlaning(varargin) % Author：Yang zeGang % Email: yangze18@live.com % Version:0.0.1 % Description: an open source matlab project for robot path planing,PSO algorithm with Bezier % curve, % Usage:user can add barriers manually(cricle.triangle.rectangle...), % Git url:https://github.com/zegangYang/PSO_PathPlaningNew % TODO: gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @pathPlaning_OpeningFcn, ... 'gui_OutputFcn', @pathPlaning_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 % --- 界面及数据初始化函数 function pathPlaning_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); % 设置axes的坐标轴的范围 set(handles.axes1,'XLim',[0 12000],'YLim',[0 12000]); axis square hold on global lastPosi_Draw curenPosi_Draw %绘制圆时圆心与终止位置的坐标 global draw_ok_flag %绘制完成标志 global barriers_num barriers_info_buf % 障碍物的个数 与 障碍物信息数组 global add_barrier_flag % 按下按钮添加障碍物的标志 global P0 P1 % 路径的起点与终点 global PSO_barriers_num % 优化时的障碍物的个数 global x_spots y_spots % 障碍物的横纵坐标点 global D_safe % 安全距离 global num_ba % 障碍物的坐标点个数 lastPosi_Draw = zeros(1,2); curenPosi_Draw = zeros(1,2); draw_ok_flag = 0; barriers_info_buf = zeros(20,3); barriers_num = 0; add_barrier_flag = 0; P0 =zeros(1,2);%路径的起始点 P1 =zeros(1,2);%路径的终点 x_spots = []; y_spots = []; PSO_barriers_num = 0; D_safe = 2; num_ba = 0; function varargout = pathPlaning_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; % --- 鼠标按下时的回调函数 (绘制障碍物) function figure1_WindowButtonDownFcn(hObject, eventdata, handles) global lastPosi_Draw draw_ok_flag global add_barrier_flag if add_barrier_flag mouse_posi = get(gca,'CurrentPoint'); if mouse_posi(1,1)>0 && mouse_posi(1,1)<12000 && mouse_posi(1,2)>0 && mouse_posi(1,2)<12000 lastPosi_Draw(1,1) = mouse_posi(1,1);%记录鼠标按下的位置 lastPosi_Draw(1,2) = mouse_posi(1,2);% end draw_ok_flag = 0; end % --- 鼠标发生移动时的回调函数. function figure1_WindowButtonMotionFcn(hObject, eventdata, handles) global draw_ok_flag lastPosi_Draw global barriers_num barriers_info_buf global add_barrier_flag if ~draw_ok_flag && add_barrier_flag %没有添加成功 则显示 cla % 设置GUI的axes控件属性 axes(handles.axes1); set(handles.axes1,'XLim',[0 12000],'YLim',[0 12000]); theta = 0:2*pi/50:2*pi; %绘制上次的历史数据 for index = 1:barriers_num x_dybamic=barriers_info_buf(index,2)+sin(theta)*barriers_info_buf(index,1); y_dybamic=barriers_info_buf(index,3)+cos(theta)*barriers_info_buf(index,1); plot(x_dybamic,y_dybamic,'*b'); hold on end mouse_posi = get(gca,'CurrentPoint'); r = sqrt((mouse_posi(1,1)-lastPosi_Draw(1,1))^2 + (mouse_posi(1,2)-lastPosi_Draw(1,2))^2); x_dybamic=lastPosi_Draw(1,1)+sin(theta)*r; y_dybamic=lastPosi_Draw(1,2)+cos(theta)*r; plot(x_dybamic,y_dybamic,'*b'); end % --- 单机鼠标键松手后的回调函数 function figure1_WindowButtonUpFcn(hObject, eventdata, handles) global curenPosi_Draw lastPosi_Draw draw_ok_flag global barriers_num barriers_info_buf global add_barrier_flag if add_barrier_flag mouse_posi = get(gca,'CurrentPoint'); if mouse_posi(1,1)>0 && mouse_posi(1,1)<12000 && mouse_posi(1,2)>0 && mouse_posi(1,2)<12000 curenPosi_Draw(1,1) = mouse_posi(1,1);%记录鼠标按下的位置 curenPosi_Draw(1,2) = mouse_posi(1,2);% end axes(handles.axes1); set(handles.axes1,'XLim',[0 12000],'YLim',[0 12000]); theta = 0:2*pi/50:2*pi; r = sqrt((curenPosi_Draw(1,1)-lastPosi_Draw(1,1))^2 + (curenPosi_Draw(1,2)-lastPosi_Draw(1,2))^2); x_dybamic=lastPosi_Draw(1,1)+sin(theta)*r; y_dybamic=lastPosi_Draw(1,2)+cos(theta)*r; plot(x_dybamic,y_dybamic,'*b'); %记录障碍物的信息(圆心坐标 半径) barriers_num = barriers_num+1; barriers_info_buf(barriers_num,1) = r; barriers_info_buf(barriers_num,2) = lastPosi_Draw(1,1); barriers_info_buf(barriers_num,3) = lastPosi_Draw(1,2); hold on draw_ok_flag = 1; end % --- Executes on button press in pushbutton5. function pushbutton5_Callback(hObject, eventdata, handles) % 添加三角形障碍物 global curenPosi_Draw lastPosi_Draw draw_ok_flag global barriers_num barriers_info_buf global add_barrier_flag global x_spots y_spots % 障碍物的横纵坐标点、 global PSO_barriers_num global num_ba add_barrier_flag = 0; triangle_corner = ginput(3); % line 1-2 ginput 1-2 axes(handles.axes1); k1 = (triangle_corner(2,2)- triangle_corner(1,2))/(triangle_corner(2,1)- triangle_corner(1,1)); b1 = triangle_corner(2,2)-k1*triangle_corner(2,1); x1 = triangle_corner(1,1):(triangle_corner(2,1)-triangle_corner(1,1))/50:triangle_corner(2,1); y1 = k1.*x1+b1; % line 1-3 k2 = (triangle_corner(3,2)- triangle_corner(1,2))/(triangle_corner(3,1)- triangle_corner(1,1)); b2 = triangle_corner(3,2)-k2*triangle_corner(3,1); x2 = triangle_corner(3,1):(triangle_corner(1,1)-triangle_corner(3,1))/50:triangle_corner(1,1); y2 = k2.*x2+b2; % line 2-3 k3 = (triangle_corner(2,2)- triangle_corner(3,2))/(triangle_corner(2,1)- triangle_corner(3,1)); b3 = triangle_corner(2,2)-k3*triangle_corner(2,1); x3 = triangle_corner(2,1):(triangle_corner(3,1)-triangle_corner(2,1))/50:triangle_corner(3,1); y3 = k3.*x3+b3; % plot lines plot(x1,y1,'*b'); hold on plot(x2,y2,'*b'); hold on plot(x3,y3,'*b'); hold on x1 = x1./100; y1 = y1./100; x2 = x2./100; y2 = y2./100; x3 = x3./100; y3 = y3./100; x_spots = [x_spots,x1]; y_spots = [y_spots,y1]; x_spots = [x_spots,x2]; y_spots = [y_spots,y2]; x_spots = [x_spots,x3]; y_spots = [y_spots,y3]; [m,n] = size(x_spots); num_ba = num_ba+n % --- Executes on button press in pushbutton6. function pushbutton6_Callback(hObject, eventdata, handles) % 添加矩形障碍物 global curenPosi_Draw lastPosi_Draw draw_ok_flag global barriers_num barriers_info_buf global add_barrier_flag global x_spots y_spots % 障碍物的横纵坐标点、 global PSO_barriers_num global num_ba add_barrier_flag = 0; corners = ginput(2); % line 1-2 ginput 1-2 axes(handles.axes1); y1 = corners(1,2):(corners(2,2)-corners(1,2))/20:corners(2,2); [m n] = size(y1); x1 = ones(1,n)*corners(1,1); x2 = corners(1,1):(corners(2,1)-corners(1,1))/20:corners(2,1); [m n] = size(x2); y2 = ones(1,n)*corners(1,2); y3 = corners(1,2):(corners(2,2)-corners(1,2))/20:corners(2,2); [m n] = size(y3); x3 = ones(1,n)*corners(2,1); x4 = corners(1,1):(corners(2,1)-corners(1,1))/20:corners(2,1); [m n] = size(x4); y4 = ones(1,n)*corners(2,2); plot(x1,y1,'*b'); hold on plot(x2,y2,'*b'); hold on plot(x3,y3,'*b'); hold on plot(x4,y4,'*b'); hold on x1 = x1./100; y1 = y1./100; x2 = x2./100; y2 = y2./100; x3 = x3./100; y3 = y3./100; x4 = x4./100; y4 = y4./100; x_spots = [x_spots,x1]; y_spots = [y_spots,y1]; x_spots = [x_spots,x2]; y_spots = [y_spots,y2]; x_spots = [x_spots,x3]; y_spots = [y_spots,y3]; x_spots = [x_spots,x4]; y_spots = [y_spots,y4]; % [m,n] = size(x_spots); num_ba = num_ba+n % --- 按键按下开始添加障碍物 function pushbutton1_Callback(hObject, eventdata, handles) global add_barrier_flag add_barrier_flag = 1; % --- 按键按下停止添加