RRT（快速随机搜索树）算法原理及代码实践

RRT算法简介

RRT 算法为一种递增式的路径规划算法，算法不断在搜索空间中随机生成采样点，如果该点位于无碰撞位置，则寻找搜索树中离该节点最近的结点为基准结点，由基准结点出发以一定步长朝着该随机结点进行延伸，延伸线的终点所在的位置被当做新的有效结点加入搜索树中。这个搜索树的生长过程一直持续，直到目标结点与搜索树的距离在一定范围以内时终止。随后搜索算法在搜索树中寻找一条连接起点到终点的最短路径。

RRT算法原理

该博客叙述得十分清楚明白，后面的代码实践基本是按照该思路得到：

链接: link

MALTAB代码实现

使用到的地图：

碰撞检测代码：

collisionChecking.m

function feasible=collisionChecking(startPose,goalPose,map)feasible=true;dir=atan2(goalPose(1)-startPose(1),goalPose(2)-startPose(2));for r=0:0.5:sqrt(sum((startPose-goalPose).^2))posCheck = startPose + 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([floor(goalPose(1)),ceil(goalPose(2))],map), feasible=false; endendfunction feasible=feasiblePoint(point,map)feasible=true;if ~(point(1)>=1 && point(1)<=size(map,2) && point(2)>=1 && point(2)<=size(map,1) && map(point(2),point(1))==255)feasible=false;end

节点编号代码：

node_index.m

function n_index = node_index(T_LIST,xval,yval)%This function returns the index of the location of a node in the T_LISTi=1;while ( T_LIST(i,1) ~= xval || T_LIST(i,2) ~= yval )i=i+1;endn_index=i;end

主函数：

RRT_main.m

%***************************************%Author: Wang Liang%Date: -06-21%***************************************%% 流程初始化clc;clear all; close all;x_I=1; y_I=1; % 设置初始点x_G=700; y_G=700; % 设置目标点 这里是地图上的像素点Thr=30; % 设置目标点阈值Delta= 30; % 设置扩展步长%% 建树初始化T.v(1).x = x_I; % T是我们要做的树，v是节点，这里先把起始点加入到T里面来T.v(1).y = y_I; % 将起始点的坐标加入到结点中T.v(1).xPrev = x_I;% 起始节点的父节点仍然是其本身T.v(1).yPrev = y_I;T.v(1).dist=0;% 从父节点到该节点的距离，这里可取欧氏距离T.v(1).indPrev = 0;% %% 开始搜索并构建树figure(1);ImpRgb=imread('map.png');Imp=rgb2gray(ImpRgb);imshow(Imp)xL=size(Imp,1);%地图x轴长度yL=size(Imp,2);%地图y轴长度hold onplot(x_I, y_I, 'ro', 'MarkerSize',5, 'MarkerFaceColor','r');plot(x_G, y_G, 'go', 'MarkerSize',5, 'MarkerFaceColor','g');% 绘制起点和目标点count=1;for iter = 1:3000p_rand=[];%Step 1: 在地图中随机采样一个点x_rand%提示：用（p_rand(1),p_rand(2)）表示环境中采样点的坐标p_rand(1)=ceil(rand()*xL); % rand()生成的是0~1均匀分布的随机数，乘以800再向上取整，数便为[1,800]间的整数p_rand(2)=ceil(rand()*yL);p_near=[];%Step 2: 遍历树，从树中找到最近邻近点x_near %提示：x_near已经在树T里min_distance = 1000;for i=1:count distance = sqrt( ( T.v(i).x - p_rand(1) )^2 + ( T.v(i).y - p_rand(2) )^2 );if distance < min_distancemin_distance = distance;index = i;endendp_near(1) = T.v(index).x;p_near(2) = T.v(index).y; % 找到采样点的最近邻近点p_new=[];%Step 3: 扩展得到x_new节点%提示：注意使用扩展步长Deltap_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 );%检查节点是否是collision-freeif ~collisionChecking(p_near,p_new,Imp) continue;endcount=count+1;%Step 4: 将x_new插入树T %提示：新节点x_new的父节点是x_nearT.v(count).x = p_new(1); T.v(count).y = p_new(2); T.v(count).xPrev = p_near(1); T.v(count).yPrev = p_near(2);T.v(count).dist = min_distance;%Step 5:检查是否到达目标点附近 %提示：注意使用目标点阈值Thr，若当前节点和终点的欧式距离小于Thr，则跳出当前for循环new_distance = sqrt( ( p_new(1) - x_G )^2 + ( p_new(2) - y_G )^2 );if new_distance <= Thrplot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_newline( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_newline( [x_G p_new(1)], [y_G p_new(2)], 'Marker','.','LineStyle','-'); %连接x_Target和x_newbreak;end%Step 6:将x_near和x_new之间的路径画出来%提示 1：使用plot绘制，因为要多次在同一张图上绘制线段，所以每次使用plot后需要接上hold on命令%提示 2：在判断终点条件弹出for循环前，记得把x_near和x_new之间的路径画出来plot(p_new(1), p_new(2), 'bo', 'MarkerSize',2, 'MarkerFaceColor','b'); % 绘制x_newline( [p_new(1) p_near(1)], [p_new(2) p_near(2)], 'Marker','.','LineStyle','-'); %连接x_near和x_newhold on;pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察end%% 画出路径T_LIST = zeros(size(T.v, 2), 5); % size(),获取矩阵T.v的函数for i=1:size(T.v, 2)T_LIST(i,1) = T.v(i).x;T_LIST(i,2) = T.v(i).y;T_LIST(i,3) = T.v(i).xPrev;T_LIST(i,4) = T.v(i).yPrev;T_LIST(i,5) = i;endpath = [];path_count = 1;path(path_count,1) = x_G;path(path_count,2) = y_G;path_count = path_count + 1;path(path_count,1) = p_new(1);path(path_count,2) = p_new(2);n_index = node_index(T_LIST, p_new(1), p_new(2));path_count = path_count + 1;path(path_count,1) = T_LIST(i,3);path(path_count,2) = T_LIST(i,4);while path(path_count,1) ~= x_I || path(path_count,2) ~= y_Inew_n_index = node_index(T_LIST, path(path_count,1), path(path_count,2));path_count = path_count + 1;path(path_count,1) = T_LIST(new_n_index,3);path(path_count,2) = T_LIST(new_n_index,4);n_index = new_n_index;endfor i=size(path,1)-1 :-1: 1line( [path(i,1) path(i+1,1)], [path(i,2) path(i+1,2)], 'Marker','.','LineStyle','-','color','r'); %连接x_near和x_newhold on;pause(0.1); %暂停0.1s，使得RRT扩展过程容易观察end

运行结果

C++代码实现

待更新…