1 RRT定义
RRT(Rapidly-Exploring Random Tree)算法是一种基于采样的路径规划算法,常用于移动机器人路径规划,适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进,有效躲避障碍物,避免路径陷入局部极小值,收敛速度快。本文通过matlab实现RRT算法,解决二维平面的路径规划问题。
2 地图
为了方便算法的实现,使用离散量来表达环境地图。其中,数值0表示无障碍物的空区域,数值1表示该区域有障碍物。
RRT算法中搜索到的顶点坐标为连续点,在地图中产生随机点,算法将通过连续的点构建树。此过程中,对树枝和顶点进行检测,检测顶点所处位置是否是空区域。下载附录中.dat文件,绘制地图。
colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1]; imshow(uint8(map),colormap)
note:数据中的列为x轴,行为y轴
3 RRT算法原理
通过matlab程序构建从起始位置到目标位置的树,并生成连接两个点的路径。使用一颗中心点在起始点的树,而不是两颗树(一个中心点在起始位置,一个中心点在目标位置)。
编写一个matlab函数,输入和输出有相同的形式。
function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)
其中:
map:.mat文件中的地图矩阵
q_start:起点的x和y坐标
q_goal:目标点的x和y坐标
k: 在目标点无法找到是,控制产生搜索树的最大迭代次数为k次
delta_q : q_new 和 q_near之间的距离
p: 将q_goal 作为q_rand 的概率,当随机产生的随机数小于p,将目标点作为随机点q_rand,当随机产生的数大于p时,产生一个随机点作为q_rand
vertices:顶点的x和y坐标,生成随机树的过程中产生的所有的点的坐标都存储在这个矩阵中,第一个点为起点,最后一个点为目标点。是一个2行n列的矩阵
deges:生成随机树的所有树枝,一共有n-1个树枝,因此该矩阵有n-1行,每一行的两列分别表示两个点的索引号。一旦搜索到目标点,最后一行将表示目标点,沿着目标点回溯,即可找到路径
path: 从起始点到目标点的索引,是一个行向量
下面用一个图来表示上面提到的算法里的一些变量:
4 障碍物检测
检测树枝(即q_near和q_new之间的edge)是否处于自由空间,可以使用增量法或者等分法,示意图如下(假设两点之间有10个点,左图为为增量检测法,右图为等分法,从示意图中可以看出使用等分法检测次数更少):
在本文中,使用k=10000,delta_q=50,p=0.3, 我们将获得如下结果:
5 路径平滑处理
完成基本的RRT算法之后,我们获得了一条从起点到终点的路径,现在对这条路径进行平滑和降噪处理,处理完成之后,我们将得到一条更短的路径。
采用贪心算法:
连接q_start和q_goal,如果检测到两个点之间有障碍物,则将q_goal替换为前一个点,直到两个点能连接上(中间无障碍物)为止。一旦q_goal被连接上,
在matlab中定义平滑函数:
function [path_smooth] = smooth(map, path, vertices, delta)
其中:
path: 从起始点到目标位置的路径索引号
vertices:树中所有的顶点坐标
delta:增量距离,用来检测路径顶点之间的直接连接是否在自由空间之内,每个edge都被delta分割成几段
path_smooth:经过平滑处理之后,路径点将会减少,用path_smooth记录平滑之后的路径,仍然是一个行向量,记录路径的索引号
平滑处理之后的路径为:
6 总结
RRT算法是一种增量式的搜索算法,基于概率的思想,它是一种概率完备的路径优化算法,具有求解速度上的优势。RRT基本算法有其自身缺陷,求解得到的路径通常质量不好,带有棱角,不够光滑。因此需要对路径进行平滑处理,才能得到适合机器人路径跟踪的路径曲线。
function result = PlanPathRRTstar(param, p_start, p_goal) % RRT* % credit : Anytime Motion Planning using the RRT*, S. Karaman, et. al. % calculates the path using RRT* algorithm % param : parameters for the problem % 1) threshold : stopping criteria (distance between goal and current % node) % 2) maxNodes : maximum nodes for rrt tree % 3) neighborhood : distance limit used in finding neighbors % 4) obstacle : must be rectangle-shaped #limitation % 5) step_size : the maximum distance that a robot can move at a time % (must be equal to neighborhood size) #limitation % 6) random_seed : to control the random number generation % p_start : [x;y] coordinates % p_goal : [x;y] coordinates % variable naming : when it comes to describe node, if the name is with % 'node', it means the coordinates of that node or it is just an index of % rrt tree % rrt struct : 1) p : coordinate, 2) iPrev : parent index, 3) cost : % distance % obstacle can only be detected at the end points but not along the line % between the points % for cost, Euclidean distance is considered. % output : cost, rrt, time_taken % whether goal is reached or not, depends on the minimum distance between % any node and goal field1 = 'p'; field2 = 'iPrev'; field3 = 'cost'; field4 = 'goalReached'; rng(param.random_seed); tic; start(); function start() rrt(1) = struct(field1, p_start, field2, 0, field3, 0, field4, 0); N = param.maxNodes; % iterations j = 1; % while endcondition>param.threshold %&& j<=N while j<=N sample_node = getSample(); % plot(sample_node(1), sample_node(2), '.g'); % text(sample_node(1), sample_node(2), strcat('random',num2str(j))) nearest_node_ind = findNearest(rrt, sample_node); % plot(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), '.g'); % text(rrt(nearest_node_ind).p(1), rrt(nearest_node_ind).p(2), strcat('nearest', num2str(j))); new_node = steering(rrt(nearest_node_ind).p, sample_node); if (isObstacleFree(new_node)==1) % plot(new_node(1), new_node(2), '.g'); % text(new_node(1), new_node(2)+3, strcat('steered: new node', num2str(j))) neighbors_ind = getNeighbors(rrt, new_node); if(~isempty(neighbors_ind)) parent_node_ind = chooseParent(rrt, neighbors_ind, nearest_node_ind,new_node); % plot(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2), '.g'); % text(rrt(parent_node_ind).p(1), rrt(parent_node_ind).p(2)+3, strcat('parent', num2str(j))); else parent_node_ind = nearest_node_ind; end rrt = insertNode(rrt, parent_node_ind, new_node); if (~isempty(neighbors_ind)) rrt = reWire(rrt, neighbors_ind, parent_node_ind, length(rrt)); end if norm(new_node-p_goal) == param.threshold rrt = setReachGoal(rrt); end end j = j + 1; end setPath(rrt); % text1 = strcat('Total number of generated nodes:', num2str(j-1)) % text1 = strcat('Total number of nodes in tree:', length(rrt)) end function rrt=setReachGoal(rrt) rrt(end).goalReached = 1; end function setPath(rrt) for i = 1: length(rrt)-1 p1 = rrt(i).p; rob.x = p1(1); rob.y=p1(2); plot(rob.x,rob.y,'.b') child_ind = find([rrt.iPrev]==i); for j = 1: length(child_ind) p2 = rrt(child_ind(j)).p; plot([p1(1),p2(1)], [p1(2),p2(2)], 'b', 'LineWidth', 1); end end [cost,i] = getFinalResult(rrt); result.cost = cost; result.rrt = [rrt.p]; while i ~= 0 p11 = rrt(i).p; plot(p11(1),p11(2),'b', 'Marker','.', 'MarkerSize', 30); i = rrt(i).iPrev; if i ~= 0 p22 = rrt(i).p; plot(p22(1),p22(2),'b', 'Marker', '.', 'MarkerSize', 30); % plot([p11(1),p22(1)],[p11(2),p22(2)], 'b', 'LineWidth', 3); end end result.time_taken = toc; end function [value,min_node_ind] = getFinalResult(rrt) goal_ind = find([rrt.goalReached]==1); if ~(isempty(goal_ind)) disp('Goal has been reached!'); rrt_goal = rrt(goal_ind); value = min([rrt_goal.cost]); min_node_ind = find([rrt.cost]==value); if length(min_node_ind)>1 min_node_ind = min_node_ind(1); end else disp('Goal has not been reached!'); for i =1:length(rrt) norm_rrt(i) = norm(p_goal-rrt(i).p); end [value,min_node_ind]= min(norm_rrt); value = rrt(min_node_ind).cost; end end % if it is obstacle-free, return 1. % otherwise, return 0 function free=isObstacleFree(node_free) free = 1; for i = 1: length(param.obstacles(:,1)) obstacle = param.obstacles(i,:); op1 = [obstacle(1), obstacle(2)]; op2 = [op1(1)+obstacle(3), op1(2)]; op3 = [op2(1), op1(2) + obstacle(4)]; op4 = [op1(1), op3(2)]; nx = node_free(1); ny = node_free(2); if ((nx>=op1(1) && nx<=op2(1)) && (ny>=op1(2) && ny<=op4(2))) free = 0; end end end function new_node=steering(nearest_node, random_node) dist = norm(random_node-nearest_node); ratio_distance = param.step_size/dist; x = (1-ratio_distance).* nearest_node(1)+ratio_distance .* random_node(1); y = (1-ratio_distance).* nearest_node(2)+ratio_distance .* random_node(2); new_node = [x;y]; end function rrt = reWire(rrt, neighbors, parent, new) for i=1:length(neighbors) cost = rrt(new).cost + norm(rrt(neighbors(i)).p - rrt(new).p); if (cost<rrt(neighbors(i)).cost) % if norm(rrt(new).p-rrt(neighbors(i)).p)<param.step_size % % plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.b'); % rrt(neighbors(i)).p = steering(rrt(new).p, rrt(neighbors(i)).p); % end % plot(rrt(neighbors(i)).p(1), rrt(neighbors(i)).p(2), '.m'); rrt(neighbors(i)).iPrev = new; rrt(neighbors(i)).cost = cost; end end end function rrt = insertNode(rrt, parent, new_node) rrt(end+1) = struct(field1, new_node, field2, parent, field3, rrt(parent).cost + norm(rrt(parent).p-new_node), field4, 0); end function parent = chooseParent(rrt, neighbors, nearest, new_node) min_cost = getCostFromRoot(rrt, nearest, new_node); parent = nearest; for i=1:length(neighbors) cost = getCostFromRoot(rrt, neighbors(i), new_node); if (cost<min_cost) min_cost = cost; parent = neighbors(i); end end end function cost = getCostFromRoot(rrt, parent, child_node) cost = rrt(parent).cost + norm(child_node - rrt(parent).p); end function neighbors = getNeighbors(rrt, node) neighbors = []; for i = 1:length(rrt) dist = norm(rrt(i).p-node); if (dist<=param.neighbourhood) neighbors = [neighbors i]; end end end
1 matlab版本
2014a
2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例(第2版)[M].电子工业出版社,2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社,2017.
[3]RRT路径规划算法