小车自动避障与路径规划

1、第3章系统总体结构及工作原理该系统主要以超声波测距为基本测距原理，并在相应的硬件和软件的支持 下，达到机器人避障的效果。3.1机器人总体硬件设计3.1.1传感器的分布要求为了全方位检测障物的分布状况，并及时为机器人系统提供全面的数据， 可将所需的八个传感器均匀排列在机器人周围，相邻每对传感器互成45度角。 为了避免相互干扰，八个传感器以程序运行周期为周期，进行循环测距。传感 器排列示意图如下：图3.1.1传感器分布图3. 1. 2避障系统总体电路框架图设计图3.1.2硬件设计总体框架图上图为支持机器人运行实用程序的硬件部分的总体设计框架图，由负责相 关任务的同学提供。在超声波信号输入单片机以后

2、，由存储在单片机中的主程 序调用避障子程序，根据输入信号执行避障指令，并使相关数据返回主程序， 转而提供给电机和LED显示器的驱动程序使用，最后，由电机执行转向指令， 结果则显小在LED显小器上。3. 1. 3避障系统总体软件框架图设计图3.1.3软件总体框架图由上图可知，本文作者负责的超声波避障程序为软件总体设计中的子程序 部分。在主程序运行过程中，若调用超声波避障程序，机器人在自行轨迹规划 后，将程序处理所得数据送给电机处理成立程序，控制电机动作。具体的避障 程序设计将在第4章进行。3. 2超声波测距原理测距原理：超声波是指频率高于20KHz的机械波。为了以超声波作为检测 手段，必须产生超

3、生波和接收超声波。完成这种功能的装置就是超声波传感 器，习惯上称为超声波换能器或超声波探头。超声波传感器有发送器和接收 器，但一个超声波传感器也可具有发送和接收声波的双重作用。超声波传感器 是利用压电效应的原理将电能和超声波相互转化即在发射超声波的时候，将电 能转换，发射超声波；而在收到回波的时候，则将超声振动转换成电信号。超声波测距的原理一般采用渡越时间法TOF (time of flight)o首先测出 超声波从发射到遇到障碍物返回所经历的时间，再乘以超声波的速度就得到二 倍的声源与障碍物之间的距离，即：8】D二ct/2其中D为传感器与障碍物之间的距离，以m计，c为超声波速度，这里以 34

4、0m/s计，t为超声波从发送到接收的总时间，以s计。据此原理可以用超声 波传感器测得的距离为避障程序提供所需的数据。同第4章 轨迹规划算法的实现方案4.1轨迹规划算法的层次化设计根据上述材料分析，可以将机器人轨迹规划算法设计分为基础控制层、行为 控制层和坐标计算层，三个层次进行。4. 1.1基础控制层设计基础控制层可定义为基本行为层，这层算法的任务是寻找目标点，并确保 机器人可以顺利到达指定目标位。在确定目的地位置的情况下，为了达到上述 目的，计算机必须对机器人的方位进行时实计算。应用人工势场法原理，可以 将目标点设为引力极，牵引机器人运动。对此动作建立相应的模型，可以使用 建立平面坐标作为虚

5、拟势场的方法來给机器人定义方位，将机器人关于目标点 的时实偏角作为虚拟引力方向，以确定机器人下一步所需转过的角度，并时实 检测，是否己到达目的地，若己到达，则可认为虚拟引力此刻为0,并发出信号控制程序终止运行总体程序。由此,可确定基础控制层所需的各参数：(1)机器人的时实坐标x,y值，由专门的坐标计算层提供，为了提高精 确度，可以采用厘米为单位制。(2)机器人的速度V,测量后设为定值使用。(3)周期T,直接设置为定值使用。偏转角de,可通过机器人与横坐标之间的夹角pe,减去机器人到目 标点连线与横坐标的夹角E得到。(5)终止信号last值先置为0,当到达目的地时，将其置为1。基础控制层程序流程

6、图如下：图4丄1基本控制层程序流程图4.1.2行为控制层行为控制层是比基本控制层更复杂，更具有决定权的层次。它的存在决定 了机器人智能避障行为的可行性，是相当重要的算法层。其主要任务是让机器 人根据超声波传感器采集的距离信息判断是否该进行避障行为，且给出避障的 转角值，及转向。这些都以左右综合距离的大小决定，当左边综合距离大于友 边综合距离时，可认为左边的斥力值大与右边斥力值，机器人左转，反之，右 转。当前方综合距离小于设定的最小允许接近距离，而左右综合距离乂相等 时，则需要设置专门的转角，对机器人施行强制性转角动作。据此，可将各控制变量之间的关系以数学公式的形式列出，并做为“人工 势场法”的

7、基本数学模型被运用于程序流程图设计。设声纳Si的输出为di,转角de和速度v表示控制层行为的输出，其中de 表示机器人下一步的运动方向，v表示机器人下一步运动的速度。那么，基于 势场的控制行为可以表示如下。de = meLMmin A RR上式中的L表示机器人左边障碍物的迫近程度，根据图3. 1. 1 ： L为第1 号到第3号传感器返回距离值的倒数和；R表示机器人右边障碍物的迫近程 度，根据图3. 1.1 : R为第5号到第7号传感器返回距离值的倒数和。Mmin为 障碍物的最小迫近程度值，相当于第L组（或第R组）超声波传感器最大探索围 的倒数和。当左右迫近程度的值都小于最小迫近程度值时，可简单

8、认为机器人 周围无障碍物，机器人按基础控制层执行程序，其中的me表示机器人要到达目 的地需要转过的角度。deO表示机器人执行行为控制层程序时，所需转动的角 度大小，一般可设为定值。以上述方案为基础，为了提高机器人的避障能力，还可对设置机器人的前 方迫近程度值，因为机器人的反转是以转角180度來实现的，而非后退，所以， 后方迫近程度值暂时无需设置。de = deO FMmaxF表示机器人前方距离值障碍物的迫近程度，根据图3.1.1： F为第4号传 感器返回距离值的倒数，Mmax就是用于判断障碍物是否已经离机器人很近，假 设机器人的最大速度为v,每两步之间的时间间隔为t,那么，为了确保机器人 不会

9、与障碍物碰撞，Mmax可以表示为速度v和间隔时间t乘积的倒数。当F值 大于Mmax值时，表示前方距离障碍物很近了，需要进行避障处理了。 行为控制层程序流程图如下：图4. 1. 2行为控制层程序流程图4.1.3坐标计算层设计坐标计算层的设计方案，主要可釆用虚拟坐标技术，它能形象地定义机器 人相对障碍物及目标点的具体位置，确保机器人及时避障，并顺利到达目标 点。在机器人初始坐标明确的条件下，机器人坐标可根据上述两层程序提供的 数据算得。设程序循环间隔的周期为T,那么，在间隔周期T时间，机器人行 走的距离P为速度v与T的乘积。乂设机器人正方向与平面坐标横轴正方向的 夹角为pe ,其初始值确定，转动值

10、为上述两层提供的角度de ,那么本周期的 pe值为上个周期的pe值与转角de的差。由此，可分别计算机器人本周期横、 纵坐标的变化值X , YoX 二 P * cos ( pe )Y 二 P * sin ( pe )因此，本周期的横、纵坐标x , y值可由上个周期的坐标减去变化值得到。而本周期的坐标值在行为控制层允许的情况下，将被代入基本行为层，检 测是否到达目的地。坐标计算层程序流程图如下：图4. 1. 3坐标计算层程序流程图上述三层算法，相互制约，相互联系，组成了整个系统的基本算法。每次 循环执行程序时，都是由行为控制层入口，通过对迫近程度值大小的判断，选 择是否中断行为控制层，转而执行基础

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

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

13、 步坐标的确立过程。“神经反射功能”的任务则是输出必要数据，并判断是否应 该结束循环。由这些功能组成模块，正是机器人轨迹规划子程序部分。图4.2.2系统程序流程图4. 3超声波传感器测距功能的模拟根据图4.2.2给出的系统程序流程，可知，在机器人系统及环境的软件化 过程中，传感器测距功能的模拟最为繁杂。主要的解决思路是建立适当的数学模型，通过对模型的具体分析，运用儿 何学知识计算超声波传感器与障碍物之间的距离值。为了方便计算，可用微分 学将障碍物设为由多个己知坐标和半径的圆组成的任意形状。而传感器到障碍 物之间的距离可参考以下几何建模的方法计算得出：图4. 3机器人测距功能数学模型图根据上图分

14、析可设机器人半径为r ,障碍物半径为R o实线M就是需要求 取的障碍物与第i号传感器之间的距离。点划线A是机器人中心与障碍物圆心 的连线，它与X正方向的夹角记为Ze。第i号传感器与第4号传感器之间的 夹角记为Ke ,在图上反映为实线M与第4号传感器之间的夹角。因为机器人圆心坐标(x ,y)可由避障子程序求得，而障碍物圆心(a ,b) 乂 是已知值，因此，可用C语言表示出Ze值。Ze=atan2(by) , (ax);根据图3.1.1可知相邻两个超声波传感器的夹角为45度，那么，Xe的值 可分为两种情况，分别求取：当i 4时，第i号传感器在第4号传感器左边。Xe = Ze-pe-(4-i)*45

15、;当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的总体软件框架中的“运行超声波避障”部分，就 可构成实用程序的流程图了。图4.4避障机器人运行实用程序流程图4. 5难点程序化优化处理在对程序详细设计的过程中，难免遇到一些难以表达，或表达不到位的地

16、 方，比如角度换算问题、障碍物放置个数问题以及虚拟超声波的穿透测距问 题。在避障子程序和仿真主程序中，大量使用了角度这一变量。为了表示机器 人转动这一动作，角度就必须带上正负符号。当然，真正需要注意的并不是角 度的符号问题，而是“角度”与“弧度”的互换问题。在C程序中，有关正 弦、余弦、正切、余切及其反函数的计算，一般都是以弧度作为角的单位被使 用的，因此，在角的单位处理上，需要特别留意。处理方法一般是根据“弧 度”和“角度”的关系式进行互相转换，“360二2 Ji弧度”，子程序中的处理如 下：E=(180*ataii2(Y,X)/3.14; /*机器人圆心到目标点的连线与横坐标X的夹角*/

17、dx=p*cos(pe*3.14)/180); /*机器人每走一步，在横坐标上产生的变量值*/ dv=p*sin(pe*3.14)/180); /*机器人每走一步，在纵坐标上产生的变量值*/障碍物的放置动作，反映在仿真程序中，相当于由调试人员输入多个定坐 标和半径的圆的动作。为了确保可被输入圆的个数尽量多，可釆用循环输入方 式置障。在循环过程中，因无法限制输入圆的个数，所以，必然会造成程序的 死循环。因此，必须给循环定义一个结束标准。通常可以在输入程序“scanf” 中多加一个结束控制变量s,将s初值置为0,当s=0时，循环继续，当s=l 时，跳出循环。子程序中的处理如下：foi(j=0;s!

18、=l;j-H-)scaiif(%f,%f,%f,%d&xb|j,&yb|j,&ib|j,&s); /*依次输入圆心坐标，圆的 半径，循环结束控制信号s的值*/setcolor(RED);setfillstyle(SOLID_FILL,RED);cncle(xbj,yb|j,ibU)；n=J；虚拟超声波的穿透测距问题，在实际环境中，这是不可能发生的事情，但 在程序中，计算机每次循环都需要对每个圆进行测距，这是由程序的智能程度 偏低造成的，也是不可避免的，为了得到真实的测量数据，需要给原算法加上 智能化较高的附加模块。根据逻辑分析，当同一传感器得到多个返回值时，最 小的那个就是真实值。因此，可在原

19、來算法的基础上加上一段比较算法，得出 最终真实值。子程序中的处理如下：foi(i=0;i=7;i+-t-) /*比较出每个传感器返回值中的最小值，并作为传感器的 真实返回值，送入避障子程序处理*/foiO=l；JpoiU-l)poiU=poij-l;nuii=poi|j;di=mm;以上设计难点仅为详细编程时碰到的儿个理论上的疑难问题，而具体调试 过程中出现的实际问题将在第5章，给出相应解决方案。第5章程序的仿真分析5.1仿真数据处理及分析为了检测避障程序能否正常运行，必须对其进行仿真处理。在开始仿真处理前，为了便于对仿真结果进行分析，首先需在仿真主程序 中将机器人起点设置为黄色，目标点设为红

20、色，障碍物为绿色，机器人自身设 为蓝色，这可以方便观察、分析。为了提高仿真的精确度，程序中衡量长度的 数据都以分米为单位。做好上述准备后，就可以在TmboC2.0环境下开始程序仿真了。为了对程 序功能进行全面的检测，障碍物放置方法越多越好。下面，从简入难对程序进 行仿真。1.对单障碍物阻挡的仿真(1) 机器人左偏避障以下图形中黄色的圆代表机器人行动的起点坐标，其圆心坐标为 (20, 20),半径为20；蓝色的圆组成的是机器人移动的轨迹；绿色圆代 表障碍物，其圆心坐标为( 150, 150),半径为50；红色的圆代表目的 地，圆心坐标为(400, 400),半径为20。程序中设定的程序循环周期为

21、4 秒，速度为10分米每秒。图5.1.1单障碍物环境下，机器人的左偏运动根据对图形5丄1中，不同颜色的圆的相对位置分析可知，机器人可以 成功的以左偏方式避开正前方的障碍物，但却穿越了目的地限定的红色区 域，这可视为没有成功到达目的地。可见，避障程序的循环周期过大了， 因此，可将程序的循环周期改为2秒，从而可以得到较完美的图形，图 5.1.2显示了程序改进后的效果。图5.1.2单障碍物环境下，机器人的左偏运动的改进图(2) 机器人右偏避障以修改后的上述仿真为基础，保持起点圆和目的地圆的圆心坐标及半 径不变，以函数输入的形式，重新调整障碍物的圆心坐标及半径大小，可 得到仿真图5.1.3。改动后的障

22、碍物圆心坐标为(150, 140),半径为50分米。图5.1.3单障碍物环境卞，机器人的右偏运动从图5.1.3可看出机器人根据程序判断出从右边到目的地的距离比从左 边去短，并选择了短距离避障的运行方式。而且，机器人可以顺利到达目 的地。但图中，机器人运行轨迹仍有与障碍物接触的点，表明程序循环周 期依然过大，继续进行改进，其改进效果在“机器人对双障碍物的同侧避 障”图5.1.4中显得较为可观。2.对多障碍物阻挡情况的仿真(1)机器人对双障碍物的同侧避障在保证起点和目的地圆心坐标及半径不变的条件下，可继续添加障碍 物來检测机器人是否有避开多个障碍的能力。输入一号障碍物圆心坐标( 150, 150)

23、,半径50分米；二号障碍物圆 心坐标(300, 300),半径50分米。考虑到新加障碍物可能影响机器人行 走的精确度，可将程序循环周期改为1秒，仿真后得到图5丄4。图5.1.4机器人对双障碍物的同侧避障1根据上图中机器人的轨迹分析可知机器人拥有对多重障碍物的避障功 能，并可以按指令到达目标点。当然，这里的多重障碍物是非连续分布。 (2)机器人对双障碍物的异侧避障为了检测机器人在进行多重障碍物避障的行为过程中是否依然具有近 距离选择，这一智能行为，可继续改变障碍物的位置分布状况。入一号障碍物圆心坐标( 150, 150),半径50分米；二号障碍物圆心 坐标(250, 150),半径50分米。仿真后，生成下图5丄5。图5.1.5机器人对双障碍物的异侧避障观察图5.1.5,可发现机器人并不是从两个障碍物的同侧饶过，而是选择 了相对较短的路程，从障碍物中间穿过，并成功避开了障碍物，到达目的 地，这说明，在该程序的支持下，机器人可以实现智能化较高的避障处理 方式了。(3) 机器人对壁形障碍物的避障在机器人运行起点和目的地之间，放置一串圆心在同一直线上，半径相 同的圆形障碍物，组成壁形进行仿真，这样可测试机器人对连续障碍物的 避障行为。根据壁形障碍物与机器人相对位置的多样性，可做出如下两种 不同的仿真。当壁形障碍物与起点到目的地的连线