资源简介
一个RRT*(RRT star)(不是基本RRT)路径规划算法的matlab m file小程序,以三维状态空间为例,简单易懂,可直接运行
代码片段和文件信息
%% RRT* 3D state space
%% - create a path from a start node to an end node
%% using the RRT algorithm.
%% - RRT* = Rapidly-exploring Random Tree star
%% This short programm is writen based other basic RRT code and used for test my hardware work .
%% Apologize no time to add the enough comments
%% Size(Adrian) Xiao DEC 2016 University of Queensland Australia uqsxiao@uq.edu.au
function RRT_star_3D;
clear all;
% create random worlddetermine the parameters of simulation
Size = 16384; % the 2D world is a 100 by 100 map
Obs_Cordi = [111;111;111];
NumObstacles = 3; % randomly obstacles distributed on the map
maxObstacleRadius = 0.05; % define the max radius of obstacle
maxturningangle = 1.05; %75deg
Heading = 0.758;% 45 degree; ccw rotation vector from positive x-axis
world = createWorld(NumObstacles[Size;Size;Size][0;0;0]maxObstacleRadiusObs_Cordi);
segmentLength = 200; % standard length of path segments
hyper_R = 450;
start_node =[129129129000]; %start node‘s idx is 1. node array format :[x y chi(indicate if this node get to the end node) cost parent‘s idx]
end_node = [180018001800000];
% establish tree starting with the start node
tree = start_node;
% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:3)-end_node(1:3)) &&(collision(start_nodeend_nodeworld)==0) )
path = [start_node; end_node];
else
numPaths = 0;
while numPaths<2000
[treeflag] = extendTree(treeend_nodesegmentLengthworldmaxturninganglehyper_R);
numPaths = numPaths + 1;
end
end
plotWorld(worldtree);
clc
tree
size(tree)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% createWorld
%% - create random world with obstacles
%% the first element is the north coordinate
%% the second element is the south coordinate
%%
function world = createWorld(NumObstacles NEcorner SWcornermaxRadius obstacle_cordi);
% check to make sure that the region is nonempty
if (NEcorner(1) <= SWcorner(1)) | (NEcorner(2) <= SWcorner(2)) | (NEcorner(3) <= SWcorner(3))%% NEcorner(1:2)=(100 100) SWcorner(1:2)=(00)
disp(‘Not valid corner specifications!‘)
world=[];
% create world data structure
else
temp=size(obstacle_cordi);
NumObstacles=temp(1);
world.NumObstacles = NumObstacles;
world.NEcorner = NEcorner;
world.SWcorner = SWcorner;
for i=1:NumObstacles
world.radius(i) = maxRadius;
world.cn(i) = obstacle_cordi(i1); % give the X coordinate to current obstacle
world.ce(i) = obstacle_cordi(i2); % give the Y coordinate to current obstacle
world.cz(i) = obstacle_cordi(i3);
end
end
%% generateRandomNode
%% create a random node (initialize)
function node=generateRandomNode(world);
% randomly pick configuration
pn = (
相关资源
- 路径规划算法Matlab仿真更新
- RRT、RRT-Connect、LazyRRT、RRTextend、RRT*的
- mclmcrrt710.dll
- path-planning 路径规划RRT算法
- 快速随机搜索树RRT算法
- mclmcrrt8_3.dll
- RRT路径规划matlab源代码
- matlab-快速搜索随机树算法实现RRT
- RRT_MATLAB程序带中文注释
- RRT路径规划
- matlab-m语言-编程实现随机树算法实现
- RRT*-Connect in 2 dimensional space
- mclmcrrt7_16.dll mclmcrrt7_16.rar
- RRT算法MATLAB实现
- matlab rrt2D/3D
- 解决含有运动力学约束的路径规划问
- rrtsanwei
- RRTstar.rar
- RRT路径规划Matlab程序
- 三维空间下的rrt程序
评论
共有 条评论