一种激光与二维码融合的AGV精确定位方法与流程

本发明涉及移动机器人小车技术领域，尤其是涉及一种基于二维码的激光导航agv精确定位方法。

背景技术：

自动引导车(automatedguidedvehicle,agv)是指配备有多种传感器装置，如激光雷达、深度相机和二维码相机等，能够在不需要人工引导的情况下，将货物从a点送至b点的搬运车。

目前国内大多数工厂生产车间内的物料搬运都是由人工完成的。人工搬运存在着工作强度大、效率低、安全隐患和人工成本越来越高等问题。agv具有多个安全保护等级，只需在前期完成建图和设定目标点等工作，后续结合多机调度系统或仓储管理系统，即可实现自动无人化车间。然而，为了确保不同的环境下，每次搬运的百分百成功，agv需要更高的停车精度。申请人经过市场调研了解，目前任何一家激光无反光板的agv停车精度都不能达到毫米级。为了使agv停车精度提高到毫米级，一般的做法都是使用二次定位技术。

在现有技术中，可以将辅助二次定位功能分为两大类：(1)二次定位装置，中国专利公布号cn203299650u，公布日2013.11.20中公开了一种智能二次精准定位装置，通过在相应的工位上安装信号标识面板，在agv上安装扫描面板，可以达到毫米级或微米级的精度；(2)使用二维码带、色带或磁条来辅助，这类方法虽然能够提供更多的位置信息且成本低廉，但agv最终只能停靠在码带、色带或磁条上，换句话说，料架必须要精准地放在码带、色带或磁条的正上方。中国专利公布号109669455a，公布日2019.4.23中公开了一种基于二维码的agv定位系统，该方法安装了三个二维码相机，分别读取两侧的控制码和中间的位置码，可以实现二维码导航。但该方法类似于磁条导航，对码带贴制的平行度要求较高，且在二次定位中，需要三个二维码相机，成本较高。

技术实现要素：

有鉴于此，本发明的目的是解决现有技术的问题，提供可实现agv停车精度达到毫米级，满足工业需求，且agv不需要额外的二次定位装置，结构更简单，成本更低的一种融合激光与二维码导航的agv精确定位方法。

为了达到上述目的，本发明采用如下技术方案：

一种激光与二维码融合的agv精确定位方法，所述agv包括具有激光导航装置的agv车体，所述agv车体包括伺服驱动器和驱动左右车轮的伺服电机，所述agv车体上还设置有二维码相机，所述agv精确定位方法包括以下步骤：

s1、确定料架的位置，以料架中心为起点，在向前方或后方平移一段固定距离的地面位置，贴上二维码标签；此时，在二维码坐标系下，已知agv和所述料架的位姿；

s2、开始激光导航，伺服驱动器以速度模式控制伺服电机使agv从起点出发，激光导航的目标点为所述二维码标签在地图坐标系下的位姿；

s3、当agv快行驶到激光导航终点时减速，所述二维码相机读取到二维码标签，获取agv在二维码坐标下的位姿偏差，立即进入二次定位步骤，此时不再使用全局坐标，使用二维码坐标系下agv的相对坐标；

s4、在二维码坐标系下，已知agv位姿和料架位姿，此时以agv为起点，以料架为终点，规划出一条曲线路径；

s5、在所述曲线路径上采样取点，相邻两点之间的弧长用一定曲率半径的圆弧来逼近，通过两轮差速运动模型计算出左右车轮所需行进的距离，然后计算出用于驱动左右车轮的伺服电机的对应的脉冲数，并存入驱动器指令序列中，伺服驱动器的速度模式切换为位置模式；

s6、把上述驱动器指令序列中的脉冲数下发给伺服驱动器，伺服电机执行相应的脉冲；

s7、使agv按照驱动器指令序列的脉冲数据行驶，直至驱动器指令序列中的脉冲数据为空，表示agv已经行驶到位，将驱动器模式切换回速度模式，agv根据规划的路径行驶至料架的位置。

其中，所述二维码相机安装在所述agv车体的正中心处，二维码相机坐标系与agv车体坐标系一致，定义所述agv行驶的前进方向为x正方向，左侧为y正方向，agv车体逆时针转向时角度为正。

优选地，所述二维码标签应贴制在激光导航的终点之前，且y方向的偏离值不超过±80mm，所述二维码相机距离地面的二维码标签的垂直距离为100mm。如果二维码标签贴放没有一致，通过简单的坐标转换即可。保证小车在二维码标签上前后运动时x值在变化，车头与二维码标签一致时，返回0°，且左转时返回角度为正值。

优选地，伺服驱动器和伺服电机必须包含速度模式和位置模式。所述速度模式是指给伺服驱动器下发速度指令序列，伺服电机不断响应该速度，直到速度为零，伺服电机停止，单位为每分钟多少转(rpm)；所述位置模式是指给伺服驱动器下发脉冲指令序列，伺服电机根据指定的脉冲位置立马进行响应，直到到位，伺服电机停止，单位为脉冲(pulse)。

在一些具体实施例中，所述步骤s4中，通过三次函数曲线y＝ax³+bx²+cx+d规划曲线路径，其中x为x方向的坐标，y为y方向的坐标，起点坐标为(x0,y0)，通过方程组求解得到四个系数a，b，c，d：

方程1：起点(x0,y0)在曲线上，因此ystart＝f(xstart)；

方程2：起点的角度是函数在该点的斜率，即f^′(xstart)＝tan(θstart)；

方程3：终点处有f(xend)＝0；

方程4：终点处斜率有f^′(xend)＝0。

优选地，所述步骤s5中，两轮差速运动模型具体为：定义两轮间距为l，轮轴所在方向为x轴，垂直于x轴的前进方向为y轴，agv行驶的方向角度为α，agv车体在曲线路径上相邻两点位姿为(x,y,α)和(x′,y′,α′)，转向角度θ，转弯半径为r，则相邻两点之间的位移θ＝α′-α，因此左轮行走的弧长为ll＝(r-l/2)θ，右轮行走的弧长为lr＝(r+l/2)θ，将其单位转化为脉冲，驱动左轮行走的脉冲pl＝llm，驱动右轮行走的脉冲pr＝lrm，其中其中red为伺服电机减速比，res为伺服电机一圈的脉冲数，d为轮子直径；在曲线路径上采样取点，分别计算pl和pr并存入脉冲指令序列中。

本发明的通过agv上的二维码相机读到二维码后，精准获取到agv在二维码坐标下的位姿偏差δx、δy和δθ，规划出一条平滑的曲线路径，在路径上采样一系列的位姿点，由相邻两个位姿点，计算出左右轮分别行进的弧长，进而转换成左右轮的脉冲，下发给伺服电机驱动器执行，小车会跟随提前规划的路径，平稳地走到指定的目标点。

由于采用上述技术方案，本发明不依赖于额外的二次定位装置，使用单个二维码标签，利用伺服电机的脉冲控制，实现agv高精度的二次定位功能。相较于现有技术，本发明达到以下有益效果：

1、通过规划曲线路径，同时纠偏agv位姿偏差δx、δy和δθ，二次定位阶段一步到位，定位过程步骤。

2、不需要二次定位机构装置，对地面的改造较小，仅需在地面贴制一张二维码标签，agv上仅设置一个二维码相机，结构简单，成本较低。

3、合理使用伺服电机的高精度特性，能够实现毫米级精度的二次定位功能。

附图说明

图1为本发明中的agv侧视示意图；

图2为本发明中基于二维码导航阶段的示意图；

图3为本发明中的两轮差速运动模型图；

图4为本发明中的二次定位流程示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

如图1所示，本实施例的一种激光与二维码融合的agv精确定位方法，应用于一种潜伏顶升式agv，该agv包括具有激光导航装置的agv车体1，agv车体包括伺服驱动器和驱动左右车轮的伺服电机，agv车体1上还设置有二维码相机2，二维码相机2安装在agv车体正中心并朝向下方地面。二维码相机坐标系与agv车体坐标系一致，定义所述agv行驶的前进方向为x正方向，左侧为y正方向，agv车体逆时针转向时角度为正。

其中，伺服驱动器和伺服电机必须包含速度模式和位置模式。所述速度模式是指给伺服驱动器下发速度指令序列，伺服电机不断响应该速度，直到速度为零，伺服电机停止，单位为每分钟多少转(rpm)；所述位置模式是指给伺服驱动器下发脉冲指令序列，伺服电机根据指定的脉冲位置立马进行响应，直到到位，伺服电机停止，单位为脉冲(pulse)。

本发明通过在agv激光导航终点附近贴制二维码标签，以二维码坐标系下agv作为起点，料架作为终点，规划出一条可行的曲线路径，使agv平滑地走到指定终点的位置。基于伺服电机的高精度，最终的停车精度可以达到毫米级，满足工业需求。如图2-4所示，基于二维码和开环控制的agv二次定位方法具体通过以下步骤实现：

s1、确定料架的位置，以料架中心为起点，在向前方或后方平移500mm(料架左右两侧的间距一般小于agv车宽，因此agv无法从料架的两侧进入)的地面位置，贴上二维码标签。二维码标签与料架之间的距离设定在500-1000是比较合适的。本发明所用agv的轮子直径d为150mm，编码器线数为2500线，减速比red为1：25，因此，电机转一圈为10000pulse，轮子转一圈为250000pulse，1pulse＝0.00188mm。此时，在二维码坐标系下，agv车体的位姿是已知的。优选地，二维码标签应贴制在激光导航的终点之前，且y方向的偏离值不超过±80mm。如果二维码标签贴放没有一致，通过简单的坐标转换即可。保证小车在二维码标签上前后运动时x值在变化，车头与二维码标签一致时，返回0°，且左转时返回角度为正值。

s2、开始激光导航，伺服驱动器以速度模式控制伺服电机使agv从起点出发，激光导航的目标点为所述二维码标签在地图坐标系下的位姿。当二维码相机2的安装高度为100mm时，x和y可以读到的范围为[-60,60]mm左右，因此二维码标签只要贴在激光导航停车精度为±60mm误差内就行。一般激光导航的精度可以达到±25mm，因此二维码标签贴的位置要求并非十分严格。

s3、当agv快行驶到激光导航终点时减速，所述二维码相机读取到二维码标签，获取agv在二维码坐标下的位姿偏差，立即进入二次定位步骤，如图4所示，此时不再使用全局坐标，使用二维码坐标系下agv的相对坐标。

s4、在二维码坐标系下，已知agv位姿和料架位姿，此时以agv为起点，以料架为终点，规划出一条曲线路径，如图2所示。在二维码坐标下，已知小车位姿和料架位姿，可以有不同的路径规划方法，本实施例通过三次函数曲线y＝ax³+bx²+cx+d规划曲线路径，其中x为x方向的坐标，y为y方向的坐标，起点坐标为(x0,y0)，需要四个方程：方程1：起点(x0,y0)在曲线上，因此ystart＝f(xstart)；方程2：起点的角度是函数在该点的斜率，即f^′(xstart)＝tan(θstart)；方程3：终点处有f(xend)＝0；方程4：终点处斜率有f^′(xend)＝0。求解该方程组得到四个系数a，b，c，d。

s5、在所述曲线路径上采样取点，相邻两点之间的弧长用一定曲率半径的圆弧来逼近，通过两轮差速运动模型计算出左右车轮所需行进的距离，然后计算出用于驱动左右车轮的伺服电机的对应的脉冲数，并存入驱动器指令序列中，伺服驱动器的速度模式切换为位置模式。如图3所示，两轮差速运动模型具体为：定义两轮间距为l，轮轴所在方向为x轴，垂直于x轴的前进方向为y轴，agv行驶的方向角度为α，agv车体在曲线路径上相邻两点位姿为(x,y,α)和(x′,y′,α′)，转向角度θ，转弯半径为r，则相邻两点之间的位移θ＝α′-α，因此左轮行走的弧长为ll＝(r-l/2)θ，右轮行走的弧长为lr＝(r+l/2)θ，将其单位转化为脉冲，驱动左轮行走的脉冲pl＝llm，驱动右轮行走的脉冲pr＝lrm，其中其中red为伺服电机减速比，res为伺服电机一圈的脉冲数，d为轮子直径；在曲线路径上采样取点，分别计算pl和pr并存入脉冲指令序列中。

s6、把上述驱动器指令序列中的脉冲数下发给伺服驱动器，伺服电机执行相应的脉冲。

s7、使agv按照驱动器指令序列的脉冲数据行驶，直至驱动器指令序列中的脉冲数据为空，表示agv已经行驶到位，将驱动器模式切换回速度模式，agv根据规划的路径行驶至料架的位置。

本实施例的agv精确定位方法解决了以下问题：

1、在不需要额外辅助装置的情况下，用二次定位解决一些定位精度要求高的应用场景；

2、使用单个二维码，贴制方便，解决码带或色带等复杂铺设，并解决使用码带或色带时料架必须放置在二维码的正上方的问题；

3、利用曲线规划路径，二次定位一步到位，结合伺服电机的高精度脉冲控制，实现最终的毫米级停车精度。

综上所述，本发明有效融合了激光导航和基于二维码导航技术，合理使用伺服电机的高精度特性，实现了毫米级的二次定位功能，可以广泛应用于需要高停车精度的应用场景，适用于所有agv车型。利用图1中的agv测试，本发明最终的停车精度在±5mm以内，完全满足工业需求。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。