CN111240330B - 一种平粮机器人同步导航和精确定位方法及系统 - Google Patents
一种平粮机器人同步导航和精确定位方法及系统 Download PDFInfo
- Publication number
- CN111240330B CN111240330B CN202010055603.7A CN202010055603A CN111240330B CN 111240330 B CN111240330 B CN 111240330B CN 202010055603 A CN202010055603 A CN 202010055603A CN 111240330 B CN111240330 B CN 111240330B
- Authority
- CN
- China
- Prior art keywords
- leveling robot
- grain leveling
- grain
- robot
- marks
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 38
- 238000001514 detection method Methods 0.000 claims abstract description 53
- 238000006073 displacement reaction Methods 0.000 claims abstract description 51
- 238000012545 processing Methods 0.000 claims description 18
- 230000007246 mechanism Effects 0.000 claims description 14
- 238000007790 scraping Methods 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000004891 communication Methods 0.000 claims description 5
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 101100134058 Caenorhabditis elegans nth-1 gene Proteins 0.000 claims description 3
- 230000002159 abnormal effect Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000002194 synthesizing effect Effects 0.000 claims description 3
- 230000000007 visual effect Effects 0.000 claims description 3
- 238000003860 storage Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 5
- 238000002474 experimental method Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000009423 ventilation Methods 0.000 description 2
- 231100000716 Acceptable daily intake Toxicity 0.000 description 1
- OKTJSMMVPCPJKN-UHFFFAOYSA-N Carbon Chemical compound [C] OKTJSMMVPCPJKN-UHFFFAOYSA-N 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 238000003958 fumigation Methods 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000011068 loading method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000003786 synthesis reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种平粮机器人同步导航和精确定位方法及系统，其中方法包括：利用摄像头获取平粮机器人行进时的走道板边沿图像，基于走道板边沿图像控制平粮机器人进行巡线导航；实时记录里程计信息；利用检测模块检测走道板上的标记并记录；标记以一定间隔设置在走道板上，相邻两侧走道板标记设置位置同步，包括导航标记和定位标记；定位标记根据走道板的行道分类；检测到标记时，将里程计信息清零，对检测到的标记进行误检测使平粮机器人主从机进行同步，再根据统计的标记种类和数量得到平粮机器人在粮仓的位置，没有检测到标记时，根据统计的标记种类和数量及里程计信息得到位移对平粮机器人主从机进行同步，同时得到平粮机器人在粮仓的位置。
Description
技术领域
本发明涉及粮食储藏技术领域，特别涉及一种平粮机器人同步导航和精确定位方法及系统。
背景技术
智能粮库是粮食发展信息化建设的重要内容，是粮食储存管理升级的必由之路。随着通信技术、物联网技术、电子信息技术和计算机网络技术的发展，自动化控制和管理技术已经广泛应用于粮库的日常生产中，涉及到粮仓入粮、通风、熏蒸、除虫等具体工作；粮食平整是粮仓的一项重要工作，对于需要长期储存粮食的粮仓来说，平整的粮面有助于更准确地把握粮食总量，便于粮情检查和均匀通风。
在通过抛粮输送机将粮食输送入粮仓后，通常需要进行初平和细平两个阶段的工作，初平工作即是将“金字塔”形的粮食堆推平为高差约为正负10cm左右，细平阶段的任务即是将高差10cm左右的粮面平整至正负2cm左右。目前粮库大都是人工完成平整工作，工人任务繁重、身体健康受到粉尘的影响，且具有被粮食埋没的危险。现有的自动化程度较高的适用于粮食细平阶段的机器人存在同步性差，关键点定位精度不够高，运行过程中无法进行全局定位等问题。
发明内容
本发明的目的在于克服现有技术中所存在的适用于粮食细平阶段的机器人存在同步性差，关键点定位精度不够高，运行过程中无法进行全局定位的不足，提供一种平粮机器人同步导航和精确定位方法及系统。
为了实现上述发明目的，本发明提供了以下技术方案：
一种平粮机器人同步导航和精确定位方法，包括以下步骤：
步骤1：利用摄像头获取平粮机器人行进时的走道板边沿图像；实时记录平粮机器人沿走道板行进的里程计信息；利用检测模块检测平粮机器人行进的走道板上的标记并记录检测到的所述标记；
其中，所述标记以一定间隔设置在所述走道板上，相邻两侧走道板上的标记位置同步，所述标记包括设置在所述走道板走道上的导航标记和定位标记；所述定位标记根据所述走道板的多个行道进行分类；
步骤2：基于所述走道板边沿图像控制平粮机器人进行巡线导航；
当所述检测模块检测到定位标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，所述检测模块输出所述定位标记相对于摄像头像素坐标的偏移量，根据偏移量调整平粮机器人的位姿，对记录的定位标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块检测到导航标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，对记录的所述标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块没有检测到所述标记时，对记录的所述标记的种类和数量进行统计，并结合所述里程计信息计算得到位移，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，得到平粮机器人在粮仓的位置。
优选的，所述步骤2中，基于所述走道板边沿图像控制平粮机器人进行巡线导航，具体包括：
将摄像头采集到的所述走道板边沿图像二值化，得到走道板边沿二值图，运用霍夫变换算法检测所述走道板边沿二值图中的直线，拟合出走道板边缘直线；
在距离所述走道板边沿二值图下边框为K和2K处分别作平行于下边框的参考线一、参考线二，所述参考线一与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于一点，取这两点连线的中点M1，所述参考线二与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于一点，取这两点连线的中点M2，连接M1，M2得到交于摄像头像素坐标X轴的点M，根据所述走道板边沿二值图横轴的像素坐标，计算出点M距离所述走道板边沿二值图中线的长度作为PID函数的反馈值，取PID函数的设定值为零，计算得到控制量，将所述控制量乘以转换系数作为平粮机器人的角速度；基于所述角速度控制底盘转动使反馈值能够保持为零，保证走道板中央的白线位于摄像头的视野中线上，平粮机器人实现巡线导航。
优选的，所述步骤1还包括利用激光测距模块获取平粮机器人距离障碍物的距离；所述步骤2还包括根据测得的平粮机器人距离障碍物的所述距离控制平粮机器人进行避障。
优选的，平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，具体包括：
根据平粮机器人主机和平粮机器人从机检测到的标记数量是否一致进行误检测处理，当平粮机器人主机检测到的标记为第n个标记，且在容许的位移误差内平粮机器人从机检测到的标记为第n个标记，则平粮机器人主机与平粮机器人从机位置同步；当平粮机器人主机检测到的标记为第n个标记，在容许的位移误差内，平粮机器人从机检测到的标记为第n-1个标记，根据陀螺仪数据判断平粮机器人从机的情况，当平粮机器人从机的陀螺仪数据变化超过阈值，则认为平粮机器人从机当前存在异常，此时控制平粮机器人主机停止，降低平粮机器人从机的速度使平粮机器人从机行进到第n个标记的位置；当平粮机器人从机的陀螺仪数据在阈值范围内，且平粮机器人主机与平粮机器人从机的位移误差均在预设范围内，则认为是平粮机器人从机检测标记失败，此时纠正平粮机器人从机进行记录，并重置底盘里程计信息。
优选的，所述检测模块为Openmv模块。
优选的，所述里程计信息的计算方法为：
将编码器测到的脉冲数转换成电机转速，所述电机转速即为底盘的多个轮子的角速度，将所述电机转速根据运动学模型合成速度，将所述速度以一定的周期进行积分得到所述里程计信息。
优选的，将所述电机转速根据运动学模型合成速度，具体的计算公式：
其中w1、w2、w3分别为底盘的三个轮子以r/min为单位的角速度，L为底盘半径，R为轮子半径，Vx、Vy分别为机器人X轴、Y轴速度，W为平粮机器人的角速度，即是平粮机器人主机或者平粮机器人从机的底盘相对于各自中心轴的角速度，从俯视角度来看，以逆时针方向为正。
优选的，所述步骤2中，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，具体包括：
将平粮机器人主机位移作为PID函数设定值，平粮机器人从机位移为反馈值，计算得到调整值，将所述调整值乘以修正系数后作为平粮机器人从机速度增量，从而调节平粮机器人从机的速度，使平粮机器人主机和平粮机器人从机位置同步。
本发明还公开了一种平粮机器人同步导航和精确定位系统，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的一种平粮机器人同步导航和精确定位方法。
本发明还公开了一种平粮机器人同步导航和精确定位系统，包括工控机、平粮机器人主机、平粮机器人从机以及连接所述平粮机器人主机和平粮机器人从机的刮粮机构；
所述平粮机器人主机和所述平粮机器人从机均包括底盘、摄像头和电动推杆；所述底盘上设有所述电动推杆，所述电动推杆与所述刮粮机构相连接；所述底盘上还设有控制单元、检测模块、编码器、电机和陀螺仪；
所述电机用于带动所述底盘运动；所述编码器采集所述电机的转速并传输给所述控制单元；所述陀螺仪检测平粮机器人的偏航角并传输给所述控制单元；所述检测模块检测走道板上的标记并传输给所述控制单元；
所述控制单元与所述工控机通讯连接，所述控制单元将转速信息、偏航角信息和标记信息进行记录并传输给所述工控机；
所述摄像头采集走道板边沿图像并传输给所述工控机；
所述工控机进行数据处理后，控制所述底盘运动和所述电动推杆升降；
所述数据处理包括：
基于所述走道板边沿图像控制平粮机器人进行巡线导航；
当所述检测模块检测到定位标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，所述检测模块输出所述定位标记相对于摄像头像素坐标的偏移量，根据偏移量调整平粮机器人的位姿，对记录的定位标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块检测到导航标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，对记录的所述标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块没有检测到所述标记时，对记录的所述标记的种类和数量进行统计，并结合所述里程计信息计算得到位移，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，得到平粮机器人在粮仓的位置。
与现有技术相比，本发明的有益效果：
1、本发明通过对摄像头采集的走道板边沿图像进行处理使平粮机器人能够进行巡线导航，同时在走道板上以一定间隔铺设导航标记和定位标记，相邻标记之间保证了机器人前进方向的位移误差，利用检测模块对标记进行检测，根据检测结果控制平粮机器人进行同步，保证了平粮机器人主从机的同步性，同时在定位标记处先矫正位姿再定位，提高了平粮机器人在定位标记处(即关键点)的定位精度，并能够准确定位平粮机器人的位置(即全局定位)。
2、本发明利用激光测距模块测量平粮机器人距离障碍物的距离，使得平粮机器人在运行过程中能够避障。
附图说明：
图1是本发明实施例1中所述的平粮机器人底盘结构框图。
图2是本发明实施例1中所述的平粮机器人结构框图。
图3是本发明实施例1所述的一种平粮机器人同步导航和精确定位方法的方法流程图。
图4是本发明实施例1所述的一种平粮机器人同步导航和精确定位方法中走道板上所述标记的分布示意图。
图5是本发明实施例1所述的一种平粮机器人同步导航和精确定位方法中所述定位标记的铺设位置图。
图6是本发明实施例1所述的一种平粮机器人同步导航和精确定位方法中所述参考线一和所述参考线二的示意图。
图7是本发明实施例1所述的一种平粮机器人同步导航和精确定位方法中平粮机器人接近第二行走道板走道交叉口时的摄像头视野的示意图。
图8是本发明实施例2所述的一种平粮机器人同步导航和精确定位系统的结构示意图。
图中标记：1-第一定位标记，2-第二定位标记，3-第三定位标记，4-第四定位标记，5-导航标记，6-平粮机器人，7-第二行走道板，310-电子设备，311-处理器，312-存储器，313-输入输出接口，314-电源。
具体实施方式
下面结合试验例及具体实施方式对本发明作进一步的详细描述。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围。
实施例1
如图1-2所示，本实施例中，平粮机器人包括平粮机器人主机、平粮机器人从机、连接在平粮机器人主机和平粮机器人从机之间的刮粮机构、电动推杆和工控机；平粮机器人主机和平粮机器人从机均包括底盘，底盘与刮粮机构之间不是固定连接，可以相对于刮粮机构旋转任意角度；每个底盘均安装有摄像头、Openmv模块、两个激光测距传感器、控制单元、3个无刷直流电机、3个编码器、3个电机驱动器、一个陀螺仪、供电电源。供电电源为总体供电，摄像头呈45度安装在底盘前进方向，采集走道板边沿图像，Openmv模块作为检测模块安装在底盘正中央且垂直于地面，无刷电机驱动器配合无刷电机产生动力，编码器采集无刷电机实际转速，陀螺仪测出平粮机器人的偏航角。
底盘控制单元一方面接收并解析工控机信息，生成控制信号输出到电机驱动器控制电机，一方面采集底盘速度、转角、合成位移信息，采集Openmv模块检测到的标记信息，并根据查询指令传送给工控机；工控机同时处理两个摄像头数据，并可直接通过IO口产生控制信号，经过电动推杆控制器，控制电动推杆升降，也可同时控制底盘转动。
如图3所示，一种平粮机器人同步导航和精确定位方法，包括以下步骤：
步骤1：利用摄像头获取平粮机器人行进时的走道板边沿图像；实时记录平粮机器人沿走道板行进的里程计信息；利用检测模块检测平粮机器人行进的走道板上的标记并记录检测到的所述标记；
其中，所述标记以一定间隔设置在所述走道板上，相邻两侧走道板同步，包括设置在所述走道板走道上的导航标记和定位标记；所述定位标记根据所述走道板的多个行道进行分类。
本实施例中，相邻两侧走道板上的标记位置同步，且标记包括设置在所述走道板走道上的导航标记和设置在所述走道板走道交叉处的定位标记。
平粮机器人在行进过程中，将编码器测到的脉冲数转换成电机转速，所述电机转速即为底盘的多个轮子的角速度，将所述电机转速根据运动学模型合成速度，将所述速度以一定的周期进行积分得到所述里程计信息。其中，运动学模型合成速度的计算公式如下：
其中w1、w2、w3分别为底盘的三个轮子以r/min为单位的角速度，L为底盘半径，R为轮子半径，Vx、Vy分别为机器人X轴、Y轴速度，W为平粮机器人的角速度，即是平粮机器人主机或者平粮机器人从机的底盘相对于各自中心轴的角速度，从俯视角度来看，以逆时针方向为正。根据速度合成位移的公式如下：
其中θ为当前机器人偏航角，Δt为一段微小时间，Δx、Δy为机器人在X轴、Y轴的位移增量，在该时间内可将机器人视为是做匀速运动，X、Y为机器人在X轴、Y轴位移，即得到里程计信息。
所述检测模块为Openmv模块，平粮机器人在走道板行进过程中，利用Openmv模块检测走道板上的标记，当Openmv模块检测到某一标记时，控制单元就对该标记进行记录。
其中，本实施例中，所述标记之间的一定间隔根据理论位移与测得的实际位移的误差确定，具体的，使平粮机器人以设定的距离和速度前行，该设定的距离为理论位移，根据底盘的里程计信息判断平粮机器人是否到达理论位移，实施过程中使平粮机器人以0.1m/s速度行驶100cm，多次测量实际距离，得到理论位移与实际位移的误差均值，再使平粮机器人以0.1m/s速度行驶200cm，多次测量实际距离，得到理论位移与实际位移的误差均值，具体实验得到如下表所示结果：
表1理论位移与实际位移数据的误差表
根据上表所测得实验数据，可以看到随着平粮机器人前行距离的增加，里程计信息累积误差越大，在实际测试中，40cm以内的误差对于平粮机器人来说是相对安全的，本实施例以固定间隔来设置标记，保守取固定间隔为2m，保证平粮机器人前进方向位移误差在10cm以内，当然，在保证误差的同时，可以设置其他长度的固定间隔，也可以自定义设置其他长度的间隔。
如图4所示，在走道板上以固定间隔2m铺设标记，其中浅灰色部分代表粮食，深灰色呈“井”字形的为粮仓走道(即走道板)，平粮机器人6在走道板上行走，图中箭头方向为平粮机器人6的一种轨迹，其中以平粮机器人6前进方向右边底盘为平粮机器人主机，左边底盘为平粮机器人从机，当前位置平粮机器人主机所在设为第一行走道板，平粮机器人从机设为第二行走道板7，所述定位标记根据所述走道板的多个行道进行分类，在第一行走道板所有交叉口铺设的标记都为第一定位标记1，第二行走道板7所有交叉口铺设的标记都为第二定位标记2，第三行走道板所有交叉口铺设的标记都为第三定位标记3，第四行走道板所有交叉口铺设的标记都为第四定位标记4，以此类推，除去交叉口处的定位标记外，走道板的走道上均铺设导航标记5，图中只画出了第一行走道板、第二行走道板7的导航标记5。
如图5所示，本实施例中的定位标记严格按照图示的第二定位标记2进行铺设，可以为带有不同位置信息Apriltag标记，铺设在走道板交叉口处正方形对角线交点处。
步骤2：基于所述走道板边沿图像控制平粮机器人进行巡线导航；
当所述检测模块检测到定位标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，所述检测模块输出所述定位标记相对于摄像头像素坐标的偏移量，根据偏移量调整平粮机器人的位姿，对记录的定位标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块检测到导航标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，对记录的所述标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块没有检测到所述标记时，对记录的所述标记的种类和数量进行统计，并结合所述里程计信息计算得到位移，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，得到平粮机器人在粮仓的位置。
本实施例中，基于所述走道板边沿图像控制平粮机器人进行巡线导航，具体包括：
将摄像头采集到的所述走道板边沿图像二值化，得到走道板边沿二值图，运用霍夫变换算法检测所述走道板边沿二值图中的直线，拟合出走道板边缘直线；实验环境中，利用OpenCV库中的直线检测功能检测所述走道板边沿二值图中的直线，且走道板行进方向的中线上铺设白线。
如图6所示，在距离所述走道板边沿二值图下边框为K和2K处分别作平行于下边框的参考线一、参考线二，2K小于Y轴的像素值，所述参考线一与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于点(XL1，Yl1)、点(Xr1，Yr1)，取这两点连线的中点M1，所述参考线二与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于点(Xl2，Yl2)、点(Xr2，Yr2)，取这两点连线的中点M2，连接M1，M2得到交于摄像头像素坐标X轴的点M(X，Y)，根据所述走道板边沿二值图横轴像素坐标Apx，计算出M点距离所述走道板边沿二值图中线的长度D＝X-A/2作为PID函数的反馈值，取PID函数的设定值为零，计算得到控制量，将所述控制量乘以转换系数作为平粮机器人的角速度；基于所述角速度控制底盘转动使反馈值能够保持为零，保证走道板中央的白线位于摄像头的视野中线上，平粮机器人实现巡线导航。
PID函数的计算公式如下：
ε(k)＝Sk-F(k) 式(1)
U(k)＝U(k-1)+ΔU(k)
＝Kp[ε(k)-ε(k-1)]+Kiε(k)+Kd[ε(k)-2ε(k-1)+ε(k-2)]
式(2)
W＝KW×U(k) 式(3)
其中，式(1)中，Sk是设定值，F(k)是k时刻的反馈值，ε(k)是偏差量，
式(2)中，Kp是比例系数，
为积分系数，
为微分系数，Ti是积分时间常数，TD是微分时间常数，T是采样周期，U(k)是控制量，k是采样序号k＝0，1，2…，式(3)中，KW为控制量U(k)转换成角速度W的转换系数，角速度W为平粮机器人主机或者平粮机器人从机的底盘相对于各自中心轴的角速度。
平粮机器人在导航前行的同时根据测得的平粮机器人距离障碍物的所述距离控制平粮机器人进行避障，每个底盘前进方向安装两个激光测距传感器，控制单元通过I2C协议读取激光测距传感器采集的距离数据，将两个距离数据做平均，当工控机查询到平粮机器人主机和平粮机器人从机中的一台所得到的平均距离小于安全值时，认为平粮机器人前进方向有障碍物，工控机发出停止和报警指令，控制平粮机器人停止运动，等待工作人员将障碍物移开，当障碍物被移开后，平粮机器人继续运动。
当Openmv模块没有检测到标记时，那么此时就对记录的标记的种类和数量进行统计，并结合所述里程计信息计算得到位移，根据位移控制平粮机器人主机和平粮机器人从机进行同步，具体的，以图4示出的平粮机器人前进方向右边为平粮机器人主机，左边为平粮机器人从机，保持平粮机器人主机恒速运动，平粮机器人主机位移为Xl，平粮机器人从机位移为Xf，则将Xl作为PID函数设定值，Xf为反馈值，算出的调整值乘以修正系数后作为平粮机器人从机速度增量，例如：当Xl＞Xf时，平粮机器人从机落后于平粮机器人主机，输出调整值瞬时大于0，平粮机器人从机速度增加，实现位置同步。
根据记录的标记种类和数量以及位移信息可以得到平粮机器人在粮仓的准确位置。例如：当平粮机器人主机检测到第一定位标记1与平粮机器人从机检测到第二定位标记2均发生一次，且在检测到定位标记后检测到两次导航标记，当前里程计信息为(X_t)m，得到当前平粮机器人从定位标记计位移为(2×2+X_t)m。
当Openmv模块检测到标记时，控制单元将里程计信息清零，这样使得里程计记录的位移信息保证在设定的位移误差范围内。在标记处，平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，具体包括：根据平粮机器人主机和平粮机器人从机检测到的标记数量是否一致进行误检测处理，当平粮机器人主机检测到的标记为第n个标记，且在容许的位移误差内平粮机器人从机检测到的标记为第n个标记，则平粮机器人主机与平粮机器人从机位置同步；当平粮机器人主机检测到的标记为第n个标记，在容许的位移误差内，平粮机器人从机检测到的标记为第n-1个标记，根据陀螺仪数据判断平粮机器人从机的情况，当平粮机器人从机的陀螺仪数据变化超过阈值，则认为平粮机器人从机当前存在异常(如受力过大出现打滑现象)，此时控制平粮机器人主机停止，降低平粮机器人从机的速度使平粮机器人从机行进到第n个标记的位置；当平粮机器人从机的陀螺仪数据在阈值范围内，且平粮机器人主机与平粮机器人从机的位移误差均在预设范围内，则认为是平粮机器人从机检测标记失败，此时纠正平粮机器人从机进行记录，并重置底盘里程计信息。当然，若主机检测到的标记数量少于从机的标记数量，上述控制方法同样适用。所述阈值为根据经验设定，本实施例可以设为30度。所述预设范围是通过实验测得的位移误差设定的，由上述的表1结果得到，在实验中选取10cm的误差为预设范围。
当检测到的标记为导航标记时，根据误检测进行平粮机器人主从机同步后，统计的检测到的标记的种类和数量，得到平粮机器人在粮仓的位置。如平粮机器人主机和平粮机器人从机均检测到导航标记两次，可以得到平粮机器人距离出发点为4米。
当检测到的标记为定位标记时，根据误检测进行平粮机器人主从机同步后，Openmv模块输出定位标记相对于摄像头像素坐标的偏移量，基于所述偏移量控制平粮机器人调整位姿，位姿包括角度、坐标等信息。具体的，实验中，Openmv模块调用其find_apriltags函数可以得到定位标记相对于摄像头的X轴方向偏移tag·Tx，Y轴方向偏移tag.Ty和旋转角度tag.rotation()，并将这些信息通过串口传给底盘控制单元，底盘控制单元将这些信息传输给平粮机器人的工控机，工控机发送指令给控制单元，底盘控制单元解析工控机指令，首先参考陀螺仪数据控制平粮机器人转动固定的角度后，再根据Openmv模块输出的偏差信息精细调整平粮机器人位置至tag.Tx＝0，tag.Ty＝0，tag.rotation＝0。
在调整好平粮机器人的位姿后根据记录的定位标记的种类和数量对其精准定位，如平粮机器人主机检测到第一定位标记1两次，平粮机器人从机检测到第二定位标记2两次，那么可以定位平粮机器人在行进方向的第二粮格和第三粮格的走道交叉口处。
另外，平粮机器人在定位标记处是停止运动的，那么需要在摄像头检测到前方走道板边缘时，如图7示出的摄像头视野中检测到垂直于走道板的走道边缘线，平粮机器人降速，直到Openmv模块检测到定位标记停止运动。
平粮机器人在粮仓中工作时，在粮食区域需要降下刮粮机构对粮食进行平整，在经过交叉走道的时候需要升起刮粮机构，因此，控制刮粮机构的升降方式有：由于里程计信息每2m更新一次，定位误差控制在了较小的范围内，所以可以根据里程计信息来控制刮粮机构的升降，如在定位标记前0.5m处升起刮粮机构，在定位标记后0.5m处放下刮粮机构；另外还可以参考巡线摄像头数据，摄像头相对地面呈45度角，如图7所示，在接近第二行走道板7的走道交叉处时，能在摄像头视野中检测到垂直于第二行走道板7一侧的走道边缘，根据该边缘线在当前摄像头像素坐标系中的Y坐标值来控制刮粮机构的升降。
实施例2
如图8所示，一种平粮机器人同步导航和精确定位系统，即电子设备310(例如具备程序执行功能的计算机服务器)，其包括至少一个处理器311，电源314，以及与所述至少一个处理器311通信连接的存储器312和输入输出接口313；所述存储器312存储有可被所述至少一个处理器311执行的指令，所述指令被所述至少一个处理器311执行，以使所述至少一个处理器311能够执行前述实施例1所公开的方法；所述输入输出接口313可以包括显示器、键盘、鼠标、以及USB接口，用于输入输出数据；电源314用于为电子设备310提供电能。
本领域技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(ReadOnlyMemory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。
当本发明上述集成的单元以软件功能单元的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、磁碟或者光盘等各种可以存储程序代码的介质。
以上实施例仅用以说明本发明而并非限制本发明所描述的技术方案，尽管本说明书参照上述的各个实施例对本发明已进行了详细的说明，但本发明不局限于上述具体实施方式，因此任何对本发明进行修改或等同替换；而一切不脱离发明的精神和范围的技术方案及其改进，其均应涵盖在本发明的权利要求范围当中。
Claims (10)
1.一种平粮机器人同步导航和精确定位方法，其特征在于，包括以下步骤：
步骤1：利用摄像头获取平粮机器人行进时的走道板边沿图像；实时记录平粮机器人沿走道板行进的里程计信息；利用检测模块检测平粮机器人行进的走道板上的标记并记录检测到的所述标记；
其中，所述标记以一定间隔设置在所述走道板上，相邻两侧走道板同步，所述标记包括设置在所述走道板走道上的导航标记和定位标记；所述定位标记根据所述走道板的多个行道进行分类；
步骤2：基于所述走道板边沿图像控制平粮机器人进行巡线导航；
当所述检测模块检测到定位标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，所述检测模块输出所述定位标记相对于摄像头像素坐标的偏移量，根据偏移量调整平粮机器人的位姿，对记录的定位标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块检测到导航标记时，将所述里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，对记录的所述标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块没有检测到所述标记时，对记录的所述标记的种类和数量进行统计，并结合所述里程计信息计算得到位移，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，得到平粮机器人在粮仓的位置。
2.根据权利要求1所述的一种平粮机器人同步导航和精确定位方法，其特征在于，所述步骤2中，基于所述走道板边沿图像控制平粮机器人进行巡线导航，具体包括：
将摄像头采集到的所述走道板边沿图像二值化，得到走道板边沿二值图，运用霍夫变换算法检测所述走道板边沿二值图中的直线，拟合出走道板边缘直线；
在距离所述走道板边沿二值图下边框为K和2K处分别作平行于下边框的参考线一、参考线二，所述参考线一与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于一点，取这两点连线的中点M1，所述参考线二与拟合出的所述走道板边缘直线中的左侧直线、右侧直线分别交于一点，取这两点连线的中点M2，连接M1，M2得到交于摄像头像素坐标X轴的点M，根据所述走道板边沿二值图横轴的像素坐标，计算出点M距离所述走道板边沿二值图中线的长度作为PID函数的反馈值，取PID函数的设定值为零，计算得到控制量，将所述控制量乘以转换系数作为平粮机器人的角速度；基于所述角速度控制底盘转动使反馈值能够保持为零，保证走道板中央的白线位于摄像头的视野中线上，平粮机器人实现巡线导航。
3.根据权利要求1所述的一种平粮机器人同步导航和精确定位方法，其特征在于，平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，具体包括：
根据平粮机器人主机和平粮机器人从机检测到的标记数量是否一致进行误检测处理，当平粮机器人主机检测到的标记为第n个标记，且在容许的位移误差内平粮机器人从机检测到的标记为第n个标记，则平粮机器人主机与平粮机器人从机位置同步；当平粮机器人主机检测到的标记为第n个标记，在容许的位移误差内，平粮机器人从机检测到的标记为第n-1个标记，根据陀螺仪数据判断平粮机器人从机的情况，当平粮机器人从机的陀螺仪数据变化超过阈值，则认为平粮机器人从机当前存在异常，此时控制平粮机器人主机停止，降低平粮机器人从机的速度使平粮机器人从机行进到第n个标记的位置；当平粮机器人从机的陀螺仪数据在阈值范围内，且平粮机器人主机与平粮机器人从机的位移误差均在预设范围内，则认为是平粮机器人从机检测标记失败，此时纠正平粮机器人从机进行记录，并重置底盘里程计信息。
4.根据权利要求1所述的一种平粮机器人同步导航和精确定位方法，其特征在于，所述检测模块为Openmv模块。
5.根据权利要求1所述的一种平粮机器人同步导航和精确定位方法，其特征在于，所述里程计信息的计算方法为：
将编码器测到的脉冲数转换成电机转速，所述电机转速即为底盘的多个轮子的角速度，将所述电机转速根据运动学模型合成速度，将所述速度以一定的周期进行积分得到所述里程计信息。
6.根据权利要求5所述的一种平粮机器人同步导航和精确定位方法，其特征在于，将所述电机转速根据运动学模型合成速度，具体的计算公式：
其中w1、w2、w3分别为底盘的三个轮子以r/min为单位的角速度，L为底盘半径，R为轮子半径，Vx、Vy分别为机器人X轴、Y轴速度，W为平粮机器人的角速度。
7.根据权利要求1所述的一种平粮机器人同步导航和精确定位方法，其特征在于，所述步骤2中，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，具体包括：
将平粮机器人主机位移作为PID函数设定值，平粮机器人从机位移为反馈值，计算得到调整值，将所述调整值乘以修正系数后作为平粮机器人从机速度增量，从而调节平粮机器人从机的速度，使平粮机器人主机和平粮机器人从机位置同步。
8.根据权利要求1-7任一所述的一种平粮机器人同步导航和精确定位方法，其特征在于，所述步骤1还包括利用激光测距模块获取平粮机器人距离障碍物的距离；所述步骤2还包括根据测得的平粮机器人距离障碍物的所述距离控制平粮机器人进行避障。
9.一种平粮机器人同步导航和精确定位系统，其特征在于，包括至少一个处理器，以及与所述至少一个处理器通信连接的存储器；所述存储器存储有被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1至8中任一项所述的方法。
10.一种平粮机器人同步导航和精确定位系统，其特征在于，包括工控机、平粮机器人主机、平粮机器人从机以及连接所述平粮机器人主机和平粮机器人从机的刮粮机构；
所述平粮机器人主机和所述平粮机器人从机均包括底盘、摄像头和电动推杆；所述底盘上设有所述电动推杆，所述电动推杆与所述刮粮机构相连接；所述底盘上还设有控制单元、检测模块、编码器、电机和陀螺仪；
所述电机用于带动所述底盘运动；所述编码器采集所述电机的转速并传输给所述控制单元；所述陀螺仪检测平粮机器人的偏航角并传输给所述控制单元；所述检测模块检测走道板上的标记并传输给所述控制单元；
所述控制单元与所述工控机通讯连接，所述控制单元将转速信息、偏航角信息和标记信息进行记录并传输给所述工控机；
所述摄像头采集走道板边沿图像并传输给所述工控机；
所述工控机进行数据处理后，控制所述底盘运动和所述电动推杆升降；
所述数据处理包括：
基于所述走道板边沿图像控制平粮机器人进行巡线导航；
当所述检测模块检测到定位标记时，将里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，所述检测模块输出所述定位标记相对于摄像头像素坐标的偏移量，根据偏移量调整平粮机器人的位姿，对记录的定位标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块检测到导航标记时，将里程计信息清零，同时平粮机器人根据记录的所述标记进行误检测处理，控制平粮机器人主机和平粮机器人从机进行同步，对记录的所述标记的种类和数量进行统计，得到平粮机器人在粮仓的位置；
当所述检测模块没有检测到所述标记时，对记录的所述标记的种类和数量进行统计，并结合里程计信息计算得到位移，根据所述位移来控制平粮机器人主机和平粮机器人从机进行同步，得到平粮机器人在粮仓的位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010055603.7A CN111240330B (zh) | 2020-01-17 | 2020-01-17 | 一种平粮机器人同步导航和精确定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010055603.7A CN111240330B (zh) | 2020-01-17 | 2020-01-17 | 一种平粮机器人同步导航和精确定位方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111240330A CN111240330A (zh) | 2020-06-05 |
| CN111240330B true CN111240330B (zh) | 2021-03-23 |
Family
ID=70863980
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010055603.7A Active CN111240330B (zh) | 2020-01-17 | 2020-01-17 | 一种平粮机器人同步导航和精确定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111240330B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114157981B (zh) * | 2021-11-29 | 2024-08-02 | 歌尔科技有限公司 | 一种距离安全检测方法及可穿戴设备 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107121981A (zh) * | 2017-04-20 | 2017-09-01 | 杭州南江机器人股份有限公司 | 一种基于视觉的agv巡线导航和定位方法 |
| CN108225303A (zh) * | 2018-01-18 | 2018-06-29 | 水岩智能科技（宁波）有限公司 | 二维码定位标签、基于二维码的定位导航系统和方法 |
| CN109556616A (zh) * | 2018-11-09 | 2019-04-02 | 同济大学 | 一种基于视觉标记的自动建图机器人建图修整方法 |
| CN109631851A (zh) * | 2019-01-31 | 2019-04-16 | 电子科大科园股份有限公司 | 一种平粮机器人的同步计算方法 |
| CN109799825A (zh) * | 2019-01-31 | 2019-05-24 | 电子科大科园股份有限公司 | 一种平粮机器人的同步走道方法 |
| CN109795900A (zh) * | 2019-01-31 | 2019-05-24 | 电子科大科园股份有限公司 | 一种精平机器人的平粮方法 |
| CN109933065A (zh) * | 2019-02-21 | 2019-06-25 | 四川阿泰因机器人智能装备有限公司 | 一种精平机器人电气控制方法 |
| CN110531772A (zh) * | 2019-09-12 | 2019-12-03 | 四川阿泰因机器人智能装备有限公司 | 一种平粮机器人的控制方法及其系统 |
| CN110660097A (zh) * | 2019-09-12 | 2020-01-07 | 四川阿泰因机器人智能装备有限公司 | 一种平粮机器人定位和动作控制方法 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9625481B2 (en) * | 2012-05-24 | 2017-04-18 | Siemens Healthcare Diagnostics Inc. | Non-contact optical encoding scheme for intelligent automation puck |
| KR20210015211A (ko) * | 2019-08-01 | 2021-02-10 | 엘지전자 주식회사 | 실시간으로 클라우드 슬램을 수행하는 방법 및 이를 구현하는 로봇과 클라우드 서버 |
-
2020
- 2020-01-17 CN CN202010055603.7A patent/CN111240330B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107121981A (zh) * | 2017-04-20 | 2017-09-01 | 杭州南江机器人股份有限公司 | 一种基于视觉的agv巡线导航和定位方法 |
| CN108225303A (zh) * | 2018-01-18 | 2018-06-29 | 水岩智能科技（宁波）有限公司 | 二维码定位标签、基于二维码的定位导航系统和方法 |
| CN109556616A (zh) * | 2018-11-09 | 2019-04-02 | 同济大学 | 一种基于视觉标记的自动建图机器人建图修整方法 |
| CN109631851A (zh) * | 2019-01-31 | 2019-04-16 | 电子科大科园股份有限公司 | 一种平粮机器人的同步计算方法 |
| CN109799825A (zh) * | 2019-01-31 | 2019-05-24 | 电子科大科园股份有限公司 | 一种平粮机器人的同步走道方法 |
| CN109795900A (zh) * | 2019-01-31 | 2019-05-24 | 电子科大科园股份有限公司 | 一种精平机器人的平粮方法 |
| CN109933065A (zh) * | 2019-02-21 | 2019-06-25 | 四川阿泰因机器人智能装备有限公司 | 一种精平机器人电气控制方法 |
| CN110531772A (zh) * | 2019-09-12 | 2019-12-03 | 四川阿泰因机器人智能装备有限公司 | 一种平粮机器人的控制方法及其系统 |
| CN110660097A (zh) * | 2019-09-12 | 2020-01-07 | 四川阿泰因机器人智能装备有限公司 | 一种平粮机器人定位和动作控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| 复合式平粮机器人系统方案设计与关键技术研究;尹强 等;《武汉轻工大学学报》;20190925;第32卷(第4期);全文 * |
| 智能平仓机器人控制系统设计与实现;杨耀明;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200715;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111240330A (zh) | 2020-06-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Fallon et al. | Continuous humanoid locomotion over uneven terrain using stereo fusion | |
| KR102502219B1 (ko) | 로봇 운동시 카펫으로 인한 드리프트 제어방법, 칩 및 청소 로봇 | |
| CN110262495A (zh) | 可实现移动机器人自主导航与精确定位的控制系统及方法 | |
| US12094195B2 (en) | Identifying stairs from footfalls | |
| CN111693050A (zh) | 基于建筑信息模型的室内中大型机器人导航方法 | |
| TW202144150A (zh) | 定位方法、機器人及儲存介質 | |
| CN114758504B (zh) | 一种基于滤波校正的网联车超速预警方法及系统 | |
| CN113804184A (zh) | 基于多传感器的地面机器人定位方法 | |
| CN111240330B (zh) | 一种平粮机器人同步导航和精确定位方法及系统 | |
| WO2022151794A1 (zh) | 基于无线测距传感器的移动机器人定位方法、系统及芯片 | |
| CN109085840A (zh) | 一种基于双目视觉的车辆导航控制系统及控制方法 | |
| CN112462762A (zh) | 一种基于路侧二维码单元的机器人室外自主移动系统及其方法 | |
| CN103472844A (zh) | 基于电子标签自校正的移动平台定位系统 | |
| CN113610910B (zh) | 一种移动机器人避障方法 | |
| CN112880683A (zh) | 基于参考直线距离的机器人定位控制方法、系统及芯片 | |
| CN103363916B (zh) | 一种信息处理方法及处理设备 | |
| CN115979249A (zh) | 巡检机器人的导航方法及装置 | |
| CN116476047A (zh) | 一种自动铺设二维码的方法、装置、机器人及系统 | |
| Liang et al. | 4D Point Cloud (4DPC)-driven real-time monitoring of construction mobile cranes | |
| Bayramoglu et al. | Mobile robot navigation in a corridor using visual odometry | |
| JP2013250795A (ja) | 移動体誘導装置及び移動体誘導方法 | |
| CN110857861B (zh) | 轨迹规划方法与系统 | |
| Banta | Advanced dead reckoning navigation for mobile robots | |
| CN104390642A (zh) | 可远程监控的全向测距室内自主侦测导航设备 | |
| US11846718B2 (en) | Location measuring system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |