【路径规划】基于粒子群算法机器人避障路径规划matlab代码

208 阅读5分钟

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代码问题可私信交流。

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