Fast random tree (RRT / path planning algorithm rapidly exploring random tree) by sampling points in the state space for collisions, avoiding the modeling space, can effectively solve the complex and high dimensional space constraints path planning issues. Features of this method is to quickly and efficiently search for a high-dimensional space, by random sampling points of the state space, the blank area searching the guide to find a planned path from the starting point to the target point, the robot is suitable to solve the complex multi-DOF planning and dynamic environment path environment. And similar PRM, which is the probability of complete and not optimal.
RRT is a multi-dimensional space efficient planning method. It is a starting point to the root node, leaf nodes increases by a random sampling method, generates a random tree extension, when the tree leaf node contains a random target point or into the target area, it can be found in a tree by a random the path from the initial point to the target point. Basic RRT algorithm pseudo code as shown below:
function feasible = collisionChecking (startPose, goalPose , map)% conflict check: determining whether there is an obstacle between the start point end Feasible = to true;% viable, executable the dir = of atan2 (goalPose (. 1) -startPose (. 1) , goalPose (2) -startPose (2 ));% angle between the starting point and the goal for R & lt = 0: (. SUM ((startPose-goalPose) ^ 2): 0.5 sqrt)% sqrt (SUM ((startPose -goalPose) ^ 2. )): distance between two points posCheck . * = R & lt startPose + [SiN (the dir) COS (the dir)];% at intervals of 0.5 to obtain an intermediate point of the 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= to false; BREAK ; End IF ~ feasiblePoint ([Floor (goalPose (. 1)), ceil (goalPose (2))], Map), Feasible = to false; End End function Feasible = feasiblePoint (Point, Map)% is determined points are in the map, and there are no obstructions Feasible = to true; IF ~ (Point (. 1)> =. 1 && Point (. 1) <= size (map,. 1) && Point (2)> =. 1 && Point (2) <= size (Map, 2) && Map (Point (2), Point (. 1)) == 255) Map% (Point (2), Point (. 1)) == 255 : no obstructions Feasible = to false; End
function distance=Distance(start_Pt,goal_Pt)
distance=sqrt((start_Pt(1)-goal_Pt(1))^2+(start_Pt(2)-goal_Pt(2))^2);
function [X_near,index]=Near(X_rand,T) min_distance=sqrt((X_rand(1)-T.v(1).x)^2+(X_rand(2)-T.v(1).y)^2); for T_iter=1:size(T.v,2) temp_distance=sqrt((X_rand(1)-T.v(T_iter).x)^2+(X_rand(2)-T.v(T_iter).y)^2); if temp_distance<=min_distance min_distance=temp_distance; X_near(1)=T.v(T_iter).x X_near(2)=T.v(T_iter).y index=T_iter; end end
function X_rand=Sample(map,goal) % if rand<0.5 % X_rand = rand(1,2) .* size(map); % random sample % else % X_rand=goal; % end if unifrnd(0,1)<0.5 X_rand(1)= unifrnd(0,1)* size(map,1); % 均匀采样 X_rand(2)= unifrnd(0,1)* size(map,2); % random sample else X_rand=goal; end
function X_new=Steer(X_rand,X_near,StepSize) theta = atan2(X_rand(1)-X_near(1),X_rand(2)-X_near(2)); % direction to extend sample to produce new node X_new = X_near(1:2) + StepSize * [sin(theta) cos(theta)]; % dir_x = X_rand(1)- X_near(1); % dir_y = X_rand(2)- X_near(2); % dir = sqrt(dir_x^2 + dir_y^2); % X_new(1) = dir_x * StepSize/dir+X_near(1); % X_new(2) = dir_y * StepSize/dir+X_near(2);
function X_rand=Sample(map,goal) % if rand<0.5 % X_rand = rand(1,2) .* size(map); % random sample % else % X_rand=goal; % end if unifrnd(0,1)<0.5 X_rand(1)= unifrnd(0,1)* size(map,1); % 均匀采样 X_rand(2)= unifrnd(0,1)* size(map,2); % random sample else X_rand=goal; end
***************************************% % Author: Qian Chenglong % a Date: 2019- 11-5 % *************************************** %% process initialization clear all; Close All; x_i =. 1; y_i =. 1;% to set the initial point x_G = 700; y_G = 700;% set the target point goal ( . 1) = x_G; goal ( 2) = y_G; Thr = 50;% set the target point threshold when this range is considered to have reached the target point of Delta = 30;% maximum extension distance setting step, allowing the extension node %% achievements initialization Tv ( . 1) .x = x_i;% T we do tree , v nodes, where T is added to the first starting point to the inside Tv ( . 1) .y =y_i; Tv ( . 1) x_i = .xPrev;% parent node is the start node itself remains Tv ( . 1) .yPrev = y_i; Tv ( . 1) .dist = 0;% node from the parent node of the distance, preferably Euclidean distance here Tv ( . 1) .indPrev = 0;% index of parent node %% start building tree - job part Figure ( . 1 ); ImpRgb = imread ( 'newmap.png' ); Imp = rgb2gray (ImpRgb) ; imshow (Imp) xL = size (Imp,. 1);% map the x-axis length yL = size (Imp, 2);% map y axis length HOLD ON Plot (x_i, y_i, 'RO', 'MarkerSize', 10 , 'MarkerFaceColor', 'R & lt' ); Plot (x_G, y_G,'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');% drawing start point and the target point COUNT =. 1 ; for ITER =. 1: 3000 % x_rand = []; % the Step. 1 : randomly sampled in map a point x_rand % Note: with (x_rand (. 1), x_rand (2 )) represents the coordinates of the environment sampling points x_rand = the sample (Imp, Goal); % x_near = []; % the Step 2 : traverse the tree, the tree find the nearest adjacent points x_near % Tip: x_near already in the tree T where [x_near, index] = near (x_rand, T); Plot (x_near ( . 1), x_near (2), 'Go', 'MarkerSize', 2 ); x_new =% []; % the Step. 3 : extended node obtained x_new %Tip: Note the use of the extended step of Delta x_new = Steer (x_rand, x_near, of Delta); whether the node is a check% collision- Free IF ~ collisionChecking (x_near, x_new, Imp)% if the obstacle out Continue ; End COUNT = COUNT + 1'd ; % the Step. 4 : the x_new inserted into the tree T % Tip: parent node is a new node x_new x_near Tv (COUNT) .x = x_new (. 1 ); Tv (COUNT) .y = x_new (2 ); Tv (COUNT ) .xPrev = x_near (. 1);% parent node is the start node itself remains Tv (COUNT) .yPrev = x_near (2 ); Tv (COUNT) .dist= Distance (x_new, x_near); % node from the parent node of the distance, Euclidean distance where desirable Tv (COUNT) .indPrev = index;% parent index % the Step. 5 : Check whether the target point reaches the vicinity % Tip: Note the use of the target point threshold value Thr, if the Euclidean distance of the current node and the endpoint is less than Thr, then out of the current for loop IF distance (x_new, goal) < Thr BREAK ; End % the Step. 6 : the path between x_near and x_new drawn % Tip 1 : use the plot to draw, because to draw the line several times on the same graph, plot it after each use need to connect hold on command % Tip 2 : before you pop the conditions for determining the end of the cycle, and remember to x_near of x_new path between the drawn % Plot ([x_near (. 1), x_near (2)], [x_new (. 1), x_new (2 )]); % HOLD ON Line ([x_near ( . 1), x_new (. 1)], [x_near (2), x_new (2 )]); PAUSE ( 0.1);% pause 0.1s, so that the expansion process is easy to observe RRT End %% path has been found, reverse lookup IF ITER <2000 path.pos ( . 1) .x = x_G; path.pos (. 1) .y = y_G; path.pos ( 2) .x = Tv (End) .x; path.pos (2) .y = Tv (End) .y; pathIndex = Tv (End) .indPrev;% end joining path J = 0 ; the while . 1 path.pos (J +3) .x = Tv (pathIndex) .x; path.pos (J +3) .y = Tv (pathIndex) .y; pathIndex = Tv (pathIndex) .indPrev; IF pathIndex 1 == BREAK End J = J +. 1 ; End % in the end point back to the starting point path.pos (End + 1'd) .x = x_i; path.pos (End) .y = y_i;% starting point for joining path 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 the else DISP ( 'Error, NO path found!' ); End
Reference link: https: //www.cnblogs.com/flyinggod/p/8727951.html