基于向量场直方图的移动机器人避障方法研究.doc

本科毕业设计（论文）题目：（中文）基于向量场直方图的移动机器人避障方法研究 （英文）STUDY OF OBSTACLE AVOIDANCE FOR THE MOBILE ROBOT BASED ON VECTOR FIELD HISTOGRAM诚 信 承 诺我谨在此承诺：本人所写的毕业论文基于向量场直方图的移动机器人避障方法研究均系本人独立完成，没有抄袭行为，凡涉及其他作者的观点和材料，均作了注释，若有不实，后果由本人承担。 承诺人（签名）： 年 月 日摘要【摘要】移动机器人广泛应用于工业生产加工制造中，尤其在危险和恶劣的环境中可以用机器人代替人工操作减少损失。避障技术在移动机器人的发展中起着至关重要的作用，避障方法有很多种，本文是基于向量场直方图的移动机器人避障方法。由于传统的向量场直方图法在给定值太大或太小时都无法安全避障，本文在此基础上，利用激光测距仪所或得的数据首先确定一个可以安全行驶的范围，然后通过算法自动的改变给定值的大小，最终选择最优给定值，通过差分驱动控制使机器人安全避障。并在RoboticStudio仿真系统中建立场景和编程来实现。【关键词】移动机器人；激光测距仪；向量场直方图 ；差分驱动；避障STUDY OF OBSTACLE AVOIDANCE FOR THE MOBILE ROBOT BASED ON VECTOR FIELD HISTOGRAMAbstract【ABSTRACT】Mobile robots are widely used in industrial production and manufacturing，especially in dangerous and harsh environments they can replace manual operations to reduce losses. Obstacle avoidance technology plays a vital role in the development of mobile robot , There are many ways about obstacle avoidance, this article is the obstacle avoidance method for mobile robot based on the vector field histogram.If the given value is too large or too small the robot can not go through obstacles safely using traditional vector field histogram method. Basing on the VFH, firstly ,determining a range of safe driving use the data from laser range finders.Then changing the given value automatically and choosing the optimal value , finally using the differential drive control method make the robot avoid obstacles successfully.And make it come ture in the RoboticStudio simulated system.【KEYWORDS】mobile robot；LRF；VFH ; differential drive; obstacle avoidance 目 录摘要2Abstract31概论51.1课题的目的和意义51.2课题的主要研究内容52移动机器人避障的基本原理621激光测距仪LMS 200基本工作原理62.2向量场直方图法72.3差分驱动法92.4 Microsoft Robotics Developer Studio103基于VFH的避障算法113.1算法流程图113.2求机器人可以安全通过的所有范围113.3移动机器人角速度和线速度133.3.1 确定给定值的原则133.3.3 角速度控制153.3.4线速度控制153.4差分驱动控制164仿真实现174.1创建场景1742编写仿真程序224.2.1新建DSS项目224.2.2获取LRF数据234.2.3在窗口和txt文本中显示254.2.4控制算法264.3运行仿真程序285结论31参考文献32致谢341概论1.1课题的目的和意义 移动机器人是美国和苏联在60年代后期，为了探测月球表面，开发成功的。通过在地面的控制，利用美国“探测者”3号成功实施了在月球上挖沟的计划并完成了其它的任务。苏联的“登月者”20号在月球表面成功着陆，控制相关设备在在月球表面钻削岩石，并把岩石样品装进设备带回实验室。日本早稻田大学于70年代初期开发出和人类相似的两足行走机器人。随着原子能利用和海洋开发的发展，极限作业机器人和水下机器人也得到了迅速发展。在人们的日常生活中，机器人也开始得到使用并引起人们的高度关注。尤其在危险、恶劣的环境中可以用机器人代替人工操作而减少人力和物力资源损失。避障在移动机器人的发展中起着举足轻重的作用。由于传感器所获得的数据的安全性、连续性、可靠性等不是太完美，并且移动机器人对周围的环境感知能力不理想。因此怎样才能够使得机器人安全的避过障碍物而到达指定目的地越来越引起人们的关注。现在有很多移动机器人的避障法，比如势场法、边沿检测法等。 1.2课题的主要研究内容本课题的主要任务与目标是在RoboticStudio仿真系统中建立差分驱动轮式移动机器人的仿真环境，根据激光测距仪的障碍物距离信息，使用向量场直方图法（VFH）设计合适的机器人避障方法，使移动机器人能够无碰撞地通过障碍区。主要任务和目标：1、学习和了解RoboticStudio仿真系统，掌握建立机器人仿真环境和编程控制机器人的基本方法； 2、学习和了解差分驱动轮式移动机器人的基本运动控制方式； 3、学习和了解激光传感器的工作原理和方式，能够根据激光数据获取周边障碍物的距离信息； 4、学习和掌握向量场直方图法（VFH），设计合理的避障控制算法； 5、根据激光传感器检测到的距离信息，在仿真系统中编程实现移动机器人无碰撞地通过障碍区2移动机器人避障的基本原理21激光测距仪LMS 200基本工作原理LMS200 实物如图2-1，它采用红外线激光光束，优点是不需要接触被测量物体的表面并且物体的形状和颜色都没有要求，不需要反射片或是另一接收器而可以直接接收反射回来的信号。它的最小测量距离是0m，最大测量距离是80 m ，有100和180两种角度范围模式可供选择，长度误差小于10 mm ，角度误差小于0. 5。障碍物的尺寸、形状能够被迅速获得，这些对于生成环境地图和机器人定位都是非常有利的。图2-1 LMS 200实物LMS200激光测距仪测距原理如图2-2所示，它 是基于飞时测量原理（激光雷达）工作的。在激光传感器工作的范围内，发射一个激光脉冲并且被物体的表面反射回来，通过这个激光脉冲从发射出去到接受之后所消耗的时间来计算物体和LMS之间的距离。 （2-1）L:物体与传感器间的距离；c：光速;t: 激光脉冲往返的时间获得从物体表面反射回来的激光脉冲后，激光脉冲经过一个旋转的镜子进行反射，形成了一个100或是180的扇形区域 图2-2 LMS工作原理2.2向量场直方图法向量场直方图法（vector field histogram,VFH）是由Borenstein和Koren一起创造出来的，向量场直方图法的关键是在传感器的周围建立一个图形，这样可以避免由于传感器所获得数据暂时延迟而或丢失可能导致的错误。任意时刻创建的图形都是一个划分的栅格，只有在传感器扫描范围内的数据才会在栅格内从而取代已有的旧数据。为了能够有效避开障碍物，它形成了一个极坐标图，其中x轴代表行走的方向与障碍物所构成的角度，y轴是根据占有栅格的多少来计算障碍物就在运动方向的概率，如图2-3所示。如图2-3 极坐标图利用此极坐标图可以计算出机器人的行走方向。首先确定出移动机器人可以安全避开障碍物所有路径范围，再在这些范围内选择损耗最低的路径，即最优路径。损耗函数L可以表示为： (2-2)其中，表示目的角度； 表示轮子角度； 表示原来角度k、u、w为比例系数，损耗最小的就是最优路径，调整这些系数可以改变移动机器人的避障效果。其中向量场直方图法存在某些缺陷，在不同的环境下，对于同一个给定值可能使得机器人无法安全避开障碍物，给定值太大或是太小都会出现错误。当给定值太大时移动机器人有时无法发现障碍物而最终与障碍物发生撞击，如图2-4所示。当给定值太小时移动机器人有时会丢弃可以安全通过的路径而最终进入死角，如图2-5所示。图2-4 给定值太小发生错误图2-5 给定值太大发生错误2.3差分驱动法如图26所示，这个移动机器人使用两个轮子采用差分驱动控制，两个轮子的直径都是r。假设两个轮子的中心点为p点，中心点p到两个轮子的距离都是l。若已知r，l，且两个轮子的转速分别是1和2。则移动机器人的总速度如式2-3： （2-3）图26差分动驱动的移动机器人2.4 Microsoft Robotics Developer Studio微软开发出来的Microsoft Robotics Developer Studio(MRDS)是用来为市场上已经存在的或是即将开发出的机器人建立软件的软件开发工具包，如果你已经有一个机器人了，可以直接用MSRS进行控制，如果没有的话，也不用担心，因为MSRS为虚拟机器人提供了一个成熟的三维仿真环境，当你下载到免费的SDK几分钟后就可以驱动机器人了。它的最大好处就是对个人、学校或者开发商都是免费的。MSRS还包含一个成熟的实时运行环境，用来编写强大的异步式和分布式应用程序很容易。微软的MRDS的SDK包含许多组件，并发同时运行（CCR）和分散软件服务（DSS）构成了实时运行环境。它们都是控制库，所以运行在这些环境中的机器人服务也是通过控制代码实现的。仿真环境是可以用来实现新算法的全物理的三维仿真器，可视化编程语言（ VPL ）是一个可以用来实现机器人服务的图形化编程环境。除了所有这些组件，微软团队还开发了很多实例并且为编程用户提供程序和块。3基于VFH的避障算法由2.2部分的VFH的缺点可以知道当给定值太大或太小时，如果使用VFH算法的话，移动机器人都有可能无法安全通过障碍物，因此在基于VFH算法的基础上做了修改。首先通过激光测距仪所获得的数据计算确定机器人可以安全通过的一些范围，然后通过算法在这些可以通过的给定值中选择最优给定值，使得机器人可以快速安全通过障碍物。3.1算法流程图:图3-1算法流程图图3-1为基于VFH算法改进的算法流程图。通过激光测距仪所获得的数据，来求可以安全通过的所有范围，即同时满足边长条件和长方形条件的范围都是可以安全通过的范围，若不满足则激光传感器需重新采集下一组数据。然后在在这些范围中确定一个最优给定值，利用这个给定值，通过差分驱动分别控制机器人的角速度和线速度，安全避开障碍物。3.2求机器人可以安全通过的所有范围步骤1：将激光测距仪LMS200 的180扫描范围分为72个扇形，每个扇形的角度为2.5，并且将每个扇形按照从右到左的顺序依次标记为Sm。其中m为1，2,3，.，72 。步骤2：确定每个扇形的最小扫描距离Lmmin，并且用该值与给定值Rg进行比较，如果该扇形区域GgLmmin，那么该扇形是预备的扇形区域。图3-2确定机器人可以安全通过的路径方向步骤3：在步骤2中得到的所有预备扇形区域中，将所有连接在一起的并接成一个待判断的大扇形区域，该待判断的大扇形区域表示为Qd，d为1，2，Td（其中Td表示待判断大扇形区域的个数且Td72）。若nf表示并接的第一个扇形，nl表示并接的最后一个扇形，d表示待判断大扇形的角平分线与移动机器人的坐标轴的x轴的夹角，那么Qd可表示为一个矩阵： （3-1）由几何知识易得d： （3-2）步骤4:在对待判断大扇形区域Sd进行判断选择，若满足以下条件，那么该区域可保证机器人能够安全通过，此区域保留，否则丢弃。边长约束条件：待判断的大扇形区域的中心角所对的边长为Ed，当Eg小于Ed时称为满足边长约束条件，其中，Kg是一个比例系数，在此算法中将其设置为1.2，Z是移动机器人的直径，在仿真环境中选用的先锋三号直径设置为0.5米。即只需要m即满足了边长约束条件。长方形约束条件：激光测距仪LMS200扫描所获得的数据点都不在长方形内。该长方形的两条边与待判断扇形的角平分线互相平行，具体长方形见图3-2，其中，Kd是比例系数且取值是1.2。如果不能同时满足以下条件的则一定不满足长方形约束条件。 （3-3） （3-4）图3-2 确定是否满足长方形约束条件其中A、B分别是长方形的宽和长，只有同时满足了式3-3和式3-4才满足长方形的约束条件如果同时符合步骤4中、的待判断区域是安全区域，机器人可以避开障碍物而安全通过，此待判断扇形区的角平分线叫做可安全行驶的路径方向。3.3移动机器人角速度和线速度3.3.1 确定给定值的原则在3、2中对于某个确定的给定值G，可以求得移动机器人安全避开障碍物的路径方向，图3-3是给定值G不同时，移动机器人所或得的安全避开障碍物的路径方向图，图3-3-1和图3-3-2分别是给定值为3和4的时候，此时所获得的路径方向角度全都超过90，但此时所获得的范围无法满足步骤4中的长方形约束条件，所以这两个给定值都是不合理的，移动机器人无法成功避开障碍物。图3-3-3和图3-3-4的给定值分别为1和2，此时获得的安全避开障碍物的路径角度分别是90和110，这两个给定值都可以保证机器人避开障碍物，但由图容易发现当给定值为2时的效果更好，因为给定值为1时要过段时间才能够发现前方障碍物。因此我们可以得出结论，再能够保证移动机器人成功避开障碍物的前提下，给定值G越大越好。图3-3-1给定值G=3 图3-3-2给定值G=4 图3-3-3给定值G=1 图3-4-4给定值G=2 3.3.2确定最优给定值Gb 由上部分可知在保证移动机器人可以安全避障的前提下，给定值G是越大越好。接下来将计算出自动改变给定值的计算方法。：假定给定值值G的最大值Gmax： （3-5）所有可能的给定值为Gj： （3-6）其中G0为最小的给定值： （3-7）K0是一比例系数，S0表示移动机器人从最大的速度开始到停止是所运行的距离: （3-8）本课题中移动机器人的最大速度Vmax为0.8，减速的加速度a为-1，G0确定为1，是固定变化量且取值为0.1， UG的值选为41，则容易得到Gmax为4。：由上部分中所获得的Gj（其中j=1,2,3，），用3、2中的方法判断移动机器人是否能够成功避开障碍物。即某个确定的给定值，判断是否满足边长约束条件和长方形约束条件。如果两个条件同时满足，那么这个给定值就是有效的保留起来，否则该给定值就是无效的直接丢弃。：假定总共有H个给定值是有效的，则移动机器人共有H个可以通过的路径方向。其中Gj表示第j个有效的给定值，表示第j个有效值对应的可以避障的安全路径方向与机器人的x轴的夹角，Ej表示第j个有效值对应的安全路径范围的中心角对应的边长。 在这H个有效值中，同时满足式3-8的安全路径方向中给定值最大的那个就是最优给定值，用Gb表示。 （3-9） 其中是先前确定的固定值，本课题中是，是移动机器人与目标之间的角度。3.3.3 角速度控制由3、3、2的可得最优给定值，Gb对应的安全路径方就是机器人的最终行走方向，用表示。控制角速度的表达式为式3-8： （3-10）其中为比例系数，取值为0.8.注：由3、3、2确定的最优给定值Gb可能对应多个安全路径方向，其中选取中心角对应边长最大的那个方向为移动机器人的最终行走方向。3.3.4线速度控制机器人在前进的过程中希望机器人运行的越快越好，即我们总是希望机器人的线速度是Vmax，但是当机器人距离障碍物较近时或者行驶方向与目标偏离较大时，速度过大反而可能会浪费时间，此时需要减小轮子的速度。线速度表达式为： （3-11）其中为： （3-12） 其中是机器人运行过程中的最大线速度，取值为0.8， 机器人运行过程中的最小线速度。是式3-13在取时且最优给定值Gb取G0时的值。 （3-13） 其中G0为最小给定值，取值为1。Gmax为最大给定值，取值为4，Gb为最优给定值。K1与K2之和为1并且K1大于K2，表示机器人的行走方向与目标方向之间的差距比最优给定值的优先权要高，这样可以保证机器人更快的避开障碍物，本课题中K1取值为0.65，K2取值为0.35。3.4差分驱动控制 根据差分驱动控制原理我们可以得到机器人的角速度、线速度与左右轮的关系如式3-14： （3-14） 其中和分别是左轮和右轮的速度，l表示轮子间的距离，表示机器人的线速度，表示机器人的角速度。由此可以得到左右轮的速度如图3-15所示： （3-15）4仿真实现4.1创建场景步骤1、添加场景和机器人打开Microsoft Visual Simulation Environment 2008，模式设置为编辑状态。打开EntityUI.manifest.xml，如图4-1所示。添加缺省场景。图4-1 添加场景和机器人在图4-1中，选择LRF。再设定机器人初始位置为0,0,0 。点击“Add Motor Base”，添加一个差分驱动的机器人本体。成功添加机器人后为图4-2图4-2 成功添加机器人步骤2、修饰机器人在图4-2的左侧编辑栏中，选择Motor Base with Drive点击“Edit Entity”出现编辑窗口如图4-3所示，在图4-3中找到EntityState，点击右端的选择按钮会出现图4-4然后修改本体的名称和式样。图4-3 修饰机器人图4-4修改机器人名字和式样这里我们把Name改成“P3DX”。在Mesh这一项里，选择安装目录下store/media/Pioneer3dx.bos作为渲染文件，然后点击确定。步骤3、修饰轮子在编辑栏中选择P3DX，点击“Edit Entity”，首先修饰左轮，找到LeftWheel后点击选项按钮，在弹出窗口中找到Entity State，出现与4类似的窗口，在Mesh项中选择PioneerWheel.bos作为轮子的渲染文件，然后点击确定。按照同样的方法修饰右轮。此时的机器人为图2-5所示。图4-5机器人P3DX步骤4、建立不可移动的环境菜单项Entity-New，出现图2-6窗口，选择FloorPlanEntity图4-6建立机器人环境点击OK，加入房间构造，然后在编辑栏中选择FloorPlan，点击“Edit Entity”进入二维的房间布置画面，选择4个wall作为走廊设置其高度为1.5，在选择6个wall作为障碍物，布置好房间布局后，点击“OK”按钮退出。将场景命名为Simrobtic2，保存在D:Microsoft Robotics Developer Studio 2008 R3 ，最终的场景如图2-7所示。图4-7 最终场景42编写仿真程序4.2.1新建DSS项目在Visual C# 2008里，新建一个项目,如图4-8所示，选择Microsoft Robotics的DSS Service模板图4-8 新建DSS Service 项目点击“确定”后，出现如下选项：图4-9 选择Partner总共需要添加3个Partner：Simulation Engine， Simulated Generic Differential Driver， Simulated Laser Range Finder。4.2.2获取LRF数据获取激光数据的思路为图4-10所示：图4-10 LRF获取数据首先使用get函数给机器人人发送消息，然后接下来判断数据是否错误，如果数据正确的话则将数据保存在sensorState的DistanceMeasurements属性中，程序为sicklrf.State sensorState = (sicklrf.State)sensorOrFault 判断数据的程序为：bool HasError(PortSet sensorOrFault) Fault fault = (Fault)sensorOrFault; if (fault != null) LogError(fault.ToException(); return true; else return false; 在最后一步中的启动计时器是为了能够不断更新激光传感器的数据，编程语句为：Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateLaser);该句表示当时间端口_dateTimePort消息到来时就会执行UpdateLaser函数一次。 TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(600), _dateTimePort);该句启动一个延时计数，当经过600毫秒后，就往_dateTimePort发送一个消息。4.2.3在窗口和txt文本中显示 （1）在窗口中显示LRF数据为了能够在窗口中更直观的观察激光传感器所获得的数据，首先在Example3项目中添加一个名为LaserData窗体，再在该窗体中添加一个名为pictureBoxLaser 的PictureBox控件， 为了能够更新pictureBoxLaser控件，在窗体中新增一个方法UpdateLaserData由于获取传感器数据是在Example3Service类中，需要在Example3Service类中能够调用该窗体的UpdateLaserData方法。首先在Example3Service中定义窗体变量：LaserData _laserDataForm;然后在start方法中启动窗体： WinFormsServicePort.Post(new RunForm() = _laserDataForm = new LaserData(); _laserDataForm.Show(); return _laserDataForm; ); 同时必须修改UpdateLaser任务函数，在UpdateLaser中增加窗体的更新方法： WinFormsServicePort.Post(new FormInvoke() = _laserDataForm.UpdateLaserData(sensorState.DistanceMeasurements); );这样，当每次获得激光传感器数据后，就会更新窗体中的pictureBoxLaser控件了。（2）在txt文本中显示机器人信息机器人是在不断运动中的，希望能够直观的观测当前的机器人的信息，例如移动机器人每个时刻的角速度和线速度。通过以下程序可实现其功能：try StreamWriter sw = new StreamWriter(d:textbook.txt,true);sw.WriteLine(response.Entity.State.Velocity.X+,+response.Entity.State.Velocity.Z+,+response.Entity.State.AngularVelocity.X +, + response.Entity.State.AngularVelocity.Z) ;sw.Close(); catch (Exception g) Console.WriteLine(Exception: + g.Message); finally Console.WriteLine(Executing finally block.); 使用该段程序之前需添加引用using System.IO; 该段程序可以将机器人P3DX的线速度和角速度写在D盘下的名为textbook的txt文件中。4.2.4控制算法（1）获得机器人位置信息在3中基于VFH的移动机器人避障算法中需得到移动机器人的X轴与目标之间的夹角，而目标信息是给定的，此时需要获得机器人的位置信息。可以通过仿真引擎端口的QuerySimulationEntity来完成。以下是获得机器人位置的函数GetEntityState()程序。 IEnumerator GetEntityState() engine.VisualEntity tmp = new engine.VisualEntity(); tmp.State = new Microsoft.Robotics.Simulation.Proxy.EntityState(); tmp.State.Name = P3DX; var stateOrFault = _simulationEnginePort.QuerySimulationEntity(tmp); yield return stateOrFault.Choice(); if (!HasError(stateOrFault) engine.QuerySimulationEntityResponseType response = (engine.QuerySimulationEntityResponseType)stateOrFault; /显示在_laserDataForm中 _laserDataForm.UpdatePoseInfo(string.Format(Position: rnX=0, Y=1, Z=2rnOrientation: rnX=3, Y=4, Z=5rn W=6,Angle=7rnVelocity=8,9,10rnA Velocity=11,12,13, response.Entity.State.Pose.Position.X, response.Entity.State.Pose.Position.Y, response.Entity.State.Pose.Position.Z, response.Entity.State.Pose.Orientation.X, response.Entity.State.Pose.Orientation.Y, response.Entity.State.Pose.Orientation.Z, response.Entity.State.Pose.Orientation.W, ConvertQuaternion(response.Entity.State.Pose.Orientation) * 180 / Math.PI, response.Entity.State.Velocity.X, response.Entity.State.Velocity.Y, response.Entity.State.Velocity.Z, response.Entity.State.AngularVelocity.X, response.Entity.State.AngularVelocity.Y, response.Entity.State.AngularVelocity.Z); 上面一段程序同时可以更新4.2.3中定义的窗口，将所获的当前数据显示在_laserDataForm中。由于仿真环境中方向表示方法与现实生活中人们的习惯不一样，为了把表示方向的四元组转换成在平面中更加熟悉的角度，特定义了ConvertQuaternion函数来实现该功能 private double ConvertQuaternion(Microsoft.Robotics.PhysicalModel.Proxy.Quaternion q) Microsoft.Robotics.PhysicalModel.AxisAngle a = new Microsoft.Robotics.PhysicalModel.AxisAngle(); Microsoft.Robotics.PhysicalModel.Quaternion q1 = new Microsoft.Robotics.PhysicalModel.Quaternion(); q1.X = q.X; q1.Y = q.Y; q1.Z = q.Z; q1.W = q.W; a = Microsoft.Robotics.PhysicalModel.Quaternion.ToAxisAngle(q1); if (float.IsNaN(a.Angle) return 0; else if (Math.Sign(a.Axis.Y) 0) return 2.0 * Math.PI - a.Angle; return a.Angle; 4.3运行仿真程序 在开始菜单，Microsoft Robotics Developer Studio组下找到DSS Manifest Editor并运行，如图4-11：图4-11 运行DSS Manifest Editor在File菜单下打开场景文件所对应的manifest文件即Simrobtic2.manifest.xml，其中包含有simulationengine, simulateddifferentialdrive, s