快速随机搜索树（RRT）的MATLAB代码实现

2023-12-10 18:02| 来源: 网络整理| 查看: 265

快速随机搜索树（Rapidly-Exploring Random Tree）算法是移动机器人或机械臂常用的路径规划算法，给定起点和终点，规划出一条路径。其主要步骤分以下几步：

1. 初始化需要规划的2D或3D地图，给出移动机器人或机械臂要运动的起点和终点坐标，树的根部在起点。

2. 开始搜索并构建树。树的构建过程为：a) 首先在地图上随机选一点Prand；b) 遍历树上所有的点，从书中找到与Prand最近的点，记作Pnear，选取Pnear到Prand方向的向量，选取一个固定长度d，得到一个和Pnear之间距离为d的，方向为Pnear到Prand方向的一个新的点Pnew；c）判断Pnear和Pnew之间是否会碰撞到周围的环境（即判断Pnear和Pnew之间是否是collision-free的），如果无碰撞，则把Pnew加入数中，并记录Pnew的父节点为Pnear，进行下一步，如果有碰撞，则回到步骤a)重新选点；d) 判断Pnew是否到达目标点附近，用欧氏距离来判断，当距离小于一个阈值后，认为Pnew到达了目标点附近，停止建树，如果还未到达目标点附近，则回到a)继续循环整个过程，直到Pnew到达目标点附近。

3. 找到目标点后，从目标点开始找到整条路径。方法为查找树上每个点的父节点，直到找到起点为止。

4. 得到整条路径后，有时会对路径进行平滑处理，处理方法有三次B样条处理等，这里不作介绍。

代码如下：

RRT.m

%*************************************** %Author: Yaoqing Hu %Date: 2020-04-09 %*************************************** %% 流程初始化 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('newmap.png'); Imp=rgb2gray(ImpRgb); imshow(Imp) xL=size(Imp,1);%地图x轴长度 yL=size(Imp,2);%地图y轴长度 hold on plot(x_I, y_I, 'ro', 'MarkerSize',5, 'MarkerFaceColor','r'); plot(x_G, y_G, 'go', 'MarkerSize',5, 'MarkerFaceColor','g');% 绘制起点和目标点 count=1; for iter = 1:3000 p_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_distance min_distance = distance; index = i; end end p_near(1) = T.v(index).x; p_near(2) = T.v(index).y; p_new=[]; %Step 3: 扩展得到x_new节点 %提示：注意使用扩展步长Delta p_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-free if ~collisionChecking(p_near,p_new,Imp) continue; end count=count+1; %Step 4: 将x_new插入树T %提示：新节点x_new的父节点是x_near T.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 =1 && point(1)=1 && point(2)

【本文地址】

公司简介

联系我们