小车自动避障及路径规划

第3章系统总体结构及工作原理该系统主要以超声波测距为基本测距原理，并在相应的硬件和软件的支持下，达到机器人避障的效果。3.1机器人总体硬件设计3.1.1传感器的分布要求为了全方位检测障物的分布状况，并及时为机器人系统提供全面的数据，可将所需的八个传感器均匀排列在机器人周围，相邻每对传感器互成45度角。为了避免相互干扰，八个传感器以程序运行周期为周期，进行循环测距。传感器排列示意图如下：图3.1.1传感器分布图d0d1d2d3d4d5d6d73.1.2避障系统总体电路框架图设计图3.1.2硬件设计总体框架图上图为支持机器人运行实用程序的硬件部分的总体设计框架图，由负责相关任务的同学提供。在超声波信号输入单片机以后，由存储在单片机中的主程序调用避障子程序，根据输入信号执行避障指令，并使相关数据返回主程序，转而提供给电机和LED显示器的驱动程序使用，最后，由电机执行转向指令，结果则显示在LED显示器上。红外传感器A/D转换单片机左侧电机超声波信号右侧电机LED显示LED显示3.1.3避障系统总体软件框架图设计图3.1.3软件总体框架图由上图可知，本文作者负责的超声波避障程序为软件总体设计中的子程序部分。在主程序运行过程中，若调用超声波避障程序，机器人在自行轨迹规划后，将程序处理所得数据送给电机处理成立程序，控制电机动作。具体的避障程序设计将在第4章进行。3.2超声波测距原理测距原理：超声波是指频率高于20KHz的机械波。为了以超声波作为检测开始初始化，开中断有按键？键盘处理按下键值X？运行红外避障运行超声波避障电机处理程序电机处理程序输出显示结束返回运行显示处理手段，必须产生超生波和接收超声波。完成这种功能的装置就是超声波传感器，习惯上称为超声波换能器或超声波探头。超声波传感器有发送器和接收器，但一个超声波传感器也可具有发送和接收声波的双重作用。超声波传感器是利用压电效应的原理将电能和超声波相互转化即在发射超声波的时候，将电能转换，发射超声波；而在收到回波的时候，则将超声振动转换成电信号。[8]超声波测距的原理一般采用渡越时间法TOF（timeofflight）。首先测出超声波从发射到遇到障碍物返回所经历的时间，再乘以超声波的速度就得到二倍的声源与障碍物之间的距离，即：[8]D=ct/2其中D为传感器与障碍物之间的距离，以m计，c为超声波速度，这里以340m/s计，t为超声波从发送到接收的总时间，以s计。据此原理可以用超声波传感器测得的距离为避障程序提供所需的数据。[8]第4章轨迹规划算法的实现方案4.1轨迹规划算法的层次化设计根据上述材料分析，可以将机器人轨迹规划算法设计分为基础控制层、行为控制层和坐标计算层，三个层次进行。4.1.1基础控制层设计基础控制层可定义为基本行为层，这层算法的任务是寻找目标点，并确保机器人可以顺利到达指定目标位。在确定目的地位置的情况下，为了达到上述目的，计算机必须对机器人的方位进行时实计算。应用人工势场法原理，可以将目标点设为引力极，牵引机器人运动。对此动作建立相应的模型，可以使用建立平面坐标作为虚拟势场的方法来给机器人定义方位，将机器人关于目标点的时实偏角作为虚拟引力方向，以确定机器人下一步所需转过的角度，并时实检测，是否已到达目的地，若已到达，则可认为虚拟引力此刻为0，并发出信号控制程序终止运行总体程序。由此，可确定基础控制层所需的各参数：(1)机器人的时实坐标x,y值，由专门的坐标计算层提供，为了提高精确度，可以采用厘米为单位制。(2)机器人的速度v，测量后设为定值使用。(3)周期T，直接设置为定值使用。(4)偏转角de，可通过机器人与横坐标之间的夹角pe，减去机器人到目标点连线与横坐标的夹角E得到。(5)终止信号last值先置为0，当到达目的地时，将其置为1。基础控制层程序流程图如下：图4.1.1基本控制层程序流程图4.1.2行为控制层行为控制层是比基本控制层更复杂，更具有决定权的层次。它的存在决定了机器人智能避障行为的可行性，是相当重要的算法层。其主要任务是让机器人根据超声波传感器采集的距离信息判断是否该进行避障行为，且给出避障的转角值，及转向。这些都以左右综合距离的大小决定，当左边综合距离大于友边综合距离时，可认为左边的斥力值大与右边斥力值，机器人左转，反之，右转。当前方综合距离小于设定的最小允许接近距离，而左右综合距离又相等时，则需要设置专门的转角，对机器人施行强制性转角动作。据此，可将各控制变量之间的关系以数学公式的形式列出，并做为“人工势场法”的基本数学模型被运用于程序流程图设计。设声纳Si的输出为di，转角de和速度v表示控制层行为的输出，其中de表示机器人下一步的运动方向，v表示机器人下一步运动的速度。那么，基于势场的控制行为可以表示如下。de=meLMmin∩RMminde=de0R=L数据读入1计算偏转角de判断是否到达目标将last值置1数据输出2YNde=-de0LR上式中的L表示机器人左边障碍物的迫近程度，根据图3.1.1：L为第1号到第3号传感器返回距离值的倒数和；R表示机器人右边障碍物的迫近程度，根据图3.1.1：R为第5号到第7号传感器返回距离值的倒数和。Mmin为障碍物的最小迫近程度值，相当于第L组(或第R组)超声波传感器最大探索范围的倒数和。当左右迫近程度的值都小于最小迫近程度值时，可简单认为机器人周围无障碍物，机器人按基础控制层执行程序，其中的me表示机器人要到达目的地需要转过的角度。de0表示机器人执行行为控制层程序时，所需转动的角度大小，一般可设为定值。以上述方案为基础，为了提高机器人的避障能力，还可对设置机器人的前方迫近程度值,因为机器人的反转是以转角180度来实现的，而非后退，所以，后方迫近程度值暂时无需设置。de=de0FMmaxF表示机器人前方距离值障碍物的迫近程度，根据图3.1.1：F为第4号传感器返回距离值的倒数，Mmax就是用于判断障碍物是否已经离机器人很近，假设机器人的最大速度为v，每两步之间的时间间隔为t，那么，为了确保机器人不会与障碍物碰撞，Mmax可以表示为速度v和间隔时间t乘积的倒数。当F值大于Mmax值时，表示前方距离障碍物很近了，需要进行避障处理了。行为控制层程序流程图如下：图4.1.2行为控制层程序流程图4.1.3坐标计算层设计坐标计算层的设计方案，主要可采用虚拟坐标技术，它能形象地定义机器人相对障碍物及目标点的具体位置，确保机器人及时避障，并顺利到达目标点。在机器人初始坐标明确的条件下，机器人坐标可根据上述两层程序提供的数据算得。设程序循环间隔的周期为T，那么，在间隔周期T时间内，机器人行走的距离P为速度v与T的乘积。又设机器人正方向与平面坐标横轴正方向的夹角为pe，其初始值确定，转动值为上述两层提供的角度de，那么本周期的pe值为上个周期的pe值与转角de的差。由此，可分别计算机器人本周期内横、纵坐标的变化值X,Y。X=P*cos(pe)Y=P*sin(pe)因此，本周期的横、纵坐标x,y值可由上个周期的坐标减去变化值得数据读入0F是否大于MmaxR是否大于等于L左转角度deL和R是否都小于Mmin数据出口1右转角度deR是否大于等于L左转角度de右转角度de数据输出2YNYNYNYN到。而本周期的坐标值在行为控制层允许的情况下，将被代入基本行为层，检测是否到达目的地。坐标计算层程序流程图如下：图4.1.3坐标计算层程序流程图上述三层算法，相互制约，相互联系，组成了整个系统的基本算法。每次循环执行程序时，都是由行为控制层入口，通过对迫近程度值大小的判断，选择是否中断行为控制层，转而执行基础控制层。最后执行坐标计算层，并把所得数据提供传给程序循环的下一周期使用。依据上述各算法层之间的关系，可以组合出轨迹规划子程序的总体程序流程图，如下图所示：数据读入2计算P计算pe计算本周期内横、纵坐标变化值X,Y计算本周期内横、纵坐标的值x,y数据输出0图4.1.4轨迹规划子程序总体程序流程图4.2机器人系统及环境的软件化硬件及环境的软件化技术是一种脱离硬件及实际环境对程序进行检测、仿真的编程技术，也是对机器人轨迹进行规划的过程中必不可少的一种技术。它将避障程序运行必需的硬件功能及环境影响，用拥有同样功效的程序表达出来，以检测避障程序正确与否。1.以下便是支持避障程序运行必需的各项硬件功能：(1)测量8个超声波传感器与障碍物之间的距离。(2)系统停止调用避障子程序的功能。(3)机器人运行轨迹的可视化功能。2.以下可视为环境对程序的影响：(1)机器人运行前，设置障碍物的大小及坐标。根据上述原理和各项需要模拟的功能，可以使用数学建模的方法进行软件化编程。首先，需要对上述逻辑化步骤进行排序。因为机器人行为是一种仿人的行为，因此，可将上述功能及影响对应相关的“神经传导过程”进行排序，并在排序后设计相应的系统程序流程图。排序行为控制层坐标计算层基础控制层数据接口1数据接口2主程序调用入口0主程序调用出口0及功能对应的图形如下：图4.2.1神经传导模型排序图图4.2.1中的“神经传递功能”相当于数据输入功能，而“行为决策功能”相当于偏角计算及算法层次选择的功能，“行为动作功能”则对应机器人下步坐标的确立过程。“神经反射功能”的任务则是输出必要数据，并判断是否应该结束循环。由这些功能组成模块，正是机器人轨迹规划子程序部分。环境识别功能神经传递功能行为决策功能行为动作功能1神经反射功能传感器测距功能轨迹规划子程序功能停止调用子程序行为动作功能2轨迹可视化功能行为动作功能0图4.2.2系统程序流程图4.3超声波传感器测距功能的模拟根据图4.2.2给出的系统程序流程，可知，在机器人系统及环境的软件化过程中，传感器测距功能的模拟最为繁杂。主要的解决思路是建立适当的数学模型，通过对模型的具体分析，运用几何学知识计算超声波传感器与障碍物之间的距离值。为了方便计算，可用微分学将障碍物设为由多个已知坐标和半径的圆组成的任意形状。而传感器到障碍物之间的距离可参考以下几何建模的方法计算得出：程序开始测量障碍物距离放置障碍物避障子程序是否停止避障子程序绘制机器人运行轨迹程序结束YN图4.3机器人测距功能数学模型图根据上图分析可设机器人半径为r，障碍物半径为R。实线M就是需要求取的障碍物与第i号传感器之间的距离。点划线A是机器人中心与障碍物圆心的连线，它与X正方向的夹角记为Ze。第i号传感器与第4号传感器之间的夹角记为Ke，在图上反映为实线M与第4号传感器之间的夹角。因为机器人圆心坐标(x,y)可由避障子程序求得，而障碍物圆心(a,b)又是已知值，因此，可用C语言表示出Ze值。Ze=atan2((b-y),(a-x));根据图3.1.1可知相邻两个超声波传感器的夹角为45度，那么，Xe的值可分为两种情况，分别求取：当i4时，第i号传感器在第4号传感器左边。Xe=Ze-pe-(4-i)*45;当i=4时，第i号传感器在第4号传感器右边。Xe=pe-Ze-(i-4)*45;已知Xe值后，通过正余弦公式便可求得机器人与障碍物之间的距离M。其C程序表达式如下：N=A*sin(Xe);VAX轴正方向第4号传感器朝向wM角Xex,ya,b机器人障碍物M=abs(A*cos(Xe))-sqrt(R*R-N*N)-r;4.4避障子程序与实用主程序的衔接组合将4.1中提及的三层算法整理到一起就可以组装成实现避障行为的子程序。将该子程序代替图3.1.3的总体软件框架中的“运行超声波避障”部分，就可构成实用程序的流程图了。开始初始化，开中断有按键？键盘处理按下键值X？运行红外避障电机处理程序电机处理程序输出显示结束返回运行显示处理行为控制层坐标计算层基础控制层数据接口1数据接口2主程序调用入口0主程序调用出口0图4.4避障机器人运行实用程序流程图4.5难点程序化优化处理在对程序详细设计的过程中，难免遇到一些难以表达，或表达不到位的地方，比如角度换算问题、障碍物放置个数问题以及虚拟超声波的穿透测距问题。在避障子程序和仿真主程序中，大量使用了角度这一变量。为了表示机器