【无标路径剪枝与平滑题】

最新推荐文章于 2024-09-14 17:25:17 发布

Ruo_su.

最新推荐文章于 2024-09-14 17:25:17 发布

阅读量290

点赞数

文章标签： matlab 算法

本文链接：https://blog.csdn.net/qq_44752711/article/details/124437128

版权

该博客主要介绍了机器人路径规划的RRT算法及其优化过程。首先通过cutPath函数进行路径剪枝，去除碰撞点和过长直线段。然后使用pullPath函数调整路径，确保轨迹远离障碍物。最后，通过adjust函数对路径点进行平滑处理，以提高路径的安全性和平滑度。整个过程涉及路径安全性检查、距离计算和轨迹调整等关键技术。

摘要由CSDN通过智能技术生成

%step4 路径剪枝与调整
for i=1:20
    path = cutPath(path,Imp);
    path = pullPath(path,Obs,ns);
    path = pullPath(path,Obs,ns);
end

path = cutPath(path,Imp);

%% RRT剪枝路径以获得更短路径
function path = cutPath(oldpath,Imp)
    oldpath = flipud(oldpath);
    startx = oldpath(1,1);
    starty = oldpath(1,2);
    path = [startx,starty];
    for i = 2:size(oldpath)-1
        goalx = oldpath(i+1,1);
        goaly = oldpath(i+1,2);
        if iscollision(startx,starty,goalx,goaly,Imp) || ((goalx - startx)^2 + (goaly - starty)^2)^0.5 >50*5
            path = [path;oldpath(i,1),oldpath(i,2)];
            startx = oldpath(i,1);
            starty = oldpath(i,2);
        end
    end
    path = [path;oldpath(end,:)];
    oldpath = path;
    path = oldpath(1,:);
    for i = 2:size(oldpath,1)
        startx = oldpath(i-1,1);
        star