CN117830679A - 货架识别方法、机器人控制方法、终端设备以及存储介质 - Google Patents

货架识别方法、机器人控制方法、终端设备以及存储介质 Download PDF

Info

Publication number
CN117830679A
CN117830679A CN202410010597.1A CN202410010597A CN117830679A CN 117830679 A CN117830679 A CN 117830679A CN 202410010597 A CN202410010597 A CN 202410010597A CN 117830679 A CN117830679 A CN 117830679A
Authority
CN
China
Prior art keywords
shelf
point cloud
leg
candidate
legs
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.)
Pending
Application number
CN202410010597.1A
Other languages
English (en)
Inventor
陈惠林
胡立志
廖琼珍
李翔
卢维
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Huaray Technology Co Ltd
Original Assignee
Zhejiang Huaray Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Huaray Technology Co Ltd filed Critical Zhejiang Huaray Technology Co Ltd
Priority to CN202410010597.1A priority Critical patent/CN117830679A/zh
Publication of CN117830679A publication Critical patent/CN117830679A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/25—Determination of region of interest [ROI] or a volume of interest [VOI]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Warehouses Or Storage Devices (AREA)

Abstract

本申请提出一种货架识别方法、机器人控制方法、终端设备以及计算机存储介质。所述货架识别方法包括：获取机器人前方的三维点云和二维点云；基于所述三维点云，获取货架腿三维点云以及确定货架模型；按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值；利用所述货架中心位姿初值和所述货架腿位姿初值，对所述二维点云进行过滤，获取二维观测点云；利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿。通过上述货架识别方法，采用了三维点云粗识别与二维点云精识别相结合的方式，精确识别货架中心的位置。

Description

货架识别方法、机器人控制方法、终端设备以及存储介质
技术领域
本申请涉及货架自动搬运技术领域，特别是涉及一种货架识别方法、机器人控制方法、终端设备以及计算机存储介质。
背景技术
在物流和工业领域，移动机器人一般指自动引导运输车(Automated GuidedVehicle，简称AGV)，是一种装备有电磁或光学等自动导引装置、能够沿规定的导引路径行驶，并且具有安全保护和移载功能的运输车，可以代替人工完成货架等容器的托举和搬运。移动机器人执行搬运货架任务需要准确移动到货架正下方，在一些情况下，货架摆放不够准确或移动机器人定位信息不够准确，使得移动机器人无法准确到达货架正下方。
发明内容
为解决上述技术问题，本申请提出了一种货架识别方法、机器人控制方法、终端设备以及计算机存储介质。
为解决上述技术问题，本申请提出了一种货架识别方法，所述货架识别方法包括：
获取机器人前方的三维点云和二维点云；
基于所述三维点云，获取货架腿三维点云以及确定货架模型；
按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值；
利用所述货架中心位姿初值和所述货架腿位姿初值，对所述二维点云进行过滤，获取二维观测点云；
利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿。
其中，所述基于所述三维点云，获取货架腿三维点云以及确定货架模型，包括：
获取所述三维点云中的若干候选货架腿点云聚类集合；
将每一候选货架腿点云聚类集合的中心点作为候选货架腿；
遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，获取剩余的候选货架腿组合；
获取所述候选货架腿组合的货架腿间距和每一货架模型的货架腿间距；
若存在任意一候选货架腿组合的货架腿间距和任意一货架模型的货架腿间距的残差小于预设残差阈值，则获取该候选货架腿组合的货架腿三维点云以及该货架模型。
其中，所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
获取所述候选货架腿组合在车体坐标系的向量；
将所述向量与所述车体坐标系的第一预设轴夹角大于预设角度阈值的候选货架腿组合排除。
其中，所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
判断所述候选货架腿组合之间的区域，或者所述候选货架腿与所述机器人之间的视野内是否存在其他候选货架腿；
若是，则排除所述候选货架腿组合。
其中，所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
获取所有货架模型的最大深度值和/或最小深度值；
利用所述最大深度值和/或所述最小深度值，以所述候选货架腿组合的识别面为边，构建候选范围区域；
响应于所述候选范围区域内存在其他候选货架腿，排除所述候选货架腿组合。
其中，所述按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值，包括：
基于所述货架腿三维点云构造的协方差矩阵进行分解，获取所述协方差矩阵的特征值和特征向量；
选取最小特征值对应的特征向量，作为货架朝向初值；
利用所述货架腿三维点云，确定货架腿前沿中心在所述特征向量为基轴的坐标系上的货架腿前沿中心坐标；
按照所述货架朝向初值、所述货架腿前沿中心坐标以及所述货架模型，获取货架中心位姿初值和所述货架腿位姿初值。
其中，所述利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿，包括：
利用所述货架模型和所述二维观测点云构建第一误差方程，求解货架腿位姿；
利用所述货架腿位姿和所述货架模型构建第二误差方程，求解所述货架中心最终位姿。
其中，所述基于所述三维点云，获取货架腿三维点云以及确定货架模型之前，所述货架识别方法还包括：
获取所述三维点云中的若干货架腿点云聚类集合；
获取每一货架腿点云聚类集合在第二预设轴坐标的最大坐标值和最小坐标值；
将所述最大坐标值和所述最小坐标值的乘积大于0的货架腿点云聚类集合剔除。
为解决上述技术问题，本申请还提出一种机器人控制方法，所述机器人控制方法包括：
获取货架中心最终位姿；
按照里程计导航方式将所述机器人按照所述货架中心最终位姿进行移动；
利用上视相机扫描货架下方的二维码，获取二维码导航信息；
按照所述二维码导航信息将所述机器人移动至所述货架中心的正下方；
其中，所述货架中心最终位姿通过上述的货架识别方法计算所得。
为解决上述技术问题，本申请还提出一种终端设备，所述终端设备包括存储器以及与所述存储器耦接的处理器；其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如上述的货架识别方法，和/或机器人控制方法。
为解决上述技术问题，本申请还提出一种计算机存储介质，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现上述的货架识别方法，和/或机器人控制方法。
与现有技术相比，本申请的有益效果是：终端设备获取机器人前方的三维点云和二维点云；基于所述三维点云，获取货架腿三维点云以及确定货架模型；按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值；利用所述货架中心位姿初值和所述货架腿位姿初值，对所述二维点云进行过滤，获取二维观测点云；利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿。通过上述货架识别方法，采用了三维点云粗识别与二维点云精识别相结合的方式，精确识别货架中心的位置。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
其中：
图1是本申请提供的移动机器人一实施例的结构示意图；
图2是本申请提供的二维栅格地图一实施例的示意图；
图3是本申请提供的货架模型一实施例的结构示意图；
图4是本申请提供的货架识别方法一实施例的流程示意图；
图5是本申请提供的货架识别方法的整体流程示意图；
图6是图4所示货架识别方法步骤S12的具体流程示意图；
图7是本申请提供的车体正前方视野范围的示意图；
图8是本申请提供的深度最大值对应的矩形视野范围的示意图；
图9是本申请提供的深度最小值对应的矩形视野范围的示意图；
图10是协方差矩阵的单位特征向量的示意图；
图11是协方差矩阵的单位特征向量β2的投影示意图；
图12是本申请提供的二维观测点云的示意图；
图13是本申请提供的利用货架腿位姿计算货架中心坐标及货架朝向的示意图；
图14是本申请提供的机器人控制方法一实施例的流程示意图；
图15是本申请提供的终端设备一实施例的结构示意图；
图16是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例，例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
本申请基于里程计、激光传感器和先验地图估计移动机器人的位姿，在进行货架对接时，先用tof(Time of Figh，飞行时间)相机，或者其他深度相机获取移动机器人正前方的3D点云，用tof的3D点云数据进行货架模型筛选及货架位姿粗估计作为初值；然后用激光雷达的二维点云数据作为观测构建误差方程，进行非线性优化求解，计算出货架的位姿，并将其位姿转换到世界坐标系下；最后切换成里程计导航方式进入货架下方，在快到货架中心时，打开上视相机，扫描检测贴在货架下方的二维码，当检测到二维码时，切换成二维码导航方式，保证二维码能到达设备的转盘中心位置，让设备成功举起货架。
本申请设计一种里程计+2D激光雷达+3D TOF传感器的移动机器人，具体请参阅图1，图1是本申请提供的移动机器人一实施例的结构示意图。
如图1所述，本申请所涉及的移动机器人移动地盘(包括运动控制器、电机、电池、嵌入式计算机、里程计等)、2D激光雷达传感器、3D TOF传感器和上视相机。其基本功能结合如下：
该移动机器人在基于仓储环境下生成的二维栅格地图，如图2所述的地图中进行高精度定位。该地图包括货架、工作台、房屋支撑架以及需要搬运的货物等部件。该移动机器人使用里程计估计出机器人运动的改变量，同时使用2D激光雷达扫描环境轮廓。而在机器人进入货架前，会在货架识别准备点进行货架识别，检测到货架中心位置，切换成里程计模式走入货架下方，再利用上视相机扫描货架下方的二维码，让移动机器人走到货架正下方成功举起货架。而对应的货架模型如图3所示，其中货架零度方向与有货架背面贴的二维码的方向来决定。
下面先介绍上述过程中的货架识别过程，具体请参阅图4和图5，图4是本申请提供的货架识别方法一实施例的流程示意图，图5是本申请提供的货架识别方法的整体流程示意图。
如图4所示，其具体步骤如下：
步骤S11：获取机器人前方的三维点云和二维点云。
在本申请实施例中，终端设备采集机器人前方的TOF激光数据，即三维点云和激光点云数据，即二维点云。
进一步地，终端设备在执行货架识别之前，需要对TOF激光数据和激光点云数据进行预处理。
终端设备需要对获取到的TOF激光数据进行预处理。终端设备会先排除无效的点云数据，例如无数据点或异常值点。然后，终端设备会将TOF点云从TOF坐标系转换到机器人车体坐标系，这样就可以在机器人车体坐标系下进行处理和判断。最后，终端设备根据预先设定的目标区域范围对点云进行裁剪，以便只保留在目标区域内的点云。例如，本申请可以将移动机器人前后+0.5m～+3m、左右-1.5m～+1.5m、上下0m～+1.5m范围设为识别ROI，在此范围外的点云全部被剔除，得到剩下的点云即为TOF点云。
相同地，终端设备也需要对激光点云数据进行预处理，剔除非法数据；然后将激光点云从激光雷达坐标系转换到机器人车体坐标系；同样的也需要根据识别的目标区域范围对点云进行裁切；最后还需要进行例如降噪、去除畸变等操作。
终端设备需要继续对预处理后的TOF点云进行欧式聚类，剔除点云数量小于阈值的聚类块，并根据聚类块拓扑结构剔除不连通的点云。
具体地，终端设备使用区域生长法根据欧式距离对TOF点云进行聚类；其中在每一个聚类块生成聚类的过程中，记录下当前聚类块中的点云坐标在y维度上的最大和最小值：ymin、ymax；聚类结束后得到一系列的聚类点云集合。需要说明的是，在机器人车体坐标系中，x轴为车体指向货架的方向，y轴为x轴的垂直方向。
终端设备遍历得到的聚类点云集合：首先判断当前聚类集合中点云数量是否少于一定阈值，如果点云数量少于阈值，则直接将此聚类集合中的所有点云剔除；然后判断当前聚类集合的ymin和ymax乘积是否小于等于零，如果乘积大于零，则说明当前聚类块的所有点云都处于机器人同一侧。而目标货架一般都放置于机器人前方，车头在两个货架腿之间，因此观测到的货架点云聚类之后的ymin和ymax一定是分别处于机器人两侧，所以需要将所有点云都处于机器人同一侧的聚类块，即ymin和ymax乘积大于零的聚类块剔除。
步骤S12：基于三维点云，获取货架腿三维点云以及确定货架模型。
在本申请实施例中，终端设备根据步骤S11的预处理和聚类处理后得到的TOF点云，即三维点云筛选货架模型以及货架腿三维点云。
具体地，终端设备根据实际场景中的货架腿长度对TOF点云进行ROI区域提取，一般z＝0～10cm是货架的轮子部分，z＝[10cm，40cm]部分是长方体货架腿或圆柱货架腿，z>40cm部分是承托货物的货架平面及货物。因此，本申请可以设定z＝[20cm，30cm]的区间来对上面步骤S11筛选之后的聚类点云集合再进行一次ROI，截取出能够提供可靠观测的货架腿点云区域。
然后，终端设备对ROI之后的点云再进行聚类，得到候选货架腿点云聚类集合，剔除点云数量不满足要求的聚类集合；计算每个候选货架腿聚类的中心点坐标作为此候选货架腿的坐标(x,y)，并对所有候选货架腿按与车体中心的距离由近到远排序。
最后，终端设备即可按照上述候选货架腿获取货架腿三维点云以及确定货架模型。
请继续参阅图6，图6是图4所示货架识别方法步骤S12的具体流程示意图。
如图6所示，其具体步骤如下：
步骤S121：获取三维点云中的若干候选货架腿点云聚类集合。
步骤S122：将每一候选货架腿点云聚类集合的中心点作为候选货架腿。
在本申请实施例中，终端设备根据上述聚类和排序，获取多个候选货架腿。
步骤S123：遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，获取剩余的候选货架腿组合。
在本申请实施例中，终端设备对候选货架腿两两组合遍历，排除不符合货架腿规则的候选货架腿组合，下面具体介绍本申请所涉及的货架腿排除规则：
第一：计算两货架腿聚类中心点在车体坐标系xoy平面连接所形成的向量，与车体坐标系的X轴夹角是否大于45°；若大于45°则直接排除当前候选货架腿组合。
第二：判断当前两个候选货架腿之间是否还夹杂其它候选货架腿。判断方法是另外任取一个候选货架腿，判定此货架腿是否在当前两个选定的候选货架腿所组成的线段内。具体算法步骤为判断夹角是否小于一定阈值，即是否接近共线，并且第三点与前两点所成线段的长度之和是否等于前两点所成线段的长度，本申请为了避免漏筛选，在判断长度时需要加上一定阈值，增加容错。若当前两个候选货架腿之间还存在其它候选货架腿则直接排除当前候选货架腿组合。
第三：判断车体正前方到当前候选货架腿之间的视野内是否存在其它候选货架腿。判断方法是另外任取一个候选货架腿，根据当前选定的两个候选货架腿所组成的线段，投影到车体坐标系y轴所围成的梯形范围，其代表了车体正前方的视野范围内是否有其它候选货架腿。具体算法步骤为，用射线法判断点是否在多边形内；若视野内还有其它候选货架腿则直接排除当前候选货架腿组合。
具体请参阅图7，图7是本申请提供的车体正前方视野范围的示意图。如图7所示，现已知梯形四个顶点坐标为：(x1，y1)、(x2，y2)、(0，y2)、(0，y1)。通过上述四个顶点坐标描述唯一确定的梯形，则为车体正前方视野范围，其中，(x1，y1)、(x2，y2)为当前候选货架腿的中心点坐标。
然后，终端设备遍历剩下的所有候选货架腿，用射线法判断是否有货架腿落在梯形范围内，若有货架腿在梯形范围内，则直接排除当前选定的候选货架腿组合。
第四：根据当前选中的两个货架腿所组成的识别面为一条边，即假设当前选中的两个货架腿所组成的识别面为货架宽度，取所有货架模型中的深度的最大值为另一条边，形成一矩形区域。要求此范围内不能有其它候选货架腿，需要说明的是，这里假设本申请识别选取的是货架四条腿中更靠近车体的这两条。
具体请参阅图8，图8是本申请提供的深度最大值对应的矩形视野范围的示意图。如图8所示，矩形区域四个顶点坐标为：(x1，y1)、(x2，y2)、(x3，y3)、(x4，y4)，其中，(x1，y1)、(x2，y2)为当前候选货架腿的中心点坐标，d为所有货架模型中的深度的最大值。
如图8所示，向量V1[(x2,y2)->(x3,y3)]与向量V2[(x1,y1)->(x2,y2)]垂直，即向量点乘为0；向量V1[(x2,y2)->(x3,y3)]模长为d＝max_depth>0；向量V1[(x2,y2)->(x3,y3)]在向量V2[(x1,y1)->(x2,y2)]的顺时针方向，即向量叉乘小于0；在车体坐标系下显然y1<y2。
终端设备可以联立以下不等式+等式约束，求解x3和y3的坐标值：
终端设备进一步联立以下不等式+等式约束，求解x4和y4的坐标值：
终端设备继续提取上述两个求解方程的公共部分，另计算可得：
通过上述求解，获得矩形区域四个顶点坐标的坐标值，可以用这四个顶点坐标描述唯一确定的矩形；遍历剩余的候选货架腿，用射线法判断是否有货架腿落在矩形范围内，若有货架腿在矩形范围内，则直接排除当前选定的候选货架腿组合。
第五：根据当前选中的两个货架腿所组成识别面为一条边，即假设当前选中的两个货架腿所组成的识别面为货架深度，取所有货架模型中的深度的最小值为另一条边，形成一矩形区域。要求此范围内不能有其它候选货架腿；如果矩形范围内允许存在其它候选货架腿，这里实际上要求当前选中的两个货架腿后侧一定范围内不能有其它货架，则和最小的货架深度这一假设相矛盾。
具体请参阅图9，图9是本申请提供的深度最小值对应的矩形视野范围的示意图。如图8所示，矩形区域四个顶点坐标为：(x1，y1)、(x2，y2)、(x3，y3)、(x4，y4)，其中，(x1，y1)、(x2，y2)为当前候选货架腿的中心点坐标，d为所有货架模型中的深度的最小值。
如图9所示，向量V1[(x2,y2)->(x3,y3)]与向量V2[(x1,y1)->(x2,y2)]垂直，即向量点乘为0；向量V1[(x2,y2)->(x3,y3)]模长为d＝min_depth>0；向量V1[(x2,y2)->(x3,y3)]在向量V2[(x1,y1)->(x2,y2)]的顺时针方向，即向量叉乘小于0；在车体坐标系下显然y1<y2。
终端设备可以联立以下不等式+等式约束，求解x3和y3的坐标值：
终端设备进一步联立以下不等式+等式约束，求解x4和y4的坐标值：
终端设备继续提取上述两个求解方程的公共部分，另计算可得：
通过上述求解，获得矩形区域四个顶点坐标的坐标值，可以用这四个顶点坐标描述唯一确定的矩形；遍历剩余的候选货架腿，用射线法判断是否有货架腿落在矩形范围内，若有货架腿在矩形范围内，则直接排除当前选定的候选货架腿组合。
步骤S124：获取候选货架腿组合的货架腿间距和每一货架模型的货架腿间距。
步骤S125：若存在任意一候选货架腿组合的货架腿间距和任意一货架模型的货架腿间距的残差小于预设残差阈值，则获取该候选货架腿组合的货架腿三维点云以及该货架模型。
在本申请实施例中，终端设备遍历所有货架模型，计算当前选定的候选货架腿组合与货架模型的货架腿间距的残差，若当前残差小于一定阈值则判定为找到了观测中的候选货架腿组合与待匹配的货架模型；若当前残差暂不满足要求，则继续遍历。多轮次遍历中，若当前残差值小于之前记录的最小残差，则将记录的最小残差更新为当前残差，最终使得残差收敛。
步骤S13：按照货架模型通过货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值。
在本申请实施例中，终端设备根据步骤S12筛选出来的货架模型的货架腿尺寸，0.75倍的货架腿宽为半径，分别以步骤S12中得到的两个货架腿点云聚类中心为圆心，货架模型腿高为圆柱的高，形成两个圆柱体ROI区域，继续对步骤11所得到的TOF点云进行ROI裁剪。
终端设备用PCA(principal components analysis，主成分分析技术)算法对上面得到的点云坐标分解到以特征向量为基的坐标系的x，y两个方向，分别进行非线性优化。
具体地，终端设备计算步骤11所得到的TOF点云进行ROI裁剪后，两个货架腿点云聚类所有点云样本的均值：
其中，pj为点云样本中的一个点，|Z为点云样本的数量。
利用上述均值，终端设备构造协方差矩阵ξi：
终端设备对ξi进行SVD分解后，得到协方差矩阵ξi的特征值{λ1,λ2}和单位特征向量{β1,β2}，其中λ1<λ2。具体请参阅图10，图10是协方差矩阵的单位特征向量的示意图。
终端设备选取最小特征值λ1对应的单位特征向量β1，即所有点云样本的法向量，作为货架朝向θ的初值：
其中，β1(y)为特征向量在y轴的分量，β1(x)为特征向量在x轴的分量。
货架腿前沿中心在以{β1,β2}为基的坐标系下的坐标为(x′,y′)，已知货架模型宽度为w，货架腿宽为wleg。
为了去除外点的影响并减少非线性优化的计算量；将所有样本点投影到特征向量β1上实现降维(特征向量*样本矩阵)，货架腿前沿中心在一维坐标轴(特征向量β1)上的坐标值y'用以下均值计算：
其中，|Z|为点云样本的数量，pi(y)为每个样本点云在以{β1,β2}为基的坐标系下的β1轴上的坐标值。
为了去除外点的影响并减少非线性优化的计算量；将所有样本点投影到特征向量β2上实现降维(特征向量*样本矩阵)，用非线性优化拟合货架腿前沿中心在一维坐标轴(特征向量β2)上的坐标值x′。
如图11所示，图11是协方差矩阵的单位特征向量β2的投影示意图。所有样本点都在线段[y1,y4]之内，即内点与两端点所成的向量模长之和等于线段[y1,y4]宽度w+wleg；所有样本点都在线段[y2,y3]之外，即外点与两端点所成的向量模长之差等于线段[y2,y3]宽度w-wleg。
其中，
利用打在货架腿上的点云构建误差方程：
εj＝εj′+εj″
其中，ωj′、ωj″为每个样本点云在残差项中的权重，pi(y)为每个样本点云在以{β1,β2}为基的坐标系下的β2轴上的坐标值。
终端设备计算出货架腿平面中心坐标，以特征向量为基的坐标系，再进行PCA逆变换，逆变换得到的是去均值的结果，最终需要加上均值得到车体坐标系原尺度下的坐标(x,y)。
终端设备根据货架前边缘平面中心(x′,y′)，货架模型宽度为w，货架腿宽为wleg，宽度深度为d，及货架朝向θ，计算举升中心的位姿及四个货架腿的位姿。
步骤S14：利用货架中心位姿初值和货架腿位姿初值，对二维点云进行过滤，获取二维观测点云。
在本申请实施例中，终端设备步骤S13确定的货架模型以及货架位姿对二维激光点云进行过滤。
具体地，终端设备根据前面通过TOF数据筛选得到的货架模型、计算得到的货架中心位姿和货架腿位姿，对激光点云进行ROI过滤，如图12所示，只保留货架腿区域的点云。
步骤S15：利用货架模型以及二维观测点云，求解货架中心最终位姿。
在本申请实施例中，终端设备用货架腿的二维观测点云观测估计货架腿位姿，再用货架腿位姿估计货架中心位姿。
具体地，经过步骤S14过滤后的剩下的激光点云是非常可靠的观测点云，终端设备即可利用货架腿激光点云构建误差方程计算货架腿中心坐标及货架腿朝向：
εi＝εi′+εi″+εi″′
其中，ld、lw为货架腿的深度和宽度，ωi′、ωi″、ωi″′为每个样本点云在残差项中的权重，pi(x)、pi(y)为每个样本点云在车体坐标系下的x,y轴坐标值，x、y、θ依次为货架腿中心的坐标以及角度朝向。
终端设备通过非线性优化求解可得货架腿中心的坐标x、y以及角度朝向θ；依次计算出四个货架腿位姿p1、p2、p3、p4。
则货架中心位姿C，初值为：
C＝(p1+p2+p3+p4)/4
如图13所示，终端设备再用货架腿位姿构建误差方程计算货架中心坐标及货架朝向：
εi＝εi′+εi″+εi″′
其中，d、w为货架的深度和宽度，ωi′、ωi″、ωi″′为每个货架腿在残差项中的权重，pi(x)、pi(y)为每个货架腿在车体坐标系下的x,y轴坐标值，Cx、Cy、Cθ为依次为货架中心的坐标以及角度朝向。
最后，终端设备通过非线性优化求解可得货架中心的坐标以及角度朝向。
在本申请实施例中，终端设备获取机器人前方的三维点云和二维点云；基于所述三维点云，获取货架腿三维点云以及确定货架模型；按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值；利用所述货架中心位姿初值和所述货架腿位姿初值，对所述二维点云进行过滤，获取二维观测点云；利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿。通过上述货架识别方法，采用了三维点云粗识别与二维点云精识别相结合的方式，精确识别货架中心的位置。
请继续参阅图14，图14是本申请提供的机器人控制方法一实施例的流程示意图。
如图14所示，其具体步骤如下：
步骤S21：获取货架中心最终位姿。
步骤S22：按照里程计导航方式将机器人按照货架中心最终位姿进行移动。
步骤S23：利用上视相机扫描货架下方的二维码，获取二维码导航信息。
步骤S24：按照二维码导航信息将机器人移动至货架中心的正下方。
本申请提出了一种精确鲁棒的货架检测方法，充分利用TOF传感器的3D信息，自动过滤人机混合动态环境下的人腿、机台腿、其它动态杂物等具有干扰性的物体，解决了2D传感器在这种动态环境下的感知歧义、误识别等问题。
本申请采用了TOF粗识别+lidar精识别相结合的方式，充分利用了TOF传感器观测信息丰富、观测数据多的优势，快速剔除干扰，计算出货架位姿和货架腿位姿对lidar点云进行精准ROI，并给出相匹配的货架模型；充分利用lidar传感器测量精度高的优势，用ROI后的lidar点云精准计算货架腿位姿及货架中心位姿。解决了TOF传感器测距不够精准导致无法精准识别的问题、视场角太小无法导致无法近距离识别的问题；解决了lidar传感器很难剔除干扰点云的问题导致识别精度差的问题、观测数据维度太单一导致误识别的问题。
本申请提出了一种多货架模型筛选、匹配方法，可以识别、区分多种不同的货架；该策略有效且高效、大量剪枝，对货架模型进行排除过滤、筛选、匹配，找到和现实环境最吻合的货架模型；扩充了该识别方法的应用场景和识别的鲁棒性。
本领域技术人员可以理解，在具体实施方式的上述方法中，各步骤的撰写顺序并不意味着严格的执行顺序而对实施过程构成任何限定，各步骤的具体执行顺序应当以其功能和可能的内在逻辑确定。
为实现上述货架识别方法，和/或机器人控制方法，本申请还提出了一种终端设备，具体请参阅图15，图15是本申请提供的终端设备一实施例的结构示意图。
本实施例的终端设备400包括处理器41、存储器42、输入输出设备43以及总线44。
该处理器41、存储器42、输入输出设备43分别与总线44相连，该存储器42中存储有程序数据，处理器41用于执行程序数据以实现上述实施例所述的货架识别方法，和/或机器人控制方法。
在本申请实施例中，处理器41还可以称为CPU(Central Processing Unit，中央处理单元)。处理器41可能是一种集成电路芯片，具有信号的处理能力。处理器41还可以是通用处理器、数字信号处理器(DSP，Digital Signal Process)、专用集成电路(ASIC，Application Specific Integrated Circuit)、现场可编程门阵列(FPGA，FieldProgrammable Gate Array)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器41也可以是任何常规的处理器等。
本申请还提供一种计算机存储介质，请继续参阅图16，图16是本申请提供的计算机存储介质一实施例的结构示意图，该计算机存储介质600中存储有计算机程序61，该计算机程序61在被处理器执行时，用以实现上述实施例的货架识别方法，和/或机器人控制方法。
本申请的实施例以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (11)

1.一种货架识别方法，其特征在于，所述货架识别方法包括：
获取机器人前方的三维点云和二维点云；
基于所述三维点云，获取货架腿三维点云以及确定货架模型；
按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值；
利用所述货架中心位姿初值和所述货架腿位姿初值，对所述二维点云进行过滤，获取二维观测点云；
利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿。
2.根据权利要求1所述的货架识别方法，其特征在于，
所述基于所述三维点云，获取货架腿三维点云以及确定货架模型，包括：
获取所述三维点云中的若干候选货架腿点云聚类集合；
将每一候选货架腿点云聚类集合的中心点作为候选货架腿；
遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，获取剩余的候选货架腿组合；
获取所述候选货架腿组合的货架腿间距和每一货架模型的货架腿间距；
若存在任意一候选货架腿组合的货架腿间距和任意一货架模型的货架腿间距的残差小于预设残差阈值，则获取该候选货架腿组合的货架腿三维点云以及该货架模型。
3.根据权利要求2所述的货架识别方法，其特征在于，
所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
获取所述候选货架腿组合在车体坐标系的向量；
将所述向量与所述车体坐标系的第一预设轴夹角大于预设角度阈值的候选货架腿组合排除。
4.根据权利要求2所述的货架识别方法，其特征在于，
所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
判断所述候选货架腿组合之间的区域，或者所述候选货架腿与所述机器人之间的视野内是否存在其他候选货架腿；
若是，则排除所述候选货架腿组合。
5.根据权利要求2所述的货架识别方法，其特征在于，
所述遍历两两候选货架腿，排除不符合货架腿规则的候选货架腿组合，包括：
获取两两候选货架腿的候选货架腿组合；
获取所有货架模型的最大深度值和/或最小深度值；
利用所述最大深度值和/或所述最小深度值，以所述候选货架腿组合的识别面为边，构建候选范围区域；
响应于所述候选范围区域内存在其他候选货架腿，排除所述候选货架腿组合。
6.根据权利要求2所述的货架识别方法，其特征在于，
所述按照所述货架模型通过所述货架腿三维点云，获取货架中心位姿初值和货架腿位姿初值，包括：
基于所述货架腿三维点云构造的协方差矩阵进行分解，获取所述协方差矩阵的特征值和特征向量；
选取最小特征值对应的特征向量，作为货架朝向初值；
利用所述货架腿三维点云，确定货架腿前沿中心在所述特征向量为基轴的坐标系上的货架腿前沿中心坐标；
按照所述货架朝向初值、所述货架腿前沿中心坐标以及所述货架模型，获取货架中心位姿初值和所述货架腿位姿初值。
7.根据权利要求1所述的货架识别方法，其特征在于，
所述利用所述货架模型以及所述二维观测点云，求解货架中心最终位姿，包括：
利用所述货架模型和所述二维观测点云构建第一误差方程，求解货架腿位姿；
利用所述货架腿位姿和所述货架模型构建第二误差方程，求解所述货架中心最终位姿。
8.根据权利要求1所述的货架识别方法，其特征在于，
所述基于所述三维点云，获取货架腿三维点云以及确定货架模型之前，所述货架识别方法还包括：
获取所述三维点云中的若干货架腿点云聚类集合；
获取每一货架腿点云聚类集合在第二预设轴坐标的最大坐标值和最小坐标值；
将所述最大坐标值和所述最小坐标值的乘积大于0的货架腿点云聚类集合剔除。
9.一种机器人控制方法，其特征在于，所述机器人控制方法包括：
获取货架中心最终位姿；
按照里程计导航方式将所述机器人按照所述货架中心最终位姿进行移动；
利用上视相机扫描货架下方的二维码，获取二维码导航信息；
按照所述二维码导航信息将所述机器人移动至所述货架中心的正下方；
其中，所述货架中心最终位姿通过权利要求1至8任一项所述的货架识别方法计算所得。
10.一种终端设备，其特征在于，所述终端设备包括存储器以及与所述存储器耦接的处理器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1至8任一项所述的货架识别方法，和/或权利要求9所述的机器人控制方法。
11.一种计算机存储介质，其特征在于，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现如权利要求1至8任一项所述的货架识别方法，和/或权利要求9所述的机器人控制方法。
CN202410010597.1A 2024-01-03 2024-01-03 货架识别方法、机器人控制方法、终端设备以及存储介质 Pending CN117830679A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202410010597.1A CN117830679A (zh) 2024-01-03 2024-01-03 货架识别方法、机器人控制方法、终端设备以及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202410010597.1A CN117830679A (zh) 2024-01-03 2024-01-03 货架识别方法、机器人控制方法、终端设备以及存储介质

Publications (1)

Publication Number Publication Date
CN117830679A true CN117830679A (zh) 2024-04-05

Family

ID=90511137

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202410010597.1A Pending CN117830679A (zh) 2024-01-03 2024-01-03 货架识别方法、机器人控制方法、终端设备以及存储介质

Country Status (1)

Country Link
CN (1) CN117830679A (zh)

Similar Documents

Publication Publication Date Title
CN108152831B (zh) 一种激光雷达障碍物识别方法及系统
Oehler et al. Efficient multi-resolution plane segmentation of 3D point clouds
US8792726B2 (en) Geometric feature extracting device, geometric feature extracting method, storage medium, three-dimensional measurement apparatus, and object recognition apparatus
US20110205338A1 (en) Apparatus for estimating position of mobile robot and method thereof
US5978504A (en) Fast planar segmentation of range data for mobile robots
US11525923B2 (en) Real-time three-dimensional map building method and device using three-dimensional lidar
CN109087345A (zh) 基于ToF成像系统的栈板识别方法及自动导引运输车
Holz et al. Towards semantic scene analysis with time-of-flight cameras
Muhammad et al. Loop closure detection using small-sized signatures from 3D LIDAR data
Maier et al. Self-supervised obstacle detection for humanoid navigation using monocular vision and sparse laser data
CN112784799A (zh) Agv后向栈板及障碍物识别方法、装置及agv
JP4429461B2 (ja) 画像処理装置、平面検出方法、及び平面検出プログラムを記録した記録媒体
CN116128841A (zh) 托盘位姿检测方法及装置、无人叉车、存储介质
Sappa et al. Range image registration by using an edge-based representation
Drwięga Features matching based merging of 3D maps in multi-robot systems
Nalpantidis et al. Stereovision-based fuzzy obstacle avoidance method
CN117830679A (zh) 货架识别方法、机器人控制方法、终端设备以及存储介质
Missura et al. Polygonal perception for mobile robots
CN113592976B (zh) 地图数据的处理方法、装置、家用电器和可读存储介质
Son et al. Optimized robot mapping and obstacle avoidance using stereo vision
Vatavu et al. Environment perception using dynamic polylines and particle based occupancy grids
Truax et al. Using prioritized relaxations to locate objects in points clouds for manipulation
Klimentjew et al. Adaptive sensor-fusion of depth and color information for cognitive robotics
Luo et al. Visual simultaneous localization and mapping using stereo vision with human body elimination for service robotics
Rink et al. Monte Carlo registration and its application with autonomous robots

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