一种基于二维码定位导航的AGV控制系统及其控制方法

一种基于二维码定位导航的agv控制系统及其控制方法
技术领域
1.本发明涉及一种基于二维码定位导航的agv控制系统及其控制方法，属于agv控制领域。

背景技术：

2.随着科技的发展进步和购物方式的重大变革，物流分拣设备在生产制造、分拣运输和资源配置领域发挥着越来越重要的作用。根据物流仓储设备的最新发展报告显示，我国绝大多数分拣仓库的自动化设备覆盖率仍旧很低，主要还是依靠人力来完成分拣工作，人工成本占总成本比例超过30％。
3.为了降低人工成本，提高分拣仓库的空间利用率，智能化无人仓储逐渐成为物流领域的主流研究方向。随着研究的日益深入，自动导航小车(automated guided vehicle，agv)作为一种能够减少搬运时间、降低管理和人工成本的新型自动化物流设备，开始进入行业视野。agv是一款以车载大功率电池为动力来源，装有电磁学或光学等非接触式导引装置的自动化运输设备，该设备能够在装载包裹任务的同时按照预先规划的导引路径行驶。agv在满充情况下能够以1-2m/s的运行速度连续工作4小时以上，同时可以将分拣过程中产生的所有相关信息储存在实时更新的数据库中，这不仅能极大提升仓库分拣效率，还可以大幅度减少人工和管理成本。随着agv功能的不断完善，该设备在相关领域也发挥着越来越重要的作用。
4.近年来，随着科技的不断进步，agv导引方式也随之不断发展，常用的agv导引方式有很多，如电磁导引、磁带导引、二维码导引、光学导引等固定路径导引，惯性导引、slam导引、gps导引等自由路径导引。根据agv自身的类型、结构和应用场景的不同，可选用不同的导引方式。
5.电磁导引方式不易污染和破损，但是无法更改行驶路径，所以电磁导引适用于路径不需要经常变动且路况比较简单的场地。惯性导引只能得到agv自身的位置姿态和方向速度，不能获得agv在地图中的位置信息，因此惯性导引经常与其他导引方式复合使用。slam导航技术定位精度髙，无需铺设导轨，线路可柔性规划，是一种非常有前景的导航方式。但是该技术所需成本相对较高，且对工作环境要求较为苛刻，因此目前成熟的产品较少。二维码导引由读码器、中央控制器等零部件组成，通过中央控制器处理读码器数据来获得agv位置和位姿信息。该方式是目前市面上常见的agv导引方式之一，实施较为简单。
6.目前基于二维码导航agv发展虽然趋于成熟，但在纠偏方式及整体系统构成方面仍然存在进步空间。
7.agv纠偏控制即是根据其行驶过程中采集到的位置偏差信息然后通过对自身速度的调节使agv保持正确的运行状态。当agv偏离预定轨迹时，能根据自身控制器的作用，使得agv在一定时间内回到正确的轨迹，即轨迹跟踪。
8.张艳针对agv停靠位置精度不高的问题，利用data matrix二维码中的位置信息进行导航定位，为适应不同路况条件，采用二步纠偏法在跟踪过程中进行位置纠偏，最终agv
路径跟踪的实现过程是先采用二步纠偏方法进行位置纠偏，再采用积分分离型比例积分微分(proportion integration differentiation，pid)实现了角度纠偏(张艳,朱振伟.二维码导航agv控制系统研究[j].山东农业大学学报(自然科学版),2019,50(03):441-444)。夏坡坡根据红外传感器获取agv车身距离引导线的偏差距离，利用动态偏差实现前视距离的判断，提出了基于改进型纯跟踪方法，解决不同初始状态的路径跟踪问题(夏坡坡,姚立健,杨自栋,张瑞峰,赵辰彦.基于改进型纯追踪模型的温室agv运输平台路径追踪[j].农机化研究,2019,41(11):52-56+61.doi:10.13427/jki.njyi.2019.11.009.)。施文提出了基于改进免疫方法寻优的p比例模糊控制结构对agv跟踪路径进行偏差修正，实现跟踪误差的减小(施文,张伟,王亚刚.基于免疫算法的p模糊控制agv路径偏正[j].控制工程,2021,28(05):870-876.doi:10.14107/jki.kzgc.20190267.)。罗哉利用最优偏差转化的方法建立agv的纠偏控制器，可以快速平稳地消除航向偏差(罗哉,唐颖奇,李冬,王岚晶.基于最优偏差路径的自动导引车纠偏方法[j].仪器仪表学报,2017,38(04):853-860.doi:10.19650/jki.cjsi.2017.04.009.)。李照以二维码作为导航信息，用模糊控制实现agv路径的纠偏(李照,舒志兵,严亮.基于模糊路径纠偏的agv视觉精定位研究[j].电子技术应用,2018,44(04):81-85.doi:10.16157/j.issn.0258-7998.173846.)。张坤采用模糊控制实现差速式agv的转向控制，提高其转向控制的准确性和快速性(张坤,许伦辉.基于模糊控制的agv差速转向控制算法研究[j].自动化与仪表,2016,31(10):1-4+66.doi:10.19557/jki.1001-9944.2016.10.001.)。
[0009]
上述文献中，控制方法在路径跟踪控制过程中有较好的跟踪效果，但是上述控制方法多根据单一数据信息进行控制，纠偏复杂度较高、耗费时间过长，且未充分考虑几何路径高度复杂导致小车高速行驶时出现的不稳定情况，从而agv控制系统的纠偏精度和稳定性均比较差。

技术实现要素：

[0010]
为了有效降低纠偏复杂度和时间耗费问题，以及改善整体系统运行的稳定性，本发明提供了一种基于二维码定位导航的agv控制系统及其控制方法，所述技术方案如下：
[0011]
本发明的第一个目的在于提供一种基于二维码定位导航的agv控制系统，包括：上位机和agv车载系统，所述上位机包括：
[0012]
地图监控模块，用于根据二维码标签生成电子地图，并在地图中实时更新小车位置和运行状态；
[0013]
状态信息模块，用于实时观测小车的状态信息和网络连接信息；
[0014]
指令发送模块，用于向小车发送动作指令；
[0015]
路径规划模块，用于根据小车的起始点和目标点，通过蚁方法进行最优路径搜索，最终得到最优路径；
[0016]
所述agv车载系统用于实现agv的实际运行；所述agv车载系统包括：主板和从板；
[0017]
所述主板包括：实时调度系统，用于轮回询问小车的不同功能模块的反馈信息，在接收到所述反馈信息后，所述主板通过无线通信模块将所述反馈信息打包上传给所述上位机；
[0018]
所述从板包括：读码器、数字量微惯导系统和电机驱动模块；在所述读码器读取当
前位姿信息后，上传至所述上位机，所述上位机根据当前位姿发出路径数据包或命令数据包；所述从板接收到所述路径数据包或命令数据包后开始执行命令，所述电机驱动模块驱动电机同步直行至指定距离或转向至指定方向；所述数字量微惯导系统基于陀螺仪与加速度计解算小车航向角，在小车经过两个二维码之间空白区域时，结合电机编码器反馈信息，推算当前小车位姿状况，同时对于运行中出现的横向和角度偏差进行纠正，当小车经过不同地面二维码时，不断上传位姿信息，不断对位姿进行调整，防止误差积累最终导致丢码。
[0019]
可选的，所述主板搭载的模块包括：避障雷达模块、无线通信模块、电源模块、指令收发模块、lcd显示模块和freertos系统；
[0020]
所述实时调度系统采用freertos系统；
[0021]
所述反馈信息包括：所述电源模块反馈的电量电压信息、所述避障雷达模块反馈的障碍物体接近信息、所述从板反馈的路径信息和命令完成反馈信息；
[0022]
所述lcd显示模块显示小车电量信息；
[0023]
所述指令收发模块通过串口通信实现主板和从板之间的指令收发功能。
[0024]
可选的，所述上位机将所述最优路径生成路径数据包下发至所述agv车载系统，所述路径数据包括多个节点信息，每个节点信息包括：地址码id、动作码、速度挡位信息和动作参数。
[0025]
可选的，所述agv车载系统基于stm32搭建而成，所述主板和从板为stm32板。
[0026]
可选的，所述电机驱动模块采用差速驱动方式，基于canopen协议开发。
[0027]
可选的，所述上位机基于javafx架构搭建而成。
[0028]
本发明的第二个目的在于提供一种基于二维码定位导航的agv纠偏方法，基于上述的一种基于二维码定位导航的agv控制系统实现，包括：
[0029]
步骤一：agv上电后，读码器读取到有效二维码信息，小车进入简单归位程序，小车进行位置调整以接近x轴和y轴，并调整角度，使小车只存在横向偏差s，所述横向偏差s为agv两轮中心点与理想行进轨迹的垂直距离；
[0030]
步骤二：根据实际场景设置第一参考偏差值m1、第二参考偏差值m2和第三参考偏差值m3，且m1》m2》m3，根据所述横向偏差的大小选择不同的纠偏方法；
[0031]
当所述横向偏差s》m1时，首先选择旋转归位方法进行纠偏，当横向误差纠正至小于所述第二参考偏差值m2时，若横向误差小于所述第三参考偏差值m3，则纠偏完成，若横向误差不小于所述第三参考偏差值m3，则执行一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0032]
当m1》s》m2时，若纠偏经过二维码节点数大于等于2，系统选择连续纠偏方法，若纠偏经过节点数只有1个，系统选择低速一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0033]
当s《m2时，系统选择一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0034]
所述旋转归位方法包括：
[0035]
判断当前车头指向，当车头指向x轴方向时，车体原地旋转90
°
，此时车头指向y轴方向，根据y坐标的大小选择前进或后退以靠近x轴，调整至距离x轴
±
pmm后停止，之后旋转指向初始x轴方向，根据x坐标大小，选择前进或后退以靠近y轴，调整至距离y轴
±
pmm后停止，直至车体中心距离二维码中心在所述第三参考偏差值m3半径圆内，角度无偏差，直行不
需纠偏，其中，p为小车距离坐标轴的允许偏差；
[0036]
当车头指向y轴方向时，采取同样的方法；
[0037]
所述一次纠偏方法包括：
[0038]
读取当前横向偏差e和角度偏差θ后，将该数据传入模糊pid控制器中，所述模糊pid控制器根据输入值与期望值输出控制参数δv，从而实现差速运动消除误差，行驶途中，依据数字量微惯导系统mins与编码器推算当前横向偏差e和角度偏差θ，判断纠偏是否完成，未完成则将该值返回至输入端，循环纠偏，直至纠偏完成；
[0039]
所述连续纠偏方法包括：
[0040]
第一次纠偏时，限制横向偏差最大为所述第二参考偏差值m2，角度偏差由于简单归位的作用忽略不计，采用一次纠偏方法进行纠偏；
[0041]
小车到达第二个二维码时，再读取横向偏差与角度偏差，直接调用一次纠偏方法完成最后纠偏量。
[0042]
可选的，所述简单归位程序包括：在读取到当前二维码值后，判断当前车头指向x轴还是y轴；
[0043]
当车头指向x轴方向时，小车将以低速直行接近y轴，在距离y轴
±
nmm时停止，若车头指向y轴方向，则接近x轴，在距离x轴
±
nmm时停止，其中，n为归位程序允许的距离误差；
[0044]
判断当前车头指向的最近角度，包括：0
°
、90
°
、180
°
、270
°
，车头差速微调指向最接近角度
±s°
时停止，若一次未到位则循环调整，直至达到目标角度，其中，s为归位程序允许的角度误差。
[0045]
可选的，所述低速一次纠偏方法是将agv运行速度降低，再执行所述一次纠偏方法。
[0046]
本发明的第三个目的在于提供一种基于二维码定位导航的agv控制方法，所述方法基于上述的一种基于二维码定位导航的agv控制系统实现，采用上述的agv纠偏方法实现小车运动过程的偏差纠正，包括：
[0047]
步骤1：根据使用场景，对二维码张贴进行布局；
[0048]
步骤2：二维码实际场景布局完成后，在上位机程序中，根据不同码号生成电子地图，展现在上位机界面；
[0049]
步骤3：小车上电，agv车载系统读取到有效二维码，进入简单归位程序，归位后agv车载系统获取小车当前位置信息，并将信息上传至上位机，上位机界面更新小车信息；
[0050]
步骤4：上位机获取小车当前位姿信息，并发送控制指令；
[0051]
若控制指令为点对点指令，上位机根据当前点与目标点通过蚁方法进行最优路径迭代，从而生成路径数据包；若控制指令为动作指令，则生成动作指令包；
[0052]
步骤5：agv车载系统接收到所述路径数据包和/或动作指令包，执行指令，小车开始运行；
[0053]
步骤6：小车运行过程中，数字量微惯导系统对于运行中出现的横向和角度偏差，采用上述的agv纠偏方法进行纠正。
[0054]
本发明有益效果是：
[0055]
本发明提出了一种新式归位方法，解决了传统agv上电后处于静默状态，无法自动简单归位的缺点，达到了启动自动消除角度偏差，简化后续纠偏复杂度的目的；通过多策略
纠偏方案，解决了传统纠偏方式的单一化，应用面窄的问题，达到了简化纠偏方式，有效减少纠偏时间的目的；通过多传感器数据融合，解决了传统纠偏方案中获取数据单一，数据获取片面化，导致纠偏策略的失效的问题，达到了高稳定性、高精度运行纠偏的目的。
[0056]
本发明优化了agv初始化偏差，保证agv始终以小偏差姿态进行纠正，其次通过多策略纠偏方式最简化纠偏过程，最后融合数字量微惯导系统mins与编码器信息用以航迹推算，保证了模糊pid返回参数的准确性，有效的优化agv纠偏能力，有效降低纠偏所需步骤和耗费时间提高了agv控制系统的稳定性。
附图说明
[0057]
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0058]
图1为本发明提供的基于二维码定位导航的agv控制系统构成总图。
[0059]
图2为本发明提供的基于二维码定位导航的agv控制系统上位机界面图。
[0060]
图3为本发明提供的基于二维码定位导航的agv控制系统驱动控制总图。
[0061]
图4为本发明提供的基于二维码定位导航的agv控制系统任务分配流程图。
[0062]
图5为本发明提供的基于二维码定位导航的agv控制系统归位纠偏流程图。
[0063]
图6为本发明提供的基于二维码定位导航的agv控制系统简单归位程序图。
[0064]
图7为本发明提供的基于二维码定位导航的agv控制系统旋转归位方法图。
[0065]
图8为本发明提供的基于二维码定位导航的agv控制系统一次纠偏方法图。
[0066]
图9为本发明提供的基于二维码定位导航的agv控制系统连续纠偏方法图。
[0067]
图10为本发明提供的基于二维码定位导航的agv控制系统模糊pid控制图。
[0068]
图11为本发明提供的基于二维码定位导航的agv控制系统位姿模型图。
具体实施方式
[0069]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。
[0070]
实施例一：
[0071]
本实施例提供一种基于二维码定位导航的agv控制系统，包括：上位机和agv车载系统，所述上位机包括：
[0072]
地图监控模块，用于根据二维码标签生成电子地图，并在地图中实时更新小车位置和运行状态；
[0073]
状态信息模块，用于实时观测小车的状态信息和网络连接信息；
[0074]
指令发送模块，用于向小车发送动作指令；
[0075]
路径规划模块，用于根据小车的起始点和目标点，通过蚁方法进行最优路径搜索，最终得到最优路径；
[0076]
所述agv车载系统用于实现agv的实际运行；所述agv车载系统包括：主板和从板；
[0077]
所述主板包括：实时调度系统，用于轮回询问小车的不同功能模块的反馈信息，在
接收到所述反馈信息后，所述主板通过无线通信模块将所述反馈信息打包上传给所述上位机；
[0078]
所述从板包括：读码器、数字量微惯导系统和电机驱动模块；在所述读码器读取当前位姿信息后，上传至所述上位机，所述上位机根据当前位姿发出路径数据包或命令数据包；所述从板接收到所述路径数据包或命令数据包后开始执行命令，所述电机驱动模块驱动电机同步直行至指定距离或转向至指定方向；所述数字量微惯导系统基于陀螺仪与加速度计解算小车航向角，在小车经过两个二维码之间空白区域时，结合电机编码器反馈信息，推算当前小车位姿状况，同时对于运行中出现的横向和角度偏差进行纠正，当小车经过不同地面二维码时，不断上传位姿信息，不断对位姿进行调整，防止误差积累最终导致丢码。
[0079]
实施例二：
[0080]
本实施例提供一种基于二维码定位导航的agv纠偏方法，基于实施例一记载一种基于二维码定位导航的agv控制系统实现，包括：
[0081]
步骤一：agv上电后，读码器读取到有效二维码信息，小车进入简单归位程序，小车进行位置调整以接近x轴和y轴，并调整角度，使小车只存在横向偏差s；
[0082]
步骤二：根据实际场景设置第一参考偏差值m1、第二参考偏差值m2和第三参考偏差值m3，且m1》m2》m3，根据所述横向偏差的大小选择不同的纠偏方法；
[0083]
当所述横向偏差s》m1时，首先选择旋转归位方法进行纠偏，当横向误差纠正至小于所述第二参考偏差值m2时，若横向误差小于所述第三参考偏差值m3，则纠偏完成，若横向误差不小于所述第三参考偏差值m3，则执行一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0084]
当m1》s》m2时，若纠偏经过节点数大于等于2，系统选择连续纠偏方法，若纠偏经过节点数只有1个，系统选择低速一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0085]
当s《m2时，系统选择一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；
[0086]
所述旋转归位方法包括：
[0087]
判断当前车头指向，当车头指向x轴方向时，车体原地旋转90
°
，此时车头指向y轴方向，根据y坐标的大小选择前进或后退以靠近x轴，调整至距离x轴
±
pmm后停止，之后旋转指向初始x轴方向，根据x坐标大小，选择前进或后退以靠近y轴，调整至距离y轴
±
pmm后停止，直至车体中心距离二维码中心在所述第三参考偏差值m3半径圆内，角度无偏差，直行不需纠偏，其中，p为小车距离坐标轴的允许偏差；
[0088]
当车头指向y轴方向时，采取同样的方法；
[0089]
所述一次纠偏方法包括：
[0090]
读取当前横向偏差e和角度偏差θ后，将该数据传入模糊pid控制器中，所述模糊pid控制器根据输入值与期望值输出控制参数δv，从而实现差速运动消除误差，行驶途中，依据mins与编码器推算当前横向偏差e和角度偏差θ，判断纠偏是否完成，未完成则将该值返回至输入端，循环纠偏，直至纠偏完成；
[0091]
所述连续纠偏方法包括：
[0092]
第一次纠偏时，限制横向偏差最大为所述第二参考偏差值m2，角度偏差由于简单归位的作用忽略不计，采用一次纠偏方法进行纠偏；
[0093]
小车到达第二个二维码时，再读取横向偏差与角度偏差，直接调用一次纠偏方法
完成最后纠偏量。
[0094]
实施例三：
[0095]
本实施例提供一种基于二维码定位导航的agv控制系统及其控制方法，所述系统和方法介绍如下：
[0096]
如图1所示，本发明的控制系统分为上位机和agv车载系统两大块。
[0097]
上位机基于javafx架构搭建而成，界面包含地图监控，状态信息，指令发送和路径规划板块。
[0098]
agv车载系统(下位机)又拆分为主从板，两块stm32板之间通过串口进行通信，通信内容主要包括指令信息和动作完成返回信息。主板主要挂载了避障雷达、无线通讯、电源监控、指令收发和lcd显示等模块。从板主要挂载了读码器、微惯导系统和伺服电机等模块。
[0099]
系统的整体工作流程为：上位机在发送小车到达指定位置命令时，结合小车上线后所在位姿信息，通过蚁算法进行最优路径搜索，最终得到最优路径。路径包由多个节点共同组成，每个节点包含地址码id、动作码、速度挡位和动作参数。这样就构成一系列完整动作执行命令，确保小车快速准确的到达指定位置。
[0100]
以下对该系统的工作方法进行细致描述。
[0101]
根据使用场景，对二维码张贴进行布局，二维码张贴上要求各码之间间距以及平行度有较高的保证，否则不可避免的产生人为误差，导致小车的运行精度和平稳性变差，本实施例中，二维码采用4x4阵列，两码之间间隔为1070mm，此类dm码可以在被严重污损的情况下依旧有效使用，满足多种应用场景。
[0102]
在对二维码实际场景布局完成后，在上位机程序中，根据不同码号生成电子地图，之后展现在上位机界面中，地图中还要实时更新小车当前所在位置，以及充电桩等信息，如图2所示。
[0103]
图2左侧即为一个简单场景下构成的电子地图，图中显示各二维码码号，同时以充电桩标准表示小车充电位置。在小车上电且读取到有效二维码后，图2中左下方小车标志即自动跳到当前小车所在码号位置。同时，图2中上部agv状态信息会得到更新，会显示当前小车id、运行状态、电量电压信息、当前所在二维码码号和在此码上方时，小车距离x、y轴的偏差以及与当前指向角度，此类信息后续将作为纠偏的初始数据来源。
[0104]
在右上方两栏为服务器配置与通信状态信息栏，通信状态信息模块显示目标小车网络连接状况，服务器配置模块为pc客户端网络连接状态栏，二者分别为pc端登陆信息和agv小车登录信息。
[0105]
图2右下角为命令发送板块，其运行方式如图4所示，在获取到位姿信息后，上位机即可以发送控制指令，主要分为两种命令方式，一种是点到点运动指令，另一种是小车动作指令。
[0106]
常见为第一种点到点运动，在使用场景中，往往执行的任务就是完成点到点运动，通过图2右下角点到点运动，填写指定到达点后，上位机根据当前点与目标点通过蚁方法进行最优路径迭代，从而生成路径数据包。路径数据包由多个节点组成，每个节点包含四项内容。分别为地址码id(用于确认当前所在码位是否是命令执行码)、动作码(前进、转弯、顶升和旋转等命令)、速度挡位(根据动作不同选择不同运行速度)和动作参数(直行距离、转弯角度、顶升高度和旋转角度等要求)。上位机再根据约定命令格式添加时间戳和校验码等
校验方式，封装完毕的路径数据包即可通过无线通信模块发送至agv车载系统主板中。
[0107]
agv车载系统主板搭载了避障雷达模块、无线通信模块、电源通信模块、指令收发模块和lcd显示模块。主板中搭载freertos系统用以实时调度各任务模块，轮回询问不同模块信息，包括读取电源模块电量电压信息，避障模块物体接近信息，以及从板上传的路径信息和命令完成反馈。在接收到相应信息后，主板通过无线通信模块将其打包上传给pc端上位机。上位机接收信息后不断更新界面信息，从而可视化agv运行状态。同时满足上位机对全局agv的调度与任务分配。
[0108]
上电启动后，freertos系统根据任务优先级的顺序不同，轮回询问各任务是否进入运行态，其他任务则处于挂起态。避障雷达模块通过设置，仅当接近目标30cm处，产生报警，并迫使当前agv停止运行，有效提高安全保障。电源模块返回当前电源信息，用以提示是否需要充电，避免电量耗尽死机。lcd显示模块则显示电量信息，此类信息可供近距离查看，此类信息亦可于上位机查看。当上位机发送指令或agv车载系统上发应答返回包时，无线通信模块保证二者之间的稳定有效通讯。由于agv车载系统分为主从板控制，主从板之间通过串口通信实现指令收发功能，其内容包含路径数据包和读码信息等。
[0109]
agv车载系统从板搭载了读码器模块、数字量微惯导系统模块和驱动模块等。读码器根据地面张贴二维码读取当前位置信息，包括标签号、x轴距离、y轴距离和角度信息。通过读取二维码信息，下位机将上传一系列信息到上位机，上位机界面更新小车信息，显现其实时位置及当前位姿状况，便于上位机进行agv的调度与任务分配。数字量惯导系统可以基于陀螺仪与加速度计解算航向角，在两个二维码之间空白区域时，结合电机编码器反馈信息，推算当前agv位姿状况，同时对于运行中出现的横向和角度偏差进行纠正，防止误差累计导致读码失败造成路径丢失，出现安全事故。驱动模块采用差速驱动方式，具有灵活性高，转弯半径小等优点。其基于canopen协议开发，具备错误检测、支持多节点、实时性高和成本低廉等优点。agv小车采用stm32作为处理器，同时管理多个电机，如图3所示。在主站下挂载四个从站，分别为左右电机和顶升电机及旋转电机。canopen可以实现同时对多从站实时控制，其控制过程如图2所示。
[0110]
agv纠偏步骤如图5所示。agv上电后，当读取的二维码非空后，即读取到有效二维码，agv即进入简单归位程序，该程序实现步骤如图6所示，在读取到当前二维码值后，判断当前车头指向x轴还是y轴，当车头指向x轴时，小车将以低速直行接近y轴，在
±
3mm时停止，若车头指向y轴，则此动作位接近x轴。完成该动作后，判断当前车头指向角度最接近0
°
、90
°
、180
°
、270
°
中的哪一个，车头差速微调指向最接近角度
±
0.5
°
时停止，若一次未到位则循环调整，直至达到目标，之后系统跳入主循环函数中。
[0111]
在接收到上位机发送的路径数据包或命令包后，小车根据横向误差大小做出如下选择，当偏差大于20mm时，agv选择旋转归位方法，该方法实现步骤如图7所示，在简单归位的基础上，判断当前车头指向，当车头指向x轴时，车体原地旋转90
°
，此时车头指向y轴，根据y轴大小，前进或后退，调整至
±
2mm后停止，之后旋转指向初始方向，此时指向x轴，根据x轴大小，前进后后退，调整至
±
2mm后停止，此时车体中心距离二维码中心在2mm半径圆内，角度无偏差，直行不需纠偏。此类消除法对转向精度及绕原点精度要求较高，往往不可一次将横向偏差消除至2mm以内，于是，若消除后大于10mm，则重复上步骤，否则判定是否小于2mm满足则完成纠偏。
[0112]
若小于10mm却大于2mm则采用一次纠偏方法完成最后纠偏目的，该方法实现过程如图8所示，读取当前横向偏差e和角度偏差θ后，将该数据传入模糊pid中，模糊pid根据输入值与期望值输出控制参数δv，从而实现差速运动消除误差，行驶途中，依据数字量微惯导系统mins与编码器推算当前e与θ，判断纠偏是否完成，未完成则将该值返回至输入端，继续新的一周期纠偏，直至纠偏完成。
[0113]
模糊pid控制器构造如图10所示，本实施例将姿态偏差(包括横向偏差与角度偏差)作为输入，传入模糊控制器中优化出合适的kp、ki、kd值，之后将之用于pid控制器中，pid控制器根据输入偏差及偏差变化率，输出合适的δv值，此类δv值根据其正负不断调节左右轮速度，不断修正横向偏差与角度偏差，同时，通过数字量微惯导系统mins(其基于陀螺仪与加速度计集成，可通过内置算法解算姿态角，本实施例用到航向角)与编码器推算当前误差，推算过程如下：
[0114]
构建准确的运动学模型是进行导航控制算法设计的基础，根据如图11所示的agv运动学模型示意图，假设t时刻agv进行差速运动，右轮速度vr小于左轮速度v
l
，左右轮的行驶轨迹均为圆弧，agv向右转向，质心速度为v(vr、v
l
、v的单位均为mm/s)。在采样时间δt(s)内，转过角度为δθ(rad)：
[0115][0116]
式中：d——两轮间距(mm)；r——行驶路径轨迹圆半径(mm)，
[0117]
r＝(d(vr+v
l
))/(2(v
r-v
l
))。
[0118]
agv在x,y方向的速度分量v
x
(mm/s)、vy(mm/s)以及横摆角速度ω(rad/s)计算公式如式(2)所示：
[0119][0120]
规定初始位姿p＝[x,y,θ]
t
＝[x0,y0,θ0]
t
，得到任意时刻位姿矩阵与左右轮速度之间的关系式(3)：
[0121][0122]
以上为航迹推算实现公式，其中左右轮速度vr与v
l
由电机内置编码器解算可实时读取，上文中提到mins可返回航向角，可实时用于返回θ，则式(3)可实现。
[0123]
返回的横向偏差与角度偏差与设定值(横向偏差与角度偏差都希望纠正到0，作为设定值)作比较，不断的作为新的输入传入模糊pid中，如图10所示，通过不断地修正，最终在较小范围内波动，此时纠偏算法完成。
[0124]
当偏差小于20mm时判断是否大于10mm，若小于则采用一次纠偏方法如图8所示，若大于则考虑此次纠偏经过节点数是否大于等于2，在多节点情况下，系统选择连续纠偏方法，该方法如图9所示，因为横向误差大于10mm小于20mm，且完整路径在两个节点以上，为提高纠偏精度，采取两步走策略，第一次纠偏时，限制横向偏差最大为10mm，角度偏差由于简单归位的作用，可以忽略不计，传入一次纠偏方法中进行纠偏，到达第二个二维码时，再读取横向偏差与角度偏差，此时横向偏差为10mm左右，此时直接调用一次纠偏方法完成最后纠偏量。此方法可保证到达目标点时，将偏差纠正到较小范围内。但是当节点数仅有一个时，在综合考虑误差大小和时间的条件下，选择低速一次纠偏方法，低速一次纠偏方法是将agv运行速度降低，保证一次纠偏方法在较短的距离内完成较大的纠偏，仅用于该类情况，亦可以满足使用需要，同时降低纠偏的复杂度，至此纠偏完成。
[0125]
以上为整个系统的实现过程以及纠偏过程，在一定程度上弥补当前市面agv对于归位和纠偏方式的缺陷。
[0126]
本实施例提出一种新式归位算法，在agv小车上电后，小车识别到二维码，并自动执行简单归位程序，自动接近当前指向方向x轴或y轴，自动调整角度，指向距离当前方向最近角度。根据横向偏差大小决定不同策略进行误差消除动作，分别为一次纠偏消除、连续纠偏消除和原地旋转消除。根据不同情况做出不同策略，最大程度上减少误差消除时间。
[0127]
本实施例提出一种结合数字量微惯导系统和编码器用以实现纠偏和路径推算功能。由于agv运行速度较快，故使用双传感器用以提高纠偏稳定性，同时鉴于读码器读取速率有限，在全速通过停止或转弯码时容易出现停止不及时跑超问题。采用编码器推算距离下个二维码距离，在需要停止或转弯时提前切换二等速度，保证停止的准确性。
[0128]
本发明实施例中的部分步骤，可以利用软件实现，相应的软件程序可以存储在可读取的存储介质中，如光盘或硬盘等。
[0129]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：

1.一种基于二维码定位导航的agv控制系统，包括：上位机和agv车载系统，其特征在于，所述上位机包括：地图监控模块，用于根据二维码标签生成电子地图，并在地图中实时更新小车位置和运行状态；状态信息模块，用于实时观测小车的状态信息和网络连接信息；指令发送模块，用于向小车发送动作指令；路径规划模块，用于根据小车的起始点和目标点，通过蚁方法进行最优路径搜索，最终得到最优路径；所述agv车载系统用于实现agv的实际运行；所述agv车载系统包括：主板和从板；所述主板包括：实时调度系统，用于轮回询问小车的不同功能模块的反馈信息，在接收到所述反馈信息后，所述主板通过无线通信模块将所述反馈信息打包上传给所述上位机；所述从板包括：读码器、数字量微惯导系统和电机驱动模块；在所述读码器读取当前位姿信息后，上传至所述上位机，所述上位机根据当前位姿发出路径数据包或命令数据包；所述从板接收到所述路径数据包或命令数据包后开始执行命令，所述电机驱动模块驱动电机同步直行至指定距离或转向至指定方向；所述数字量微惯导系统基于陀螺仪与加速度计解算小车航向角，在小车经过两个二维码之间空白区域时，结合电机编码器反馈信息，推算当前小车位姿状况，同时对于运行中出现的横向和角度偏差进行纠正，当小车经过不同地面二维码时，不断上传位姿信息，不断对位姿进行调整，防止误差积累最终导致丢码。2.根据权利要求1所述的控制系统，其特征在于，所述主板搭载的模块包括：避障雷达模块、无线通信模块、电源模块、指令收发模块、lcd显示模块和freertos系统；所述实时调度系统采用freertos系统；所述反馈信息包括：所述电源模块反馈的电量电压信息、所述避障雷达模块反馈的障碍物体接近信息、所述从板反馈的路径信息和命令完成反馈信息；所述lcd显示模块显示小车电量信息；所述指令收发模块通过串口通信实现主板和从板之间的指令收发功能。3.根据权利要求1所述的控制系统，其特征在于，所述上位机将所述最优路径生成路径数据包下发至所述agv车载系统，所述路径数据包括多个节点信息，每个节点信息包括：地址码id、动作码、速度挡位信息和动作参数。4.根据权利要求1所述的控制系统，其特征在于，所述agv车载系统基于stm32搭建而成，所述主板和从板为stm32板。5.根据权利要求1所述的控制系统，其特征在于，所述电机驱动模块采用差速驱动方式，基于canopen协议开发。6.根据权利要求1所述的控制系统，其特征在于，所述上位机基于javafx架构搭建而成。7.一种基于二维码定位导航的agv纠偏方法，其特征在于，所述agv纠偏方法基于权利要求1-6任一项所述的一种基于二维码定位导航的agv控制系统实现，包括：步骤一：agv上电后，读码器读取到有效二维码信息，小车进入简单归位程序，小车进行位置调整以接近x轴和y轴，并调整角度，使小车只存在横向偏差s，所述横向偏差s为agv两轮中心点与理想行进轨迹的垂直距离；
步骤二：根据实际场景设置第一参考偏差值m1、第二参考偏差值m2和第三参考偏差值m3，且m1>m2>m3，根据所述横向偏差的大小选择不同的纠偏方法；当所述横向偏差s>m1时，首先选择旋转归位方法进行纠偏，当横向误差纠正至小于所述第二参考偏差值m2时，若横向误差小于所述第三参考偏差值m3，则纠偏完成，若横向误差不小于所述第三参考偏差值m3，则执行一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；当m1>s>m2时，若纠偏经过二维码节点数大于等于2，系统选择连续纠偏方法，若纠偏经过节点数只有1个，系统选择低速一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；当s<m2时，系统选择一次纠偏方法，直至横向误差小于所述第三参考偏差值m3；所述旋转归位方法包括：判断当前车头指向，当车头指向x轴方向时，车体原地旋转90
°
，此时车头指向y轴方向，根据y坐标的大小选择前进或后退以靠近x轴，调整至距离x轴
±
pmm后停止，之后旋转指向初始x轴方向，根据x坐标大小，选择前进或后退以靠近y轴，调整至距离y轴
±
pmm后停止，直至车体中心距离二维码中心在所述第三参考偏差值m3半径圆内，角度无偏差，直行不需纠偏，其中，p为小车距离坐标轴的允许偏差；当车头指向y轴方向时，采取同样的方法；所述一次纠偏方法包括：读取当前横向偏差e和角度偏差θ后，将该数据传入模糊pid控制器中，所述模糊pid控制器根据输入值与期望值输出控制参数δv，从而实现差速运动消除误差，行驶途中，依据数字量微惯导系统mins与编码器推算当前横向偏差e和角度偏差θ，判断纠偏是否完成，未完成则将该值返回至输入端，循环纠偏，直至纠偏完成；所述连续纠偏方法包括：第一次纠偏时，限制横向偏差最大为所述第二参考偏差值m2，角度偏差由于简单归位的作用忽略不计，采用一次纠偏方法进行纠偏；小车到达第二个二维码时，再读取横向偏差与角度偏差，直接调用一次纠偏方法完成最后纠偏量。8.根据权利要求7所述的agv纠偏方法，其特征在于，所述简单归位程序包括：在读取到当前二维码值后，判断当前车头指向x轴还是y轴；当车头指向x轴方向时，小车将以低速直行接近y轴，在距离y轴
±
nmm时停止，若车头指向y轴方向，则接近x轴，在距离x轴
±
nmm时停止，其中，n为归位程序允许的距离误差；判断当前车头指向的最近角度，包括：0
°
、90
°
、180
°
、270
°
，车头差速微调指向最接近角度
±
s
°
时停止，若一次未到位则循环调整，直至达到目标角度，其中，s为归位程序允许的角度误差。9.根据权利要求7所述的agv纠偏方法，其特征在于，所述低速一次纠偏方法是将agv运行速度降低，再执行所述一次纠偏方法。10.一种基于二维码定位导航的agv控制方法，其特征在于，所述方法基于权利要求1-6任一项所述的一种基于二维码定位导航的agv控制系统实现，采用权利要求7-9任一项所述的agv纠偏方法实现小车运动过程的偏差纠正，包括：
步骤1：根据使用场景，对二维码张贴进行布局；步骤2：二维码实际场景布局完成后，在上位机程序中，根据不同码号生成电子地图，展现在上位机界面；步骤3：小车上电，agv车载系统读取到有效二维码，进入简单归位程序，归位后agv车载系统获取小车当前位置信息，并将信息上传至上位机，上位机界面更新小车信息；步骤4：上位机获取小车当前位姿信息，并发送控制指令；若控制指令为点对点指令，上位机根据当前点与目标点通过蚁方法进行最优路径迭代，从而生成路径数据包；若控制指令为动作指令，则生成动作指令包；步骤5：agv车载系统接收到所述路径数据包和/或动作指令包，执行指令，小车开始运行；步骤6：小车运行过程中，数字量微惯导系统对于运行中出现的横向和角度偏差，采用权利要求7-9任一项所述的agv纠偏方法进行纠正。

技术总结

本发明公开了一种基于二维码定位导航的AGV控制系统及其控制方法，属于AGV控制领域。所述方法优化了小车上电初始化步骤，提供一种上电自动简单归位程序，通过新式归位方法，解决了传统AGV上电后处于静默状态，无法自动简单归位的缺点，达到了启动自动消除角度偏差，简化后续纠偏复杂度的目的；通过多策略纠偏方案，解决了传统纠偏方式的单一化，应用面窄的问题，达到了简化纠偏方式，有效减少纠偏时间的目的；通过多传感器数据融合，解决了传统纠偏方案中获取数据单一，数据获取片面化，导致纠偏策略的失效的问题，有效地提高了AGV控制系统的稳定性和纠偏精度。系统的稳定性和纠偏精度。系统的稳定性和纠偏精度。