一种偏向目标型的RRT算法实现(2)

《机器人控制理论与技术》课程论文

1009080706050403020100010203040506070809010010090807060504030201000102030405060708090100图 5路径规划图，RRT算法(左)，改进RRT算法(右)

通过图5，我们也可以看出改进后的RRT算法在复杂随机地图中也表现优异，证明了偏向目标型RRT算法的优越性。 五. 总结

本文针对基本RRT算法存在搜索过于平均，算法效率低下，规划路径偏离最短路径较大的缺陷，分析其缺陷原因在于随机点的确定在全空间分布过于平均导致的。借鉴启发式算法的思想，我们提出了一种确定随机点的新方法，即让随机点以一定的概率等于目标点，这样就使随机树的扩展有一种趋向于目标点的趋势，从而解决了随机点分布过于平均的缺点。最后通过Matlab仿真对两种算法的结果对比分析得到，改进后的偏向目标型RRT算法相对于基本RRT算法，无论在算法效率还是路径质量，都体现出了很大的优越性。 参考文献

[1]王全.基于RRT的全局路径规划方法及其应用研究[D].国防科学技术大学,2014.

[2]李加东.基于RRT算法的非完整移动机器人运动规划[D].华东理工大学,2014. [3]冯林,贾菁辉.基于对比优化的RRT路径规划改进算法[J].计算机工程与应用,2011,47(3):210-213,228.

[4]贾菁辉.移动机器人的路径规划与安全导航[D].大连理工大学,2009. [5]周培培.未知环境下机器人路径规划算法的研究[D].青岛科技大学,2014. [6]李猛.基于智能优化与RRT算法的无人机任务规划方法研究[D].南京航空航天大学,2012.

[7]王滨,金明河,谢宗武等.基于启发式的快速扩展随机树路径规划算法[J].机械制

6

《机器人控制理论与技术》课程论文

造,2007,45(12):1-4.

[8]宋金泽,戴斌,单恩忠等.一种改进的RRT路径规划算法[J].电子学报,2010,38(z1):225-228.

[9]乔永兴.自主式移动机器人的路径规划[D].广西大学,2003.

[10]李磊,叶涛,谭民等.移动机器人技术研究现状与未来[J].机器人,2002,24(5):475-480.

[11]陈刚,沈林成.复杂环境下路径规划问题的遗传路径规划方法[J].机器人,2001,23(1):40-44. 附录

本文中使用的Matlab程序

%主程序

function BiasGoal_RRT

Map=CreateMap(1); %创建有障碍物的模拟地图，输入参数为不同的地图类型 step=5; %步长 startNode=[1,1,0]; %起点 endNode=[90,90,0]; %终点 tree=startNode; %根结点

if((norm(startNode(1,1:2)-endNode(1,1:2))<=step)&(collision(startNode,endNode,Map)==0)) path=[startNode(1,1:2);endNode(1,1:2)]; else

success=0;

while success==0

[tree,flag]=extendTree(tree,endNode,step,Map); success=flag; end end

path=findPath(tree); plotmap(Map,path,tree);

%创建地图，有三种不同类型的地图 function Map=CreateMap(num)

if num==1 %分布障碍地图 Map.BarNum=3;

Map.LLCd=[0;0]; %地图左下角坐标 Map.URCd=[100;100]; %地图右上角坐标 radius=[20;15;20]; %障碍物半径

barCenter=[33,75;38,30;75,50]; %障碍物中心点坐标

7

《机器人控制理论与技术》课程论文

for i=1:Map.BarNum

Map.radius(i)=radius(i); Map.cx(i)=barCenter(i,1); Map.cy(i)=barCenter(i,2); end end

if num==2 %狭窄通道地图 Map.BarNum=2; Map.LLCd=[0;0];

Map.URCd=[100;100]; radius=[23.75;23.75];

barCenter=[50,76.25;50,23.75]; for i=1:Map.BarNum

Map.radius(i)=radius(i); Map.cx(i)=barCenter(i,1); Map.cy(i)=barCenter(i,2); end end

if num==3 %复杂随机地图 Map.BarNum=100; Map.LLCd=[0;0];

Map.URCd=[100;100]; MaxRadius=2.5; for i=1:Map.BarNum

Map.radius(i)=MaxRadius*rand;

Map.cx(i)=Map.LLCd(1)+Map.radius(i)+(Map.URCd(1)-Map.LLCd(1)-2*Map.radius(i))*rand;Map.cy(i)=Map.LLCd(2)+Map.radius(i)+(Map.URCd(2)-Map.LLCd(2)-2*Map.radius(i))*rand; end end

%基本RRT算法程序

function [newTree,flagReach]=extendTree(tree,endNode,step,Map) flag=1; while flag

randPoint=[(Map.URCd(1)-Map.LLCd(1))*rand,(Map.URCd(2)-Map.LLCd(2))*rand]; distmp=tree(:,1:2)-ones(size(tree,1),1)*randPoint; [mindis,minnum] = min(diag(distmp*distmp')); pvector=randPoint-tree(minnum,1:2);

newPoint=tree(minnum,1:2)+pvector/norm(pvector)*step; if collision(newPoint,tree(minnum,1:2),Map)==0 newNode=[newPoint,minnum]; newTree=[tree;newNode]; flag=0; end

8

《机器人控制理论与技术》课程论文

end

if((norm(newPoint-endNode(1,1:2))<=step)&(collision(newPoint,endNode(1,1:2),Map)==0)) flagReach=1; else

flagReach=0; end

%改进后的偏向目标型RRT算法程序

function [newTree,flagReach]=BiasextendTree(tree,endNode,step,Map) flag=1; Bias=0.5; while flag

randPoint=[(Map.URCd(1)-Map.LLCd(1))*rand,(Map.URCd(2)-Map.LLCd(2))*rand]; if rand

randPoint=endNode(1:2); end

distmp=tree(:,1:2)-ones(size(tree,1),1)*randPoint; [mindis,minnum] = min(diag(distmp*distmp')); pvector=randPoint-tree(minnum,1:2);

newPoint=tree(minnum,1:2)+pvector/norm(pvector)*step; if collision(newPoint,tree(minnum,1:2),Map)==0 newNode=[newPoint,minnum]; newTree=[tree;newNode]; flag=0; end end

if((norm(newPoint-endNode(1,1:2))<=step)&(collision(newPoint,endNode(1,1:2),Map)==0)) flagReach=1; else

flagReach=0; end

%检测新节点和每一个障碍物是否有碰撞，若有则返回1 function collision_flag = collision(node, parent, Map); collision_flag = 0; for sigma = 0:.2:1,

p = sigma*node(1:2) + (1-sigma)*parent(1:2); for i=1:Map.BarNum,

if (norm([p(1);p(2)]-[Map.cx(i); Map.cy(i)])<=Map.radius(i)), collision_flag = 1; break; end end end

9

《机器人控制理论与技术》课程论文

%随机扩展树完成后，根据每个节点父节点的序号找出可行路径 function path=findPath(tree) lastnodenum=size(tree,1); path=[tree(lastnodenum,1:2)];

parentnodenum=tree(lastnodenum,3); while(parentnodenum~=0)

path=[tree(parentnodenum,1:2);path]; parentn …… 此处隐藏：2692字，全部文档内容请下载后查看。喜欢就下载吧 ……