小车自动避障及路径规划(共23页).doc

《小车自动避障及路径规划(共23页).doc》由会员分享，可在线阅读，更多相关《小车自动避障及路径规划(共23页).doc（24页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、精选优质文档-倾情为你奉上第3章 系统总体结构及工作原理该系统主要以超声波测距为基本测距原理，并在相应的硬件和软件的支持下，达到机器人避障的效果。3.1机器人总体硬件设计3.1.1传感器的分布要求为了全方位检测障物的分布状况，并及时为机器人系统提供全面的数据，可将所需的八个传感器均匀排列在机器人周围，相邻每对传感器互成45度角。为了避免相互干扰，八个传感器以程序运行周期为周期，进行循环测距。传感器排列示意图如下：d0d1d2d3d4d5d6d7图3.1.1 传感器分布图3.1.2避障系统总体电路框架图设计红外传感器A/D转换单 片 机左侧电机超声波信号右侧电机LED显示LED显示图3.1.2

2、硬件设计总体框架图上图为支持机器人运行实用程序的硬件部分的总体设计框架图，由负责相关任务的同学提供。在超声波信号输入单片机以后，由存储在单片机中的主程序调用避障子程序，根据输入信号执行避障指令，并使相关数据返回主程序，转而提供给电机和LED显示器的驱动程序使用，最后，由电机执行转向指令，结果则显示在LED显示器上。3.1.3避障系统总体软件框架图设计开始初始化，开中断有按键？键盘处理按下键值X？运行红外避障运行超声波避障电机处理程序电机处理程序输出显示结束返回运行显示处理图3.1.3 软件总体框架图由上图可知，本文作者负责的超声波避障程序为软件总体设计中的子程序部分。在主程序运行过程中，若调用

3、超声波避障程序，机器人在自行轨迹规划后，将程序处理所得数据送给电机处理成立程序，控制电机动作。具体的避障程序设计将在第4章进行。3.2超声波测距原理测距原理：超声波是指频率高于20KHz的机械波。为了以超声波作为检测手段，必须产生超生波和接收超声波。完成这种功能的装置就是超声波传感器，习惯上称为超声波换能器或超声波探头。超声波传感器有发送器和接收器，但一个超声波传感器也可具有发送和接收声波的双重作用。超声波传感器是利用压电效应的原理将电能和超声波相互转化即在发射超声波的时候，将电能转换，发射超声波；而在收到回波的时候，则将超声振动转换成电信号。8超声波测距的原理一般采用渡越时间法TOF（tim

4、e of flight）。首先测出超声波从发射到遇到障碍物返回所经历的时间，再乘以超声波的速度就得到二倍的声源与障碍物之间的距离，即：8D=ct/2 其中D为传感器与障碍物之间的距离，以m计，c为超声波速度，这里以340m/s计，t为超声波从发送到接收的总时间，以s计。据此原理可以用超声波传感器测得的距离为避障程序提供所需的数据。8第4章 轨迹规划算法的实现方案4.1轨迹规划算法的层次化设计 根据上述材料分析，可以将机器人轨迹规划算法设计分为基础控制层、行为控制层和坐标计算层，三个层次进行。4.1.1基础控制层设计基础控制层可定义为基本行为层，这层算法的任务是寻找目标点，并确保机器人可以顺利到

5、达指定目标位。在确定目的地位置的情况下，为了达到上述目的，计算机必须对机器人的方位进行时实计算。应用人工势场法原理，可以将目标点设为引力极，牵引机器人运动。对此动作建立相应的模型，可以使用建立平面坐标作为虚拟势场的方法来给机器人定义方位，将机器人关于目标点的时实偏角作为虚拟引力方向，以确定机器人下一步所需转过的角度，并时实检测，是否已到达目的地，若已到达，则可认为虚拟引力此刻为0，并发出信号控制程序终止运行总体程序。由此，可确定基础控制层所需的各参数：(1) 机器人的时实坐标x, y值，由专门的坐标计算层提供，为了提高精确度，可以采用厘米为单位制。(2) 机器人的速度v，测量后设为定值使用。(

6、3) 周期T，直接设置为定值使用。(4) 偏转角de，可通过机器人与横坐标之间的夹角pe，减去机器人到目标点连线与横坐标的夹角E得到。(5) 终止信号last值先置为0，当到达目的地时，将其置为1。基础控制层程序流程图如下：数据读入1计算偏转角de判断是否到达目标将last值置1数据输出2YN图4.1.1 基本控制层程序流程图4.1.2行为控制层行为控制层是比基本控制层更复杂，更具有决定权的层次。它的存在决定了机器人智能避障行为的可行性，是相当重要的算法层。其主要任务是让机器人根据超声波传感器采集的距离信息判断是否该进行避障行为，且给出避障的转角值，及转向。这些都以左右综合距离的大小决定，当左

7、边综合距离大于友边综合距离时，可认为左边的斥力值大与右边斥力值，机器人左转，反之，右转。当前方综合距离小于设定的最小允许接近距离，而左右综合距离又相等时，则需要设置专门的转角，对机器人施行强制性转角动作。据此，可将各控制变量之间的关系以数学公式的形式列出，并做为“人工势场法”的基本数学模型被运用于程序流程图设计。设声纳Si的输出为di，转角de和速度v表示控制层行为的输出，其中de表示机器人下一步的运动方向，v表示机器人下一步运动的速度。那么，基于势场的控制行为可以表示如下 。de = me LMminR=L de = -de0 LR上式中的L表示机器人左边障碍物的迫近程度，根据图3.1.1

8、：L 为第1号到第3号传感器返回距离值的倒数和；R表示机器人右边障碍物的迫近程度，根据图3.1.1 ：R 为第5号到第7号传感器返回距离值的倒数和。Mmin为障碍物的最小迫近程度值，相当于第L组(或第R组)超声波传感器最大探索范围的倒数和。当左右迫近程度的值都小于最小迫近程度值时，可简单认为机器人周围无障碍物，机器人按基础控制层执行程序，其中的me表示机器人要到达目的地需要转过的角度。de0表示机器人执行行为控制层程序时，所需转动的角度大小，一般可设为定值。以上述方案为基础，为了提高机器人的避障能力，还可对设置机器人的前方迫近程度值,因为机器人的反转是以转角180度来实现的，而非后退，所以，后

9、方迫近程度值暂时无需设置。de = de0 FMmaxF表示机器人前方距离值障碍物的迫近程度，根据图3.1.1：F 为第4号传感器返回距离值的倒数，Mmax就是用于判断障碍物是否已经离机器人很近，假设机器人的最大速度为v，每两步之间的时间间隔为t，那么，为了确保机器人不会与障碍物碰撞，Mmax可以表示为速度v和间隔时间t乘积的倒数。当F值大于Mmax值时，表示前方距离障碍物很近了，需要进行避障处理了。行为控制层程序流程图如下：数据读入0F是否大于MmaxR是否大于等于L左转角度deL和R是否都小于Mmin数据出口1右转角度deR是否大于等于L左转角度de右转角度de数据输出2YNYNYNYN图

10、4.1.2 行为控制层程序流程图4.1.3坐标计算层设计坐标计算层的设计方案，主要可采用虚拟坐标技术，它能形象地定义机器人相对障碍物及目标点的具体位置，确保机器人及时避障，并顺利到达目标点。在机器人初始坐标明确的条件下，机器人坐标可根据上述两层程序提供的数据算得。设程序循环间隔的周期为T，那么，在间隔周期T时间内，机器人行走的距离P为速度v与T的乘积。又设机器人正方向与平面坐标横轴正方向的夹角为pe ，其初始值确定，转动值为上述两层提供的角度de ，那么本周期的pe值为上个周期的pe值与转角de的差。由此，可分别计算机器人本周期内横、纵坐标的变化值 X , Y。X = P * cos ( pe

11、 )Y = P * sin ( pe )因此，本周期的横、纵坐标 x , y 值可由上个周期的坐标减去变化值得到。而本周期的坐标值在行为控制层允许的情况下，将被代入基本行为层，检测是否到达目的地。坐标计算层程序流程图如下：数据读入2计算P计算pe计算本周期内横、纵坐标变化值X , Y计算本周期内横、纵坐标的值x , y数据输出0图4.1.3 坐标计算层程序流程图上述三层算法，相互制约，相互联系，组成了整个系统的基本算法。每次循环执行程序时，都是由行为控制层入口，通过对迫近程度值大小的判断，选择是否中断行为控制层，转而执行基础控制层。最后执行坐标计算层，并把所得数据提供传给程序循环的下一周期使用

12、。依据上述各算法层之间的关系，可以组合出轨迹规划子程序的总体程序流程图，如下图所示：行为控制层坐标计算层基础控制层数据接口1数据接口2主程序调用入口0主程序调用出口0图4.1.4 轨迹规划子程序总体程序流程图4.2机器人系统及环境的软件化硬件及环境的软件化技术是一种脱离硬件及实际环境对程序进行检测、仿真的编程技术，也是对机器人轨迹进行规划的过程中必不可少的一种技术。它将避障程序运行必需的硬件功能及环境影响，用拥有同样功效的程序表达出来，以检测避障程序正确与否。1.以下便是支持避障程序运行必需的各项硬件功能：(1) 测量8个超声波传感器与障碍物之间的距离。(2) 系统停止调用避障子程序的功能。(

13、3) 机器人运行轨迹的可视化功能。2.以下可视为环境对程序的影响：(1)机器人运行前，设置障碍物的大小及坐标。根据上述原理和各项需要模拟的功能，可以使用数学建模的方法进行软件化编程。首先，需要对上述逻辑化步骤进行排序。因为机器人行为是一种仿人的行为，因此，可将上述功能及影响对应相关的“神经传导过程”进行排序，并在排序后设计相应的系统程序流程图。排序及功能对应的图形如下：环境识别功能神经传递功能行为决策功能行为动作功能1神经反射功能传感器测距功能轨迹规划子程序功能停止调用子程序行为动作功能2轨迹可视化功能行为动作功能0图4.2.1 神经传导模型排序图图4.2.1中的“神经传递功能”相当于数据输入

14、功能，而“行为决策功能”相当于偏角计算及算法层次选择的功能，“行为动作功能”则对应机器人下步坐标的确立过程。“神经反射功能”的任务则是输出必要数据，并判断是否应该结束循环。由这些功能组成模块，正是机器人轨迹规划子程序部分。程序开始测量障碍物距离放置障碍物避障子程序是否停止避障子程序绘制机器人运行轨迹程序结束YN图4.2.2 系统程序流程图4.3超声波传感器测距功能的模拟根据图4.2.2给出的系统程序流程，可知，在机器人系统及环境的软件化过程中，传感器测距功能的模拟最为繁杂。主要的解决思路是建立适当的数学模型，通过对模型的具体分析，运用几何学知识计算超声波传感器与障碍物之间的距离值。为了方便计算

15、，可用微分学将障碍物设为由多个已知坐标和半径的圆组成的任意形状。而传感器到障碍物之间的距离可参考以下几何建模的方法计算得出：VAX轴正方向第4号传感器朝向wM角Xex , ya , b机器人障碍物图4.3 机器人测距功能数学模型图根据上图分析可设机器人半径为r ，障碍物半径为R 。实线M就是需要求取的障碍物与第i号传感器之间的距离。点划线A是机器人中心与障碍物圆心的连线，它与X正方向的夹角记为Ze 。第i号传感器与第4号传感器之间的夹角记为Ke ，在图上反映为实线M与第4号传感器之间的夹角。因为机器人圆心坐标(x ,y)可由避障子程序求得，而障碍物圆心(a ,b)又是已知值，因此，可用C语言表

16、示出Ze值。Ze=atan2(b-y),(a-x);根据图3.1.1可知相邻两个超声波传感器的夹角为45度，那么，Xe的值可分为两种情况，分别求取：当i = 4时，第i号传感器在第4号传感器右边。Xe = pe-Ze-(i-4)*45;已知Xe值后，通过正余弦公式便可求得机器人与障碍物之间的距离M 。其C程序表达式如下：N=A*sin(Xe);M = abs(A*cos(Xe)-sqrt(R*R-N*N)-r;4.4避障子程序与实用主程序的衔接组合将4.1中提及的三层算法整理到一起就可以组装成实现避障行为的子程序。将该子程序代替图3.1.3的总体软件框架中的“运行超声波避障”部分，就可构成实用

17、程序的流程图了。开始初始化，开中断有按键？键盘处理按下键值X？运行红外避障电机处理程序电机处理程序输出显示结束返回运行显示处理行为控制层坐标计算层基础控制层数据接口1数据接口2主程序调用入口0主程序调用出口0图4.4 避障机器人运行实用程序流程图4.5难点程序化优化处理在对程序详细设计的过程中，难免遇到一些难以表达，或表达不到位的地方，比如角度换算问题、障碍物放置个数问题以及虚拟超声波的穿透测距问题。在避障子程序和仿真主程序中，大量使用了角度这一变量。为了表示机器人转动这一动作，角度就必须带上正负符号。当然，真正需要注意的并不是角度的符号问题，而是“角度”与“弧度”的互换问题。在C程序中，有关

18、正弦、余弦、正切、余切及其反函数的计算，一般都是以弧度作为角的单位被使用的，因此，在角的单位处理上，需要特别留意。处理方法一般是根据“弧度”和“角度”的关系式进行互相转换，“360=2弧度”，子程序中的处理如下：E=(180*atan2(Y,X)/3.14; /*机器人圆心到目标点的连线与横坐标X的夹角*/dx=p*cos(pe*3.14)/180); /*机器人每走一步，在横坐标上产生的变量值*/dy=p*sin(pe*3.14)/180); /*机器人每走一步，在纵坐标上产生的变量值*/障碍物的放置动作，反映在仿真程序中，相当于由调试人员输入多个定坐标和半径的圆的动作。为了确保可被输入圆的

19、个数尽量多，可采用循环输入方式置障。在循环过程中，因无法限制输入圆的个数，所以，必然会造成程序的死循环。因此，必须给循环定义一个结束标准。通常可以在输入程序“scanf”中多加一个结束控制变量s，将s初值置为0，当s=0时，循环继续，当s=1时，跳出循环。子程序中的处理如下：for(j=0;s!=1;j+)scanf(%f,%f,%f,%d,&xbj,&ybj,&rbj,&s); /*依次输入圆心坐标，圆的半径，循环结束控制信号s的值*/setcolor(RED);setfillstyle(SOLID_FILL,RED);circle(xbj,ybj,rbj);n=j;虚拟超声波的穿透测距问题

20、，在实际环境中，这是不可能发生的事情，但在程序中，计算机每次循环都需要对每个圆进行测距，这是由程序的智能程度偏低造成的，也是不可避免的，为了得到真实的测量数据，需要给原算法加上智能化较高的附加模块。根据逻辑分析，当同一传感器得到多个返回值时，最小的那个就是真实值。因此，可在原来算法的基础上加上一段比较算法，得出最终真实值。子程序中的处理如下：for(i=0;i=7;i+) /*比较出每个传感器返回值中的最小值，并作为传感器的真实返回值，送入避障子程序处理*/for(j=1;jpoij-1)poij=poij-1;min=poij;di=min;以上设计难点仅为详细编程时碰到的几个理论上的疑难问

21、题，而具体调试过程中出现的实际问题将在第5章，给出相应解决方案。第5章 程序的仿真分析5.1仿真数据处理及分析为了检测避障程序能否正常运行，必须对其进行仿真处理。在开始仿真处理前，为了便于对仿真结果进行分析，首先需在仿真主程序中将机器人起点设置为黄色，目标点设为红色，障碍物为绿色，机器人自身设为蓝色，这可以方便观察、分析。为了提高仿真的精确度，程序中衡量长度的数据都以分米为单位。做好上述准备后，就可以在TurboC2.0环境下开始程序仿真了。为了对程序功能进行全面的检测，障碍物放置方法越多越好。下面，从简入难对程序进行仿真。1. 对单障碍物阻挡的仿真(1) 机器人左偏避障以下图形中黄色的圆代表

22、机器人行动的起点坐标，其圆心坐标为（20，20），半径为20；蓝色的圆组成的是机器人移动的轨迹；绿色圆代表障碍物，其圆心坐标为（150，150），半径为50；红色的圆代表目的地，圆心坐标为（400，400），半径为20。程序中设定的程序循环周期为4秒，速度为10分米每秒。图5.1.1 单障碍物环境下，机器人的左偏运动根据对图形5.1.1中，不同颜色的圆的相对位置分析可知，机器人可以成功的以左偏方式避开正前方的障碍物，但却穿越了目的地限定的红色区域，这可视为没有成功到达目的地。可见，避障程序的循环周期过大了，因此，可将程序的循环周期改为2秒，从而可以得到较完美的图形，图5.1.2显示了程序改进后

23、的效果。图5.1.2单障碍物环境下，机器人的左偏运动的改进图(2) 机器人右偏避障以修改后的上述仿真为基础，保持起点圆和目的地圆的圆心坐标及半径不变，以函数输入的形式，重新调整障碍物的圆心坐标及半径大小，可得到仿真图5.1.3。改动后的障碍物圆心坐标为（150，140），半径为50分米。图5.1.3单障碍物环境下，机器人的右偏运动从图5.1.3可看出机器人根据程序判断出从右边到目的地的距离比从左边去短，并选择了短距离避障的运行方式。而且，机器人可以顺利到达目的地。但图中，机器人运行轨迹仍有与障碍物接触的点，表明程序循环周期依然过大，继续进行改进，其改进效果在“机器人对双障碍物的同侧避障”图5.

24、1.4中显得较为可观。2. 对多障碍物阻挡情况的仿真(1) 机器人对双障碍物的同侧避障在保证起点和目的地圆心坐标及半径不变的条件下，可继续添加障碍物来检测机器人是否有避开多个障碍的能力。输入一号障碍物圆心坐标（150，150），半径50分米；二号障碍物圆心坐标（300，300），半径50分米。考虑到新加障碍物可能影响机器人行走的精确度，可将程序循环周期改为1秒，仿真后得到图5.1.4。图5.1.4机器人对双障碍物的同侧避障1根据上图中机器人的轨迹分析可知机器人拥有对多重障碍物的避障功能，并可以按指令到达目标点。当然，这里的多重障碍物是非连续分布。(2) 机器人对双障碍物的异侧避障为了检测机器人

25、在进行多重障碍物避障的行为过程中是否依然具有近距离选择，这一智能行为，可继续改变障碍物的位置分布状况。入一号障碍物圆心坐标（150，150），半径50分米；二号障碍物圆心坐标（250，150），半径50分米。仿真后，生成下图5.1.5。图5.1.5机器人对双障碍物的异侧避障 观察图5.1.5，可发现机器人并不是从两个障碍物的同侧饶过，而是选择了相对较短的路程，从障碍物中间穿过，并成功避开了障碍物，到达目的地，这说明，在该程序的支持下，机器人可以实现智能化较高的避障处理方式了。 (3) 机器人对壁形障碍物的避障在机器人运行起点和目的地之间，放置一串圆心在同一直线上，半径相同的圆形障碍物，组成壁形

26、进行仿真，这样可测试机器人对连续障碍物的避障行为。根据壁形障碍物与机器人相对位置的多样性，可做出如下两种不同的仿真。当壁形障碍物与起点到目的地的连线成任意不垂直的角度时，这时，由于机器人左右迫近程度的不同，机器人将按照最短路径行走，如图5.1.6；当壁形障碍物与起点到目的地的连线垂直，且左右迫近程度相同时，根据人工势场法，机器人只能根据在该情况下设定的程序，选择固定角度运行。如图5.1.7。图5.1.6机器人对壁形障碍物的避障1 从上图可直观地看出，机器人再次以最短路径绕过壁形障碍物，到达目的地，这再一次证明了程序的智能化。图5.1.7 机器人对壁形障碍物的避障2 从图5.1.7观察到，机器人

27、在避障程序的指导下，绕过障碍物，在每个程序循环周期中，其运行方向都是与从第0号传感器算起，第一个检测到障碍物的传感器的前一号传感器朝向是一致的。之所以，不使用每个程序循环周期转动相同角度的老方法，是因为，这种老方法在连续避障行为中，可能出现死循环。若这还不够证明该程序避障的智能性，可继续进行更复杂的仿真。(4) 机器人在半包围式障碍物内的避障基于以上仿真原理排列出的半包围结构障碍物更具测试意义。对如果说，机器人对壁形障碍物避障仿真的成功证明了机器人具有躲避平面墙的功能，那么，在半包围障碍物环境下的避障仿真则证明了机器人具有躲避垂直形墙角的功能，是在前一种仿真基础上的进步，也表明，该程序支持的机器人初步具有从房形障碍物中绕转出来能力。仿真图如下：图5.1.8机器人在半包围式障碍物内的避障 上图显示的机器人成功躲避开障碍物，并顺利到达目的地。(5) 机器人关于前方两边障碍物之间距离小于机器人直径的多障碍物避障为了测试机器人是否会穿过小于自身直径的间隙，可做如下仿真，模拟小间隙情况。 专心-专注-专业