【二维路径规划】基于人工势场求解机器人路径规划问题附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.

⛳️ 代码获取关注我

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

❤️ 关注我领取海量matlab电子书和数学建模资料