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++代码实现
待更新…