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

1、机器人控制理论与技术课程论文基于RRT及其改进型的路径规划算法报告摘要：本设计学习并分析了基本RRT路径规划的原理，并通过参考资料及自己分析，提出了一种改进的RRT路径规划算法。为了验证改进型RRT算法的正确性以及合理性。最后在VS2010开发环境下用C+编写了两种RRT算法的程序代码及演示界面。通过一定量的实验得到了大量数据。经过数据分析，验证了改进型RRT是正确的，并且在不破坏基本RRT算法的随机性的前提下，有效的将随机性和目的性结合起来，提高了RRT算法的效率和路径的质量。关键词：机器人 路径规划 快速扩展随机树(RRT) 随机性与目的性一. 引言路径规划是智能机器人研究领域的一个重要方

2、向，主要解决在有障碍的环境中，机器人如何自主寻找一条从给定起点到终点适当的运动路径，使其能在运动过程中安全、无碰地绕过障碍物。传统的路径规划有多边形拟合法、遗传算法、栅格法、人工势能法等。但这些方法都需要在一个确定性空间内对障碍物进行确定的建模和描述，计算复杂度与机器人自由度呈指数关系，不适合于解决多自由度机器人在复杂环境中的规划 3。最近十几年来，基于随机采样的路径规划算法渐渐增多，该类算法可以适用于不确定环境的高维空间。快速扩展随机树(Rapidly-Exploring Random Tree，RRT)算法，是由SMLaValle于1998年提出。RRT是一种基于随机采样的单查询运动规划方

3、法。该方法特点是能够实现随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合于解决多自由度机器人在复杂环境下和变化场景中的运动规划。二. RRT算法原理介绍快速扩展随机树算法是一种高效的数据结构和算法，该算法适用于解决未知环境下的完整性规划、以及涉及动力运动学约束的非完整性规划。RRT算法是一种随机算法，不需要特定的启发式函数的帮助。在随机树扩展时，每次产生一个随机点，由于随机点产生的随机性，可以引导随机树向着空间中的任意一个方向生长，从而该算法对未知空间有着强烈的搜索倾向。最终，随机树经过有限次的生长，将覆盖整个区域。这是RRT算法的一个非常显著的优点。这种特殊的扩

4、展方式能够渐渐的较少树节点与目标点之间的距离，并最终能够使树延伸到目标点。在每次规划中，RRT选取状态空间中的起始点作为根节点，通过随机采样，逐渐增加叶节点，生成一个随机扩展树。当随机树的叶节点中包含了目标点或目标区域中的点，便停止生长，并从目标点或目标区域开始逆向查找可以找到从目标点到起始点的连通路径，即为需要的路线。基本RRT的运行过程如图1所示：图1.快速扩展随机树的扩展过程示意图三. 一种改进型的RRT算法机器人RRT路径规划的核心思想是利用概率论中的随机性，使机器人通过RRT算法，能够探索所有的自由空间。这是RRT相对于其他路径规划方式的最显著优点。但是这一优点同时也带来了缺点，即扩

5、展方式过于平均，路径的质量不高。这是由于随机性带来的。如果能在不破坏随机性，即RRT的概率完整性的前提下，将目的性或者方向性引入到RRT算法中，使RRT的随机性与目的性达到一个协调统一，那么将能够在一定程度上克服基本RRT扩展方式过于平均，路径的质量不高的缺点。目前国内外有关学者，针对RRT的缺点提出了各种改进型的RRT算法。其中有，根据A *搜索算法中寻找最短路径的思想，在构建扩展树时引入了启发式估价函数。这种方法确实可以有效地提高路径质量，但是却带了个很大的计算量。本文中，在研究了RRT算法的思想，以及参考其他各种改进型RRT的基础上，也提出了一种改进型的RRT算法。即在产生随机点Qran

6、d之后，判断随机点Qrand与目标点Qtarget之间的距离。如果随机点Qrand落在Qend的一个圆形邻域中（|Qrand-Qend|<=d，d为一个根据机器人所处的环境设置的一个判断值），那么就用目标点Qend代替随机点Qrand，否则不做替换。公式如下： Qrand=Qend 当|Qrand-Qend|<=d Qrand=Qrand 当|Qrand-Qend|>d 其中d为一个根据机器人所处的环境设置的一个判断值 这种改进不会破坏基本RRT的概率完整性，并且使RRT带有了偏向目标方向扩展的趋势，即实现了RRT随机性与目的性的协调，并且不会增加算法的运算量。通过实验调试验

7、证，这种方式确实可以提高路径的质量，以及RRT算法的速度。也就是说新的改进算法继承了基本RRT算法的全部优点，同时，收敛速度快。具体分析见第五部分，程序调试。四. 本设计中的构思，算法实现，程序介绍4.1构思本设计中，选取二维平面作为机器人的运动环境。即在二维平面下进行RRT算法设计，将机器人看做二维平面上的一个点。机器人的运动是连续的。用C+语言实现算法，在VS2010集成开发环境下运行调试程序。在演示软件中，将机器人所处的环境大小设置为600*400。机器人的起始位置用一个半径为30的大圆的圆心表示。目标点用一个半径为20的圆的圆心表示。并且为了程序设计的方便，障碍物设计成一个个的圆形区域

8、，半径为10。如果需要的话，可以将多个圆形区域连在一起构成大的障碍物。程序的设计主要包括两个方面：1. RRT及改进型RRT程序设计。2. 演示界面设计。4.2算法实现4.2.1基本RRT算法流程Build-RRT(Qinit，Qend )1 T.init(Qinit)；2 for i= 1 to K do3 if(Extend-Tree(T )= Reached)then4 return Path(Qinit，Qend )；5 return Failure；Extend_Tree(T)1 Qrand=Random - Configuration()；2 Qnear= Nearest-Neig

9、hbor(T，Qrand)；3 Qnew =New- Configuration(Qrand，Qnear)4 if Qnew Null then5 T.add-node(Qnew)6 T.add-edge(Qnew )；7 if |Qnew-Qend|<dmin then8 return Reached；9 else10 return Advanced；11 else12 return Trapped；其中：函数Extend-Tree( )是表示扩展树的叶节点，它有3个返回值，Trapped代表没有找到新的叶节点，Advanced代表找到新的叶节点，Reached代表到达目标节点 。当扩

10、展树到达目标节点后，返回找到的路径Path(Qinit，Qend )。在Extend-Tree( )中，函数Random- Configuration()表示随机选取采样点Qrand；函数Nearest-Neighbor(T，Qrand)表示在扩展树中寻找距Qrand 最近的叶节点Qnear ；函数New- Configuration(Qrand，Qnear)完成从Qnear到 Qrand的方向上以固定的步长L计算出新的叶节点 ，计算公式为：Qnew=Qnear+L*（Qrand-Qnear）/|Qrand-Qnew| 其中在本程序中L=5；基本RRT流程图如图2（在下一页）所示；4.2.2改

11、进型RRT算法流程产生随机点QrandQrand=Qend找出树Tk上的最近点Qnear|Qrand-Qend|<=d？NY改进型RRT算法和基本RRT算法总体上是一样的。唯一不同的一点是在产生了随机点之后，判断随机点Qrand与Qend之间的距离，如果它们之间的距离小于一个给定的值d（d为一个根据机器人所处的环境设置的一个判断值，在程序调试过程中，d=30，可以根据具体情况设定）。以下仅给出改进的那部分流程图，其他同基本RRT的流程图。图3 改进型RRT算法流程图(仅仅是改进部分的)开始结束输入起始点Qstart，目标点QendQnew=Qend？Qnew在障碍物中？Qstart=Qe

12、nd？将Qnew添加到树Tk上，Tk成为T（k+1）生成随机点Qrand，找到随机树Tk上的最近点Qnear算出新节点QnewYYYNNN图2 基本RRT算法流程图4.3程序介绍1. 定义存储随机树每个节点的结构体为：struct Treestrdouble x,y;int prtpos; ; 其中x和y是树上节点的坐标，因为地图是连续的，所以定义成double型，prtpos为当前节点的父节点在randomTree中的编号。2. 定义存储随机树的数据为：vector<Treestr>randomTree 因为树的大小是未知的，所以定义成vector。3. 定义存储路径每个节点的结

13、构体：struct posdouble x,y;pos()x=-20,y-20;4. 定义存储路径的数据为：vector<pos>TreePath5. 各功能函数如下：基本RRT路径规划：randTree_build()改进型RRT路径规划：OPT_randTree_build()测距函数：double getDis(pos a,pos b)产生随机点的函数：pos Random_Configuration()寻找新点的函数：pos New_Configuration(pos randPos,pos nearPos )其他功能模块都包含在OPT_randTree_build()和r

14、andTree_build()函数中，不在具体说明。6. 在程序中设置每次路径生成节点大小的上线为10000.超过之后程序结束。如果没有找到路径，可以再找一次。通过多次实验发现，在本环境下，一般在5000次以内都可以找到路径。五. 程序调试分析5.1程序调试过程中有错误的程序介绍由于在对随机数数组排序时，程序设计有一点错误，产生了下图错误，经过调试发现并改正了错误。图4 错误程序示例图5.2演示界面介绍以图7为例。其中在环境设置一栏，有三个选项，分别在演示区域放置起点，终点，和障碍物。有四个按钮，分别为rrt路径规划对应基本RRT，改进型rrt路径对应改进型RRT，只显示路径对应基本RRT使只

15、显示最终生成的路径而不显示树。关闭对应关闭演示界面。输出栏显示随机树生成的结果。在上边的空白区域为演示区。注意：由于没有设置清除按钮，如果在执行了一次路径规划后，如果需要在执行一次，需要重新选择起点。5.3两种树的展示下面分别是在无障碍物的情况下生成的随机树。其中，图5是基本RRT无障碍物时生成的随机树，图6是改进型RRT无障碍物时生成的随机树。从两幅图中可以看出改进型RRT明显优于基本型。 图5 基本RRT无障碍物时生成的随机树 图6 改进型RRT无障碍物时生成的随机树5.4数据分析由于一次的路径搜索具有随机性，下面将在相同复杂的环境下分别用基本RRT和改进型RRT进行20次路径规划。在相同

16、环境下实验的随机树如图7和8所示。通过实验测试得到的数据如下表格所示：表一 基本RRT数据随机树大小271334962210266828002780380525424401路径大小231247231242229218226213232随机树大小225928122273175130092635242816441771路径大小236200202221221202209199229随机树大小41373060路径大小246243表二 改进型RRT数据随机树大小171316302083166124342278257017932660路径大小201220221245192197234203197随机树大小

17、178120722264244419011956166821532916路径大小207205240204208201222208218随机树大小19312305路径大小205209表三 数据比较20次平均值随机数大小路径大小基本型RRT2760224改进型RRT2100212图7 相同环境下基本RRT生成的随机树实例图8 相同环境下改进型RRT生成的随机树实例通过数据比较，以及随机树的示例图7和8可以看出改进型RRT在减小随机树方面有明显的优势，并能较好的减小路径，找到更短的路径，而不会增加计算量。即验证了前面提出的RRT改进思想是正确的。这种改进不会破坏基本RRT的概率完整性，并且使RRT带

18、有了偏向目标方向扩展的趋势，即实现了RRT随机性与目的性的协调，并且不会增加算法的运算量。通过实验调试验证，这种方式确实可以提高路径的质量，以及RRT算法的速度。也就是说新的改进算法继承了基本RRT算法的全部优点，同时，收敛速度快。六. 总结及展望1. 由于存储树节点的数组vector<Treestr>randomTree在存储数据时，顺序比较复杂，给整个随机树的最后排序显示带来了一定的困难。花费了较多的时间来解决这个问题。后来发现通过指针数据结构，可以有效解决这个问题，提高程序的执行效率。2. 通过自己编写RRT算法，程序，以及程序调试。进一步加深了对机器人RRT路径规划算法思想的理解。这种算法的优势在于，由于每一步探索的方向是随机的，可以指向自由空间的任意一点。所以随机树可以扩展到自由空间中的任何一个小的区域，可以说能够遍历整个自由空间。所以这种路径搜索方式特别适合机器人在未知的复杂环境下进行路径规划。并且从某种角度说，在调试程序，解决程序中的错误时，如果把错误看做目标点的话，也在使用这种方式，即随机性与目的性的统一。3. 在复杂程序调试过程中，如果感觉算法和程序整体上是对的，而运行的结果却和期望的有一定差距，可以写一些功能相似而相对简单的程序段代替复杂程序段，从而使程序调试更容易些，从而能更快的发现错误的那个点。4. 在程