【路径规划】基于RRT算法实现多机器人路径规划，多起点，统一终点matlab源码

一、RRT算法

传统的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。基于快速扩展随机树（RRT / rapidly exploring random tree）的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。与PRM类似，该方法是概率完备且不最优的。

 　　

　　RRT是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。基本RRT算法如下面伪代码所示：

 1 function BuildRRT(qinit, K, Δq)
 2     T.init(qinit)
 3     for k = 1 to K
 4         qrand = Sample()  -- chooses a random configuration
 5         qnearest = Nearest(T, qrand) -- selects the node in the RRT tree that is closest to qrand
 6         if  Distance(qnearest, qgoal) < Threshold then
 7             return true
 8         qnew = Extend(qnearest, qrand, Δq)  -- moving from qnearest an incremental distance in the direction of qrand
 9         if qnew ≠ NULL then
10             T.AddNode(qnew)
11     return false
12 
13 
14 function Sample() -- Alternatively,one could replace Sample with SampleFree(by using a collision detection algorithm to reject samples in C_obstacle
15     p = Random(0, 1.0)
16     if 0 < p < Prob then
17         return qgoal
18     elseif Prob < p < 1.0 then
19         return RandomNode()

　　初始化时随机树T只包含一个节点：根节点qinit。首先Sample函数从状态空间中随机选择一个采样点qrand（4行）；然后Nearest函数从随机树中选择一个距离qrand最近的节点qnearest（5行）；最后Extend函数通过从qnearest向qrand扩展一段距离，得到一个新的节点qnew（8行）。如果qnew与障碍物发生碰撞，则Extend函数返回空，放弃这次生长，否则将qnew加入到随机树中。重复上述步骤直到qnearest和目标点qgaol距离小于一个阈值，则代表随机树到达了目标点，算法返回成功（6~7行）。为了使算法可控，可以设定运行时间上限或搜索次数上限（3行）。如果在限制次数内无法到达目标点，则算法返回失败。
　　为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。在Sample函数中设定参数Prob，每次得到一个0到1.0的随机值p，当0<p<Prob的时候，随机树朝目标点生长行；当Prob<p<1.0时，随机树朝一个随机方向生长。

　　上述算法的效果是随机采样点会“拉着”树向外生长，这样能更快、更有效地探索空间（The effect is that the nearly uniformly distributed samples “pull” the tree toward them, causing the tree to rapidly explore C-Space）。随机探索也讲究策略，如果我们从树中随机取一个点，然后向着随机的方向生长，那么结果是什么样的呢？见下图（Left: A tree generated by applying a uniformly-distributed random motion from a randomly chosen tree node does not explore very far. Right: A tree generated by the RRT algorithm using samples drawn randomly from a uniform distribution. Both trees have 2000 nodes ）。可以看到，同样是随机树，但是这棵树并没很好地探索空间。

二、部分代码

clearvars
close all
rand('state',1);


%% env 
env.x_max = 500;
env.y_max = 500;
x_max=500;
y_max=500;
width_c=50;
width_b=100;
l_block=0.5*(x_max-width_c);
h_block=(y_max-2*width_b);
obstacle1 = [0,width_b,l_block,h_block];
obstacle2 = [x_max-l_block,width_b,l_block,h_block];
ob_vec=[obstacle1;obstacle2];
env.ob_vec=ob_vec;

%% constant definition
EPS = 10;
rho=40;
    BETA=EPS/rho; 
numNodes=1000;

%% Agent1
agent1.q_start.coord = [20 20];
agent1.q_start.dir=pi/4; 
agent1.q_start.cost = 0;
agent1.q_start.parent = 0;

agent1.q_goal.coord = [x_max-20 y_max-20];
agent1.q_goal.cost = 0;
agent1.q_goal.dir=0;
% agent1.nodes(1) =agent1.q_start;

% agent1.q_ci=agent1.q_start.coord;
% agent1.phi_ci=agent1.q_start.dir;
% agent1.i=1;
% agent1.confliction.state=zeros(num_agent,1);
% agent1.confliction.agents=[];


%% Agent2
agent2.q_start.coord = [x_max-20 20];
agent2.q_start.dir=-3*pi/4; 
agent2.q_start.cost = 0;
agent2.q_start.parent = 0;

agent2.q_goal.coord = [x_max-20 y_max-20];
agent2.q_goal.cost = 0;
agent2.q_goal.dir=0;

plot_env(env,1);
hold on
plot(agent1.q_goal.coord(1),agent1.q_goal.coord(2),'ro','LineWidth',2)
plot(agent2.q_goal.coord(1),agent2.q_goal.coord(2),'ro','LineWidth',2)
 
path1=RRTStar_FindPath(agent1,EPS,rho,numNodes,env);
path2=RRTStar_FindPath(agent2,EPS,rho,numNodes,env);
newpath1=fixPath(path1,rho,7);
newpath2=fixPath(path2,rho,7);

save('newpath_data2','newpath1','newpath2')
plot(newpath1(:,1),newpath1(:,2),'y.')  
plot(newpath2(:,1),newpath2(:,2),'b.') 
plot(path1(:,1),path1(:,2),'g--')  
plot(path2(:,1),path2(:,2),'m--') 

plot(newpath1(:,1),newpath1(:,2),'y--')  
plot(newpath2(:,1),newpath2(:,2),'p--') 

三、仿真结果