rrt* 算法

@wangqi

已于 2024-04-21 22:32:46 修改

阅读量1.3k

点赞数 12

分类专栏：规划算法文章标签：算法 matlab

于 2024-04-21 22:31:32 首次发布

本文链接：https://blog.csdn.net/qq_54644501/article/details/138046411

版权

规划算法专栏收录该内容

5 篇文章 0 订阅

订阅专栏

rrt算法由于没有考虑cost值，使得最终规划的路径质量很差。rrt*算法在此基础上，引入贪心思想，使得规划路径可以随着采样次数增多逐步优化。

一、算法流程

二、rrt*与rrt的不同

1、父节点的选取不同

rrt扩展完 $q_{new}$ 后，将距离其最近的节点作为其父节点；而rrt*是划定一个范围，将范围的节点均作为待选父节点，然后分别计算以它们为父节点时， $q_{new}$ 的cost值，选择使 $q_{new}$ 的cost最小的节点作为其父节点。这一步使得扩展得新节点得cost尽可能得小。

2、重布线（rewire）

在完成新节点的父节点选取后，选取新节点的周边节点，假设将刚扩展的新节点作为它们的父节点，比较此时它们的cost值与其原先得cost值，如果此时cost值较小，则rewire，将新节点作为其父节点，并更新cost值。

三、matlab代码

这里只放两处关键代码，后面会上传github。

1、父节点的选取

%以newnode节点为中心，半径R为半径搜索节点
R=30/ceil(0.001*numel(nodes_list));
dis2new=nodes_list(ind).cost+stepLength;
minCostIndex=ind;
nearNodeList=[];
for index_near=1:N
    if norm(nodes_list(index_near).position-newnode)<R
        nearNodeList=[nearNodeList,nodes_list(index_near)]; %if node in the range, put it in the nearNodeList
        if norm(nodes_list(index_near).position-newnode)+nodes_list(index_near).cost<dis2new
            dis2new=norm(nodes_list(index_near).position-newnode)+nodes_list(index_near).cost;
            flag=collisionCheck(newnode,nodes_list(index_near).position,fig);
            if flag
                minCostIndex=index_near;
            end
        end
    end
end
lhandle=plot([nodes_list(minCostIndex).position(1),newnode(1)],[nodes_list(minCostIndex).position(2),newnode(2)],'b-','LineWidth',1);
lhandlelist=[lhandlelist,lhandle];
phandle=plot(newnode(1),newnode(2),'ro','MarkerFaceColor','r','MarkerSize',2);
phandlelist=[phandlelist,phandle];
drawnow;
nodes_list(N+1).position=newnode;
nodes_list(N+1).parentind=minCostIndex;
nodes_list(N+1).cost=nodes_list(minCostIndex).cost+norm(near2rand);
nodes_list(N+1).index=N+1;

2、rewire

%% rewire
NearNums=length(nearNodeList);
for iterNear=1:NearNums
    nearNewCost=norm(nearNodeList(iterNear).position-newnode);
    if nearNewCost+nodes_list(N+1).cost<nearNodeList(iterNear).cost
        flag=collisionCheck(newnode,nearNodeList(iterNear).position,fig);
        if flag
            nodes_list(nearNodeList(iterNear).index).parentind=N+1;
            nodes_list(nearNodeList(iterNear).index).cost=nearNewCost+nodes_list(N+1).cost;
            %delete(lhandlelist(nearNodeList(iterNear).index-1));
            lhandlelist(nearNodeList(iterNear).index-1)=plot([nearNodeList(iterNear).position(1),newnode(1)],...
                [nearNodeList(iterNear).position(2),newnode(2)],'r-','LineWidth',2);
            drawnow;
        end
    end
end
if norm(newnode-goalPoint)<=stepLength && ~findflag
    findflag=1;
    nodes_list(N+2).position=goalPoint;
    nodes_list(N+2).parentind=N+1;
    nodes_list(N+2).cost=nodes_list(N+1).cost+norm(newnode-goalPoint);
    nodes_list(N+2).index=N+2;
    goalindex=N+2;
    %plot([newnode(1),goalPoint(1)],[newnode(2),goalPoint(2)],'b-');
end
title(['RRT* algorithm ','sampleNums:',num2str(j)]);
if findflag
    updatepathcount=updatepathcount+1;
    if updatepathcount==50
        updatepathcount=0;
        m=2;
        path=[];
        path(1).position=goalPoint;
        pathIndex=nodes_list(goalindex).parentind;
        while 1
            path(m).position = nodes_list(pathIndex).position;
            pathIndex = nodes_list(pathIndex).parentind;    % 沿终点回溯到起点
            if pathIndex == -1
                break
            end
            m=m+1;
        end
        for delete_index=1:length(pathhandlelist)
            delete(pathhandlelist(delete_index));
        end
        for m=2:length(path)
            pathhandle=plot([path(m).position(1),path(m-1).position(1)],[path(m).position(2),path(m-1).position(2)],'g-','LineWidth',3);
            pathhandlelist=[pathhandlelist,pathhandle];
        end
    end
end