小车自动避障及路径规划样本文档格式.docx

小车自动避障及路径规划样本文档格式.docx

《小车自动避障及路径规划样本文档格式.docx》由会员分享，可在线阅读，更多相关《小车自动避障及路径规划样本文档格式.docx（23页珍藏版）》请在冰豆网上搜索。

而在收到回波时候，则将超声振动转换成电信号。

[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。

基本控制层程序流程图如下：

图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值时，表达前方距离障碍物很近了，需要进行避障解决了。

行为控制层程序流程图如下：

图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坐标计算层程序流程图

上述三层算法，互相制约，互相联系，构成了整个系统基本算法。

每次循环执行程序时，都是由行为控制层入口，通过对逼近限度值大小判断，选取与否中断行为控制层，转而执行基本控制层。

最后执行坐标计算层，并把所得数据提供传给程序循环下一周期使用。

根据上述各算法层之间关系，可以组合出轨迹规划子程序总体程序流程图，如下图所示：

图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给出系统程序流程，可知，在机器人系统及环境软件化过程中，传感器测距功能模仿最为繁杂。

重要解决思路是建立恰当数学模型，通过对模型详细分析，运用几何学知识计算超声波传感器与障碍物之间距离值。

为了以便计算，可用微分学将障碍物设为由各种已知坐标和半径圆构成任意形状。

而传感器到障碍物之间距离可参照如下几何建模办法计算得出：

图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）机器人关于前方两边障碍物之间距离不大于机器人直径多障碍物避障

为了测试机器人与否会穿过不大于自身直径间隙，可做如下仿真，模仿小间隙状况。