CN103268116B - 基于复合型磁钉校正的agv纠偏控制方法 - Google Patents

Abstract

本发明涉及一种基于复合型磁钉校正的AGV纠偏控制系统及方法，该系统包括在每个磁钉的附近敷设有与磁钉相对应的RFID射频标签，该AGV小车包括AGV车体、安装在AGV车体前后两侧和左右两侧的电磁栅尺和RFID读卡单元，在AGV车体底部安装有与电磁栅尺和RFID读卡单元相连接的主控制器以对AGV小车进行纠偏控制；该方法包括以下步骤：1、主控制器通过电磁栅尺、RFID读卡单元获取RFID射频标签信号；2、计算得到AGV小车当前的位姿偏差；3、根据AGV小车当前的位姿偏差通过AGV驱动模块对AGV小车进行纠偏控制。本发明自动采集磁钉附近的RFID射频标签以得到磁钉的绝对值坐标，有效地解决了传统方法不能同步获取磁钉绝对坐标值的难题，大幅度提高了工作效率。

Description

基于复合型磁钉校正的AGV纠偏控制方法

技术领域

本发明属于AGV技术系统，尤其是一种基于复合型磁钉校正的AGV纠偏控制方法。

背景技术

在AGV控制过程中，电磁栅尺和磁钉可以用来对AGV的行驶过程进行校正。其实现方法是：在路段上每相隔一段距离设置一对间隔固定的磁钉，由于磁钉不具有方向性，无法对AGV的姿态进行测定，因此，需要采用双磁钉、双磁尺或双磁钉、单磁尺的方法来实现，即采用平行布设两个磁钉的方法，AGV在行驶时，用两把电磁尺去检测按要求埋设在路径上的两颗磁钉相对于AGV的位置，再根据检测到的数据进行“测量计算”，计算出AGV的当前位置和方向，进而就可以按照预先设定的程序确定AGV行驶到下一个测量点所要执行的动作。

AGV双磁钉导引技术具有计算简便、结构简单、运行可靠、使用方便等特点，但是，其存在的问题是：由于磁钉内不能含有信息，磁钉的绝对值坐标必须事先标定，对于磁钉坐标标定方法一般是采用示教方法由人工运行AGV对所有磁钉进行一次人工扫描，即由AGV自身的定位系统对磁钉进行测量和标定，如论文《惯性导航自动引导车磁钉校正路径迭代学习方法》中所述：当AGV通过磁钉CJ时,定位系统测得的AGV车体位姿为P_I(X_I,Y_I,θ_I)，电磁栅尺测得AGV中心到磁钉的距离为c，磁钉CJ的坐标为：XC_J＝X_I-c*SINθ_I，YC_J＝Y_I-c*COSθ_I，这样经过示教扫描后可得到每一个磁钉的坐标及每一对磁钉的连线与XOY绝对坐标系的夹角θ。上述人工示教方法对于大面积的磁钉敷显然设费时费力，实际应用意义不大。

发明内容

本发明的目的在于克服现有技术的不足，提供一种设计合理、效率高且实现简便的基于复合型磁钉校正的AGV纠偏控制方法。

本发明解决其技术问题是采取以下技术方案实现的：

一种基于复合型磁钉校正的AGV纠偏控制方法，是在如下系统实现的：该系统包括AGV小车、在地面上敷设有磁钉,在每个磁钉的附近敷设有与磁钉相对应的RFID射频标签，所述的AGV小车包括AGV车体、安装在AGV车体前后两侧和左右两侧的电磁栅尺和RFID读卡单元，在每个电磁栅尺上均布安装有传感器，在AGV车体底部安装有与电磁栅尺和RFID读卡单元相连接的主控制器以对AGV小车进行纠偏控制；包括以下步骤：

步骤1：主控制器通过电磁栅尺、RFID读卡单元获取RFID射频标签信号，具体方法为：电磁栅尺和RFID读卡单元为分时操作：当电磁栅尺采集到磁钉信号后发送给微处理器，微处理器通知RFID读卡单元，RFID读卡单元采集地面上的AGV小车周围的RFID射频标签信号并发送给微处理器，该RFID射频标签信号为磁钉的绝对坐标；

步骤2：主控制器对获取的RFID射频标签信号进行计算得到AGV小车当前的位姿偏差；

步骤3：主控制器根据AGV小车当前的位姿偏差通过AGV驱动模块对AGV小车进行纠偏控制。

而且，所述步骤2的具体方法包括以下步骤：

⑴建立磁钉坐标系、小车坐标系、世界坐标系；

⑵计算AGV小车在小车坐标系下相对于磁钉坐标系的偏离位置和偏离角度；

⑶计算AGV小车在世界坐标系下的偏离角度以及AGV小车在世界坐标系下相对于磁钉坐标系的偏离位置；

⑷计算AGV小车在世界坐标系的坐标。

而且，所述步骤⑶计算AGV小车在世界坐标系下的偏离角度为：分别计算AGV小车相对于磁钉坐标系的偏离角度θ1、磁钉坐标系相对于世界坐标系的偏离角度θ2，再将偏离角度θ1和偏离角度θ2相加便得到计算AGV小车在世界坐标系下的偏离角度θ。

而且，所述步骤⑷计算AGV小车在世界坐标系的坐标的方法为：通过AGV小车在世界坐标系下的偏离角度θ并利用三角函数关系，分别计算得到小车在世界坐标系下相对于磁钉坐标系的X轴偏离位置和小车在世界坐标系下相对于磁钉坐标系的Y轴偏离位置。

本发明的优点和积极效果是：

本发明通过在地面上敷设磁钉和对应的RFID射频标签、在AGV车体上安装电磁栅尺和对应的RFID读卡单元，通过微处理器分时采集磁钉和RFID射频标签数据并进行分析处理，即采用自带绝对值坐标的复合型磁钉的方法读取磁钉绝对值坐标，有效地解决了用电磁栅尺和磁钉校正AGV时不能同步获取磁钉绝对坐标值的难题，大幅度提高了工作效率。

附图说明

图1为本发明的系统的结构示意图；

图2为电磁栅尺和RFID读卡单元的安装示意图；

图3为主控制器的电路方框图；

图4为电磁栅尺上传感器分布示意图；

图5为图2的侧视图；

图6为本发明的磁场强度示意图；

图7为本发明计算AGV在世界坐标系下的坐标和偏离角度的示意图。

具体实施方式

以下结合附图对本发明实施例做进一步详述：

一种基于复合型磁钉校正的AGV纠偏控制系统，如图1及图2所示，包括AGV小车、在AGV小车行驶的地面上敷设有磁钉，在每个磁钉的附近敷设有与磁钉相对应的RFID射频标签，每对磁钉和RFID射频标签组合在一起构成自带绝对值坐标的复合型磁钉，AGV小车包括AGV车体4、安装在AGV车体前后两侧和左右两侧的电磁栅尺1、5和RFID读卡单元3、7，在每个电磁栅尺上分别均布安装有霍尔传感器2、6，电磁栅尺和RFID读卡单元分别用于读取地面上的磁钉和RFID射频标签，在AGV车体底部还安装有用于处理磁钉信号和RFID射频标签信号的主控制器8。如图3所示，主控制器包括微处理器和AGV驱动模块，微处理器分别与四个电磁栅尺和四个RFID读卡单元相连接，电磁栅尺首先采集信号，采集后由微处理器通知RFID读卡单元，RFID读卡单元接到微处理器通知后开始采集地面上的RFID射频标签信号并发送给微处理器，微处理器对RFID读卡单元采集的射频标签信号进行分析计算得到AGV小车当前的位姿偏差(偏离位置和偏离角度)，微处理器通过位姿偏差控制AGV驱动模块对AGV进行纠偏处理。下面对系统中的各个部分详细进行说明：

在AGV行驶的地面上敷设磁钉及RFID射频标签(复合型磁钉)的方法是：围绕AGV车体的尺寸在长度方向或宽度方向分别布设2个磁钉，每2个磁钉组成一对，设AGV车体长为L、宽为W，长度方向两个磁钉的间距为AGV车体的长度L+2l,其中l为磁钉中心点和该方向AGV车体的间隙，在本实施例中l设为1厘米；宽度方向上的两个磁钉的间距为AGV车体的宽度W+2l，其中l也为磁钉中心点和该方向AGV车体的间隙，在本实施例中l也设为1厘米。在每一个磁钉的旁边是与之对应的RFID射频标签，RFID射频标签敷设位置应是RFID读卡单元可以读取的范围。RFID射频标签内含唯一ID号，每一个唯一ID号分别对应一个绝对坐标值，该绝对坐标值用于唯一标识磁钉的绝对位置(在世界坐标系的坐标)。

安装在AGV车体四周的电磁栅尺为矩阵式分布的电磁栅尺，如图4所示，电磁栅尺包括至少2排水平矩阵分布的传感器，第一排传感器用于首次采集信号，第二排传感器用于补充采集漏采集的信号。本实施例采用霍尔传感器矩阵分布的方式，霍尔传感器能够抗地磁干扰(地面磁场平均达到每平方米0.5GS，霍尔传感器的变化量每GS变化1mv-2.1mv，磁尺位于磁钉正上方4cm处变化量可达到100MV以上，远远大于地球磁场引起的变化量)，但检测范围相对比较窄(实验测得：霍尔传感器水平方向有效检测范围为从中心点算起外延2厘米的半径范围，垂直方向有效检测范围为4厘米)，本实施例兼顾霍尔传感器的优、缺点，采用霍尔传感器矩阵分布的方式，既克服了地磁干扰，又解决了检测精度问题。根据传感器的检测范围确定分布方式：采用直径为5毫米的霍尔传感器，两个霍尔传感器横向间隙或前后两排分布的间隙都为5毫米、两个传感器中心点距离为1厘米，两个传感器总长度为1.5厘米，假设电磁栅尺的长度为车宽的一半或车长的一半(1米或2米)，按照2个霍尔传感器占用1.5CM长度计算，需要67个传感器(1.05米)；本实施例中，AGV车体前后两端的电磁栅尺用于测量AGV车体沿着车头方向行进的偏离位置和角度，AGV车体左右两端的电磁栅尺用于测量车头不转AGV车体横向移动时的偏离位置和角度，其原理是基于在先专利申请(专利名称为：一种用于AGV的紧凑式万向运动机构及AGV运输车，申请号为：201110449887.9)的AGV运输车，该AGV车体由紧凑式万向运动机构组成，能够实现车身不动，车轮转动，从而实现在车头不转的情况下通过扭转车轮的角度而改变AGV车体行进的方向。在图5中，电磁栅尺底端和AGV车体底部齐平或略高于AGV车体底部，距离地面约4厘米(在传感器垂直方向有效读取范围内)，电磁栅尺高度约为5厘米，RFID读卡单元距离AGV车体1厘米间隙。

如图6所示，本发明的纠偏控制方法基于如下原理实现：AGV车体上的电磁栅尺距离磁钉越近，则磁场强度越大，当电磁栅尺位于磁钉的正上方时为最佳测量时机。AGV车体四周的传感器可同时感应到四个最大的磁场强度，并且AGV主控制器能够同时(微观上分时，宏观上同时)输出小车的偏离位置和偏离角度。AGV主控制器输出小车偏离位姿的原理为：

⑴采用双排传感器增加了数据采集的可靠性：第一排传感器未能检测到磁钉信号，用第二排传感器进行补充采集。

⑵小车通过1个磁钉的直径需要的时间：已知车速为3毫米/毫秒，磁钉直径为5毫米，小车需要1.6毫秒走完磁钉5毫米的路程。

⑶主控制器完成一轮采集和处理需要的时间：从微处理器采集信号到主控制器接收采集的信号、对采集的信号进行处理和输出(输出小车偏离位姿)为1个周期，如果采用AD方式采集信号，1个周期为1微秒，分别采集67个传感器需要67微秒。

⑷主控制器在1.6毫秒(走完磁钉5毫米直径的路程)内可重复采集的次数。由于微处理器采集67个传感器所需要时间为1微秒，1毫秒＝1000微秒，用1000/67，约为15次，也就是说主控制器在1.6毫秒的时间可以重复采集信号15次。

从以上结论可以看出，当电磁栅尺突然感受到磁场的变化到磁场变化突然减弱的时间应该是1.6毫秒的时间，而在1.6毫秒的时间内，主控制器可以连续采集信号15个轮回，也就是说，AGV主控制器在突然感受到磁场增强以及突然感受到磁场减弱的瞬间能够针对同样的环境作出15次同样结果的重复输出。

本发明的纠偏控制方法仅仅是计算测量沿着小车电磁栅尺路经上电磁栅尺上中心点到磁钉的位置偏离，假设电磁栅尺中心点和磁钉没有纵向偏差，即计算位置偏差的时机是电磁栅尺刚好到达磁钉的正上方，则不存在纵向偏差。

本发明的纠偏控制方法设计思想为：

⑴目标：最终目的是给出机器人一个随时随地在世界坐标系的(X、Y、θ)，解决方法是通过获得两个中心点的绝对值坐标(世界坐标系坐标)，再进行简单的加减法计算。两个中心点包括双磁钉连线的中心点和小车坐标系的中心点坐标。小车坐标系的中心点坐标是动态的，双磁钉连线是静态的，可以通过计算得到。

⑵获取小车坐标系的中心点绝对值坐标：用小车距离磁钉的位置偏差和角度偏差计算AGV小车中心点的绝对值坐标。

⑶小车距离磁钉的位置偏差从电磁栅尺获得(磁钉最大值距离电磁栅尺中心点的距离)，小车偏离磁钉连线的角度偏差通过三角函数获得。

本发明的基于复合型磁钉校正的AGV纠偏控制方法包括以下步骤：

步骤1：主控制器通过电磁栅尺、RFID读卡单元获取RFID射频标签信号。其具体步骤为：

电磁栅尺和RFID读卡单元为分时操作，用以防止电磁尺受到RFID电磁波的干扰：首先，电磁栅尺采集到磁钉信号后发送给微处理器，然后，微处理器通知RFID读卡单元，最后，RFID读卡单元开始采集地面上的RFID射频标签信号(四个磁钉坐标)并发送给微处理器。

步骤2：主控制器对获取的RFID射频标签信号进行计算得到AGV小车当前的位姿偏差，位姿偏差包括偏离位置和偏离角度；

本步骤是基于复合型磁钉校正的AGV纠偏控制方法的核心内容，包括以下内容：

⑴建立磁钉坐标系(X_钉、Y_钉)、小车坐标系(x_小车、y_小车)、世界坐标系(x_世界、y_世界)，如图7所示。分别说明如下：

①建立磁钉坐标系(X_钉、Y_钉)：

建立互相垂直的二条双磁钉连线并求中心点坐标b(X_中，Y_中)。D1(X1_钉、Y1_钉)和D2(X2_钉、Y2_钉)连线的长度为小车的长度，D3(X3_钉、Y3_钉)和D4(X4_钉、Y4_钉)连线的长度为小车的宽度，它们互相垂直，b为它们的交点，也是中心点,四个磁钉的坐标可以从四个相应的RFID射频标签读取到。设双磁钉连线中心点b坐标为：b(X_中，Y_中)，由于双磁钉连线中心点到四个顶点的距离相等：

X1_钉-X_中＝X_中-X3_钉；X4_钉-X_中＝X_中-X2_钉；

X1_钉+X3_钉＝2X_中；X4_钉+X2_钉＝2X_中；

因此，X1_钉+X3_钉+X4_钉+X2_钉＝4X_中；

X_中＝(X1_钉+X3_钉+X4_钉+X2_钉)/4；

同理:Y_中＝(Y1_钉+Y3_钉+Y4_钉+Y2_钉)/4。

②建立小车坐标系(X_小车、Y_小车)：在小车长度方向和宽度方向分别作两条相互垂直的中心线，a为小车坐标系的原点。

③建立世界坐标系(x_世界、y_世界)：即绝对值坐标系，Y_世界，O_世界，X_世界。

⑵计算AGV小车在小车坐标系下相对于磁钉坐标系的偏离位置和偏离角度。

其计算方法为：

①计算偏离位置；小车磁尺中心点相对于磁钉D2偏离距离为e；

②计算偏离角度：设两个磁钉D1、D2之间的距离为d，磁钉D2偏离磁尺中心的距离为e,所以；小车偏角θ1＝arcsin(e/d)。

⑶计算小车在世界坐标系下的偏离角度(θ)和AGV小车在世界坐标系下相对于磁钉坐标系的偏离位置(bc、ca)；

计算方法概述：①小车在世界坐标系下的偏离角度(θ)分以下步骤完成，先分别计算AGV小车相对于磁钉坐标系的偏离角度θ1、磁钉坐标系相对于世界坐标系的偏离角度θ2，再将已知θ1和θ2相加，最后得到θ，即：θ＝θ1+θ2；②通过已经计算的θ，进而计算AGV小车在世界坐标系下相对于双磁钉连线中心点的偏离位置bc、ca。

具体步骤为：

①计算θ1：从前面的计算结果已知小车偏角θ1＝arcsin(e/d)；

②计算θ2：θ2为D3(X_磁钉、Y_磁钉)、D4(X_磁钉、Y_磁钉)的连线相对于世界坐标系的偏离角度：

tanθ2＝(YD4_磁钉-YD3_磁钉)/(XD4_磁钉-XD3_磁钉)；

其中YD4_磁钉、YD3_磁钉、XD4_磁钉、XD3_磁钉为D3、D4磁钉的绝对值坐标；

③计算AGV小车在世界坐标系下的偏离角度θ：θ＝θ1+θ2；

④计算AGV小车在世界坐标系下相对于磁钉坐标系的偏离位置bc、ca；

计算方法概述：首先计算三角型abc中∠abc的值，以及线段ab的长度，最后通过三角函数计算出bc、ca。

具体步骤为：

①作一条射线O_世界A2，该射线平行于小车倾斜于世界坐标系的方向(小车左右两侧电磁栅尺中心点的连线与世界坐标系X轴的夹角)，则A2与世界坐标系X_世轴的夹角为θ，已知：θ＝θ1+θ2；

②在三角型abc中，由于线段bc平行于世界坐标系的X_世轴，线段ab平行于O_世A2，

因此，∠abc＝θ；

③计算ab的长度。ab为两个中心点之间的连线，a为小车坐标系的中心点，b为两对磁钉连线的中心点，由于θ1已知，tanθ1＝ab/(d/2),因此，ab＝2tanθ1/d；

④根据已经求出的θ和ab求出bc、ca

bc＝COSθ*ab；bc为小车在世界坐标系下相对于磁钉坐标系的X轴偏离位置；

ca＝SINθ*ab；ca为小车在世界坐标系下相对于磁钉坐标系的Y轴偏离位置。

⑷计算AGV小车相在世界坐标系的坐标a(X0、Y0)。

计算方法为：由于b(X_中、Y_中)为磁钉坐标系中心点绝对坐标，通过四个磁钉的绝对坐标能够计算出磁钉坐标系中心点b(X_中、Y_中)的坐标；因此，X0＝X_中+bc；Y0＝Y_中+ac,小车相在世界坐标系的坐标:

a(X0、Y0)＝a(X_中+bc、Y0＝Y_中+ac)；X_中＝(X1_钉+X3_钉+X4_钉+X2_钉)/4；Y_中＝(Y1_钉+Y3_钉+Y4_钉+Y2_钉)/4。

步骤3：主控制器根据AGV小车当前的位姿偏差通过AGV驱动模块对AGV小车进行纠偏控制。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (4)

1.一种基于复合型磁钉校正的AGV纠偏控制方法，是在如下系统实现的：该系统包括AGV小车、在地面上敷设有磁钉,在每个磁钉的附近敷设有与磁钉相对应的RFID射频标签，所述的AGV小车包括AGV车体、安装在AGV车体前后两侧和左右两侧的电磁栅尺和RFID读卡单元，在每个电磁栅尺上均布安装有传感器，在AGV车体底部安装有与电磁栅尺和RFID读卡单元相连接的主控制器以对AGV小车进行纠偏控制；其特征在于：包括以下步骤：

步骤1：主控制器通过电磁栅尺、RFID读卡单元获取RFID射频标签信号，具体方法为：电磁栅尺和RFID读卡单元为分时操作：当电磁栅尺采集到磁钉信号后发送给微处理器，微处理器通知RFID读卡单元，RFID读卡单元采集地面上的AGV小车周围的RFID射频标签信号并发送给微处理器，该RFID射频标签信号为磁钉的绝对坐标；

步骤2：主控制器对获取的RFID射频标签信号进行计算得到AGV小车当前的位姿偏差；

步骤3：主控制器根据AGV小车当前的位姿偏差通过AGV驱动模块对AGV小车进行纠偏控制。

2.根据权利要求1所述的基于复合型磁钉校正的AGV纠偏控制方法，其特征在于：所述步骤2的具体方法包括以下步骤：

⑴建立磁钉坐标系、小车坐标系、世界坐标系；

⑵计算AGV小车在小车坐标系下相对于磁钉坐标系的偏离位置和偏离角度；

⑶计算AGV小车在世界坐标系下的偏离角度以及AGV小车在世界坐标系下相对于磁钉坐标系的偏离位置；

⑷计算AGV小车在世界坐标系的坐标。

3.根据权利要求2所述的基于复合型磁钉校正的AGV纠偏控制方法，其特征在于：所述步骤⑶计算AGV小车在世界坐标系下的偏离角度为：分别计算AGV小车相对于磁钉坐标系的偏离角度θ1、磁钉坐标系相对于世界坐标系的偏离角度θ2，再将偏离角度θ1和偏离角度θ2相加便得到计算AGV小车在世界坐标系下的偏离角度θ。

4.根据权利要求2所述的基于复合型磁钉校正的AGV纠偏控制方法，其特征在于：所述步骤⑷计算AGV小车在世界坐标系的坐标的方法为：通过AGV小车在世界坐标系下的偏离角度θ并利用三角函数关系，分别计算得到小车在世界坐标系下相对于磁钉坐标系的X轴偏离位置和小车在世界坐标系下相对于磁钉坐标系的Y轴偏离位置。