浅谈A*算法如何演变为rrt算法

1、A*算法流程

在开始寻找路径时，需要建立两张遍历表close、open。close中存放所经过的所有路径点。open中存放当前点下一步所可能过的路径点如下图所示（p1…p8为下步所能达到的点）。为下一步的每一个点添加能量值以便选择最路点，能量值公式如下：
fit=abs（下一步点−目标点）−abs（当前点−目标点）+步长fit=abs（下一步点-目标点）-abs（当前点-目标点）+步长 fit=abs（下一步点−目标点）−abs（当前点−目标点）+步长

开始寻找路径：
流出为：
1.在open中添加当前点
2.遍历下一步路点加入open中
3.判断是否无路可走（在无法往回走的前提下，无路可走）
4.有路劲点则在close中添加当前点。无路可走就结束。
5.在open中删除当前点并计算所有路点的能量值，并把能量值最低的当做新的当前点
6.判断时都是目标点，是就结束，不是就重复2-6。流程图如下：

Created with Raphaël 2.3.0开始open添加当前点遍历下一步路点存入open没有路点？没有找到路径结束把当前点存入close并在open中删除当前点计算open中路点能量值，并把能量值最小的路点当做当前点是否是目标点？结束yesnoyesno

下面展示A*的主要部分 代码。

function path=AStar(obstacle,p)path=[];open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)];%第三个为能量值
close=[];
next=MotionModel();
findFlag=false;%目标点判断
while ~findFlagif isempty(open(:,1)) disp('No path to goal!!');return;end[Y,I] = sort(open(:,3))open=open(I(1),:)if isSamePosi(open(1,1:2),p.goal)disp('Find Goal!!');close=[open(1,:);close]  findFlag=true;breakendfor in=1:length(next(:,1))m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];%next为步长%m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal);m(3)=h(m(1:2),p.goal);%曼哈顿距离if isObstacle(m,obstacle) %障碍物判断continue;end[flag, targetInd]=FindList(m,open,close)%m为下一步路点if flag==1 %是否为open中点disp('cao')continue;elseif flag==2 %是否为close路点continue;else open=[open;[m open(1,1) open(1,2)]]endendif findFlag==falseclose=[close; open(1,:)]open(1,:)=[];endendpath=GetPath(close,p.start);

最终可以跑出如下图（黄色为起始点，蓝色为目标点）所示：

2、A*算法的缺陷和改进（增加RRT特性）

由于A*算法没有回头路，在一些特点场合下，就会在流程第4步出现无路可走的地步，如下图所示：

从图中可以发现，m点距离目标点（蓝色）较近，当前点就会朝m点方向跑，又可以知道m点周围都是障碍物，下一步的结果可想而知是无路可走的，此时无法找到路径，但地图上确实有路可以走到目标点。
如何避免这种现象，在无路可走的情况下，可以增加一个随机目标点，让他荡出局部路径。代码如下：

// An highlighted blockpick=0.3;if rand()>pickgoal=p.goal;%目标点elsegoal=[p.XYMAX*rand,p.XYMAX*rand];%随机目标点end

最终可以出现的结果如下图所示：

到这里你们就会奇怪：说好的rrt呢！！！其实个人理解A星算法加上随机扰动就是RRT啦，不信可以看如下图所示的示意图（地图放大5倍）

蓝色三角形为利用随机扰动目标在地图上寻找的路（到这里你们会发现他是不是和RRT的树枝蔓延一样哈），绿色为优化路径，红色+绿色为未优化路径。由于存在随机目标，所以得到的路径需要优化从而减少路程。优化过程与open中遍历最短路点一致。
改进后的代码如下：

function [path path1 path2]=AStar(obstacle,p)path1=[];
path2=[];
path=[];
index=2;
index1=1;
open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2) 1 1];
close=[];
next=MotionModel();
findFlag=false;%目标标志
goal=p.goal;
for i=1:5000if isempty(open(:,1)) num=length(close(:,1));num=ceil(num*rand);         open(1,:)=close(num,:);index1=open(1,6);open(1,6)=index-1;pick=0.3;if rand()>pickgoal=p.goal;elsegoal=[p.XYMAX*rand,p.XYMAX*rand];endend[Y,I] = sort(open(:,3));open=open(I(1),:);if isSamePosi(open(1,1:2),p.goal)disp('Find Goal!!');close=[close;open(1,:)]  ;findFlag=true;breakendfor in=1:length(next(:,1))m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];m(3)=(m(3)+next(in,3)+ h(m(1:2),goal))-h(open(1,1:2),p.goal);      if isObstacle(m,obstacle) continue;end[flag, targetInd]=FindList(m,open,close);if flag==1 continue;elseif flag==2 continue;else              open=[open;[m open(1,1) open(1,2)] index index1];endendif findFlag==false  close=[close; open(1,1:5) open(1,6) open(1,7)];index=index+1;index1=index-1;open(1,:)=[];endend%寻找连续路径
index=length(close);
path(1,1:2)=close(index,1:2);
flag=true;
i=2;
while flag==1index=close(index,7);path(i,1:2)=close(index,1:2);if index==1break;endi=i+1;
end
path1=close(:,1:2)  ;
path=flipud(path);
%优化路劲
index=1;
path2=path(1,1:2);
while(1)s=abs(path(index+1:end,:)-path(index,:)) ; s=sum(s,2);I=find(s<=1);I=index+I;ss=abs(path(I,:)-path(end,:));ss=sum(ss,2);[Y1,I1]=min(ss);index=I(I1);index=index(1);path2=[path2;path(index,:)];if ss(I1)==0break;end
end

延伸

随机扰动不仅可以用在A中（其实A加随机目标就是RRT），也经常用在各种路径寻优算法中，如遗传算法、粒子滤波等等，又如人工势场法的局部最优突破还是得靠随机扰动等等，所以，随机扰动真的神！！！