【路径规划】基于RRT+APF实现机器人二值地图动态避障算法附matlab仿真

✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。

🍎个人主页：Matlab科研工作室

🍊个人信条：格物致知。

更多Matlab仿真内容点击👇

智能优化算法       神经网络预测       雷达通信       无线传感器        电力系统

信号处理              图像处理               路径规划       元胞自动机        无人机 

⛄ 内容介绍

一种APFRRT算法的机器人避障路径规划方法,在已知起始位置,目标位置和障碍物环境信息的条件下,利用APFRRT算法,引入障碍物数目参数和算法切换策略避免传统APF算法无法到达目标点和陷入局部最小值,引入双边最近节点选择策略;提出的改进APFRRT算法减少了路径长度,提升了算法效率,可以很好地适用于机器人避障路径规划问题.

⛄ 部分代码

clc

clear all

close all

%% 初始化

path='scc.png';

map=loadMap(path);

% 最大迭代次数

RRTCountMax = 30000;

APFCountMax = 30000;

% 步长

RRTstep = 20;

APFstep = 2;

% 起始点、目标点

%select = 5;

starts = [100, 100; 100, 100; 100, 100; 1, 3; 4,4];

targets = [300, 400; 950,750; 100, 500; 5, 9; 9,8];

%

select = 1;

start = starts(select, :);

target = targets(select, :);

% x_I = 100; y_I = 100;           % 设置初始点 100,100

% x_G = 200; y_G = 200;       % 设置目标点  750,750

% 障碍物 x y r

obs = [

    960, 690, 20;

    540, 690, 20;

    90,200,10;

    55,400,10;

    60,550,10;

    ];    % 动态障碍点圆心坐标及半径

obsv = [

    2, 2, 0;

    -2, -2, 0;

    -2,0,0;

    1,0,0;

    -1,0,0;

    ];

% kAttr, kRep

kAttr = 0.01;  % 引力系数

kRep = 10;    % 斥力系数

kObs = 5;   % 作用延伸范围

figure

imshow(map)

hold on;

plot(start(1), start(2), 'p', markersize=10, color='red');

plot(target(1), target(2), 'p', markersize=10, color='green');

ok = false;

result = [];

while ~ok

    ok = true;

    rrt_result = RRTstar(map, start, target, obs, RRTstep, RRTCountMax);

    if isempty(rrt_result)

        disp("rrt star cannot find path")

        return

    end

    if size(rrt_result, 1) == 1

        disp('start == target')

        return

    end

    hflag=0;

    h1=[];

    for i = 2: size(rrt_result, 1)

        apf_start = rrt_result(i - 1, :);

        apf_target = rrt_result(i, :);

        [h1,hflag,apf_result, success, newStart, count, obs] = APF(h1,hflag,map, start, target,apf_start, apf_target, obs,obsv, APFstep, APFCountMax, kAttr, kRep, kObs);

        result = [result; apf_result];

        if (success == false)

            ok = false;

            start = newStart;

            break;

        end

    end

end

plot(result(1:end, 1), result(1:end, 2), '--', color='g');

⛄ 运行结果

⛄ 参考文献

[1] 陈少斌, 蒋静坪. 基于神经网络和粒子群优化算法的移动机器人动态避障路径规划[J]. 系统仿真技术, 2006(4):6.

[2] 彭鹏张志安黄学功. 基于APF-RRT的双机械臂动态避障路径规划[J]. 机械传动, 2022, 46(4):48-54.

[3] 黄永良, 李燕, 张小龙. 基于改进蚁群算法的机器人路径规划[J]. 电脑与信息技术, 2023, 31(2):4.

[4] 闵凤, 江翠翠, 姚忠福. 基于Matlab和V-REP的移动机器人路径规划仿真平台[J]. 科技经济导刊, 2019(5):2.

[5] 唐新星, 刘博聪, 项天野,等. 一种改进APF-RRT算法的机器人避障路径规划方法:, CN202211189700.0[P]. 2023.

[6] 杨姗姗, 戴学丰, 唱江华. 实现机器人动态路径规划的仿真系统[J]. 计算机工程与应用, 2009, 45(32):4.

⛳️ 代码获取关注我

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

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