最近研究了一下机器人运动规划,基于搜索的dfs,bfs,dijstra,A*之类的算法课上大多都讲过,所以选了这个基于采样的方法,做了个小实验。
自己进行了一些算法优化,使其更快收敛。效果还算不错吧。
下面是代码和动画:
clear all;
close all;
global n obstacle dst range step
map=zeros(255,255);
n=9;
obstacle=[170 50;50 36 ;78 89;90 140;205 169;123 94;124 215;150 170;40 200];
range=round(4*rand(1,n)+14);
src=round(25*rand(2,1)+1);
dst=round(45*rand(2,1)+190);
for i=1:n
x=obstacle(i,1);
y=obstacle(i,2);
for j=x-range(1,i):x+range(1,i)
for k=y-range(1,i):y+range(1,i)
map(j,k)=1;
end
end
end
clear x y;
map(src(1,1),src(2,1))=0;
map(dst(1,1),dst(2,1))=0;
step=3;
iteration=2000;
i=0;
x.parent=0;
x.pos=src;
x.length=0;
T={x};
min_dis=inf;
while i<iteration
if min_dis>=50
x_rand=round(253*rand(2,1)+1);
elseif min_dis>=25
x_rand=round(100*rand(2,1)+dst-50);
elseif min_dis>=12
x_rand=round(50*rand(2,1)+dst-25);
else
x_rand=round(50*rand(2,1)+dst-12);
end
min=inf;
x_min=0;
for j=1:length(T)
x_pos=T{j}.pos;
dis=norm(x_rand-x_pos);
if dis<min
min=dis;
x_min=j;
end
end
if min<=0
continue
end
x_new.pos=T{x_min}.pos+round((3+2*rand)*(x_rand-T{x_min}.pos)/norm(x_rand-T{x_min}.pos));
if x_new.pos(1,1)<=255 && x_new.pos(1,1)>=1 && x_new.pos(2,1)<=255 && x_new.pos(2,1)>=1 && map(x_new.pos(1,1),x_new.pos(2,1))==0
x_new.parent=x_min;
x_new.length=norm(x_new.pos-T{x_min}.pos);
T=[T x_new];
if min_dis>norm(x_new.pos-dst)
min_dis=norm(x_new.pos-dst);
end
if norm(x_new.pos-dst)<=5
x_final.pos=dst;
x_final.parent=length(T);
x_final.length=norm(x_new.pos-dst);
T=[T x_final];
break;
end
end
i=i+1;
end
times=i;
for i=1:n
xk=[obstacle(i,1)-range(1,i),obstacle(i,1)+range(1,i),obstacle(i,1)+range(1,i),obstacle(i,1)-range(1,i)];
yk=[obstacle(i,2)-range(1,i),obstacle(i,2)-range(1,i),obstacle(i,2)+range(1,i),obstacle(i,2)+range(1,i)];
fill(xk,yk,'k')
hold on
end
plot(src(1,1),src(2,1),'o',"Markersize",10,"MarkerFaceColor",'g')
plot(dst(1,1),dst(2,1),'o',"Markersize",10,"MarkerFaceColor","r")
for i=2:length(T)
x=[T{i}.pos(1,1) T{T{i}.parent}.pos(1,1)];
y=[T{i}.pos(2,1) T{T{i}.parent}.pos(2,1)];
plot(x,y,'b-')
pause(0.03)
end
clear x y;
num=length(T);
x=[];
y=[];
while num~=0
x=[x T{num}.pos(1,1)];
y=[y T{num}.pos(2,1)];
num=T{num}.parent;
end
plot(x,y,'g-',"LineWidth",2);