s = [50;50];              %起始點
g = [250;250];            %目標點

obstaclesx = [100,150,150,100,100; %障礙物
    170,200,200,170,170;
    190,220,220,190,190];
obstaclesy = [100,100,150,150,100;
    50,50,200,200,50;
    230,230,250,250,230];

figure(1)
hold on
axis([0 300 0 300])
xlabel('x');
ylabel('y');
for i = 1:size(obstaclesx,1)
    plot(obstaclesx(i,:),obstaclesy(i,:),'r')
end
plot(s(1),s(2),'s',g(1),g(2),'x')
title('Obstacles (polygon), initial vehicle (square) and goal (x)');
hold off