小车自动避障及路径规划Word格式文档下载.docx
《小车自动避障及路径规划Word格式文档下载.docx》由会员分享，可在线阅读，更多相关《小车自动避障及路径规划Word格式文档下载.docx（20页珍藏版）》请在冰点文库上搜索。
3.2超声波测距原理
测距原理：
超声波是指频率高于20KHz的机械波。
为了以超声波作为检测手段，必须产生超生波和接收超声波。
完成这种功能的装置就是超声波传感器，习惯上称为超声波换能器或超声波探头。
超声波传感器有发送器和接收器，但一个超声波传感器也可具有发送和接收声波的双重作用。
超声波传感器是利用压电效应的原理将电能和超声波相互转化即在发射超声波的时候，将电能转换，发射超声波；
而在收到回波的时候，则将超声振动转换成电信号。
[8]
超声波测距的原理一般采用渡越时间法TOF（timeofflight）。
首先测出超声波从发射到遇到障碍物返回所经历的时间，再乘以超声波的速度就得到二倍的声源与障碍物之间的距离，即：
D=ct/2
其中D为传感器与障碍物之间的距离，以m计，c为超声波速度，这里以340m/s计，t为超声波从发送到接收的总时间，以s计。
据此原理可以用超声波传感器测得的距离为避障程序提供所需的数据。
第4章轨迹规划算法的实现方案
4.1轨迹规划算法的层次化设计
根据上述材料分析，可以将机器人轨迹规划算法设计分为基础控制层、行为控制层和坐标计算层，三个层次进行。
4.1.1基础控制层设计
基础控制层可定义为基本行为层，这层算法的任务是寻找目标点，并确保机器人可以顺利到达指定目标位。
在确定目的地位置的情况下，为了达到上述目的，计算机必须对机器人的方位进行时实计算。
应用人工势场法原理，可以将目标点设为引力极，牵引机器人运动。
对此动作建立相应的模型，可以使用建立平面坐标作为虚拟势场的方法来给机器人定义方位，将机器人关于目标点的时实偏角作为虚拟引力方向，以确定机器人下一步所需转过的角度，并时实检测，是否已到达目的地，若已到达，则可认为虚拟引力此刻为0，并发出信号控制程序终止运行总体程序。
由此，可确定基础控制层所需的各参数：
（1）机器人的时实坐标x,y值，由专门的坐标计算层提供，为了提高精确度，可以采用厘米为单位制。
（2）机器人的速度v，测量后设为定值使用。
（3）周期T，直接设置为定值使用。
（4）偏转角de，可通过机器人与横坐标之间的夹角pe，减去机器人到目标点连线与横坐标的夹角E得到。
（5）终止信号last值先置为0，当到达目的地时，将其置为1。
基础控制层程序流程图如下：
数据读入1
计算偏转角de
判断是否到达目标
将last值置1
数据输出2
Y
N
图4.1.1基本控制层程序流程图
4.1.2行为控制层
行为控制层是比基本控制层更复杂，更具有决定权的层次。
它的存在决定了机器人智能避障行为的可行性，是相当重要的算法层。
其主要任务是让机器人根据超声波传感器采集的距离信息判断是否该进行避障行为，且给出避障的转角值，及转向。
这些都以左右综合距离的大小决定，当左边综合距离大于友边综合距离时，可认为左边的斥力值大与右边斥力值，机器人左转，反之，右转。
当前方综合距离小于设定的最小允许接近距离，而左右综合距离又相等时，则需要设置专门的转角，对机器人施行强制性转角动作。
据此，可将各控制变量之间的关系以数学公式的形式列出，并做为“人工势场法”的基本数学模型被运用于程序流程图设计。
设声纳Si的输出为di，转角de和速度v表示控制层行为的输出，其中de表示机器人下一步的运动方向，v表示机器人下一步运动的速度。
那么，基于势场的控制行为可以表示如下。
de=meL<
Mmin∩R<
Mmin
de=de0R>
=L
de=-de0L>
R
上式中的L表示机器人左边障碍物的迫近程度，根据图3.1.1：
L为第1号到第3号传感器返回距离值的倒数和；
R表示机器人右边障碍物的迫近程度，根据图3.1.1：
R为第5号到第7号传感器返回距离值的倒数和。
Mmin为障碍物的最小迫近程度值，相当于第L组（或第R组）超声波传感器最大探索范围的倒数和。
当左右迫近程度的值都小于最小迫近程度值时，可简单认为机器人周围无障碍物，机器人按基础控制层执行程序，其中的me表示机器人要到达目的地需要转过的角度。
de0表示机器人执行行为控制层程序时，所需转动的角度大小，一般可设为定值。
以上述方案为基础，为了提高机器人的避障能力，还可对设置机器人的前方迫近程度值,因为机器人的反转是以转角180度来实现的，而非后退，所以，后方迫近程度值暂时无需设置。
de=de0F>
Mmax
F表示机器人前方距离值障碍物的迫近程度，根据图3.1.1：
F为第4号传感器返回距离值的倒数，Mmax就是用于判断障碍物是否已经离机器人很近，假设机器人的最大速度为v，每两步之间的时间间隔为t，那么，为了确保机器人不会与障碍物碰撞，Mmax可以表示为速度v和间隔时间t乘积的倒数。
当F值大于Mmax值时，表示前方距离障碍物很近了，需要进行避障处理了。
行为控制层程序流程图如下：
数据读入0
F是否大于Mmax
R是否大于等于L
左转角度de
L和R是否都小于Mmin
数据出口1
右转角度de
图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值可由上个周期的坐标减去变化值得到。
而本周期的坐标值在行为控制层允许的情况下，将被代入基本行为层，检测是否到达目的地。
坐标计算层程序流程图如下：
图4.1.3坐标计算层程序流程图
上述三层算法，相互制约，相互联系，组成了整个系统的基本算法。
每次循环执行程序时，都是由行为控制层入口，通过对迫近程度值大小的判断，选择是否中断行为控制层，转而执行基础控制层。
最后执行坐标计算层，并把所得数据提供传给程序循环的下一周期使用。
依据上述各算法层之间的关系，可以组合出轨迹规划子程序的总体程序流程图，如下图所示：
行为控制层
坐标计算层
基础控制层
数据接口1
数据接口2
主程序调用入口0
主程序调用出口0
图4.1.4轨迹规划子程序总体程序流程图
4.2机器人系统及环境的软件化
硬件及环境的软件化技术是一种脱离硬件及实际环境对程序进行检测、仿真的编程技术，也是对机器人轨迹进行规划的过程中必不可少的一种技术。
它将避障程序运行必需的硬件功能及环境影响，用拥有同样功效的程序表达出来，以检测避障程序正确与否。
1.以下便是支持避障程序运行必需的各项硬件功能：
（1）测量8个超声波传感器与障碍物之间的距离。
（2）系统停止调用避障子程序的功能。
（3）机器人运行轨迹的可视化功能。
2.以下可视为环境对程序的影响：
（1）机器人运行前，设置障碍物的大小及坐标。
根据上述原理和各项需要模拟的功能，可以使用数学建模的方法进行软件化编程。
首先，需要对上述逻辑化步骤进行排序。
因为机器人行为是一种仿人的行为，因此，可将上述功能及影响对应相关的“神经传导过程”进行排序，并在排序后设计相应的系统程序流程图。
排序及功能对应的图形如下：
图4.2.1神经传导模型排序图
图4.2.1中的“神经传递功能”相当于数据输入功能，而“行为决策功能”相当于偏角计算及算法层次选择的功能，“行为动作功能”则对应机器人下步坐标的确立过程。
“神经反射功能”的任务则是输出必要数据，并判断是否应该结束循环。
由这些功能组成模块，正是机器人轨迹规划子程序部分。
图4.2.2系统程序流程图
4.3超声波传感器测距功能的模拟
根据图4.2.2给出的系统程序流程，可知，在机器人系统及环境的软件化过程中，传感器测距功能的模拟最为繁杂。
主要的解决思路是建立适当的数学模型，通过对模型的具体分析，运用几何学知识计算超声波传感器与障碍物之间的距离值。
为了方便计算，可用微分学将障碍物设为由多个已知坐标和半径的圆组成的任意形状。
而传感器到障碍物之间的距离可参考以下几何建模的方法计算得出：
V
A
X轴正方向
第4号传感器朝向w
M
角Xe
x,y
a,b
机器人
障碍物
图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的值可分为两种情况，分别求取：
当i<
4时，第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）;
M=abs（A*cos（Xe））-sqrt（R*R-N*N）-r;
4.4避障子程序与实用主程序的衔接组合
将4.1中提及的三层算法整理到一起就可以组装成实现避障行为的子程序。
将该子程序代替图3.1.3的总体软件框架中的“运行超声波避障”部分，就可构成实用程序的流程图了。
图4.4避障机器人运行实用程序流程图
4.5难点程序化优化处理
在对程序详细设计的过程中，难免遇到一些难以表达，或表达不到位的地方，比如角度换算问题、障碍物放置个数问题以及虚拟超声波的穿透测距问题。
在避障子程序和仿真主程序中，大量使用了角度这一变量。
为了表示机器人转动这一动作，角度就必须带上正负符号。
当然，真正需要注意的并不是角度的符号问题，而是“角度”与“弧度”的互换问题。
在C程序中，有关正弦、余弦、正切、余切及其反函数的计算，一般都是以弧度作为角的单位被使用的，因此，在角的单位处理上，需要特别留意。
处理方法一般是根据“弧度”和“角度”的关系式进行互相转换，“360°
=2π弧度”，子程序中的处理如下：
E=（180*atan2（Y,X））/3.14;
/*机器人圆心到目标点的连线与横坐标X的夹角*/
dx=p*cos（（pe*3.14）/180）;
/*机器人每走一步，在横坐标上产生的变量值*/
dy=p*sin（（pe*3.14）/180）;
/*机器人每走一步，在纵坐标上产生的变量值*/
障碍物的放置动作，反映在仿真程序中，相当于由调试人员输入多个定坐标和半径的圆的动作。
为了确保可被输入圆的个数尽量多，可采用循环输入方式置障。
在循环过程中，因无法限制输入圆的个数，所以，必然会造成程序的死循环。
因此，必须给循环定义一个结束标准。
通常可以在输入程序“scanf”中多加一个结束控制变量s，将s初值置为0，当s=0时，循环继续，当s=1时，跳出循环。
子程序中的处理如下：
for（j=0;
s!
=1;
j++）
{
scanf（"
%f,%f,%f,%d"
&
xb[j],&
yb[j],&
rb[j],&
s）;
/*依次输入圆心坐标，圆的半径，循环结束控制信号s的值*/
setcolor（RED）;
setfillstyle（SOLID_FILL,RED）;
circle（xb[j],yb[j],rb[j]）;
n=j;
}
虚拟超声波的穿透测距问题，在实际环境中，这是不可能发生的事情，但在程序中，计算机每次循环都需要对每个圆进行测距，这是由程序的智能程度偏低造成的，也是不可避免的，为了得到真实的测量数据，需要给原算法加上智能化较高的附加模块。
根据逻辑分析，当同一传感器得到多个返回值时，最小的那个就是真实值。
因此，可在原来算法的基础上加上一段比较算法，得出最终真实值。
for（i=0;
i<
=7;
i++）/*比较出每个传感器返回值中的最小值，并作为传感器的真实返回值，送入避障子程序处理*/
for（j=1;
j<
=n;
if（po[i][j]>
po[i][j-1]）
po[i][j]=po[i][j-1];
min=po[i][j];
d[i]=min;
以上设计难点仅为详细编程时碰到的几个理论上的疑难问题，而具体调试过程中出现的实际问题将在第5章，给出相应解决方案。
第5章程序的仿真分析
5.1仿真数据处理及分析
为了检测避障程序能否正常运行，必须对其进行仿真处理。
在开始仿真处理前，为了便于对仿真结果进行分析，首先需在仿真主程序中将机器人起点设置为黄色，目标点设为红色，障碍物为绿色，机器人自身设为蓝色，这可以方便观察、分析。
为了提高仿真的精确度，程序中衡量长度的数据都以分米为单位。
做好上述准备后，就可以在TurboC2.0环境下开始程序仿真了。
为了对程序功能进行全面的检测，障碍物放置方法越多越好。
下面，从简入难对程序进行仿真。
1.对单障碍物阻挡的仿真
（1）机器人左偏避障
以下图形中黄色的圆代表机器人行动的起点坐标，其圆心坐标为（20，20），半径为20；
蓝色的圆组成的是机器人移动的轨迹；
绿色圆代表障碍物，其圆心坐标为（150，150），半径为50；
红色的圆代表目的地，圆心坐标为（400，400），半径为20。
程序中设定的程序循环周期为4秒，速度为10分米每秒。
图5.1.1单障碍物环境下，机器人的左偏运动
根据对图形5.1.1中，不同颜色的圆的相对位置分析可知，机器人可以成功的以左偏方式避开正前方的障碍物，但却穿越了目的地限定的红色区域，这可视为没有成功到达目的地。
可见，避障程序的循环周期过大了，因此，可将程序的循环周期改为2秒，从而可以得到较完美的图形，图5.1.2显示了程序改进后的效果。
图5.1.2单障碍物环境下，机器人的左偏运动的改进图
（2）机器人右偏避障
以修改后的上述仿真为基础，保持起点圆和目的地圆的圆心坐标及半径不变，以函数输入的形式，重新调整障碍物的圆心坐标及半径大小，可得到仿真图5.1.3。
改动后的障碍物圆心坐标为（150，140），半径为50分米。
图5.1.3单障碍物环境下，机器人的右偏运动
从图5.1.3可看出机器人根据程序判断出从右边到目的地的距离比从左边去短，并选择了短距离避障的运行方式。
而且，机器人可以顺利到达目的地。
但图中，机器人运行轨迹仍有与障碍物接触的点，表明程序循环周期依然过大，继续进行改进，其改进效果在“机器人对双障碍物的同侧避障”图5.1.4中显得较为可观。
2.对多障碍物阻挡情况的仿真
（1）机器人对双障碍物的同侧避障
在保证起点和目的地圆心坐标及半径不变的条件下，可继续添加障碍物来检测机器人是否有避开多个障碍的能力。
输入一号障碍物圆心坐标（150，150），半径50分米；
二号障碍物圆心坐标（300，300），半径50分米。
考虑到新加障碍物可能影响机器人行走的精确度，可将程序循环周期改为1秒，仿真后得到图5.1.4。
图5.1.4机器人对双障碍物的同侧避障1
根据上图中机器人的轨迹分析可知机器人拥有对多重障碍物的避障功能，并可以按指令到达目标点。
当然，这里的多重障碍物是非连续分布。
（2）机器人对双障碍物的异侧避障
为了检测机器人在进行多重障碍物避障的行为过程中是否依然具有近距离选择，这一智能行为，可继续改变障碍物的位置分布状况。
入一号障碍物圆心坐标（150，150），半径50分米；
二号障碍物圆心坐标（250，150），半径50分米。
仿真后，生成下图5.1.5。
图5.1.5机器人对双障碍物的异侧避障
观察图5.1.5，可发现机器人并不是从两个障碍物的同侧饶过，而是选择了相对较短的路程，从障碍物中间穿过，并成功避开了障碍物，到达目的地，这说明，在该程序的支持下，机器人可以实现智能化较高的避障处理方式了。
（3）机器人对壁形障碍物的避障
在机器人运行起点和目的地之间，放置一串圆心在同一直线上，半径相同的圆形障碍物，组成壁形进行仿真，这样可测试机器人对连续障碍物的避障行为。
根据壁形障碍物与机器人相对位置的多样性，可做出如下两种不同的仿真。
当壁形障碍物与起点到目的地的连线成任意不垂直的角度时，这时，由于机器人左右迫近程度的不同，机器人将按照最短路径行走，如图5.1.6；
当壁形障碍物与起点到目的地的连线垂直，且左右迫近程度相同时，根据人工势场法，机器人只能根据在该情况下设定的程序，选择固定角度运行。
如图5.1.7。
图5.1.6机器人对壁形障碍物的避障1
从上图可直观地看出，机器人再次以最短路径绕过壁形障碍物，到达目的地，这再一次证明了程序的智能化。
图5.1.7机器人对壁形障碍物的避障2
从图5.1.7观察到，机器人在避障程序的指导下，绕过障碍物，在每个程序循环周期中，其运行方向都是与从第0号传感器算起，第一个检测到障碍物的传感器的前一号传感器朝向是一致的。
之所以，不使用每个程序循环周期转动相同角度的老方法，是因为，这种老方法在连续避障行为中，可能出现死循环。
若这还不够证明该程序避障的智能性，可继续进行更复杂的仿真。
（4）机器人在半包围式障碍物内的避障
基于以上仿真原理排列出的半包围结构障碍物更具测试意义。
对如果说，机器人对壁形障碍物避障仿真的成功证明了机器人具有躲避平面墙的功能，那么，在半包围障碍物环境下的避障仿真则证明了机器人具有躲避垂直形墙角的功能，是在前一种仿真基础上的进步，也表明，该程序支持的机器人初步具有从房形障碍物中绕转出来能力。
仿真图如下：
图5.1.8机器人在半包围式障碍物内的避障
上图显示的机器人成功躲避开障碍物，并顺利到达目的地。
（5）机器人关于前方两边障碍物之间距离小于机器人直径的多障碍物避障
为了测试机器人是否会穿过小于自身直径的间隙，可做如下仿真，模拟小间隙情况。
升级会员 