一.基于采样的路径规划
1.内容来源:深蓝学院课程
二. RRT( Rapidly-exploring Random Tree)
1.RRT的基本思想:
RRT的基本思想是通过随机抽样来增长一棵树,树的每一个节点代表一个可能的机器人位置。每次迭代,RRT都会随机选择一个点并试图连接到现有的树中,使得树逐渐扩展到整个搜索空间
2.RRT的基本步骤如下:
- 初始化:开始时,只有起始点在树中。
- 随机抽样:在搜索空间中随机选择一个点,称为随机点(x_rand)。
- 查找最近邻:遍历树中距离随机点最近的点,称为最近邻点(x_near)。
- 扩展:从最近邻点朝随机点的方向扩展一个固定的小距离,产生一个新的点(x_new)。
- 连接:将新点添加到树中,并连接到最近邻点。
- 终止条件:当达到某个终止条件时(例如树达到一定大小、找到了终点或执行了一定次数的迭代),算法结束。
- 返回路径:如果找到了一条从起点到终点的路径,返回这条路径。
伪代码 :
3.RRT的主要步骤:
3.1树的结构T.v:
3.2 得到x_near
遍历树,从树中找到最近邻近点x_near
function [x_near ,ind_x_near] = Near(x_rand,T)
%在树中搜索离x_rand最近的点,并返回 x_near 的坐标以及 索引值
len = size(T.v,2) ;
dis_tmp = 10000 ;
for node = 1:len
get_dist = sqrt( ((T.v(node).x - x_rand(1))^2) + ((T.v(node).y - x_rand(2))^2));
if(get_dist < dis_tmp)
dis_tmp = get_dist ;
x_near(1) = T.v(node).x;
x_near(2) = T.v(node).y;
ind_x_near = node ;
end
end
end
3.3 扩展得到x_new
function x_new = Steer(x_rand,x_near,Stepsize)
%使用线性插值的办法得到x_new
dist = DIST(x_rand,x_near);
x_new(1) =((dist - Stepsize)*x_near(1) + Stepsize*x_rand(1) )/ dist ;
x_new(2) =((dist - Stepsize)*x_near(2) + Stepsize*x_rand(2) )/ dist ;
end
线性插值 (来源于gpd)
3.4检查是否是collision-free
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;
end
end
if ~feasiblePoint([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; end
function feasible=feasiblePoint(point,map)
feasible=true;
if ~(point(1)>=1 && point(1)<=size(map,2) && ... % x in map
point(2)>=1 && point(2)<=size(map,1) && ... % y in map
map(point(2),point(1))==255) % x,y is Free
feasible=false;
end
3.5 将x_new 插入树中
function T = AddNode(T,x_new,x_near,near_indx)
count = size(T.v,2)+1 ;
T.v(count).x = x_new(1);
T.v(count).y = x_new(2);
T.v(count).xPrev = x_near(1);
T.v(count).yPrev = x_near(2);
T.v(count).dist = DIST(x_new,x_near) + T.v(near_indx).dist ;
T.v(count).indPrev = near_indx ;
end
3.6检查是否到达目标点附近
dist_goal = sqrt(power(x_new(1)-x_G,2) + power(x_new(2) - y_G, 2) );
if(dist_goal <Thr)
bFind = true ;
break
end
3.7 路径返回
%% 路径已经找到,反向查询
if bFind
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
3.8 结果
RRT的优点:
- 能够快速地探索大的搜索空间。
- 能够找到复杂环境中的路径。
- 对于高维空间也能够工作。
RRT的缺点:
- 产生的路径可能不是最优的。
- 可能需要很多迭代才能找到一个解决方案