【路径规划】基于RRT实现路径规划
⼀、简介
(快速探索随机树) 是⼀种通⽤的⽅法，不管什么机器⼈类型、不管⾃由度是多少、不管约束有多复杂都能⽤。⽽且它的原理很简单，这是它在机器⼈领域流⾏的主要原因之⼀。不过它的缺点也很明显，它得到的路径⼀般质量都不是很好，例如可能包含棱⾓，不够光滑，通常也远离最优路径。
RRT 能在众多的规划⽅法中脱颖⽽出，它到底厉害在哪⾥呢？ \ 天下武功唯快不破，“快”是 RRT 的⼀⼤优点。RRT 的思想是快速扩张⼀群像树⼀样的路径以探索(填充)空间的⼤部分区域，伺机找到可⾏的路径。之所以选择“树”是因为它能够探索空间。我们知道，阳光⼏乎是树⽊唯⼀的能量来源。为了最⼤程度地利⽤阳光，树⽊要⽤尽量少的树枝占据尽量多的空间。当然了，能探索空间的不⼀定⾮得是树，⽐如也可以做到，⽽且要多密有多密，如上图左所⽰的例⼦。虽然像Peano曲线这样的单条连续曲线也能探索空间，但是它太“确定”了。在搜索轨迹的时候我们可不知道出路应该在哪⾥，如果不在“确定”的搜索⽅向上，我们怎么找也找不到(找到的概率是0)。这时“随机”的好处就体现出来了，虽然不知道出路在哪⾥，但是通过随机的反复试探还是能碰对的，⽽且碰对的概率随着试探次数的增多越来越⼤，就像买彩票⼀样，买的数量越多中奖的概率越⼤(RRT名字中“随机”的意思)。可是随机试探也讲究策略，如果我们从树中随机取⼀个点，然后向着随机的⽅向⽣长，那么结果是什么样的呢？见上图右。可以看到，同样是随机树，但是这棵树并没很好地探索空间，它⼀直在起点(红点)附近打转。这可不好，我们希望树尽量经济地、均匀地探索空间，不要过度探索⼀个地⽅，更不能漏掉⼤部分地⽅。这样的⼀棵树怎么构造呢？
RRT 的基本步骤是： \ 1. 起点作为⼀颗种⼦，从它开始⽣长枝丫； \ 2. 在机器⼈的“构型”空间中，⽣成⼀个随机点 ； \ 3. 在树上找到距离 最近的那个点，记为 吧； \ 4. 朝着 的⽅向⽣长，如果没有碰到障碍物就把⽣长后的树枝和端点添加到树上，返回 2； \ 随机点⼀般是均匀分布的，
所以没有障碍物时树会近似均匀地向各个⽅向⽣长，这样可以快速探索空间(RRT名字中“快速探索”的意思)。当然如果你事先掌握了最有可能发现路径的区域信息，可以集中兵⼒重点探索这个区域，这时就不宜⽤均匀分布了。 RRT 的⼀个弱点是难以在有狭窄通道的环境找到路径。因为狭窄通道⾯积⼩，被碰到的概率低，找到路径需要的时间要看运⽓了。下图展⽰的例⼦是 RRT 应对⼀个⼈为制作的很短的狭窄通道，有时RRT很快就找到了出路，有时则⼀直被困在障碍物⾥⾯。
RRT探索空间的能⼒还是不错的，例如下图左所⽰的例⼦，障碍物多⽽且杂乱(借助 Mathematica 本⾝具有的强⼤函数库，实现这个例⼦所需的所有代码应该不会超过30⾏)。还有没有环境能难住RRT呢？下图右所⽰的迷宫对RRT就是个挑战。这个时候空间被分割得⾮常严重，RRT显得有些⼒不从⼼了，可见随机策略不是什么时候都有效的。
“随机”使得RRT有很强的探索能⼒。但是成也萧何败也萧何，“随机”也导致 RRT 很盲⽬，像个⽆头苍蝇⼀样到处乱撞。如果机器⼈对环境⼀⽆所知，那么采⽤随机的策略可以接受。可实际情况是，机器
⼈对于它的⼯作环境多多少少是知道⼀些的(即使不是完全知道)。我的博客⼀直强调信息对于机器⼈的重要性。这些已知的信息就可以⽤来改进算法。⼀个改进的办法就是给它⼀双“慧眼”：在势场法中，势函数携带了障碍物和⽬标的信息，如果能把这个信息告诉 RRT，让它在探索空间时有倾向地沿着势场的⽅向前进会更好。这样，RRT 出⾊的探索能⼒刚好可以弥补势场法容易陷⼊局部极⼩值的缺点。
⼆、代码
``` clo(findobj('type','figure','name','RRT w/ Dubins curve')); clo(findobj('type','figure','name','RRT growing')); clear;
% Planning area initialization height = 10; width = 10; center = [0, 0]; % Initial condition of [x, y, direction] origin = [1,2,
20*pi/180]; turning_rad = 0.5; % dubins turning raduis % Define iterations count iteration = 100;
offt = center - [width, height]./2; th range = 2*pi; % do not change without knowledge about Dubins th center = 0; th_offt = 0;
% prelocation of data edges.x = zeros(iteration,2); edges.y = zeros(iteration,2); edges.th = zeros(iteration,2);
edges.param(iteration).p init = [0, 0, 0]; % the initial configuration edges.param(iteration).g param = [0, 0, 0]; % the lengths of the three gments edges.param(iteration).r = turning_rad; % model forward velocity / model angular velocity turning radius edges.param(iteration).type = -1; % path type. one of LSL, LSR, ... edges.param(iteration).flag = 0;
vertecies = origin; vert count = 1; ind nearest = zeros(iteration,1); edge_count = 0;
% figure('name', 'RRT growing'); % originally for real-time animation tic; for i=1:iteration % random point generation x rand = width*rand() + offt(1); y rand = height rand() + offt(2); th_rand = th_range rand() + th_offt;
% connect to nearest point
[ind_nearest(i),param_nearest] = dubins_archn(vertecies, [x_rand, y_rand, th_rand], turning_rad);
% edge_rand = [x_rand, y_rand, th_rand ; vertecies(ind_nearest(i),:)];
% check availablility, e dubins_core.m for more info
if( param_nearest.flag < 0)
% goto next loop
%i = i-1; %doesn't work under MATLAB
el
% append the newly generated point and edge to the existing list
vertecies(vert_count+1,:) = [x_rand, y_rand, th_rand];
vert_count = vert_count + 1;
edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), x_rand];
edges.y(edge_count+1,:) = [vertecies(ind_nearest(i),2), y_rand];
edges.th(edge_count+1,:) = [vertecies(ind_nearest(i),3), th_rand];
edges.param(edge_count+1) = param_nearest;
edge_count = edge_count + 1;
end
% plot animation here is undoable probably due to MATLAB running
%
% scatter(x_rand, y_rand, 10,'filled'); hold on;
% plot([vertecies(ind_nearest(i),1); x_rand], [vertecies(ind_nearest(i),2); y_rand]); hold on;
end toc; clear i x rand y rand edge rand th rand param_nearest
figure('name', 'RRT w/ Dubins curve'); plot RRT Dubins(gca, vertecies, edges, vert_count);
% plot bounderies boundary = [offt; offt+[width 0]; offt+[width height]; offt+[0 height]; offt]; plot(boundary(:,1),boundary(:,2),'--r'); ```
三、结果展⽰
