全局路径规划：图搜索算法介绍4(RRT/RRT*)

本文课件来自香港科技大学，我的母校，感谢ELEC

本节介绍RRT/RRT*的算法：
RRT的基本原理是：

我们首先初始化我们的起点，接下来随机撒点，选出一个x_rand, 在x_near 和 x_rand之间选择一个x_new, 再在原有的已经存在的x中找到离这个点最近的点将这两个点连接起来，同时这个最近的点也会作为x_new的父节点。

RRT算法的伪代码如下：

对照着图，再看一次：
首先我们随机生成一个点，x_rand

然后再tree上面找到最近点，作为x_near

然后取两者中间的点作为x_new
最后，还要做一次collision checking, 看看生成的点是不是和x_near 连接起来后会碰撞障碍物：

按照这个方法一直搜索，一直打到停止搜索的标准，比如x_new与终点的距离小于某个极小的epsilon。另外一个，在搜索最近的x_near的时候，我们可以使用KD tree来加速搜索：

具体看一下(https://blog.csdn.net/junshen1314/article/details/51121582)

接下来我们分析一下RRT的优缺点：

RRT比概率图方法更加有效，但是这依然不是个高效的搜索方法，并且获得的解也不是最优解。

接下里，我们有一些对RRT的优化方法：
第一种方法就是双向RRT，意思就是从起点和终点同时搜索，一直到两棵树交汇

下图中可以看到，起点和终点同时生成树进行搜索。

第二种方法我们介绍RRT*
伪代码流程如下：

这个算法是在RRT基础上的改进，改进的地方是这个x_near 和 x_new不会直接连接起来，而是做一个优化处理，方式就是：

我们在x_near附近圈一个圆，将被圈在圈内的各个点与x_new的距离作对比：

如果x_near 到 x_new的距离比通过x1,x2后再到x_new的距离低，就将x_near和x_new连接起来。

同时我们还要对比，从x_near出发，到x2的距离最短值：

我们发现通过x_new之后到达x2的距离更短，所以就将x2的父节点更新为x_new. 这个步骤我们称之为rewire function.

相关视频演示参考：
https://www.youtube.com/watch?v=YKiQTJpPFkA

当然我们还有其他的优化，包括考虑动力学限制的优化：

接下来我们提供一系列源代码供读者进行测试：
首先提供2D RRT

%***************************************
%Author: Yuncheng JIANG
%***************************************
%% 流程初始化
clc;
clear all
close all
x_I=1; y_I=1;           % 设置初始点
x_G=700; y_G=700;       % 设置目标点
Thr=50;                 %设置目标点阈值
delta = 0.35 ;   % 设置扩展步长
iteration = 10000;
%% 建树初始化
T.v(1).x = x_I;         % T是我们要做的树，v是节点，这里先把起始点加入到T里面来
T.v(1).y = y_I;
T.v(1).xPrev = x_I;     % 起始节点的父节点仍然是其本身
T.v(1).yPrev = y_I;
T.v(1).dist=0;          %从父节点到该节点的距离，这里可取欧氏距离
T.v(1).indPrev = 0;     %
%% 开始构建树——作业部分
figure(1);
ImpRgb=imread('newmap.png');
Imp=rgb2gray(ImpRgb);
imshow(Imp)
xL=size(Imp,1);%地图x轴长度
yL=size(Imp,2);%地图y轴长度
hold on
plot(x_I, y_I, 'ro', 'MarkerSize',10, 'MarkerFaceColor','r');
plot(x_G, y_G, 'go', 'MarkerSize',10, 'MarkerFaceColor','g');% 绘制起点和目标点
count=1;
j =1;
for iter = 1:iterationiterx_rand(1) = rand(1)*800;x_rand(2) = rand(1)*800;x_rand = [x_rand(1),x_rand(2)];%Step 1: 在地图中随机采样一个点x_rand%提示：用（x_rand(1),x_rand(2)）表示环境中采样点的坐标dist =zeros(length(T),1);for i =1: countdist(i) = (T.v(i).x- x_rand(1))^2 + (T.v(i).y - x_rand(2))^2;[value, location] = min(dist);x_near = [T.v(location).x,T.v(location).y];end%Step 2: 遍历树，从树中找到最近邻近点x_near %提示：x_near已经在树T里x_new=[x_near(1)+delta * (x_rand(1)-x_near(1)), x_near(2)+ delta * (x_rand(2)-x_near(2))];%Step 3: 扩展得到x_new节点%提示：注意使用扩展步长Delta%检查节点是否是collision-freeif ~collisionChecking(x_near,x_new,Imp) continue;endcount=count+1;   pos =count;%Step 4: 将x_new插入树T %提示：新节点x_new的父节点是x_nearT.v(pos).x = x_new(1);         % T是我们要做的树，v是节点，这里先把起始点加入到T里面来T.v(pos).y = x_new(2); T.v(pos).xPrev = x_near(1);     % 起始节点的父节点仍然是其本身T.v(pos).yPrev = x_near(2);T.v(pos).dist=sqrt((x_new(1)-x_near(1))^2 + (x_new(2)-x_near(2))^2);          %从父节点到该节点的距离，这里可取欧氏距离T.v(pos).indPrev = T.v(pos-1).indPrev + 1;%Step 5:检查是否到达目标点附近 %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环epsilon =10;%Step 6:将x_near和x_new之间的路径画出来%提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令%提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来plot([x_near(1),x_new(1)],[x_near(2),x_new(2)],'-r')hold onpause(0.1); %暂停0.1s，使得RRT扩展过程容易观察if sqrt((x_G-x_new(1))^2 + (y_G - x_new(2))^2)<=epsilonbreak;endend
%% 路径已经找到，反向查询
% if iter < iteration
%     path.pos(1).x = x_G; path.pos(1).y = y_G;
%     path.pos(2).x = T.v(end).x; path.pos(2).y = T.v(end).y;
%     pathIndex = T.v(end).indPrev; % 终点加入路径
%     j=0;
%     while 1
%         path.pos(j+3).x = T.v(pathIndex).x;
%         path.pos(j+3).y = T.v(pathIndex).y;
%         pathIndex = T.v(pathIndex).indPrev;
%         if pathIndex == 1
%             break
%         end
%         j=j+1;
%     end  % 沿终点回溯到起点
%     path.pos(end+1).x = x_I; path.pos(end).y = y_I; % 起点加入路径
%     for j = 2:length(path.pos)
%         plot([path.pos(j).x; path.pos(j-1).x;], [path.pos(j).y; path.pos(j-1).y], 'b', 'Linewidth', 3);
%     end
% else
%     disp('Error, no path found!');
% end
%%
path(1,:) =[T.v(pos).x, T.v(pos).y];
k =1;pin =[T.v(pos).xPrev, T.v(pos).yPrev];condi = 0;while condi ==0
for i = 1:posiif T.v(i).x == pin(1) && T.v(i).y ==pin(2)path(k+1,:)= [pin(1), pin(2)]k = k+1;pin =[T.v(i).xPrev, T.v(i).yPrev];end
endif pin(1)== T.v(1).x &&pin(2) ==T.v(1).ycondi =1; endpath = [path;[x_I,y_I]];   end
plot(path(:,1),path(:,2),'b', 'Linewidth', 3)
plot([path(1,1),x_G],[path(1,2),y_G],'b', 'Linewidth', 3)

另外请调用函数：

function feasible=collisionChecking(startPose,goalPose,map)feasible=true;
dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));
for r=0:0.5:sqrt(sum((startPose-goalPose).^2))posCheck = startPose + r.*[sin(dir) cos(dir)];if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ...feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))feasible=false;break;endif ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; endendfunction feasible=feasiblePoint(point,map)
feasible=true;
if ~(point(1)>=1 &&  point(1)<=size(map,2) && point(2)>=1 && point(2)<=size(map,1) && map(point(2),point(1))==255)feasible=false;
end

还需要图片：

把这个图片保存在运行目录下。

最后运行的效果是这样的：
Matlab社区中，我们也找到了参考的代码，RRT* 2D/3D

% RRT* algorithm in 2D with collision avoidance.
%
% Author: Sai Vemprala
%
% nodes:    Contains list of all explored nodes. Each node contains its
%           coordinates, cost to reach and its parent.
%
% Brief description of algorithm:
% 1. Pick a random node q_rand.
% 2. Find the closest node q_near from explored nodes to branch out from, towards
%    q_rand.
% 3. Steer from q_near towards q_rand: interpolate if node is too far away, reach
%    q_new. Check that obstacle is not hit.
% 4. Update cost of reaching q_new from q_near, treat it as Cmin. For now,
%    q_near acts as the parent node of q_new.
% 5. From the list of 'visited' nodes, check for nearest neighbors with a
%    given radius, insert in a list q_nearest.
% 6. In all members of q_nearest, check if q_new can be reached from a
%    different parent node with cost lower than Cmin, and without colliding
%    with the obstacle. Select the node that results in the least cost and
%    update the parent of q_new.
% 7. Add q_new to node list.
% 8. Continue until maximum number of nodes is reached or goal is hit.clearvars
close allx_max = 1000;
y_max = 1000;
obstacle = [500,150,200,200];
EPS = 20;
numNodes = 3000;        q_start.coord = [0 0];
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [999 999];
q_goal.cost = 0;nodes(1) = q_start;
figure(1)
axis([0 x_max 0 y_max])
rectangle('Position',obstacle,'FaceColor',[0 .5 .5])
hold onfor i = 1:1:numNodesq_rand = [floor(rand(1)*x_max) floor(rand(1)*y_max)];plot(q_rand(1), q_rand(2), 'x', 'Color',  [0 0.4470 0.7410])% Break if goal node is already reachedfor j = 1:1:length(nodes)if nodes(j).coord == q_goal.coordbreakendend% Pick the closest node from existing list to branch out fromndist = [];for j = 1:1:length(nodes)n = nodes(j);tmp = dist(n.coord, q_rand);ndist = [ndist tmp];end[val, idx] = min(ndist);q_near = nodes(idx);q_new.coord = steer(q_rand, q_near.coord, val, EPS);if noCollision(q_rand, q_near.coord, obstacle)line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], 'Color', 'k', 'LineWidth', 2);drawnowhold onq_new.cost = dist(q_new.coord, q_near.coord) + q_near.cost;% Within a radius of r, find all existing nodesq_nearest = [];r = 60;neighbor_count = 1;for j = 1:1:length(nodes)if noCollision(nodes(j).coord, q_new.coord, obstacle) && dist(nodes(j).coord, q_new.coord) <= rq_nearest(neighbor_count).coord = nodes(j).coord;q_nearest(neighbor_count).cost = nodes(j).cost;neighbor_count = neighbor_count+1;endend% Initialize cost to currently known valueq_min = q_near;C_min = q_new.cost;% Iterate through all nearest neighbors to find alternate lower% cost pathsfor k = 1:1:length(q_nearest)if noCollision(q_nearest(k).coord, q_new.coord, obstacle) && q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord) < C_minq_min = q_nearest(k);C_min = q_nearest(k).cost + dist(q_nearest(k).coord, q_new.coord);line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], 'Color', 'g');                hold onendend% Update parent to least cost-from nodefor j = 1:1:length(nodes)if nodes(j).coord == q_min.coordq_new.parent = j;endend% Append to nodesnodes = [nodes q_new];end
endD = [];
for j = 1:1:length(nodes)tmpdist = dist(nodes(j).coord, q_goal.coord);D = [D tmpdist];
end% Search backwards from goal to start to find the optimal least cost path
[val, idx] = min(D);
q_final = nodes(idx);
q_goal.parent = idx;
q_end = q_goal;
nodes = [nodes q_goal];
while q_end.parent ~= 0start = q_end.parent;line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], 'Color', 'r', 'LineWidth', 2);hold onq_end = nodes(start);
end

其中调用函数：

function val = ccw(A,B,C)val = (C(2)-A(2)) * (B(1)-A(1)) > (B(2)-A(2)) * (C(1)-A(1));
endfunction d = dist(q1,q2)
d = sqrt((q1(1)-q2(1))^2 + (q1(2)-q2(2))^2);
endfunction nc = noCollision(n2, n1, o)A = [n1(1) n1(2)];B = [n2(1) n2(2)];obs = [o(1) o(2) o(1)+o(3) o(2)+o(4)];C1 = [obs(1),obs(2)];D1 = [obs(1),obs(4)];C2 = [obs(1),obs(2)];D2 = [obs(3),obs(2)];C3 = [obs(3),obs(4)];D3 = [obs(3),obs(2)];C4 = [obs(3),obs(4)];D4 = [obs(1),obs(4)];% Check if path from n1 to n2 intersects any of the four edges of the% obstacleints1 = ccw(A,C1,D1) ~= ccw(B,C1,D1) && ccw(A,B,C1) ~= ccw(A,B,D1); ints2 = ccw(A,C2,D2) ~= ccw(B,C2,D2) && ccw(A,B,C2) ~= ccw(A,B,D2);ints3 = ccw(A,C3,D3) ~= ccw(B,C3,D3) && ccw(A,B,C3) ~= ccw(A,B,D3);ints4 = ccw(A,C4,D4) ~= ccw(B,C4,D4) && ccw(A,B,C4) ~= ccw(A,B,D4);if ints1==0 && ints2==0 && ints3==0 && ints4==0nc = 1;elsenc = 0;end
endfunction A = steer(qr, qn, val, eps)qnew = [0 0];% Steer towards qn with maximum step size of epsif val >= epsqnew(1) = qn(1) + ((qr(1)-qn(1))*eps)/dist(qr,qn);qnew(2) = qn(2) + ((qr(2)-qn(2))*eps)/dist(qr,qn);elseqnew(1) = qr(1);qnew(2) = qr(2);end   A = [qnew(1), qnew(2)];
end

运行效果是这样的：

RRT* 3D

clearvars
close all
x_max = 640;
y_max = 480;
z_max = 400;EPS = 20;
numNodes = 2000;        q_start.coord = [0 0 0];
q_start.cost = 0;
q_start.parent = 0;
q_goal.coord = [640 400 180];
q_goal.cost = 0;nodes(1) = q_start;
figure(1)for i = 1:1:numNodesq_rand = [rand(1)*x_max rand(1)*y_max rand(1)*z_max];plot3(q_rand(1), q_rand(2), q_rand(3), 'x', 'Color',  [0 0.4470 0.7410])% Break if goal node is already reachedfor j = 1:1:length(nodes)if nodes(j).coord == q_goal.coordbreakendend% Pick the closest node from existing list to branch out fromndist = [];for j = 1:1:length(nodes)n = nodes(j);tmp = dist_3d(n.coord, q_rand);ndist = [ndist tmp];end[val, idx] = min(ndist);q_near = nodes(idx);q_new.coord = steer3d(q_rand, q_near.coord, val, EPS);line([q_near.coord(1), q_new.coord(1)], [q_near.coord(2), q_new.coord(2)], [q_near.coord(3), q_new.coord(3)], 'Color', 'k', 'LineWidth', 2);drawnowhold onq_new.cost = dist_3d(q_new.coord, q_near.coord) + q_near.cost;% Within a radius of r, find all existing nodesq_nearest = [];r = 50;neighbor_count = 1;for j = 1:1:length(nodes)if (dist_3d(nodes(j).coord, q_new.coord)) <= rq_nearest(neighbor_count).coord = nodes(j).coord;q_nearest(neighbor_count).cost = nodes(j).cost;neighbor_count = neighbor_count+1;endend% Initialize cost to currently known valueq_min = q_near;C_min = q_new.cost;% Iterate through all nearest neighbors to find alternate lower% cost pathsfor k = 1:1:length(q_nearest)if q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord) < C_minq_min = q_nearest(k);C_min = q_nearest(k).cost + dist_3d(q_nearest(k).coord, q_new.coord);line([q_min.coord(1), q_new.coord(1)], [q_min.coord(2), q_new.coord(2)], [q_min.coord(3), q_new.coord(3)], 'Color', 'g');            hold onendend% Update parent to least cost-from nodefor j = 1:1:length(nodes)if nodes(j).coord == q_min.coordq_new.parent = j;endend% Append to nodesnodes = [nodes q_new];
endD = [];
for j = 1:1:length(nodes)tmpdist = dist_3d(nodes(j).coord, q_goal.coord);D = [D tmpdist];
end% Search backwards from goal to start to find the optimal least cost path
[val, idx] = min(D);
q_final = nodes(idx);
q_goal.parent = idx;
q_end = q_goal;
nodes = [nodes q_goal];
while q_end.parent ~= 0start = q_end.parent;line([q_end.coord(1), nodes(start).coord(1)], [q_end.coord(2), nodes(start).coord(2)], [q_end.coord(3), nodes(start).coord(3)], 'Color', 'r', 'LineWidth', 4);hold onq_end = nodes(start);
end

调用的函数是：

function A = steer(qr, qn, val, eps)qnew = [0 0];if val >= epsqnew(1) = qn(1) + ((qr(1)-qn(1))*eps)/dist_3d(qr,qn);qnew(2) = qn(2) + ((qr(2)-qn(2))*eps)/dist_3d(qr,qn);qnew(3) = qn(3) + ((qr(3)-qn(3))*eps)/dist_3d(qr,qn);elseqnew(1) = qr(1);qnew(2) = qr(2);qnew(3) = qr(3);endA = [qnew(1), qnew(2), qnew(3)];
endfunction d = dist_3d(q1,q2)d = sqrt((q1(1)-q2(1))^2 + (q1(2)-q2(2))^2 + (q1(3)-q2(3))^2);
end

运行的结果是：

全局路径规划：图搜索算法介绍4(RRT/RRT*)相关推荐

路径规划 | 图搜索算法：JPS
JPS算法全称为Jump Point Search,也就是跳点算法,可以视为A*算法的一种改进算法,它保留了A*算法的主体框架,区别在于:A*算法是将当前节点的所有未访问邻居节点加入openlist, ...
全局路径规划AstarDijkstra---ROS算法入门学习
文章目录概述 Astar 原理 Dijkstra 原理代码涉及知识点补充 1. 类类的定义形式类的实例化 2. 操作列表列表创建添加元素概述
全局路径规划：图搜索算法介绍6(A star)Matlab算法实现
本文接:全局路径规划:图搜索算法介绍2(A star) https://blog.csdn.net/gophae/article/details/103061702 % This is Yunchen ...
全局路径规划：图搜索算法介绍1(BFS/DFS)
对于全局路径规划的设计,我们先要了解什么是图搜索,在此之前,要先知道什么是图: 可以看到,图有很多种,有无向图,有向图,节点之间还可以有不同的weight, 用于表述从节点与节点直接迁移的代价. 而图 ...
苏宁 11.11：仓库内多 AGV 协作的全局路径规划算法研究
本文为『InfoQ x 苏宁 2018双十一』技术特别策划系列文章之一. 1. 背景随着物联网和人工智能的发展,越来越多的任务渐渐的被机器人取代,机器人逐渐在发展中慢慢进入物流领域,"智能 ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...
ROS1云课→20迷宫不惑之A*大法（一种虽古老但实用全局路径规划算法）
ROS1云课→19仿真turtlebot(stage) 19提及的机器人如何实现全局路径规划?A*算法是一种可行的选择. www.gamedev.net/reference/articles/arti ...
(一)路径规划算法---Astar实现自定义的全局路径规划插件
Astar实现自定义的全局路径规划插件文章目录 Astar实现自定义的全局路径规划插件 1.插件功能包的建立 2. 相关步骤 2.1 建立工作空间和环境变量的配置 2.2 建立功能包 2.3 添加源 ...
视觉学习笔记5——FAR Planner全局路径规划算法学习研究
文章目录一.Far Planner是什么? 二.配置安装Far Planner 1.运行环境 2.下载编译开发环境包 3.编译Far_Planner 一.Far Planner是什么? FAR Pl ...
【硬核】 ROS Navigation 局部路径规划常见算法
简介最近,作者参加了关于RMUS 高校 SimReal挑战赛,首次接触到了机器人导航领域,这里记录一下这段时间的收货.sim2real的全称是simulation to reality,是强化学 ...

全局路径规划：图搜索算法介绍4(RRT/RRT*)

全局路径规划：图搜索算法介绍4(RRT/RRT*)相关推荐

最新文章

热门文章