[MATLAB] 7. Fast Search random tree (RRT---rapidly-exploring random Trees) path planning

RRT is a kind of efficient planning method in multidimensional space. It takes an initial point as the root node, increases the leaf node by random sampling, generates a random extension tree, and when the leaf node in the random tree contains the target point or enters the target area, it can find a path from the initial point to the target point in the random tree. The RRT method is a complete and suboptimal probability.

function Buildrrt (qinit, K,δq) t.init (qinit) forK = 1To K Qrand= Sample ()--chooses a random configuration qnearest= Nearest (T, Qrand)--Selects the nodeinchThe RRT tree that isclosest to QrandifDistance (Qnearest, Qgoal) <Threshold Thenreturntrue Qnew= Extend (Qnearest, Qrand,δq)--moving fromQnearest an incremental distanceinchThe direction of QrandifQnew≠null then T.addnode (qnew)returnfalsefunction Sample ()--Alternatively,one could replace Sample with Samplefree (by using a collision detection algorithm to reject samplesinchc_obstacle P= Random (0, 1.0)    if0 < P <Prob Thenreturnqgoal ElseIf Prob< P < 1.0 ThenreturnRandomnode ()

RRT Pseudo-code

The

Random tree T contains only one node when initializing: root node qinit. First, the sample function randomly selects a sample point Qrand (4 rows) from the state space, and then the nearest function selects a distance qrand nearest node Qnearest (5 rows) from the random tree. The last extend function expands a distance from qnearest to Qrand, resulting in a new node qnew (8 rows). If the qnew collides with an obstacle, the Extend function returns empty, discarding this growth, otherwise qnew is added to the random tree. Repeating the above steps until the qnearest and target points qgaol distance is less than a threshold, then the random tree arrives at the target point, and the algorithm returns successfully (6~7 line). In order to control the algorithm, you can set the running time limit or the maximum number of searches (3 lines). If the target point cannot be reached within the limit count, the algorithm returns failure.
in order to speed up the random tree to reach the target point, the simple improvement method is to determine whether the Qrand is a target or a random point according to the stochastic probability during each growth of the random tree. In the sample function to set the parameter prob, each time a random value of 0 to 1.0 p, when 0<p<prob, the random tree to the target point of growth line, when the prob<p<1.0, random trees in a random direction of growth.

The effect of the above algorithm is that the random sampling point will grow out of the "pull" tree, which will enable faster and more efficient exploration of the space (the effect is, the nearly uniformly distributed samples the tree tow ard them, causing the tree to rapidly explore C-space). Random exploration also pays attention to strategy, if we take a random point from the tree and then grow in a random direction, then what is the result? See (Left: a tree generated by applying a uniformly-distributed random motion from A randomly chosen tree node does n OT explore very far. Right: a tree generated by the RRT algorithm using samples drawn randomly from a uniform distribution. Both trees has a nodes). As you can see, it is also a random tree, but this tree does not have a good space to explore.

Here is the specific code

%%RRT Parametersmap=IM2BW (Imread ('map.bmp')); % Input Map Read fromA BMP file. fornew Maps Write the file name Heresource=[490 490]; % Source PositioninchY, X formatgoal=[10 900]; % goal PositioninchY, X formatstepsize= 20; %size of each step of the Rrtthreshold= 20; %nodes closer than this threshold is taken as almost the samemaxfailedattempts= 10000;d isplay= true; %Display of RRTif~feasiblepoint (Source,map), error ('source lies on an obstacle or outside map'); Endif~feasiblepoint (Goal,map), error ('goal lies on an obstacle or outside map'); EndifDisplay,imshow (map); Rectangle ('position', [1 1 size (map)-1],'Edgecolor','k');  endtic; % tic-toc:functions forElapsed Timerrtree= Double ([source-1]); % RRT rooted at the source, representation node andParent Indexfailedattempts=0;counter=0;pathfound=false; whileFailedattempts <= maxfailedattempts%loop to grow Rrts%%chooses a random configurationifRand < 0.5Sample= Rand. * Size (map); %Random SampleElseSample= goal; %sample taken as goal to bias tree generation to goal endPercent selects the nodeinchThe RRT tree that isclosest to Qrand [A, I]= Min (Distancecost (Rrtree (:, 1:2), sample), [],1]; %find the minimum value of each column Closestnode= Rrtree (I (1), 1:2); Percent moving fromQnearest an incremental distanceinchThe direction of Qrand Theta= atan2 (sample (1)-closestnode (1), sample (2)-closestnode (2)); %direction to extend sample to produce new node Newpoint= Double (Int32 (Closestnode (1:2) + stepsize *[sin (theta) cos (theta)])); if~checkpath (Closestnode (1:2), newpoint, map)%ifExtension of closest nodeinchTree to the new point isfeasible failedattempts= failedattempts + 1; Continue; EndifDistancecost (Newpoint,goal) < threshold, Pathfound = true; Break; End%goal reached [A, I2]= Min (Distancecost (Rrtree (:, 1:2), Newpoint), [],1]; % CheckifNew node is  notAlready pre-existinginchThe treeifDistancecost (Newpoint,rrtree (I2 (1), 1:2) < threshold, failedattempts = failedattempts + 1;Continue; end Rrtree= [Rrtree; newpoint I (1)]; %Add node failedattempts=0; ifDisplay, line ([Closestnode (2); Newpoint (2)],[closestnode (1); Newpoint (1)]); counter = counter + 1; M (counter) = GetFrame; End%Capture movie Frame End% GetFrame Returns a movie frame, which isa structure having both fieldsifDisplay && Pathfound, line ([Closestnode (2); goal (2)],[closestnode (1); goal (1)]); counter = counter+1; M (counter) =getframe; endifDisplay, disp ('click/press any key'); waitforbuttonpress; Endif~pathfound, Error ('no path found. Maximum attempts reached'); EndPercent Retrieve path fromParent Informationpath=[goal];p Rev= I (1); whilePrev >0 Path= [Rrtree (prev,1:2);    Path]; Prev= Rrtree (prev,3); Endpathlength=0; forI=1:length (Path)-1, pathlength = pathlength + distancecost (Path (i,1:2), Path (i+1,1:2)); End%Calculate path lengthfprintf ('processing time=%d \npath length=%d \ n', TOC, pathlength); Imshow (map); Rectangle ('position', [1 1 size (map)-1],'Edgecolor','R'); line (Path (:,2), Path (:, 1));

rrt_main.m

Percent distancecost.mfunction h=distancecost (A,    b)= sqrt (sum ((a). ^2, 2));

DISTANCECOST.M

%%checkpath.m function Feasible=Checkpath (n,newpos,map) feasible=True;dir=atan2 (Newpos (1)-N (1), Newpos (2)-N (2)); forR=0:0.5:sqrt (sum (n-newpos). ^2)) Poscheck=n+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 (Newpos,map), feasible=false; EndEnd

CHECKPATH.M

Percent feasiblepoint.mfunction feasible=feasiblepoint (point,map) Feasible=true; if  and Inside Maps if ~ (Point (1) >=1 && Point (1) <=size (map,1) && point (2) >=1 && Point (2) <= Size (map,2) && Map (Point (1), point (2)) ==1)    feasible=False;end

FEASIBLEPOINT.M

Results

[MATLAB] 7. Fast Search random tree (RRT---rapidly-exploring random Trees) path planning