✅作者简介:热爱科研的Matlab仿真开发者,修心和技术同步精进,matlab项目合作可私信。
🍎个人主页:Matlab科研工作室
🍊个人信条:格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
移动机器人的路径规划是移动机器人研究领域中的一个热点问题。
编辑
编辑
编辑
编辑
⛄ 部分代码
function [route, plan_succeeded] = GradientPlanner(potential_field, start_coordinate, dest_coordinate, max_iteration)
[gx, gy] = gradient (-potential_field);
Size = size(potential_field);
[x, y] = meshgrid (1:Size(1), 1:Size(2));
%%
figure;
skip = 5;
xidx = 1:skip:Size(2);
yidx = 1:skip:Size(1);
%q = quiver(x(yidx,xidx), y(yidx,xidx), gx(yidx,xidx), gy(yidx,xidx),1.2);
q = quiver(x(yidx,xidx), y(yidx,xidx), gx(yidx,xidx), gy(yidx,xidx),'k');
q.LineWidth = 1;
%% 梯度下降导航
route = [start_coordinate(1),start_coordinate(2)];
currunt_coordinate = start_coordinate;
plan_succeeded = 0;
for i = 2:max_iteration+1 %%max_iteration最大迭代次数
if((currunt_coordinate(1)-dest_coordinate(1))^2 + (currunt_coordinate(2)-dest_coordinate(2))^2 > 1)
a = [gx(currunt_coordinate(2),currunt_coordinate(1));gy(currunt_coordinate(2),currunt_coordinate(1))];%没有到达终点
step = a/norm(a);
if(route((i-1),1)+step(1) >= 1 && route((i-1),1)+step(1) <= Size(1)...
&& route((i-1),2)+step(2) >= 1 && route((i-1),2)+step(2) <= Size(2))
route(i,1) = route((i-1),1)+step(1);
route(i,2) = route((i-1),2)+step(2);
else
plan_succeeded = 0;
break;
end
currunt_coordinate = [round(route(i,1)),round(route(i,2))];
else
plan_succeeded = 1;
break;
end
end
hold on;
plot(start_coordinate(1), start_coordinate(2), 'g.', 'MarkerSize', 30)
plot(dest_coordinate(1), dest_coordinate(2), 'b.', 'MarkerSize', 30);
plot (route(:,1), route(:,2), 'r','LineWidth', 2);
title('梯度下降导航');
end
⛄ 运行结果
⛄ 参考文献
[1]李欣, 朱大奇. 基于人工势场法的自治水下机器人路径规划[J]. 上海海事大学学报, 2010, 31(2):5.