(19)国家知识产权局 (12)发明 专利 (10)授权公告 号 (45)授权公告日 (21)申请 号 202210737623.1 (22)申请日 2022.06.28 (65)同一申请的已公布的文献号 申请公布号 CN 114820391 A (43)申请公布日 2022.07.29 (73)专利权人 山东亚历山大智能科技有限公司 地址 250061 山东省济南市历下区经十路 17923号山东大学千佛山校区创新大 厦西楼4层 (72)发明人 周军　林乐彬　杨旭浩　李文广　 李留昭　叶丰　 (74)专利代理 机构 济南圣达知识产权代理有限 公司 372 21 专利代理师 祖之强(51)Int.Cl. G06T 5/00(2006.01) G06T 5/20(2006.01) G06T 7/11(2017.01) G06T 7/33(2017.01) G06T 7/73(2017.01) G06V 10/75(2022.01) G06V 10/762(2022.01) G06K 9/62(2022.01) 审查员 王宇莉 (54)发明名称 基于点云处理的仓储托盘检测定位方法及 系统 (57)摘要 本发明提供了一种基于点云处理的仓储托 盘检测定位方法及系统， 属于仓储托盘定位技术 领域， 获取位于叉车式AGV升降机构上传感器采 集的点云数据； 对获取的点云数据进行预处理； 对预处理后的点云进行聚类分割， 进行再次筛选 后与托盘模板点云进行匹配， 计算每簇分割点云 的匹配误差； 选取匹配误差计算最小的点云进行 平面拟合， 得到仓储托盘的转角和位置； 本发明 能够应用在不同的可以获取点云的设备中， 可以 实现不同种类仓储托盘的检测， 有效地提高了仓 储托盘的定位精度。 权利要求书3页 说明书10页 附图3页 CN 114820391 B 2022.10.11 CN 114820391 B 1.一种基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 包括以下 过程： 获取位于叉 车式AGV升降机构上传感器采集的点云数据； 对获取的点云数据进行 预处理； 对预处理后的点云进行聚类分割， 进行再次筛选后与托盘模板点云进行匹配， 计算每 簇分割点云的匹配误差； 选取匹配误差计算 最小的点云进行平面拟合， 得到 仓储托盘的转角和位置； 对预处理后的点云进行聚类分割， 包括： 基于法向量的区域生长算法对场景点云进行分割， 计算输入点云的法向量和曲率， 并 设置算法的曲率阈值K、 法向量夹角阈值θ和搜索近邻点的个数N， 根据曲率大小对 各点进行 排序， 选取曲率 最小的点 为初始种子点加入种子点 集合Q中； 搜索该种子点的N个近邻点， 采用KD树的结构加速搜索， 若近邻点的法线夹角与种子点 的夹角小于θ 且曲率 值小于K则将该点加入种子点 集合Q中； 在该种子点所有近邻点搜索完后将该种子点移出Q， 在Q中选择新的种子点并重复以上 步骤， 直到Q中为空完成一个区域 生长的聚类； 判断所有聚类的点云数， 删除不满足最小聚类点数和最大聚类点数的点云； 对得到的托盘点云进行基于RANSAC算法的平面拟合， 设置最大迭代次数N， 距离阈值d： 从点云中随机 选取三个点拟合平面方程； 用其他点计算到该平面的距离， 小于阈值d的认为是平面内点， 统计剩余点中是平面内 点的个数并重复以上步骤， 记录内点个数最多的平面方程， 直到达到迭代 次数N为止， 得到 平面方程 为Ax+By+C z+D＝0， 保存在拟合平面内的点； 计算所得平面的法向量， X和Y方向的法向量分量a和b， 由此可以得到托盘相对于传感 器X方向和Y方向的夹角arctan(b/a)和arctan(a/b)； 将得到平面点云中的点按X、 Y方向位置大小进行排序， 分别删除这两个方向上最大和 最小的多个点， 计算剩余点中两个最大及最小多个点的平均值xmin、 xmax和ymin、 ymax， 进而得 到仓储托盘的中心位置； 将仓储托盘的中心位置转 化到叉车坐标系下， 得到 仓储托盘相对于叉 车的实际位置 。 2.如权利要求1所述的基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 对获取的点云数据进行 预处理， 包括： 根据叉齿长度以及升降机构处于最低高度时传感器相对于地面的高度， 对获得的原始 点云进行直通滤波， 去除地面点云和叉齿上 的点云， 补偿预设高度以去除托盘与地面接触 的部分。 3.如权利要求2所述的基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 通过直通滤波保留的点云满足如下 条件： 其中， xs为点云的x坐标值， xmax为点云的x坐标值的最大值， l为叉齿长度， Δh为补偿的 预设高度， hp为检测的仓储托盘高度， zs为点云的z轴坐标值， hs为升降机构处于最低高度时权　利　要　求　书 1/3 页 2 CN 114820391 B 2传感器相对于地 面的高度。 4.如权利要求1所述的基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 对获取的点云数据进行 预处理， 包括： 采用统计学 滤波的方式和中值滤波的方式去除点云噪声， 包括： 假设点云中每个点到其邻域的距离符合高斯分布， 设点云中两点之间的距离为d， 第i 个点到k个临近点的平均距离为 μ， 标准差为σ， 设置标准差倍数n， 通过 统计滤波能够保留的 点云满足： μ‑nσ ＜di＜ μ+nσ； 对于点云一个采样点p(x0， y0， z0)， 设置邻域半径为r内的所有点， 即D(x0， y0， z0)＝{x， y， z)|(x‑x0)2+(y‑y0)2+(z‑z0)2＜r2}， 采用基于KD树的结构进行采样 点的近邻搜索， 只对邻 域内所有点的X方向的坐标值大小进行从小到大排列， 取其中值xmedian作为采样点， 则采样 点坐标更新公式为： p＝(xmedian， y0， z0)； 遍历统计滤波后的所有点， 得到中值滤波后的场景点云。 5.如权利要求1所述的基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 进行再次筛选， 包括： 计算每个聚类的平均法向量， 删除X 方向法向量分量低于设定值的点云集 合。 6.如权利要求5所述的基于点云处 理的仓储托盘 检测定位方法， 其特 征在于： 与托盘模板点云进行匹配， 计算每簇分割点云的匹配误差， 包括： 加载仓储托盘模板点云， 将剩下的每个聚类分别与模板点云进行基于ICP算法的点云 配准： 对于一个聚类点云中某个点进行一个初始变换T0得到pAi， 从模板点云中找到距离pAi 最近的点pBi， 则最优变换T为： 将该聚类的点云施以最优变换T， 得到配准后的点云， 计算配准后pAi′与对应点pBi的距 离的平方和， 以均方根 误差最小的聚类为检测到的托盘点云。 7.一种基于点云处 理的仓储托盘 检测定位系统， 其特 征在于： 包括： 数据获取模块， 被 配置为： 获取位于叉 车式AGV升降机构上传感器采集的点云数据； 预处理模块， 被 配置为： 对获取的点云数据进行 预处理； 点云匹配模块， 被配置为： 对预处理后的点云进行聚类分割， 进行再次筛选后与托盘模 板点云进行匹配， 计算每簇分割点云的匹配误差； 托盘定位模块， 被配置为： 选取匹配误差计算最小的点云进行平面拟合， 得到仓储托盘 的转角和位置； 对预处理后的点云进行聚类分割， 包括： 基于法向量的区域生长算法对场景点云进行分割， 计算输入点云的法向量和曲率， 并 设置算法的曲率阈值K、 法向量夹角阈值θ和搜索近邻点的个数N， 根据曲率大小对 各点进行 排序， 选取曲率 最小的点 为初始种子点加入种子点 集合Q中； 搜索该种子点的N个近邻点， 采用KD树的结构加速搜索， 若近邻点的法线夹角与种子点 的夹角小于θ 且曲率 值小于K则将该点加入种子点 集合Q中；权　利　要　求　书 2/3 页 3 CN 114820391 B 3