1 简介

基于粒子群算法的移动机器人路径规划,通过建立目标函数,变换坐标等对环境建模,再引入粒子群优化算法,得到全局最优路径.MATLAB仿真结果显示,此方法可有效地解决空间作业机器人路径规划及避障问题.与传统遗传算法比,该法建模容易,计算快捷,可以在不同的障碍物环境下得到不同的优化轨迹。

路径规划是移动机器人导航中最重要的任务之一 。路径规划技术是移动机器 人研究领域中的一个重要分支 。所谓机器人的最优路径规划问题 , 就是依据某个 或某些优化准则 如工作代价最小 、 行走路线最短 、行走时间最短等 , 在其工作 空间中找到一条从起始状态到 目标状态的能避开障碍物的最优路径 。机器人路径 规划问题可以建模为一个有约束条件的连续函数优化问题 , 要完成路径规划 、定 位和避障等任务 。路径规划主要涉及的问题包括 利用获得的移动机器人环境信 息建立较为合理的模型 , 再用某种算法寻找一条从起始状态到 目标状态的最优或 近似最优的无碰撞路径 能够处理环境模型中的不确定因素和路径跟踪中出现的 误差 , 使外界物体对机器人的影响降到最小 如何利用己知的所有信息来引导机 器人的动作 , 从而得到相对更优的行为决策 。移动机器人路径规划是机器人应用中的一项重要技术 , 例如 , 在执行装配 、 焊接及抢险救灾等任务时 , 采用良好的移动机器人路径规划技术可以节省大量机 器人作业时间 、 减少机器人磨损 , 同时也可以节约人力资源 , 减小资金投入 , 为 机器人在多种行业中的应用奠定良好的基础 。将遗传算法 、模糊逻辑以及神经网 络等方法相结合 , 可以组成新的智能型路径规划方法 , 从而提高机器人路径规划 的避障精度 , 加快规划速度 , 满足实际应用的需要 。同时 , 多机器人协调作业环 境下的路径规划技术也将是研究的热点及难点问题 , 越来越受到人们的重视 。由 于障碍物及机器人数 目的增加 , 极大地增加了路径规划的难度 , 引入可以优化约 简知识的粗糙集理论 , 简化规划条件 , 提取路径规划特征参数 , 有可能进一步解 决诸如机器人路径规划速度等难题 , 因此 , 这将是一个有意义的研究课题 。近年来 , 移动机器人技术在工业 、农业 、航天及空间探测等许多领域都起到 了重要的作用 , 同时又显示了广泛的应用前景 , 因而成为国际机器人学术界研究 和关注的热点问题 。目前 , 对于移动机器人路径规划技术的研究已经取得了大量 的成果 , 许多问题获得了比较满意的答案 。

2 部分代码

clc;clear;close all; %close(1);close(2);close(3);close(4); Object_num=50;%%%障碍物的数目 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%随机产生障碍物点%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% num=Object_num; x=rand(num,1); y=rand(num,1); point=[x,y]; % % load('D:\MATLAB71\work\毕业设计\goodpoint.mat') % % x=point(:,1);y=point(:,2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图上画出障碍物环境%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global photo_ll photo_cc photo; photo_ll=300;photo_cc=300; photo=ones(photo_ll,photo_cc); [photo_point,aaa]=plot2photo(point); [photo_point_amount,aaa]=size(photo_point); for i=1:photo_point_amount photo_point_around=square(photo_point(i,:),15);%%%%%%%%%%%%%%%%%%%%画出的图形的大小 aaa=blacken_photo(photo_point_around); end global photo_connect_points; photo_connect_points=photo; figure imshow(photo) title('障碍物环境') %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%取初始点和目标点%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% global start_point aim_point; [input_x,input_y] =ginput; input_x=input_x/photo_cc;input_y=1-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_amount=50; ant_step_amount=floor(4*norm(start_point-aim_point)*sqrt(Object_num)); ant_circle_amount=20; global piece_amount piece; piece_amount=100; %%%%%%%%%%%获得其Voronoi图 figure voronoi(x,y) title('障碍物质点的Voronoi图') [connect_point,round_point_cell]=voronoin(point); connect_point(1,:)=[]; tri=delaunay(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 i=1: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_sim=cell2mat(round_point_cell(num_min_distance_start)); aim_point_sim=cell2mat(round_point_cell(num_min_distance_aim)); start_point_sim=start_point_sim-1; aim_point_sim=aim_point_sim-1; start_point_sim(find(start_point_sim==0))=[]; aim_point_sim(find(aim_point_sim==0))=[]; start_point_sim_new=[]; for i=1:max(size(start_point_sim)) connect_points=points_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); isblack=0; for connect_points_num=1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))==0 isblack=1; break; end end if isblack==0 start_point_sim_new=[start_point_sim_new;start_point_sim(i)]; end end start_point_sim=start_point_sim_new; aim_point_sim_new=[]; for i=1:max(size(aim_point_sim)) connect_points=points_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); isblack=0; for connect_points_num=1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))==0 isblack=1; break; end end if isblack==0 aim_point_sim_new=[aim_point_sim_new;aim_point_sim(i)]; end end aim_point_sim=aim_point_sim_new; if max(size(start_point_sim))<1 || max(size(aim_point_sim))<1 error('起始点或目标点被障碍物包围'); end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%寻找路径点间的关系%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [ll,cc]=size(tri); relation=zeros(ll,3); connect_point_distance=[]; for i=1:ll if abs(connect_point(i,1))>3 || abs(connect_point(i,2))>3 continue; end connect_num=0; for j=1:ll if i~=j num=0; for ii=1:3 for jj=1:3 if tri(i,ii)==tri(j,jj) num=num+1; end end end if num==2 connect_points=points_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); isblack=0; for connect_points_num=1:connect_points_amount if photo(connect_points(connect_points_num,1),connect_points(connect_points_num,2))==0 isblack=1; break; end end if isblack==0 connect_num=connect_num+1; aaa=blacken_photo_connect_points(connect_points); % aaa=blacken_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_num<ll 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_cell=cell(connect_point_num,1); connect_point_start_mindistance=inf(connect_point_num,1); connect_point_aim_cell=cell(connect_point_num,1); connect_point_aim_mindistance=inf(connect_point_num,1); %%%%%%%%%%%定义蚂蚁 mindistance=[]; mindistance_cell=cell(0); for ant_circle_num=1:ant_circle_amount %%%%定义蚂蚁走的波数 ant_start_cell=cell(0); ant_aim_cell=cell(0); ant_start_position=[]; ant_aim_position=[]; ant_start_mindistance=zeros(ant_amount,1); ant_aim_mindistance=zeros(ant_amount,1); %%%%初始化目标点和初始点 [cccc,start_point_sim_num]=size(start_point_sim); for ant_num=1:ant_amount choose_point_randnum=floor(rand*start_point_sim_num)+1; choose_point=start_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_num=1:ant_amount choose_point_randnum=floor(rand*aim_point_sim_num)+1; choose_point=aim_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_num=1:ant_step_amount %%%%一次蚂蚁走的步数 for ant_num=1:ant_amount %%%初始蚂蚁的一步行走 ant_start_per_relation=relation(ant_start_position(ant_num,1),:); ant_start_choose=ant_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_distance=connect_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_relation=relation(ant_aim_position(ant_num,1),:); ant_aim_choose=ant_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_distance=connect_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 i=1: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_path=connect_point_aim_cell{i,1}; connect_point_aim_path=fliplr(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_num==0 min_path_cell=cell(0); min_distance=[]; else min_path_cell=mindistance_cell(1,1); min_distance=mindistance(1); for i=2:mindistance_num if_num=0; [min_distance_num,aaaa]=size(min_distance); for j=1:min_distance_num if round(mindistance(i,1)*100000)==round(min_distance(j,1)*100000) if_num=1; continue; end end if if_num==0 min_distance=[min_distance;mindistance(i,:)]; min_path_cell=[min_path_cell;mindistance_cell{i,:};]; end end end mindistance=min_distance; mindistance_cell=min_path_cell; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%找出最小路径点集，并画出%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [min_path_distance,min_path_distance_num]=sort(mindistance); min_path_num=mindistance_cell{min_path_distance_num(1),1}; min_path=[]; for i=1: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_num=1: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_num=floor((upright(min_path)-start_point(1))/piece); % % pathpoint_upright_num(find(pathpoint_upright_num<1))=1; % if pathpoint_upright_num(1)<0 % pathpoint_upright_num(1)=0; % end pathpoint_piece_point=[]; [pathpoint_amount,aaa]=size(pathpoint_upright_num); for i=1:pathpoint_amount-1 for j=pathpoint_upright_num(i)+1:pathpoint_upright_num(i+1) %pathpoint_piece_point(j,:)=node(min_path(i,:),min_path(i+1,:),[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(i+1,:),[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); aaa=blacken_photo_connect_points(test_pointss); figure; imshow(photo_connect_points) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图中寻找坐标范围%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% phototest=photo; global piece_x; slope=-(start_point(1)-aim_point(1))/(start_point(2)-aim_point(2)); piece_x=1/photo_cc; slop_start_aim=-slope^(-1); [pathpoint_piece_point_amount,aaa]=size(pathpoint_piece_point); up_num=[]; for i=1:pathpoint_piece_point_amount %%%%%沿着新y轴方向向上 x_up_status=0; y_up_num=0; while x_up_status==0 y_up_num=y_up_num+1; new_uppoint=[piece_x*y_up_num+pathpoint_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_status==0 || photo(photo_newpoint(1),photo_newpoint(2))==0 % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % %%%%%%%%%%在点状图中缩减坐标维数(相对坐标缩减)%%%%%%%%%%%%%%%%%%%%%%% % % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % % y_range_new=[]; % % d_num_mean=mean(d_num);up_num_mean=mean(up_num); % % % % for i=2:pathpoint_piece_point_amount-1 % % %%if d_num(i)>d_num(i-1) && d_num(i)>d_num(i+1) || up_num(i)<up_num(i-1) && up_num(i)<up_num(i+1) || d_num(i)==d_num(i-1) && d_num(i)==d_num(i+1) || up_num(i)==up_num(i-1) && up_num(i)==up_num(i+1) % % if d_num(i)>d_num(i-1) && d_num(i)>d_num(i+1) || up_num(i)<up_num(i-1) && up_num(i)<up_num(i+1) % % 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_uppoint=points_in_two_point(new_uppoint_d,new_uppoint_up,1/photo_cc); % % [photo_newpoint,photo_status]=plot2photo(new_uppoint); % % aaaa=blacken_photo(photo_newpoint); % % % % end % % end % % y_range=[y_range_new(:,1),y_range_new(:,2)]; % % global y_range_pointnum; % % y_range_pointnum=y_range_new(:,3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%在点状图中缩减坐标维数(绝对坐标缩减)%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% y_range_new=[]; %y_d_mean=mean(y_d);y_up_mean=mean(y_up); d_num_mean=mean(d_num);up_num_mean=mean(up_num); for i=2:pathpoint_piece_point_amount-1 %%if d_num(i)>d_num(i-1) && d_num(i)>d_num(i+1) || up_num(i)<up_num(i-1) && up_num(i)<up_num(i+1) || d_num(i)==d_num(i-1) && d_num(i)==d_num(i+1) || up_num(i)==up_num(i-1) && up_num(i)==up_num(i+1) if y_d(i)>y_d(i-1) && y_d(i)>y_d(i+1) || y_up(i)<y_up(i-1) && y_up(i)<y_up(i+1) %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_uppoint=points_in_two_point(new_uppoint_d,new_uppoint_up,1/photo_cc); [photo_newpoint,photo_status]=plot2photo(new_uppoint); aaaa=blacken_photo(photo_newpoint); end end y_range=[y_range_new(:,1),y_range_new(:,2)]; global y_range_pointnum; y_range_pointnum=y_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_result=PSO_result'; final_path_point=[]; PSO_result_amount=max(size(PSO_result)); for i=1: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(1+slop_start_aim^2),start_point(2)+(aim_point(2)-start_point(2))/piece_amount*y_range_pointnum(i)-PSO_result(i)/sqrt(1+slop_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 i=1:max(size(final_path_point))-1 new_uppoint=points_in_two_point(final_path_point(i,:),final_path_point(i+1,:),1/photo_cc); [photo_newpoint,photo_status]=plot2photo(new_uppoint); aaaa=blacken_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代码问题可私信交流。

部分理论引用网络文献，若有侵权联系博主删除。