导购机器人程序部分Word文件下载.docx
《导购机器人程序部分Word文件下载.docx》由会员分享，可在线阅读，更多相关《导购机器人程序部分Word文件下载.docx（17页珍藏版）》请在冰豆网上搜索。
下面给出A星算法的伪代码实现：
heap.add（firstnode）//将开始节点加入堆,同时也是加入openlist
do
{
fleast=heap.pop（）;
//从openlist取fcost最小的元素
current=fleast;
//取出的元素作为当前节点
fleast.flag=1//1:
onclose0:
notonopennoronclose
for（inti=current.x-1;
i<
current.x+1;
i++）//考察当前节点的所有相邻节点
for（intj=current.y-1;
j<
current.y+1;
j++）
{
if（map[i][j]!
=unwalkable||!
onclose（node（i,j）））//相邻节点可通并且没有考察过
if（!
onopen（node（i,j）））//相邻节点不在openlist中
heap.add（node（i,j））;
//加入openlist
node（i,j）.parent=current;
//设置相邻节点的父节点为当前节点
node（i,j）.calculate（f,g,h）;
//重新计算fcost,gcost,hcost
if（node（i,j）==dest）//如果加入的节点是目标点则找到路径
path=find;
}
else
temp.gcost=parent.gcost+addcost;
//相邻节点已经在openlist中
node（i,j）.hcost=10*（abs（i-parent.x）+abs（j-parent.y））;
node（i,j）.fcost=node（i,j）.gcost+node（i,j）.hcost;
if（tempgcost<
node（i,j）.gcost）
node（i,j）.recaculate（f,g,h）;
}while（!
hp.isempty（））//如果堆空则没有找到路径
if（path==find）
outpath;
//输出路径
else
cout<
<
"
pathnotfind;
!
endl;
A星算法应用在本次机器人制作需要以下几个工作：
1、地图的划分
A星算法需要知道地图的分布，因此合理将场地划分是执行程序的第一步。
考虑到现实情形，机器人有碰撞体积，并不能将场上所有的空地设置为活节点，可以设置机器人型心为质点，考虑该型心所能到达的地方，如果机器人型心超出此范围，则其边界有可能碰到货架或者场地边界，地图的分块是建立在此范围之内，下图显示了计算过机器人大小的型心运动区域分布图，其中阴影部分就是机器人的型心可以到达的区域。
之后在这个区域上划分小块，考虑到机器人绕障碍的精度要求，我们将可行区域划分为200X200MM的小区域，以区域中心作为节点所在位置，如下图所示，所画区域恰好分成诸多小块
2、节点的储存
为了避免不稳定性，我们设定机器人只能沿节点横向或纵向移动（G值不能为14），将每个节点与其邻居节点用结构体储存，结构体应该包括节点编号，节点死活，节点邻居指针，节点中心坐标等元素。
3、节点的定标
我们拟采用加速度计与陀螺仪判定机器人当前位移与身姿（详见电路部分）加速度的积分是速度，速度的积分是位移，只需知道水平面上的加速度，通过计算机的积分运算可以得到位移数据，只要知道机器人的具体坐标值，就可以判定机器人现在位置属于哪个具体的节点。
这对于绕障碍以及出现故障重新开始时重新定标起到帮助作用。
4、相邻节点间的移动
控制转向的节点结构体具体包含的几个变量如下：
typedefstruct{
floatx;
//该方块的x坐标
floaty;
//该方块的y坐标
floatG;
//该方块的G值
floatH;
//该方块的H值
rectR;
//该方块的右邻方块（right）
rectL;
//该方块的左邻方块（left）
rectT;
//该方块的上邻方块（top）
rectD;
//该方块的下邻方块（down）
intisLive;
//该方块的死活状态
IntN//该方块的编号
}*rect;
为了使机器人能够判断怎样走到相邻的方块（如应该先旋转多少角度后再移动），则机器人必须先知道自己现在的朝向（朝向可以通过之前走过的路径判断，如果出现故障，可以采用电子罗盘辅助寻找朝向，具体见电路部分）为此，我们将四个方向标记为不同的值以资区分，各个方向对应的值如下图：
设机器人当前朝向方向的值为direct，则机器人要走到R方块（右邻方块），则需要顺时针旋转的角度为：
angle=（direct%4-1）*（-90）%360；
下面为各个值对应的顺时针旋转角度：
若direct==1,则angle=0*（-90）%360=0；
若direct==2,则angle=1*（-90）%360=-90；
若direct==3,则angle=2*（-90）%360=-180；
若direct==4,则angle=（-1）*（-90）%360=90；
同理可以得到要走到T方块需要顺时针旋转的角度：
angle=（（direct%4-1）*（-90）-90）%360;
同理可以得到要走到L方块需要顺时针旋转的角度：
angle=（（direct%4-1）*（-90）-180）%360;
同理可以得到要走到D方块需要顺时针旋转的角度：
angle=（（direct%4-1）*（-90）+90）%360;
当角度转完后，再前行200mm即可到达下一节点
这样就可以从一个节点移动到它任意邻居节点。
5、卡位节点的删除
如果机器人在行进过程中偶遇障碍物，需要将障碍物所在处的节点删除并重新生成最优路径（绕障碍或者换路）。
首先发现障碍并且确定障碍位置和障碍所在节点（发现障碍的算法见程序部分的识别板块），之后并不能简单删除该节点，如果只删除单一节点有时并不能生成符合实际的最优路径。
如图，假设机器人处于蓝色方块的位置检测到障碍物的存在，如果仅删除一个节点，其余节点仍然可以行走，则机器人可能走到如图所示位置，会碰到障碍
因此为了保证行走的可行性与避障不碰障碍的要求，我们采用以下方法：
首先检测到障碍中心所在节点，其次删除其所有相邻节点，删除相邻节点时检测相邻节点的相邻节点（邻居的邻居），如果邻居邻居节点中心到障碍中心的横坐标（或纵坐标）之差的绝对值小于400mm，就将该节点删除，否则不删除。
改进以后的删除方式见下图
这样机器人若想到对面去只有一条路可以选择就是绕道（我们设定机器人转向只能原地旋转再前行）。
经多次测试，这种方法可以保证准确删除障碍节点，并完成绕路算法。
如果路被堵死的情况：
机器人前行检测到障碍并删除节点
机器人重新生成最优路径并转向。
6、商品的定位
商品的位置可以对应到相应最近的节点上，由于我们分块的时候已经照顾到商品的位置，故只需将机器人目的地设置在商品旁边的位置，这样也方便机器人接近货架200mm以内
如图绿色框图所在节点就是商品所在的节点，可以将商品编号与节点进行一一映射，从而让机器人方便的找到目的地。
综上，我们可以对基于A星算法的导购过程进行一个概述：
二、跟随部分：
跟随部分不再需要机器人自己寻找最优路径，而是只要跟着人走就可以了，因此也就不再需要使用划分的方块，考虑到前后的一致性，机器人按照如下图的路径行走比较合适（与导购阶段的路径相比只是将方块去掉了）：
跟随阶段有几个主要的问题需要解决：
1.识别人和障碍。
识别需要摄像头。
但由于导购阶段人跟随在机器人后方，因此机器人只需要通过超声波传感器测出与人之间的距离，就可以通过调整自己的速度来达到距离上的要求，这样来看机器人后方并没有安装摄像头的必要。
而跟随阶段机器人需要跟随着人行走，并且还有可能遇到障碍，因此机器人前方一定至少有一个摄像头，以识别人和障碍。
2.测量与人之间的距离以及与障碍之间的距离。
机器人通过超声波传感器测量与人之间的距离。
但是由于超声波传感器基本上只能测量与它正对的物体的距离，而机器人需要测量的人的位置不是固定的，因此在机器人上安装多个超声波传感器是必要的；
由于测量与障碍之间的距离是非常重要的（这涉及到导购阶段的挖掉死方块），为了保证尽可能准确地测出机器人与障碍之间的距离，我们通过双目识别来测量，这需要机器人前方有两个摄像头。
这些装置我们认为按下图（俯视图）放置比较合适。
其中前方的两个摄像头的高度在500mm（障碍的高度）以下，通过双目识别技术，这两个摄像头可以比较有效精准地识别圆柱体障碍物到机器人的距离。
由于人的中心位置不好测定，因此我们不打算也用双目识别来测量机器人与人之间的距离，而是通过超声波传感器直接测量。
由于测量人的上半身的距离比较容易，因此前后的超声波传感器都放置在机器人的顶部，这样可以达到人的腰部的高度；
由于安装在机器人前面（后面）的超声波传感器之间的距离为200mm，超声波传感器与机器人边缘的距离为150mm，而人的腰部宽度一般大于200mm，因此可以保证在跟随过程中只要人大致在机器人前方时，就能保证至少有一个超声波传感器是正对着前方的人的，如下图所示：
在导购和跟随过程中，只要人的中心位置在上图所示两条绿色虚线范围之内，机器人就能比较准确地测定与人之间的距离。
由于超声波传感器有一定的发射角（约15°
），因此图中所示范围与实际相比还要略小一些。
跟随阶段的大致流程图如下：
分类装袋与报价过程
由于已知商品分为食品类与生活类两种，机器人需要准确区分两种商品，并自主将其分类在不同的位置。
我们的机械结构设计留出了这种装置。
简单而言，通过已经储存在电脑中的商品位置来判断商品种类，通过带轮正反转来将放置在平台的商品转送到不同的袋子中。
导购阶段商品的位置是提前输入给电脑，而跟随阶段的商品由摄像机识别条形码，由电脑将条形码信息与商品信息对应并顺利完成计算与报价、分类等过程。
下简单介绍条形码的实现。
在跟随过程中，商品的识别我们决定采用条形码识别的方案，现今的商品上都贴有条形码标识，很容易通过商品上的条形码来判别商品的各项信息（类别，价格等）。
条形码（barcode）是将宽度不等的多个黑条和空白，按照一定的编码规则排列，用以表达一组信息的图形标识符。
常见的条形码是由反射率相差很大的黑条（简称条）和白条（简称空）排成的平行线图案。
条形码的扫描需要扫描器，扫描器利用自身光源照射条形码，再利用光电转换器接受反射的光线，将反射光线的明暗转换成数字信号。
不论是采取何种规则印制的条形码，都由静区、起始字符、数据字符与终止字符组成。
有些条码在数据字符与终止字符之间还有校验字符。
关于识别的部分，我们可以采用摄像头的拍照传输给计算机，然后通过特定的识别软件将分析得出的结果传给单片机，从而达到机器人识别商品的目的。
相对于手动输入，条形码识别主要有以下几点好处：
1．输入速度快：
与键盘输入相比，条形码输入的速度是键盘输入的5倍，并且能实现“即时数据输入”。
2．可靠性高：
键盘输入数据出错率为三百分之一，利用光学字符识别技术出错率为万分之一，而采用条形码技术误码率低于百万分之一。
机器人需要实现的另一项任务是计算总价并报价，我们计划用扩音器让机器人发声，机器人将每件已购产品储存并计算出总价值并通过电脑传输指令播放音频，通过扩音器发出声音。
该过程用简图表示如下
三、基于双目视觉的障碍识别系统
由之前的循迹算法可知，定位绕路等算法的核心在于如何准确识别障碍物所在的节点，我们考虑使用计算机视觉中的双目视觉来实现对障碍物的识别。
双目立体视觉，即由不同位置的两台或者一台摄像机（CCD）经过移动或旋转拍摄同一幅场景，通过计算空间点在两幅图像中的视差，获得该点的三维坐标值。
当一个摄像机拍摄图像时，由于图像中的像素点坐标相对于真实的世界坐标并不是唯一的，这就造成深度信息的丢失。
然而用两个摄像机同时拍摄图像时，可以获取同一场景的两幅不同的图像，通过三角测量原理计算图像像素间的位置偏差，复原三维世界坐标中的深度信息。
融合两只眼睛获得的图像并观察它们之间的差别，使我们可以获得明显的深度感，建立特征间的对应关系，将同一空间物理点在不同图像中的映像点对应起来，这个差别，我们称作视差（Disparity）图像，如图。
以下对双目视觉原理进行简单阐述：
立体视觉系统由左右两部摄像机组成。
如图所示，图中分别以下标l和r标注左、右摄像机的相应参数。
世界空间中一点A（X，Y，Z）在左右摄像机的成像面Cl和Cr上的像点分别为al（ul，vl）和ar（ur，vr）。
这两个像点是世界空间中同一个对象点A的像，称为“共轭点”。
知道了这两个共轭像点，分别作它们与各自相机的光心Ol和Or的连线，即投影线alOl和arOr，它们的交点即为世界空间中的对象点A（X，Y，Z）。
这就是立体视觉的基本原理。
在平行光轴的立体视觉系统中，左右两台摄像机的焦距及其它内部参数均相等，光轴与摄像机的成像平面垂直，两台摄像机的x轴重合，y轴相互平行，因此将左摄像机沿着其x轴方向平移一段距离b（称为基线baseline）后与右摄像机重合。
由空间点A及左右两摄像机的光心Ol、Or确定的极平面（Epipolarplane）分别与左右成像平面Cl、Cr的交线pl、pr为共轭极线对，它们分别与各自成像平面的坐标轴ul、ur平行且共线。
在这种理想的结构形式中，左右摄像机配置的几何关系最为简单，极线已具有很好的性质，为寻找对象点A在左右成像平面上的投影点al和ar之间的匹配关系提供了非常便利的条件。
将上述原理应用于计算机识别还需要进行特征点匹配，特征点匹配分为稀疏点匹配和密集点匹配，由于本次任务对识别对象形状要求并不高（红色圆柱体），并且只需要定出圆柱体型心所在的坐标值，故只需采用基于窗口的稀疏点匹配。
下面将结合圆柱实物简单介绍特征匹配的方法并定出型心。
实验所用的圆柱
对上述识别图像进行轮廓提取
由于图形轮廓具有很好的连续性，在一些弱角点的地方无法提取到角点却可以提取到轮阔点。
提取到特征点以后，以左摄像机的图像为参考对象，对右图进行匹配，用极点坐标法中的向量夹角匹配的方法计算两图同一行特征点的cos值，将取值最大且大于0.9的特征点作为对应匹配点，两个点x坐标差值为视差值，视差越大则三维点离摄像机距离越近，反之越远。
上式中的最大视差
，最小视差
，则视差图中的灰度值
就可以帮助计算出视点离摄像头的距离。
在获取多个视点之后，就是圆柱型心的判定。
上面通过灰度值算出了圆柱边缘到摄像机的距离，为了简化起见，我们现将机器人视为质点，计算出圆柱的位置，如下图：
设测出的机器人到圆柱边缘的距离为d1，圆柱半径为r（已知量），设圆心到摄像机的距离为x，则由勾股定理容易得到：
x^2=（d1^2-r^2）,又因为h为已知量，则圆心到摄像机的水平距离s=x^2+h^2=（d1^2+h^2-r^2）^0.5，摄像机的位置是已知的，则圆柱圆心的坐标也就很容易得到，为（x摄-s,y摄+h）。
升级会员 