👨🎓个人主页：研学社的博客
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
📋📋📋本文目录如下：🎁🎁🎁
目录
💥1 概述
1.1快速探索随机树算法
1.2Dubins 理论
📚2 运行结果
2.1 RRT
2.2RRT_Dubins
2.3RRT_obstacles
2.4RRT_Dubins_obstacles
🎉3参考文献
💥4Matlab代码实现
💥1 概述
1.1快速探索随机树算法
令 Tk 为一个有 K 个节点的 RRT ， Q 为 Tk 的节点，Cfree 为与障碍物无碰撞的机器人自由状态空间。 定义 Qinit 为初始状态即起点，Qgoal 为目标状态即终点 。 令 Qrand 为空间中一个随机选取的位姿状态，然后找出 Tk 中距离 Qrand 最近的节点Qnear。 设 p， q∈ Cfree ，令 D ( Qnear ， Qrand ) ≤ D ( Q ， Qrand ) 。 在 Qrand 与 Qnear 连线上求 Qnew ， Qnew 必须满足 Qnew ∈ Cfree 且 D ( Qnear ， Qrand ) = ρ 的条件，其中 ρ ＞ 0 ，为 RRT 生长的最小单位长度，称为步长。 如果存在 Qnew ，则 Tk 增加一个新节点; 否则重新选取 Qrand ，重复以上过程 。 初始的 RRT 算法的构建过程如图 1 所示 。1.2Dubins理论
Dubins 于 1957 年提出了 Dubins 曲线理论，主要内容是在满足一定曲率条件下，连接同一平面内具有特定方向向量的任意两点的最短轨迹是曲线[20]。在没有其他约束因素的情况下，同一平
面上的任意两点的最短路径是直线，但是在有一定的曲率约束的情况下，最短的路径则是圆弧。 Dubins 曲线理论主要用来求解有曲率约束的最短路径。通过选择两条与两个圆相切的切线中的一 条，可以得到 Dubins 路径，其中起始和终止位置都在圆弧上。圆弧的半径是曲率半径，它由无人 系统的转弯半径决定，圆弧中心就是曲率中心，于是该问题就简化为寻找两个圆弧的公切线。两 圆之间有两种公切线：内切线、外切线。在给定位姿点下，无人系统可以向左转和向右转，路径可以以顺时针方向和逆时针方向结束。一个给定的位姿点上有两种转弯方式，因此有两个相切圆，如图 3 所示。一个向右转弯，在弧段 C1上标记R，另一个是向左转弯，在弧段 C2 上标记L，一个位姿点可以产生 4 条 Dubins 路径。即 LSL， LSR， RSL ，RSR， 如图 4 所示。
📚2 运行结果
2.1 RRT
2.2RRT_Dubins
2.3RRT_obstacles
2.4RRT_Dubins_obstacles
部分代码：
main_fig_name = 'RRT with dubins curve and obstacle detection';
close(findobj('type','figure','name',main_fig_name));
%%%%%%%%%%%%%%%%%%%% User Configuration Area %%%%%%%%%%%%%%%%%%%%
% define the map
map.height = 20;
map.width = 20;
map.center = [0, 0];
map.offset = map.center - [map.width, map.height]./2; % bottom-left corner
% starting vertex
origin = [1,2, 20*pi/180];
% define the obstacles using polygones, should be a cell stack of arrays.
poly = { [-4,-4; -1.5, -4; 0, -2.5; -0.5, -1; -3, 0],...
[0,3; 3,3; 3, 6; 4, 6; 1.5, 8; -1, 6; 0, 6] };
% RRT parameters
iteration = 1000;
th_range = 2*pi; % do not change without knowledge about Dubins
th_center = 0;
th_offset = 0;
% dubins parameters
turning_rad = 0.5;
exp = 0.2;
%%%%%%%%%%%%%%%%%%%% End of configuration area %%%%%%%%%%%%%%%%%%%%
% main program starts here
poly = poly_verify(poly, map);
if ~iscell(poly)
error("Ending due to incorrect obsticle polygon setting");
return;
end
% prelocation of data
edges.x = zeros(iteration,2);
edges.y = zeros(iteration,2);
edges.th = zeros(iteration,2);
edges.param(iteration).p_init = [0, 0, 0]; % the initial configuration
edges.param(iteration).seg_param = [0, 0, 0]; % the lengths of the three segments
edges.param(iteration).r = turning_rad;% model forward velocity / model angular velocity turning radius
edges.param(iteration).type = -1; % path type. one of LSL, LSR, ...
edges.param(iteration).flag = 0;
% initial conditions, DO NOT CHANGE
vertecies = origin;
vert_count = 1;
ind_nearest = zeros(iteration,1);
edge_count = 0;
% figure('name', 'RRT growing'); % originally for real-time animation
tic;
for i=1:iteration
% random point generation
x_rand = map.width*rand() + map.offset(1);
y_rand = map.height*rand() + map.offset(2);
th_rand = th_range*rand() + th_offset;
% check if (x,y) is available or not
if chk_xy_available([x_rand,y_rand], map, poly) == 0
continue;
end
% connect to nearest point
[ind_nearest(i),param_nearest] = dubins_searchn(vertecies, [x_rand, y_rand, th_rand], turning_rad);
% check availablility, see dubins_core.m for more info
if( param_nearest.flag < 0)
continue; % goto next loop, reset i doesn't work in MATLAB
elseif( chk_dubins_collision(param_nearest, map, poly, exp)==0 )
%%%%%%%%%%%%%%%%% edit line %%%%%%%%%%%%%%%%
% append the newly generated point and edge to the existing list
vertecies(vert_count+1,:) = [x_rand, y_rand, th_rand];
vert_count = vert_count + 1;
edges.x(edge_count+1,:) = [vertecies(ind_nearest(i),1), x_rand];
🎉3参考文献
部分理论来源于网络，如有侵权请联系删除。
[1]莫栋成,刘国栋.改进的快速探索随机树双足机器人路径规划算法[J].计算机应用,,33(01):199-201+206.
[2]邹启杰,刘世慧,张跃,侯英鹂.基于强化学习的快速探索随机树特殊环境中路径重规划算法[J].控制理论与应用,,37(08):1737-1748.
[3]李艳,郭继峰,罗汝斌.基于遗传算法与Dubins理论的高速无人系统在多障碍环境中的路径规划[J].无人系统技术,,4(06):37-45.DOI:10.19942/j.issn.2096-5915..6.053.
[4]李寰宇,陈延龙,张振兴,刘夏锐.基于Dubins的无人机自动避撞路径规划[J].飞行力学,,38(05):44-49.DOI:10.13645/ki.f.d.2028.001.
[5]Steven M. LaValle "Rapidly-Exploring Random Trees: A New Tool for Path Planning" 1998, tech. rpt C.S.Dept, Iowa State U
[6]Sertac Karaman and Emilio Frazzoli "Sampling-based algorithms for optimal motion planning",, The International Journal of Robotics Research
[7]Yoshiaki Kuwata, Gaston A. Fiore, Justin Teo, Emilio Frazzoli, and Jonathan P. How, "Motion Planning for Urban Driving using RRT
