快速扩展随机数(RRT)算法,是近十几年应用比较广泛的一种运动规划算法。
它的大致原理为:原始的RRT算法通过一个初始点作为根节点,通过随机采样,增加叶子节点的方式,生成一个随机扩展数,当随机树中的叶子节点包含了目标点或进入了目标区域,边可以在随机树中通过回溯的方式,找到这条从初始点到目标点的路径。
RRT总体是一种基于概率采样的搜索方法,通过状态空间的随机采样点,把搜索导向空白区域,从而孕照到一条从起始点到目标点的规划路径,通过对状态空间中的采样点进行碰撞检测,避免了对空间的建模,当然它也有自身的缺点,这个做完后续的实验便可以体会到。
总体步骤
1.建立树,根节点为起始点,指定目标点
2.读取地图数据
3.在地图中随机得到一个点,记作p_rand
4.遍历当前的整个树,找到距离随机点最近的点,记作p_near
5.由p_near向随机点p_rand扩展一个步长的距离,记步长的记录为Delta,扩展后的点记作p_new
6.检测此p_new点是否在障碍物上,如果在障碍物上,则跳出本次循环,从第3步重新开始
7.将新产生的点p_new插入到整个树中
8.当p_new点距离目标点小于某个范围时,退出搜索,然后将整个路径画出来
实现
在实现上述步骤的时候,会有一些细节上的处理过程,上述的步骤只是大致的步骤描述。
先建立树以及地图:
%设置起始点
init_x = 10;
init_y = 10;
%设置目标点
end_x = 450;
end_y = 450;
%建立树,根节点为起始点
Tree.v(1).x = init_x;
Tree.v(1).y = init_y;
Tree.v(1).xFather = init_x; %根节点的父节点仍是它本身
Tree.v(1).yFather = init_y;
Tree.v(1).dis = 0; %从父节点到该节点的距离
%读取地图
figure(1);
map = im2bw(imread('map1.bmp')); %读取图片并二值化作为地图
imshow(map);
mapX = size(map,1);%地图x轴长度
mapY = size(map,2);%地图y轴长度
hold on;
plot(init_x, init_y, 'ro', 'MarkerSize',5, 'MarkerFaceColor','r');
plot(end_x, end_y, 'go', 'MarkerSize',5, 'MarkerFaceColor','b');% 绘制起点和目标点
注意上述建立树的过程中,结构体中保存的是当前点的x,y坐标,以及其父节点的坐标xFather 与 yFather ,这个是为了后续回溯路线时方便,当我们找到最后一个点时,从最后一个点一直向上回溯其父节点,然后将其保存,必然可以回溯到起始点,然后就可以将整个路径保存下来。
在地图中随机得到一个点,但是这个点不能在障碍物上,并且需要注意此随机点不能超过整个地图的距离:
%Step 1: 在地图中随机采样一个点p_rand 但是p_rand不能是在障碍物上
%当是原点坐标时,重新随机
while (p_rand(1) == 1 && p_rand(2) == 1) || (map(p_rand(1),p_rand(2)) == 0)
p_rand(1) = round(rand() * mapX); % rand()生成的是0~1均匀分布的随机数,乘以800再向上取整,数便为[1,800]间的整数
p_rand(2) = round(rand() * mapY);
if p_rand(1) < 1
p_rand(1) = 1;
else if p_rand(1) > mapX
p_rand(1) = mapX;
end
end
if p_rand(2) < 1
p_rand(2) = 1;
else if p_rand(2) > mapY
p_rand(2) = mapY;
end
end
end
%是在障碍物上时就一直随机
while map(p_rand(1),p_rand(2)) == 0
p_rand(1) = round(rand() * mapX); % rand()生成的是0~1均匀分布的随机数,乘以800再向上取整,数便为[1,800]间的整数
p_rand(2) = round(rand() * mapY);
if p_rand(1) < 1
p_rand(1) = 1;
else if p_rand(1) > mapX
p_rand(1) = mapX;
end
end
if p_rand(2) < 1
p_rand(2) = 1;
else if p_rand(2) > mapY
p_rand(2) = mapY;
end
end
end
遍历树,从树中找到最近邻近点p_near:
p_near=[];
%Step 2: 遍历树,从树中找到最近邻近点p_near
min_distance = 1000;
index = 1; %index先指向根节点
for i = 1:count
distance = sqrt( ( Tree.v(i).x - p_rand(1) )^2 + ( Tree.v(i).y - p_rand(2) )^2 );
if distance < min_distance
min_distance = distance;
index = i;
end
end
p_near(1) = Tree.v(index).x; %保存距离随机点距离最近的点
p_near(2) = Tree.v(index).y;
由距离随机点最近的点向随机点已Delta的补偿扩展得到p_new节点,并且检测p_new节点是否在障碍物上:
p_new=[];
%Step 3: 由距离随机点最近的点向随机点已Delta的补偿扩展得到p_new节点
Delta = 10; %步长设置为10
p_new(1) = p_near(1) + round( ( p_rand(1) - p_near(1) ) * Delta/min_distance ); %求出扩展一个步长后的坐标
p_new(2) = p_near(2) + round( ( p_rand(2) - p_near(2) ) * Delta/min_distance );
%防止坐标计算到负数造成错误
if p_new(1) < 1
p_new(1) = 1;
else if p_new(1) >= mapX
p_new(1) = mapX - 1;
end
end
if p_new(2) < 1
p_new(2) = 1;
else if p_new(2) >= mapY
p_new(2) = mapY;
end
end
%检查p_new节点是否触碰到障碍物 因为是二值化图像,白色为1,黑色为0 黑色即为障碍物
%注意p_new(1)代表的是列x,p_new(2)是行y,因此判断的时候需要写map(p_new(2),p_new(1))
%而不是map(p_new(1),p_new(2))
if map(p_new(2),p_new(1)) ~= 1
continue; %舍弃本次采样点,跳出本次循环
end
count = count + 1; %采样点可用时,继续后面循环
将新点插入到整个树中:
%Step 4: 将p_new插入树
Tree.v(count).x = p_new(1);
Tree.v(count).y = p_new(2);
Tree.v(count).xFather = p_near(1);
Tree.v(count).yFather = p_near(2);
Tree.v(count).dist = min_distance;
检查是否到达目标点附近:
new_distance = sqrt( ( p_new(1) - end_x )^2 + ( p_new(2) - end_y )^2 ); %计算新点相对终点的距离,小于一定阈值则判定到达终点
if new_distance <= ArriveDis
plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_new
line( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_new
line( [end_x p_new(1)], [end_y p_new(2)], 'Marker','.','LineStyle','-'); %连接x_Target和x_new
break;
end
将各个路径画出来,并且在while循环中增加一个延时,这样可以让算法搜索路径的过程更好的显示出来:
%Step 6:将x_near和x_new之间的路径画出来
plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_new
line( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_new
hold on; %要多次在同一张图上绘制线段,所以使用plot后需要接上hold on命令
pause(0.01); %暂停时间
之后从终点开始逆序寻找各个父节点就可以将整个路径画出来:
%一直检测到坐标点是起始点坐标
while path(path_count,1) ~= init_x || path(path_count,2) ~= init_y
%从终点逆序一直寻找当前点的父节点,就可以最终找到所有节点的编号
i2=1;
while ( Result_LIST(i2,1) ~= path(path_count,1) || Result_LIST(i2,2) ~= path(path_count,2) )
i2 = i2 + 1;
end
new_n_index = i2;
path_count = path_count + 1;
%将此节点的父节点保存下来
path(path_count,1) = Result_LIST(new_n_index,3);
path(path_count,2) = Result_LIST(new_n_index,4);
n_index = new_n_index;
end
最终的效果如下:
RRT路径规划
RRT,红色起始,蓝色目标
上述的代码展示出了关键步骤的代码,不过RRT算法也有其缺点,一个是其找到的路径只是可行路径,并不是最优路径,另外的,当地图很复杂的时候,RRT算法就容易陷入死循环而出不去,就像这样:
不过这个RRT也只是最简单的一个RRT了,后续还需要再对此算法进行变形优化,让其达到更好的效果。