【路径规划】基于matlab RRT三维路径规划【含Matlab源码 151期】

一、简介

对RRT算法感兴趣，是因为我对它不仅在二维平面上适用，在高维空间也能使用而感到心动，最近比较忙，下周或者什么时候要要用代码亲自实现一下。
路径规划都有哪些方法呢？比较流行的有：图搜索、势场法、RRT 等等。
RRT（快速探索随机树）是一种通用的方法，不管什么机器人类型、不管自由度是多少、不管约束有多复杂都能用。而且它的原理很简单，这是它在机器人领域流行的主要原因之一。不过它的缺点也很明显，它得到的路径一般质量都不是很好，例如可能包含棱角，不够光滑，通常也远离最优路径。
RRT 能在众多的规划方法中脱颖而出，它到底厉害在哪里呢？
　　天下武功唯快不破，“快”是 RRT 的一大优点。RRT 的思想是快速扩张一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。之所以选择“树”是因为它能够探索空间。我们知道，阳光几乎是树木唯一的能量来源。为了最大程度地利用阳光，树木要用尽量少的树枝占据尽量多的空间。当然了，能探索空间的不一定非得是树，比如Peano曲线也可以做到，而且要多密有多密，如上图左所示的例子。虽然像Peano曲线这样的单条连续曲线也能探索空间，但是它太“确定”了。在搜索轨迹的时候我们可不知道出路应该在哪里，如果不在“确定”的搜索方向上，我们怎么找也找不到（找到的概率是0）。这时“随机”的好处就体现出来了，虽然不知道出路在哪里，但是通过随机的反复试探还是能碰对的，而且碰对的概率随着试探次数的增多越来越大，就像买彩票一样，买的数量越多中奖的概率越大（RRT名字中“随机”的意思）。可是随机试探也讲究策略，如果我们从树中随机取一个点，然后向着随机的方向生长，那么结果是什么样的呢？见上图右。可以看到，同样是随机树，但是这棵树并没很好地探索空间，它一直在起点（红点）附近打转。这可不好，我们希望树尽量经济地、均匀地探索空间，不要过度探索一个地方，更不能漏掉大部分地方。这样的一棵树怎么构造呢？
　RRT 的基本步骤是：
　　1. 起点作为一颗种子，从它开始生长枝丫；
　　2. 在机器人的“构型”空间中，生成一个随机点；
　　3. 在树上找到距离最近的那个点，记为吧；
　　4. 朝着的方向生长，如果没有碰到障碍物就把生长后的树枝和端点添加到树上，返回 2；
　　随机点一般是均匀分布的，所以没有障碍物时树会近似均匀地向各个方向生长，这样可以快速探索空间（RRT名字中“快速探索”的意思）。当然如果你事先掌握了最有可能发现路径的区域信息，可以集中兵力重点探索这个区域，这时就不宜用均匀分布了。
　　RRT 的一个弱点是难以在有狭窄通道的环境找到路径。因为狭窄通道面积小，被碰到的概率低，找到路径需要的时间要看运气了。下图展示的例子是 RRT 应对一个人为制作的很短的狭窄通道，有时RRT很快就找到了出路，有时则一直被困在障碍物里面。
　 RRT探索空间的能力还是不错的，例如下图左所示的例子，障碍物多而且杂乱（借助 Mathematica 本身具有的强大函数库，实现这个例子所需的所有代码应该不会超过30行）。还有没有环境能难住RRT呢？下图右所示的迷宫对RRT就是个挑战。这个时候空间被分割得非常严重，RRT显得有些力不从心了，可见随机策略不是什么时候都有效的。
　　“随机”使得RRT有很强的探索能力。但是成也萧何败也萧何，“随机”也导致 RRT 很盲目，像个无头苍蝇一样到处乱撞。如果机器人对环境一无所知，那么采用随机的策略可以接受。可实际情况是，机器人对于它的工作环境多多少少是知道一些的（即使不是完全知道）。我的博客一直强调信息对于机器人的重要性。这些已知的信息就可以用来改进算法。一个改进的办法就是给它一双“慧眼”：在势场法中，势函数携带了障碍物和目标的信息，如果能把这个信息告诉 RRT，让它在探索空间时有倾向地沿着势场的方向前进会更好。这样，RRT 出色的探索能力刚好可以弥补势场法容易陷入局部极小值的缺点。
　　将RRT方法用在机械臂上的效果如下图所示（绿色表示目标状态）。我设置了4个障碍物（其中一个是大地），这对机械臂是个小小的挑战。由于我们生活在三维空间，没办法看到六维关节空间，所以我把六维关节空间拆成了两个三维空间，分别对应前三个关节和后三个关节（严格来说，六维转动关节空间不是一个欧式空间，它是个流形，不过这里我们不必纠结这个差别）：

二、源代码

function c;
clc
clear all
close all
%map1 随机地表。
% a=10;
% b=0.2;
% c=0.1;
% d=0.6;
% e=1;
% f=0.1;
% g=0.1;
% for x=1:80
%     for y=1:80
% Z1=sin(y+a)+b*sin(x)+cos(d*(x^2+y^2)^(1/2))+e*cos(y)+f*sin(f*(x^2+y^2)^(1/2))+g*cos(y);
% % Z1=SquareDiamond(6,2,8);
%  figure(1);
%  surf(Z1); %画出三维曲面 
%  shading flat; %各小曲面之间不要网格 
% %map2 山峰图
tic;
% h=[20,35,25,38,20,25];
% x0=[20,40,45,60,20,20];
% y0=[10,25,50,30,45,10];
% xi=[5.5,8,5,4.5,5.5,3.5];
% yi=[5,7,6,5.5,6,4.5];
h=[20,35,25,38,20,25];
x0=[20,40,45,60,20,20];
y0=[10,25,50,30,45,10];
xi=[5.5,8,5,4.5,5.5,3.5];
yi=[5,7,6,5.5,6,4.5];
Z2=CeatHill(6,h,x0,y0,xi,yi,80); 
figure(2);
surf(Z2); %画出三维曲面 
shading flat; %各小曲面之间不要网格 
%map3 合成图
%  Z3=max(Z1,Z2);
%  figure(3);
%  surf(Z3); %画出三维曲面 
%  shading flat; %各小曲面之间不要网格 
segmentLength =5;
start_node =  [10,80,5,0,0,0];
end_node   =[60,0,5,1,0,0];
hold on
plot3(start_node(:,1),start_node(:,2),start_node(:,3),'r*');
plot3(end_node(:,1),end_node(:,2),end_node(:,3),'r*');
tree = start_node;
if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )...
    &(collision(start_node,end_node)==0) )
  path = [start_node; end_node];
  else
  numPaths = 0;
  while numPaths<1,
      [tree,flag] = extendTree(tree,end_node,segmentLength,Z2);
      numPaths = numPaths + flag;
  end
end
 path = findMinimumPath(tree);
 plot3(path(:,1),path(:,2),path(:,3),'r');  
 toc;
 function [data]=CeatHill(N,h,x0,y0,xi,yi,num) 
x=1:1:num;y=1:1:num;
for m=1:num
    for n=1:num
        Sum=0;
        for k=1:N
           s=h(k)*exp(-((x(m)-x0(k))/xi(k))^2-((y(n)-y0(k))/yi(k))^2);
           Sum=Sum+s;
        end
        data(m,n)=Sum;
    end
end

三、运行结果

在这里插入图片描述

四、备注