CN113537096A - 基于ros的agv叉车库位托盘识别及辅助定位方法及*** - Google Patents

Abstract

本发明公开了一种基于ROS的AGV叉车库位托盘识别及辅助定位方法及***，包括对库位中托盘的检测识别步骤，使用深度学习训练好的模型预测图像中托盘支撑柱及其像素位置，利用深度相机计算出各根支撑柱中心在相机坐标系下的三维坐标；托盘位姿计算步骤，通过对深度相机坐标系、AGV叉车货叉坐标系、AGV叉车本体坐标系之间的坐标转化关系计算，求解托盘相对于AGV叉车本体的位姿；控制AGV叉车及货叉运动至可插取位置步骤，使得AGV叉车的货叉能够正对托盘的空洞。本发明检测速度更快，能够实时对库位中的托盘进行精确检测与定位，提升了工作效率，更好的辅助叉车插取托盘。

Description

基于ROS的AGV叉车库位托盘识别及辅助定位方法及***

技术领域

本发明涉及一种基于ROS的AGV叉车库位托盘识别及其插取辅助定位方法及***。

背景技术

随着人工智能技术的飞速发展，机器人的智能化也是必然趋势。同时随着机器人技术的成熟，机器人的应用范围也在不断的拓展，有适用于工业生产制造的工业机器人、服务人们日常生活的家庭服务机器人、辅助医生和病人的医疗机器人、用于国防部队的军事机器人等。

AGV(Automated Guided Vehicle)叉车是工业机器人的一种，具有移动、自动导航、多传感器控制、网络交互、装卸托运等功能。AGV叉车作为一种人工智能化工业装卸车，完成的任务主要有对托盘的货物进行装卸和用作短途交通运输的装卸车等。随着仓库周转率和订单量增加，为保证工作效率，越来越多的用户将传统人工叉车换成无人的AGV叉车，无人的AGV叉车有着更为广阔的应用前景。利用独有的视觉和人工智能技术，未来机器人为工厂和仓库的物流无人化升级提供柔性解决方案。计算机视觉的深层应用赋予了无人叉车更高的定位与感知精度，结合运动控制、深度学习等技术，AGV叉车更加聪明，可快速筛选并执行最优的任务完成策略。

在AGV叉车进行货物装卸搬运工作的工厂中，因库位中的托盘及其货物也会存在被人工插走的可能，所以AGV叉车在取货时，需要在进工位前使用视觉相机去实时检测识别对应的目标库位点有无托盘，进而告知***该库位点货物已被搬运走，同时AGV叉车本身也不用继续去该库位点取货。另一方面，有时货物也会出现放错位置的情况，如将货物错放在其他种类货物的库位中。这时如果用不同类型的托盘放置不同种类的货物，利用视觉相机对不同托盘进行识别，就可以区分开不同货物，进而判断所插取的货物是否正确。传统机器视觉感知方法主要通过人工设计的特征，利用特征点进行匹配等来识别目标对象。这种算法在实际的应用中，由于目标物体的形态复杂、环境的光线变化常常表现的不尽人意。目前基于深度神经网络的检测技术已经成为主流方法，并且在开放目标检测数据集上的检测效果和性能都很出色。

在库位摆放托盘时，有时未能严格按照规定的要求摆放，托盘摆放不整齐，这就导致托盘的位置与原定设置的摆放位置存在倾斜偏差。如果AGV叉车继续按原定的规划路径和程序取执行插取任务，会出现货叉插不准的情况出现。此时利用搭载的传感器设备，在原定规划的路径基础上来辅助AGV叉车对托盘进行定位，可以使得叉车能够更加精准地对托盘进行插取装卸作业。

ROS:ROS(Robot Operating System,机器人操作***)提供一系列程序库和工具以帮助软件开发者创建机器人应用软件。它提供了硬件抽象、设备驱动、函数库、可视化工具、消息传递和软件包管理等诸多功能。ROS执行若干种类型的通讯，包括基于服务的同步RPC(Remote Procedure Call，远程过程调用)通讯、基于Topic(话题)的异步数据流通讯，还有参数服务器上的数据存储。

YOLOv4:YOLO(You Only Look Once,你只要看一次)是一种深度学习的目标检测方法，该方法的特点是实现快速检测的同时还达到较高的准确率，YOLO采用单个神经网络直接预测物品边界和类别概率，实现端到端的物品检测。YOLOv4为YOLO系列***目标检测方法，继承了YOLO系列的思想和理念，可以使用传统的GPU(Graphics Processing Unit,图像处理单元)进行训练和测试，并能够获得实时、高精度的检测结果。

发明内容

1、本发明的目的

发明的目的是辅助AGV叉车在进入库位插取托盘前检测托盘是否还在原位置，以及检测托盘种类是否正确，避免重复无用操作。同时由于托盘放置不严格导致的错位，插取辅助定位***可以协助AGV叉车准确的对托盘进行定位，从而更好地完成AGV叉车插取拖运作业。

2、本发明所采用的技术方案

本发明公开了一种基于ROS的AGV叉车库位托盘识别及辅助定位方法，包括：

对库位中托盘的检测识别步骤，使用深度学习训练好的模型预测图像中托盘支撑柱及其像素位置，利用深度相机计算出各根支撑柱中心在相机坐标系下的三维坐标；

托盘位姿计算步骤，通过对深度相机坐标系、AGV叉车货叉坐标系、AGV叉车本体坐标系之间的坐标转化关系计算，求解托盘相对于AGV叉车本体的位姿；

控制AGV叉车及货叉运动至可插取位置步骤，使得AGV叉车的货叉能够正对托盘的空洞。

更进一步，检测识别步骤S1：

S11、对库位中的托盘样本数据进行拍摄采集；

S12、使用图像标注软件LabelImg对样本数据进行标注，框选出各图中的不同种类托盘的支撑柱，标记不同种类托盘的标签保存并输出标注文件，之后划分数据集；

S13、物体检测模型的训练，搭建基于深度学习YOLOv4-CSP目标检测网络；YOLOv4的网络结构分为四部分，包括输入、主干、颈部、头部；YOLOv4在输入端采用丰富检测数据集的方法，包括马赛克数据增强、SAT策略；YOLOv4的BackBone以CSPDarknet53网络框架作为网络提取骨干用于特征提取；颈部部分主要采用了SPP模块、FPN和PAN的方式，用SPP模块来融合不同尺度大小的特征图，增加主干特征的接收范围，利用自顶向下的FPN特征金字塔和自底向上的PAN特征金字塔来提升网络的特征提取能力；YOLOv4的头部部分训练的损失函数为L_CIOU，L_CIOU是在计算边界框回归时，同时考虑了预测框A和真实框B的重叠面积、中心点距离和宽高比的损失函数；L_CIOU具体的计算方法如下式：

式中，Distance_2为预测框和真实框中心点的欧式距离，Distance_C为预测框和真实框最小外接矩形的对角线距离；IOU(Intersection over Union,交并比)是测量在特定数据集中检测相应物体准确度的一个标准，其计算公式为：

其中∩表示两者的并集，∩表示两者的交集；式(1)中的v是用于衡量宽高比一致性的参数，计算公式为：

式中，w^gt、h^gt分别为真实框的宽和高，w^p、h^p分别为预测框的宽和高，arctan是反正切函数；

YOLOv4-CSP在Neck部分的FPN、PAN的上采样和下采样阶段和SPP模块中加入了跨阶段局部网络结构CSPNet，将基础层的特征图分割成两部分，然后通过跨阶段分层结构进行合并；

S14、在GPU计算机上通过训练得到效果好的托盘支撑柱检测识别模型。

更进一步，托盘位姿计算步骤S2：

S21、打开叉车货叉上的深度相机，运行YOLOv4-CSP检测模型，检测的数据会通过ROS发送bounging_boxes的话题，bounging_boxes包含检测框信息并通过ROS发送的话题消息；该话题中包含了每个被识别到的托盘支撑柱、托盘的种类、托盘支撑柱的置信度、检测框的左上和右下点的像素坐标；

S22、在ROS下创建YoloObject和YoloObjects两个消息；YoloObject和YoloObjects是自定义的消息类型，用于存放转换后的坐标信息；YoloObject消息中包含着单个托盘支撑柱的标签信息和中心点的三维坐标，YoloObjects消息用于存放所有YoloObject消息；

S23、通过ROS订阅bounging_boxes和相机的彩色图像、深度数据、内参话题消息，将支撑柱中心点的二维像素坐标(u,v)转化为三维坐标(x,y,z)，转化计算如下：

z＝0.001*d (4)

其中，d为该像素点的深度数据，f_x，f_y为相机的焦距，c_x，c_y为相机主点；

S24、将托盘支撑柱中心点的三维坐标(x,y,z)信息和托盘的种类信息存放在YoloObject中。

更进一步，所述的步骤S24，种类信息存放在YoloObject中：

判断每个支撑柱的x轴坐标大小，从小到大依次排列，分别为左侧的支撑柱a、中间的支撑柱b、右侧的支撑柱c；然后按顺序将每个YoloObject添加到YoloObjects中，利用ROS将YoloObjects通过自定义的pose_point话题发布出去；pose_point话题是被创建用于发送YoloObjects消息的ROS话题。

更进一步，S3、AGV叉车接收到检测信息，控制AGV叉车及货叉运动至可插取位置；具体步骤如下：

S31、AGV叉车运动至需要插取的托盘库位处，设置货叉的控制参数，控制货叉升降至初始准备位置；根据升降位置确定货叉坐标系{H}与叉车本体坐标系{B}变换矩阵

S32、通过手眼标定，确定相机坐标系{C}到货叉坐标系{H}下的变换矩阵

打开深度相机进行S2的检测步骤，通过相机检测托盘的支撑架来判断此处库位的托盘是否被取走以及判断托盘类型，如果没有检测到托盘的支撑架，则提示控制***托盘已被取走；如果检测到托盘类型不正确，提示控制***托盘托盘货物放错位置；如果检测到了正确托盘的支撑架，则将结果通过pose_point话题发送出去；

S33、AGV叉车订阅pose_point话题，将托盘三根支撑柱a、b、c在相机坐标系{C}下中心点的三维坐标^CP_a、^CP_b、^CP_c转化到叉车本体坐标系{B}下：

式中，^HP_a、^HP_b、^HP_c、^BP_a、^BP_b、^BP_c分别是三根支撑柱的中心点在货叉坐标系和AGV叉车本体坐标系的坐标；

S34、要求AGV叉车能够插取托盘则需要货叉正对托盘，即托盘面的中心与货叉的中心点对齐，且托盘插取面平行于叉车的门架面；托盘支撑柱a、b、c在AGV叉车本体坐标系的三维坐标分别为^BP_a＝[x_a,y_a,z_a]^T、^BP_b＝[x_b,y_b,z_b]^T、^BP_c＝[x_c,y_c,z_c]^T；托盘的中心位置即为支撑柱b的坐标^BP_b＝[x_b,y_b,z_b]^T，求解托盘插取面相对于叉车门架面的偏转角度θ，其中l为托盘的长度：

Δz＝z_c-z_a (7)

式中，arcsin为反正弦函数；

S35、AGV叉车根据获得的托盘位姿信息{x_b,y_b,z_b,θ}，包含托盘插取面中心在叉车本体坐标系下的x、y、z轴下的坐标[x_b,y_b,z_b]^T以及托盘插取面相对于叉车门架面的偏转角度θ；通过这些信息对叉车本体及货叉升降的高度位置进行调整，使得AGV叉车的货叉正对着托盘的托孔，AGV叉车向前行驶插取托盘，完场任务。

本发明提出了一种基于ROS的AGV叉车库位托盘识别及辅助定位***，包括存储器和处理器，存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述的方法步骤。

3、本发明所采用的有益效果

1)本发明利用深度学习的检测模型能够很好的对托盘进行识别。传统的方法是利用点云信息对托盘进行轮廓提取，通过轮廓的边缘之间宽度来识别是否为托盘的孔洞和支撑柱，从而判断是否为托盘。本发明提出的深度学习的检测方法相较于传统的方法，识别效果更佳，准确性更高，鲁棒性更好。

2)本发明利用改进的YOLOv4深度学习网络结构YOLOv4-CSP对托盘样本进行训练，训练好的网络模型检测速度更快，使得AGV叉车能够实时对库位中的托盘进行检测，提升了工作效率。

3)本发明中的AGV叉车插取时的辅助定位***与传统的AGV预设好坐标轨迹相比，当托盘由于摆放不严格等原因导致与原定位置不一致时，利用深度相机和深度学习的检测模型对托盘进行定位，能够更好的辅助叉车插取托盘。

4)本发明均在ROS框架下运行，模块之间的结果通过ROS***以话题的形式发布订阅进行通讯传输，***运行更加实时通畅。

附图说明

图1为深度学习检测识别模型的训练流程图。

图2为AGV叉车库位托盘识别及其插取辅助定位***的流程图。

图3为AGV叉车需要识别搬运的托盘类型，a、b、c分别表示托盘的支撑柱。

图4为托盘相对AGV叉车位姿的俯视图。

图5为检测识别模块和AGV叉车控制模块在ROS下的话题发布和订阅的可视化图。

图6为AGV叉车结构示意简图。

具体实施方式

下面结合本发明实例中的附图，对本发明实例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域技术人员在没有做创造性劳动前提下所获得的所有其他实施例，都属于本发明的保护范围。

下面将结合附图对本发明实例作进一步地详细描述。

实施例1

一种基于ROS的AGV叉车库位托盘识别及其插取辅助定位***，其包含对库位中托盘的检测识别和对托盘位姿计算两部分。库位托盘的检测部分包含了深度相机和搭载GPU的计算机，计算机通过使用深度学习训练好的模型预测图像中托盘支撑柱及其像素位置，利用深度相机计算出各根支撑柱中心在相机坐标系下的三维坐标。托盘位姿计算部分通过对深度相机坐标系、AGV叉车货叉坐标系、AGV叉车本体坐标系之间的坐标转化关系计算，求解托盘相对于AGV叉车本体的位姿，从而控制AGV叉车运动使得AGV叉车的货叉能够正对托盘的空洞。通过基于ROS的AGV叉车库位托盘识别及其插取辅助定位***可以使得AGV叉车能够更有效的对库位中的托盘进行插取拖运工作，提高生产效率。

本发明主要实现步骤包含如下：

S1、基于改进的YOLOv4目标检测网络YOLOv4-CSP来训练托盘的检测识别模型，具体步骤为：

S11、对库位中的托盘样本数据进行拍摄采集。

S12、使用图像标注软件LabelImg对样本数据进行标注，框选出各图中的不同种类托盘的支撑柱，标记不同种类托盘的标签。保存并输出标注文件，之后划分数据集，训练集和测试集的比例为9:1。

S13、物体检测模型的训练，搭建基于深度学习YOLOv4-CSP目标检测网络。YOLOv4的网络结构可以分为四部分，Input(输入)、BackBone(主干)、Neck(颈部)、Head(头部)。YOLOv4在Input输入端有马赛克数据增强、SAT(Self-adversarial-training,自对抗训练)等策略，丰富了检测数据集。YOLOv4的BackBone以CSPDarknet53网络框架作为网络提取骨干用于特征提取。Neck部分主要采用了SPP(Spatial Pyramid Pooling,空间金字塔池化)模块、FPN(Feature Pyramid Networks,特征图金字塔网络)和PAN(Path AggregationNetwork,路径聚合网络)的方式，用SPP模块来融合不同尺度大小的特征图能有效的增加主干特征的接收范围，利用自顶向下的FPN特征金字塔和自底向上的PAN特征金字塔来提升网络的特征提取能力。YOLOv4的Head部分训练的损失函数为L_CIOU，L_CIOU是在计算边界框回归时，同时考虑了预测框A和真实框B的重叠面积、中心点距离和宽高比的损失函数。L_CIOU具体的计算方法如下式：

式中，Distance_2为预测框和真实框中心点的欧式距离，Distance_C为预测框和真实框最小外接矩形的对角线距离。IOU(Intersection over Union,交并比)是一种测量在特定数据集中检测相应物体准确度的一个标准，其计算公式为：

其中∩表示两者的并集，∩表示两者的交集。式(1)中的v是用于衡量宽高比一致性的参数，计算公式为：

式中，w^gt、h^gt分别为真实框的宽和高，w^p、h^p分别为预测框的宽和高，arctan是反正切函数。

与YOLOv4相比，YOLOv4-CSP在Neck部分的FPN、PAN的上采样和下采样阶段和SPP模块中加入了跨阶段局部网络结构CSPNet(Cross Stage Partial Network)。CSPNet将基础层的特征图分割成两部分，然后通过跨阶段分层(cross-stage hierarchy)结构进行合并。CSPNet网络结构可以增强CNN(Convolutional Neural Network,卷积神经网络)的学习能力，轻量化模型的同时保持模型的精度，降低整个模型的计算瓶颈，同时降低算法的内存成本。CSPNet的主要目的是使这个架构能够实现更丰富的梯度组合，同时减少计算量。YOLOv4-CSPNet网络模型相较于YOLOv4在普通GPU处理器上有更佳均衡的速度和精度。

S14、在GPU计算机上通过训练得到效果好的托盘支撑柱检测识别模型。

S2、利用训练好的模型检测托盘，通过深度相机计算托盘三根支撑柱中心的三维坐标，通过ROS将检测的数据消息发送给AGV叉车，具体步骤如下：

S21、打开叉车货叉上的深度相机，运行YOLOv4-CSP检测模型，检测的数据会通过ROS发送bounging_boxes的话题。bounging_boxes是包含着检测框信息并通过ROS发送的话题消息。该话题中包含了每个被识别到的托盘支撑柱、托盘的种类、托盘支撑柱的置信度、检测框的左上和右下点的像素坐标等。

S22、在ROS下创建YoloObject和YoloObjects两个消息。YoloObject和YoloObjects是自定义的消息类型，主要用于存放转换后的坐标信息。YoloObject消息中包含着单个托盘支撑柱的标签信息和中心点的三维坐标，YoloObjects消息用于存放所有YoloObject消息。

S23、通过ROS订阅bounging_boxes和相机的彩色图像、深度数据、内参话题消息，将支撑柱中心点的二维像素坐标(u,v)转化为三维坐标(x,y,z)，转化计算如下：

z＝0.001*d (4)

其中，d为该像素点的深度数据，f_x，f_y为相机的焦距，c_x，c_y为相机主点。

S24、将托盘支撑柱中心点的三维坐标(x,y,z)信息和托盘的种类信息存放在YoloObject中。判断每个支撑柱的x轴坐标大小，从小到大依次排列，分别为左侧的支撑柱a、中间的支撑柱b、右侧的支撑柱c。然后按顺序将每个YoloObject添加到YoloObjects中，利用ROS将YoloObjects通过自定义的pose_point话题发布出去。pose_point话题是被创建用于发送YoloObjects消息的ROS话题。

S3、AGV叉车接收到检测信息，控制AGV叉车及货叉运动至可插取位置。具体步骤如下：

S31、AGV叉车运动至需要插取的托盘库位处，设置货叉的控制参数，控制货叉升降至初始准备位置。根据升降位置确定货叉坐标系{H}与叉车本体坐标系{B}变换矩阵

S32、通过手眼标定，确定相机坐标系{C}到货叉坐标系{H}下的变换矩阵

打开深度相机进行S2的检测步骤，通过相机检测托盘的支撑架来判断此处库位的托盘是否被取走以及判断托盘类型，如果没有检测到托盘的支撑架，则提示控制***托盘已被取走；如果检测到托盘类型不正确，提示控制***托盘托盘货物放错位置；如果检测到了正确托盘的支撑架，则将结果通过pose_point话题发送出去。

S33、AGV叉车订阅pose_point话题，将托盘三根支撑柱a、b、c在相机坐标系{C}下中心点的三维坐标^CP_a、^CP_b、^CP_c转化到叉车本体坐标系{B}下：

式中，^HP_a、^HP_b、^HP_c、^BP_a、^BP_b、^BP_c分别是三根支撑柱的中心点在货叉坐标系和AGV叉车本体坐标系的坐标。

S34、要求AGV叉车能够插取托盘则需要货叉正对托盘，即托盘面的中心与货叉的中心点对齐，且托盘插取面平行于叉车的门架面。托盘支撑柱a、b、c在AGV叉车本体坐标系的三维坐标分别为^BP_a＝[x_a,y_a,z_a]^T、^BP_b＝[x_b,y_b,z_b]^T、^BP_c＝[x_c,y_c,z_c]^T。托盘的中心位置即为支撑柱b的坐标^BP_b＝[x_b,y_b,z_b]^T，求解托盘插取面相对于叉车门架面的偏转角度θ，其中l为托盘的长度：

Δz＝z_c-z_a (7)

式中，arcsin为反正弦函数。

S35、AGV叉车根据获得的托盘位姿信息{x_b,y_b,z_b,θ}，这些信息包含托盘插取面中心在叉车本体坐标系下的x、y、z轴下的坐标[x_b,y_b,z_b]^T以及托盘插取面相对于叉车门架面的偏转角度θ。通过这些信息对叉车本体及货叉升降的高度位置进行调整，使得AGV叉车的货叉正对着托盘的托孔，AGV叉车向前行驶插取托盘，完场任务。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (6)

1.一种基于ROS的AGV叉车库位托盘识别及辅助定位方法，其特征在于，包括：

对库位中托盘的检测识别步骤，使用深度学习训练好的模型预测图像中托盘支撑柱及其像素位置，利用深度相机计算出各根支撑柱中心在相机坐标系下的三维坐标；

托盘位姿计算步骤，通过对深度相机坐标系、AGV叉车货叉坐标系、AGV叉车本体坐标系之间的坐标转化关系计算，求解托盘相对于AGV叉车本体的位姿；

控制AGV叉车及货叉运动至可插取位置步骤，使得AGV叉车的货叉能够正对托盘的空洞。

2.根据权利要求1所述的基于ROS的AGV叉车库位托盘识别及辅助定位方法，其特征在于检测识别步骤S1：

S11、对库位中的托盘样本数据进行拍摄采集；

S12、使用图像标注软件LabelImg对样本数据进行标注，框选出各图中的不同种类托盘的支撑柱，标记不同种类托盘的标签保存并输出标注文件，之后划分数据集；

S13、物体检测模型的训练，搭建基于深度学习YOLOv4-CSP目标检测网络；YOLOv4的网络结构分为四部分，包括输入、主干、颈部、头部；YOLOv4在输入端采用丰富检测数据集的方法，包括马赛克数据增强、SAT策略；YOLOv4的BackBone以CSPDarknet53网络框架作为网络提取骨干用于特征提取；颈部部分主要采用了SPP模块、FPN和PAN的方式，用SPP模块来融合不同尺度大小的特征图，增加主干特征的接收范围，利用自顶向下的FPN特征金字塔和自底向上的PAN特征金字塔来提升网络的特征提取能力；YOLOv4的头部部分训练的损失函数为L_CIOU，L_CIOU是在计算边界框回归时，同时考虑了预测框A和真实框B的重叠面积、中心点距离和宽高比的损失函数；L_CIOU具体的计算方法如下式：

式中，Distance_2为预测框和真实框中心点的欧式距离，Distance_C为预测框和真实框最小外接矩形的对角线距离；IOU(Intersection over Union,交并比)是测量在特定数据集中检测相应物体准确度的一个标准，其计算公式为：

其中∩表示两者的并集，∩表示两者的交集；式(1)中的v是用于衡量宽高比一致性的参数，计算公式为：

式中，w^gt、h^gt分别为真实框的宽和高，w^p、h^p分别为预测框的宽和高，arctan是反正切函数；

YOLOv4-CSP在Neck部分的FPN、PAN的上采样和下采样阶段和SPP模块中加入了跨阶段局部网络结构CSPNet，将基础层的特征图分割成两部分，然后通过跨阶段分层结构进行合并；

S14、在GPU计算机上通过训练得到效果好的托盘支撑柱检测识别模型。

3.根据权利要求2所述的基于ROS的AGV叉车库位托盘识别及辅助定位方法，其特征在于，所述的托盘位姿计算步骤S2：

S21、打开叉车货叉上的深度相机，运行YOLOv4-CSP检测模型，检测的数据会通过ROS发送bounging_boxes的话题，bounging_boxes包含检测框信息并通过ROS发送的话题消息；该话题中包含了每个被识别到的托盘支撑柱、托盘的种类、托盘支撑柱的置信度、检测框的左上和右下点的像素坐标；

S22、在ROS下创建YoloObject和YoloObjects两个消息；YoloObject和YoloObjects是自定义的消息类型，用于存放转换后的坐标信息；YoloObject消息中包含着单个托盘支撑柱的标签信息和中心点的三维坐标，YoloObjects消息用于存放所有YoloObject消息；

S23、通过ROS订阅bounging_boxes和相机的彩色图像、深度数据、内参话题消息，将支撑柱中心点的二维像素坐标(u,v)转化为三维坐标(x,y,z)，转化计算如下：

z＝0.001*d (4)

其中，d为该像素点的深度数据，f_x，f_y为相机的焦距，c_x，c_y为相机主点；

S24、将托盘支撑柱中心点的三维坐标(x,y,z)信息和托盘的种类信息存放在YoloObject中。

4.根据权利要求2所述的基于ROS的AGV叉车库位托盘识别及辅助定位方法，其特征在于，所述的步骤S24，种类信息存放在YoloObject中：

判断每个支撑柱的x轴坐标大小，从小到大依次排列，分别为左侧的支撑柱a、中间的支撑柱b、右侧的支撑柱c；然后按顺序将每个YoloObject添加到YoloObjects中，利用ROS将YoloObjects通过自定义的pose_point话题发布出去；pose_point话题是被创建用于发送YoloObjects消息的ROS话题。

5.根据权利要求3所述的基于ROS的AGV叉车库位托盘识别及辅助定位方法，其特征在于，所述的S3、AGV叉车接收到检测信息，控制AGV叉车及货叉运动至可插取位置；具体步骤如下：

S31、AGV叉车运动至需要插取的托盘库位处，设置货叉的控制参数，控制货叉升降至初始准备位置；根据升降位置确定货叉坐标系{H}与叉车本体坐标系{B}变换矩阵

S32、通过手眼标定，确定相机坐标系{C}到货叉坐标系{H}下的变换矩阵

打开深度相机进行S2的检测步骤，通过相机检测托盘的支撑架来判断此处库位的托盘是否被取走以及判断托盘类型，如果没有检测到托盘的支撑架，则提示控制***托盘已被取走；如果检测到托盘类型不正确，提示控制***托盘托盘货物放错位置；如果检测到了正确托盘的支撑架，则将结果通过pose_point话题发送出去；

S33、AGV叉车订阅pose_point话题，将托盘三根支撑柱a、b、c在相机坐标系{C}下中心点的三维坐标^CP_a、^CP_b、^CP_c转化到叉车本体坐标系{B}下：

式中，^HP_a、^HP_b、^HP_c、^BP_a、^BP_b、^BP_c分别是三根支撑柱的中心点在货叉坐标系和AGV叉车本体坐标系的坐标；

S34、要求AGV叉车能够插取托盘则需要货叉正对托盘，即托盘面的中心与货叉的中心点对齐，且托盘插取面平行于叉车的门架面；托盘支撑柱a、b、c在AGV叉车本体坐标系的三维坐标分别为^BP_a＝[x_a,y_a,z_a]^T、^BP_b＝[x_b,y_b,z_b]^T、^BP_c＝[x_c,y_c,z_c]^T；托盘的中心位置即为支撑柱b的坐标^BP_b＝[x_b,y_b,z_b]^T，求解托盘插取面相对于叉车门架面的偏转角度θ，其中l为托盘的长度：

Δz＝z_c-z_a (7)

式中，arcsin为反正弦函数；

S35、AGV叉车根据获得的托盘位姿信息{x_b,y_b,z_b,θ}，包含托盘插取面中心在叉车本体坐标系下的x、y、z轴下的坐标[x_b,y_b,z_b]^T以及托盘插取面相对于叉车门架面的偏转角度θ；通过这些信息对叉车本体及货叉升降的高度位置进行调整，使得AGV叉车的货叉正对着托盘的托孔，AGV叉车向前行驶插取托盘，完场任务。

6.一种基于ROS的AGV叉车库位托盘识别及辅助定位***，包括存储器和处理器，存储器存储有计算机程序，其特征在于：所述处理器执行所述计算机程序时实现如权利要求1-5任一所述的方法步骤。

Images