1 简介基于粒子群算法的移动机器人路径规划,通过建立目标函数,变换坐标等对环境建模,再引入粒子群优化算法,得到全局最优路径.MATLAB仿真结果显示,此方法可有效地解决空间作业机器人路径规划及避障问题.与传统遗传算法比,该法建模容易,计算快捷,可以在不同的障碍物环境下得到不同的优化轨迹。路径规划是移动机器人导航中最重要的任务之一 。路径规划技术是移动机器 人研究领域中的一个重要分支 。所谓机器人的最优路径规划问题 , 就是依据某个 或某些优化准则 如工作代价最小 、 行走路线最短 、行走时间最短等 , 在其工作 空间中找到一条从起始状态到 目标状态的能避开障碍物的最优路径 。机器人路径 规划问题可以建模为一个有约束条件的连续函数优化问题 , 要完成路径规划 、定 位和避障等任务 。路径规划主要涉及的问题包括 利用获得的移动机器人环境信 息建立较为合理的模型 , 再用某种算法寻找一条从起始状态到 目标状态的最优或 近似最优的无碰撞路径 能够处理环境模型中的不确定因素和路径跟踪中出现的 误差 , 使外界物体对机器人的影响降到最小 如何利用己知的所有信息来引导机 器人的动作 , 从而得到相对更优的行为决策 。移动机器人路径规划是机器人应用中的一项重要技术 , 例如 , 在执行装配 、 焊接及抢险救灾等任务时 , 采用良好的移动机器人路径规划技术可以节省大量机 器人作业时间 、 减少机器人磨损 , 同时也可以节约人力资源 , 减小资金投入 , 为 机器人在多种行业中的应用奠定良好的基础 。将遗传算法 、模糊逻辑以及神经网 络等方法相结合 , 可以组成新的智能型路径规划方法 , 从而提高机器人路径规划 的避障精度 , 加快规划速度 , 满足实际应用的需要 。同时 , 多机器人协调作业环 境下的路径规划技术也将是研究的热点及难点问题 , 越来越受到人们的重视 。由 于障碍物及机器人数 目的增加 , 极大地增加了路径规划的难度 , 引入可以优化约 简知识的粗糙集理论 , 简化规划条件 , 提取路径规划特征参数 , 有可能进一步解 决诸如机器人路径规划速度等难题 , 因此 , 这将是一个有意义的研究课题 。近年来 , 移动机器人技术在工业 、农业 、航天及空间探测等许多领域都起到 了重要的作用 , 同时又显示了广泛的应用前景 , 因而成为国际机器人学术界研究 和关注的热点问题 。目前 , 对于移动机器人路径规划技术的研究已经取得了大量 的成果 , 许多问题获得了比较满意的答案 。2 部分代码clc;clear;close all; %close(1);close(2);close(3);close(4); Object_num50;%%%障碍物的数目 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%随机产生障碍物点%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% numObject_num; xrand(num,1); yrand(num,1); point[x,y]; % % load(D:\MATLAB71\work\毕业设计\goodpoint.mat) % % xpoint(:,1);ypoint(:,2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图上画出障碍物环境%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global photo_ll photo_cc photo; photo_ll300;photo_cc300; photoones(photo_ll,photo_cc); [photo_point,aaa]plot2photo(point); [photo_point_amount,aaa]size(photo_point); for i1:photo_point_amount photo_point_aroundsquare(photo_point(i,:),15);%%%%%%%%%%%%%%%%%%%%画出的图形的大小 aaablacken_photo(photo_point_around); end global photo_connect_points; photo_connect_pointsphoto; figure imshow(photo) title(障碍物环境) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%取初始点和目标点%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global start_point aim_point; [input_x,input_y] ginput; input_xinput_x/photo_cc;input_y1-input_y/photo_ll; start_point[input_x(1),input_y(1)];aim_point[input_x(2),input_y(2)]; %start_point[0,0];aim_point[1,1]; ant_amount50; ant_step_amountfloor(4*norm(start_point-aim_point)*sqrt(Object_num)); ant_circle_amount20; global piece_amount piece; piece_amount100; %%%%%%%%%%%获得其Voronoi图 figure voronoi(x,y) title(障碍物质点的Voronoi图) [connect_point,round_point_cell]voronoin(point); connect_point(1,:)[]; tridelaunay(x,y); hold on; plot(start_point(1),start_point(2),r*); plot(aim_point(1),aim_point(2),r*); hold off; %%%%%%%%%%%起始点和目标点的距离初始化 distance_start[];distance_aim[]; for i1:num distance_start[distance_start,norm(point(i,:)-start_point)]; distance_aim[distance_aim,norm(point(i,:)-aim_point)]; end [min_distance_start,num_min_distance_start]min(distance_start); [min_distance_aim,num_min_distance_aim]min(distance_aim); start_point_simcell2mat(round_point_cell(num_min_distance_start)); aim_point_simcell2mat(round_point_cell(num_min_distance_aim)); start_point_simstart_point_sim-1; aim_point_simaim_point_sim-1; start_point_sim(find(start_point_sim0))[]; aim_point_sim(find(aim_point_sim0))[]; start_point_sim_new[]; for i1:max(size(start_point_sim)) connect_pointspoints_in_two_point(start_point,connect_point(start_point_sim(i),:),3/photo_cc); [connect_points,n]plot2photo(connect_points); [connect_points_amount,aaa]size(connect_points); isblack0; for connect_points_num1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))0 isblack1; break; end end if isblack0 start_point_sim_new[start_point_sim_new;start_point_sim(i)]; end end start_point_simstart_point_sim_new; aim_point_sim_new[]; for i1:max(size(aim_point_sim)) connect_pointspoints_in_two_point(aim_point,connect_point(aim_point_sim(i),:),3/photo_cc); [connect_points,n]plot2photo(connect_points); [connect_points_amount,aaa]size(connect_points); isblack0; for connect_points_num1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))0 isblack1; break; end end if isblack0 aim_point_sim_new[aim_point_sim_new;aim_point_sim(i)]; end end aim_point_simaim_point_sim_new; if max(size(start_point_sim))1 || max(size(aim_point_sim))1 error(起始点或目标点被障碍物包围); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%寻找路径点间的关系%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [ll,cc]size(tri); relationzeros(ll,3); connect_point_distance[]; for i1:ll if abs(connect_point(i,1))3 || abs(connect_point(i,2))3 continue; end connect_num0; for j1:ll if i~j num0; for ii1:3 for jj1:3 if tri(i,ii)tri(j,jj) numnum1; end end end if num2 connect_pointspoints_in_two_point(connect_point(i,:),connect_point(j,:),3/photo_cc); [connect_points,n]plot2photo(connect_points); [connect_points_amount,aaa]size(connect_points); isblack0; for connect_points_num1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))0 isblack1; break; end end if isblack0 connect_numconnect_num1; aaablacken_photo_connect_points(connect_points); % aaablacken_photo(connect_points); relation(i,connect_num)j; connect_point_distance(i,connect_num)norm(connect_point(i,:)-connect_point(j,:)); end end end end end [relation_num,aaa]size(relation); if relation_numll relation[relation;zeros(ll-relation_num,3)]; end figure imshow(photo_connect_points) title(障碍物环境的Voronoi图); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%蚁群算法找离散最优%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%定义蚂蚁路径点 [connect_point_num,cc]size(connect_point); connect_point_start_cellcell(connect_point_num,1); connect_point_start_mindistanceinf(connect_point_num,1); connect_point_aim_cellcell(connect_point_num,1); connect_point_aim_mindistanceinf(connect_point_num,1); %%%%%%%%%%%定义蚂蚁 mindistance[]; mindistance_cellcell(0); for ant_circle_num1:ant_circle_amount %%%%定义蚂蚁走的波数 ant_start_cellcell(0); ant_aim_cellcell(0); ant_start_position[]; ant_aim_position[]; ant_start_mindistancezeros(ant_amount,1); ant_aim_mindistancezeros(ant_amount,1); %%%%初始化目标点和初始点 [cccc,start_point_sim_num]size(start_point_sim); for ant_num1:ant_amount choose_point_randnumfloor(rand*start_point_sim_num)1; choose_pointstart_point_sim(choose_point_randnum); ant_start_position(ant_num,1)choose_point; ant_start_cell{ant_num,1}choose_point; ant_start_mindistance(ant_num,1)norm(connect_point(ant_start_position(ant_num,1))-start_point); end [cccc,aim_point_sim_num]size(aim_point_sim); for ant_num1:ant_amount choose_point_randnumfloor(rand*aim_point_sim_num)1; choose_pointaim_point_sim(choose_point_randnum); ant_aim_position(ant_num,1)choose_point; ant_aim_cell{ant_num,1}choose_point; ant_aim_mindistance(ant_num,1)norm(connect_point(ant_aim_position(ant_num,1))-aim_point); end %%%%%%%%%%%%%%%%%%%%%% %%%%蚁群优化%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%% for ant_step_num1:ant_step_amount %%%%一次蚂蚁走的步数 for ant_num1:ant_amount %%%初始蚂蚁的一步行走 ant_start_per_relationrelation(ant_start_position(ant_num,1),:); ant_start_chooseant_choose(ant_start_position(ant_num,1),ant_start_per_relation); if ant_start_choose~0 ant_start_cell{ant_num,1}[ant_start_cell{ant_num,1},ant_start_choose]; ant_start_per_distanceconnect_point_distance(ant_start_position(ant_num,1),find(relation(ant_start_position(ant_num,1),:)ant_start_choose)); ant_start_position(ant_num,1)ant_start_choose; ant_start_mindistance(ant_num,1)ant_start_mindistance(ant_num,1)ant_start_per_distance; if ant_start_mindistance(ant_num,1)connect_point_start_mindistance(ant_start_choose) connect_point_start_mindistance(ant_start_choose,1)ant_start_mindistance(ant_num,1); connect_point_start_cell{ant_start_choose,1}ant_start_cell{ant_num,1}; end end %%%目标蚂蚁的一步行走 ant_aim_per_relationrelation(ant_aim_position(ant_num,1),:); ant_aim_chooseant_choose(ant_aim_position(ant_num,1),ant_aim_per_relation); if ant_aim_choose~0 ant_aim_cell{ant_num,1}[ant_aim_cell{ant_num,1},ant_aim_choose]; ant_aim_per_distanceconnect_point_distance(ant_aim_position(ant_num,1),find(relation(ant_aim_position(ant_num,1),:)ant_aim_choose)); ant_aim_position(ant_num,1)ant_aim_choose; ant_aim_mindistance(ant_num,1)ant_aim_mindistance(ant_num,1)ant_aim_per_distance; if ant_aim_mindistance(ant_num,1)connect_point_aim_mindistance(ant_aim_choose) connect_point_aim_mindistance(ant_aim_choose,1)ant_aim_mindistance(ant_num,1); connect_point_aim_cell{ant_aim_choose,1}ant_aim_cell{ant_num,1}; end end end end end for i1:connect_point_num if connect_point_aim_mindistance(i)~inf connect_point_start_mindistance(i)~inf mindistance[mindistance;connect_point_aim_mindistance(i)connect_point_start_mindistance(i);]; connect_point_aim_pathconnect_point_aim_cell{i,1}; connect_point_aim_pathfliplr(connect_point_aim_path); connect_point_aim_path(1)[]; mindistance_cell[mindistance_cell;[connect_point_start_cell{i,1},connect_point_aim_path;]]; end end [mindistance_num,aaaa]size(mindistance); if mindistance_num0 min_path_cellcell(0); min_distance[]; else min_path_cellmindistance_cell(1,1); min_distancemindistance(1); for i2:mindistance_num if_num0; [min_distance_num,aaaa]size(min_distance); for j1:min_distance_num if round(mindistance(i,1)*100000)round(min_distance(j,1)*100000) if_num1; continue; end end if if_num0 min_distance[min_distance;mindistance(i,:)]; min_path_cell[min_path_cell;mindistance_cell{i,:};]; end end end mindistancemin_distance; mindistance_cellmin_path_cell; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%找出最小路径点集并画出%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [min_path_distance,min_path_distance_num]sort(mindistance); min_path_nummindistance_cell{min_path_distance_num(1),1}; min_path[]; for i1:size(min_path_num,2) min_path[min_path;connect_point(min_path_num(i),:);]; end min_path[start_point;min_path;aim_point]; plot(min_path(:,1),min_path(:,2),r) axis([0 1 0 1]); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%找出原始图中能够连接成最短路径的点集%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% piece(aim_point(1)-start_point(1))/piece_amount; for piece_num1:piece_amount piece_x(piece_num,1)start_point(1)piece_num*piece; piece_y(piece_num,1)aim_point(2)(start_point(2)-aim_point(2))/(start_point(1)-aim_point(1))*(piece_x(piece_num,1)-aim_point(1)); end pathpoint_upright_numfloor((upright(min_path)-start_point(1))/piece); % % pathpoint_upright_num(find(pathpoint_upright_num1))1; % if pathpoint_upright_num(1)0 % pathpoint_upright_num(1)0; % end pathpoint_piece_point[]; [pathpoint_amount,aaa]size(pathpoint_upright_num); for i1:pathpoint_amount-1 for jpathpoint_upright_num(i)1:pathpoint_upright_num(i1) %pathpoint_piece_point(j,:)node(min_path(i,:),min_path(i1,:),[start_point(1)j*piece,aim_point(2)(start_point(2)-aim_point(2))/(start_point(1)-aim_point(1))*(start_point(1)j*piece-aim_point(1))]); pathpoint_piece_point[pathpoint_piece_point;node(min_path(i,:),min_path(i1,:),[start_point(1)j*piece,aim_point(2)(start_point(2)-aim_point(2))/(start_point(1)-aim_point(1))*(start_point(1)j*piece-aim_point(1))]);]; end end [test_pointss,nn]plot2photo(pathpoint_piece_point); aaablacken_photo_connect_points(test_pointss); figure; imshow(photo_connect_points) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图中寻找坐标范围%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% phototestphoto; global piece_x; slope-(start_point(1)-aim_point(1))/(start_point(2)-aim_point(2)); piece_x1/photo_cc; slop_start_aim-slope^(-1); [pathpoint_piece_point_amount,aaa]size(pathpoint_piece_point); up_num[]; for i1:pathpoint_piece_point_amount %%%%%沿着新y轴方向向上 x_up_status0; y_up_num0; while x_up_status0 y_up_numy_up_num1; new_uppoint[piece_x*y_up_numpathpoint_piece_point(i,1),pathpoint_piece_point(i,2)slope*piece_x*y_up_num]; [photo_newpoint,photo_status]plot2photo(new_uppoint); phototest(photo_newpoint(1),photo_newpoint(2))0; if photo_status0 || photo(photo_newpoint(1),photo_newpoint(2))0 % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % %%%%%%%%%%在点状图中缩减坐标维数(相对坐标缩减)%%%%%%%%%%%%%%%%%%%%%%% % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % y_range_new[]; % % d_num_meanmean(d_num);up_num_meanmean(up_num); % % % % for i2:pathpoint_piece_point_amount-1 % % %%if d_num(i)d_num(i-1) d_num(i)d_num(i1) || up_num(i)up_num(i-1) up_num(i)up_num(i1) || d_num(i)d_num(i-1) d_num(i)d_num(i1) || up_num(i)up_num(i-1) up_num(i)up_num(i1) % % if d_num(i)d_num(i-1) d_num(i)d_num(i1) || up_num(i)up_num(i-1) up_num(i)up_num(i1) % % d_num(i)max(d_num(i),d_num_mean);up_num(i)min(up_num(i),up_num_mean); % % new_uppoint_d[piece_x*d_num(i)pathpoint_piece_point(i,1),pathpoint_piece_point(i,2)slope*piece_x*d_num(i)]; % % new_uppoint_up[piece_x*up_num(i)pathpoint_piece_point(i,1),pathpoint_piece_point(i,2)slope*piece_x*up_num(i)]; % % y_range_new[y_range_new;uprightdistance(new_uppoint_d),uprightdistance(new_uppoint_up),i]; % % % % new_uppointpoints_in_two_point(new_uppoint_d,new_uppoint_up,1/photo_cc); % % [photo_newpoint,photo_status]plot2photo(new_uppoint); % % aaaablacken_photo(photo_newpoint); % % % % end % % end % % y_range[y_range_new(:,1),y_range_new(:,2)]; % % global y_range_pointnum; % % y_range_pointnumy_range_new(:,3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图中缩减坐标维数(绝对坐标缩减)%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% y_range_new[]; %y_d_meanmean(y_d);y_up_meanmean(y_up); d_num_meanmean(d_num);up_num_meanmean(up_num); for i2:pathpoint_piece_point_amount-1 %%if d_num(i)d_num(i-1) d_num(i)d_num(i1) || up_num(i)up_num(i-1) up_num(i)up_num(i1) || d_num(i)d_num(i-1) d_num(i)d_num(i1) || up_num(i)up_num(i-1) up_num(i)up_num(i1) if y_d(i)y_d(i-1) y_d(i)y_d(i1) || y_up(i)y_up(i-1) y_up(i)y_up(i1) %y_d(i)max(y_d(i),y_d_mean);y_up(i)min(y_up(i),y_up_mean); d_num(i)max(d_num(i),d_num_mean);up_num(i)min(up_num(i),up_num_mean); new_uppoint_d[piece_x*d_num(i)pathpoint_piece_point(i,1),pathpoint_piece_point(i,2)slope*piece_x*d_num(i)]; new_uppoint_up[piece_x*up_num(i)pathpoint_piece_point(i,1),pathpoint_piece_point(i,2)slope*piece_x*up_num(i)]; y_range_new[y_range_new;uprightdistance(new_uppoint_d),uprightdistance(new_uppoint_up),i]; new_uppointpoints_in_two_point(new_uppoint_d,new_uppoint_up,1/photo_cc); [photo_newpoint,photo_status]plot2photo(new_uppoint); aaaablacken_photo(photo_newpoint); end end y_range[y_range_new(:,1),y_range_new(:,2)]; global y_range_pointnum; y_range_pointnumy_range_new(:,3); imshow(photo) figure %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%粒子群优化并显示结果%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [PSO_result,on,off,minmax]PsoProcess(8*max(size(y_range_new)),max(size(y_range)),y_range,initswarm,BasestepPSO,Griewank,0,0,8*max(size(y_range_new)),0); PSO_result(max(size(PSO_result)))[]; PSO_resultPSO_result; final_path_point[]; PSO_result_amountmax(size(PSO_result)); for i1:PSO_result_amount new_uppoint[start_point(1)(aim_point(1)-start_point(1))/piece_amount*y_range_pointnum(i)PSO_result(i)*slop_start_aim/sqrt(1slop_start_aim^2),start_point(2)(aim_point(2)-start_point(2))/piece_amount*y_range_pointnum(i)-PSO_result(i)/sqrt(1slop_start_aim^2)]; final_path_point[final_path_point;new_uppoint]; [photo_newpoint,photo_status]plot2photo(new_uppoint); photo(photo_newpoint(1),photo_newpoint(2))0; end final_path_point[start_point;final_path_point;aim_point]; for i1:max(size(final_path_point))-1 new_uppointpoints_in_two_point(final_path_point(i,:),final_path_point(i1,:),1/photo_cc); [photo_newpoint,photo_status]plot2photo(new_uppoint); aaaablacken_photo(photo_newpoint); end figure imshow(photo) figure plot(final_path_point(:,1),final_path_point(:,2),b-*); hold on plot([start_point(1);aim_point(1)],[start_point(2);aim_point(2)],r-*) hold off axis([0 1 0 1])3 仿真结果4 参考文献[1]黄祎, 孙德宝, 秦元庆. 基于粒子群算法的移动机器人路径规划[J]. 兵工自动化, 2006, 25(4):3.博主简介擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真相关matlab代码问题可私信交流。部分理论引用网络文献若有侵权联系博主删除。