一种放射性废物包识别定位方法与装置与流程

本发明涉及货物的识别定位技术领域，特别是涉及一种放射性废物包识别定位方法与装置。

背景技术：

货包识别定位技术是无人货物装卸场景下最为基础和关键的技术，识别定位的稳定性和准确性直接影响整个装卸系统的可靠性。为了完成准确的装卸载过程，需要分别对转运车上的转运容器、容器内的废物包进行识别定位。

针对外箱内货包的场景，目前尚无有效的识别定位方法。传统的货包定位识别方法针对独立的货包进行定位，方法主要分为依靠位置传感器定位、依靠感知传感器定位和两者结合三种思路，依靠位置传感器定位的方法包括基于gps/gnss定位、基于布设的轨道或位置传感器定位、基于rfid标签定位等，依靠感知传感器定位方法包括基于单目视觉的定位、基于双目视觉的定位、基于(多个)二维激光雷达的定位、基于毫米波雷达的定位等，两者结合的方法包括图像和gps融合定位、激光雷达和位置传感器融合定位等。例如公布号为cn107507167a、公布日为2017.12.22的中国专利：一种基于点云平面轮廓匹配的货物托盘检测方法及系统，其为基于单目视觉的定位，当物体不在视场中央时容易产生投影畸变，只有摄像头处在正视位置才能获得较好的识别定位效果，存在识别范围小、灵活性低等问题，并且对遮挡敏感，难以识别检测容器内物体。

当前主流方法中，使用gps的方法存在精度差、受环境影响等问题，gps传感器只能在室外使用且精度为m级，误差较大；基于轨道或rfid传感器的方法存在设备布设繁琐、标定困难等问题，并且需要较近的检测距离，难以检测容器内放置的物体；依靠单目视觉传感器的方法对深度和观测角度敏感，往往需要激光雷达或者毫米波雷达辅助检测深度信息，且物体不在视场中央时容易产生投影畸变，只有摄像头处在正视位置才能获得较好的识别定位效果，存在识别范围小、灵活性低等问题，并且对遮挡敏感，难以识别检测容器内物体；依靠双目视觉的方法对特征敏感，双目视觉需要特征匹配，只有特征明显的地方才能获取准确的三维坐标，不适用于对容器内大面积平面物体的识别，且双目视觉对遮挡敏感，难以实现对容器内部物体的识别定位；而传感器融合的方案，当前定位方法大多是视觉传感器和定位传感器的融合，例如图像和毫米波雷达融合、图像和位置传感器融合，但这种方法同样存在设备布置标定繁琐的问题，并且数据融合存在标定误差，同时识别范围较窄，难以解决遮挡、识别对象特征稀疏等问题。

技术实现要素：

本发明提供一种放射性废物包识别定位方法与装置，实现对屏蔽容器姿态与内部状态识别与精确定位，实现了屏蔽外箱内货包的精确识别与定位，解决了对环境光照、遮挡等敏感的传统技术无法解决的箱内货包识别定位问题。

本发明的技术方案为：

一种放射性废物包识别定位方法，制作废物包点云模板及屏蔽容器点云模板，利用两阶段匹配算法识别出屏蔽容器的位姿及废物包的位姿，以此进行识别定位，根据识别定位的结果对废物包进行装载或卸载，具体步骤如下：

s1、通过激光扫描传感器采集场景点云，提取标定好的车道有效区域点云，进行降噪滤波去除杂乱随机离群点，并提取地面点云；

s2、去除地面点云，并对场景内物体点云进行聚类，获得各个物体点云，并通过筛选获取聚类的车辆点云及屏蔽容器点云；

s3、应用屏蔽容器点云模板对获取的屏蔽容器点云进行两阶段位姿匹配，识别得到屏蔽容器的位姿，如位姿正常，则根据位姿指导装卸装置卸载屏蔽容器的顶盖，如位姿异常，则重新识别屏蔽容器的位姿；

s4、根据识别得到的屏蔽容器位姿，对屏蔽容器内点云进行分析，设定点云垂向密度最小阈值和最大阈值，检测屏蔽容器内是否装载废物包及屏蔽容器内部状态是否正常；

s41、如点云垂向密度大于最大阈值，则屏蔽容器内装载有废物包，当屏蔽容器内装载有废物包时，要进行卸载，卸载过程如下：

s411、提取屏蔽容器内的废物包点云并提取边界；

s412、应用废物包点云模板对提取的废物包点云进行两阶段位姿匹配，获得废物包的位姿并传输给卸载系统，卸载系统发送操作指令给装卸装置抓取废物包进行卸载；

s413、在卸载过程中实时监测废物包的位姿并反馈给卸载系统；

s42、如点云垂向密度小于最小阈值，则屏蔽容器内为空，当屏蔽容器内为空时，要进行装载，装载过程如下：

s421、直接将屏蔽容器的内部空间位姿传输给装载系统，装载系统发送操作指令给装卸装置抓取废物包进行装载；

s422、在装载完成后检测废物包的位姿并反馈给装载系统；

s43、其他状态则表明容器内存在异物，当屏蔽容器内存在异物时，则上报中控系统，在中控系统进行检查排除异常之后重新识别屏蔽容器的位姿。

在本发明的应用场景中，为了实现外箱内货条件下的支持装卸的识别与定位，装卸对象-放射性废物包为角上带有装夹孔的立方体，废物包被放置在特制转运的屏蔽容器中，废物包在屏蔽容器内位置不固定，并与容器内壁保持一定的间隔，屏蔽容器固定在运输车辆上。装载货包时，运输车辆位于装卸装置下方，应用屏蔽容器点云模板识别得到屏蔽容器的位姿，然后检测屏蔽容器的位姿是否正常，再卸载屏蔽容器的顶盖，之后再检测容器内部空间是否正常(有无异物)，之后装载废物包到屏蔽容器内，最后检测装载完成的废物包姿态是否正常。卸载货包时，运输车辆位于装卸装置下方，检测屏蔽容器的位姿是否正常，再卸载屏蔽容器顶盖，然后应用废物包点云模板识别并定位屏蔽容器内部的废物包位姿，指导装卸装置进行卸载废物包操作。

进一步，制作废物包点云模板包括以下过程：

采集点云，包括从1:1三维模型采集点云或者使用激光雷达扫描采集拼接点云，因实际情况激光扫描到的废物包点云存在遮挡情况，所以仅对其顶面进行定位即能获得精确的位姿，即只需采集废物包的顶面点云；

之后将采集到的顶面点云对齐到原点并均匀采样，方便后续求解精确三维位姿并避免点云密度不均带来的干扰，因为废物包的顶部几何特征集中在边缘处，并且有四处方孔被装卸装置抓取，故还需提取边界点云，最大化保留废物包的几何特征并剔除冗余特征点。

进一步，制作屏蔽容器点云模板包括以下过程：

采集点云，包括从1:1三维模型采集点云或者使用激光雷达扫描采集拼接点云，因实际情况下激光扫描到的屏蔽容器点云存在自身遮挡的情况，只能扫描到屏蔽容器的内外侧和顶部点云，所以只采集屏蔽容器的内外侧和顶部点云；

之后将采集到的点云对齐到原点并均匀采样，以便后续求解精确三维位姿并避免点云密度不均带来的干扰。

进一步，步骤s1中，通过随机采样一致性算法提取地面点云。

进一步，步骤s1中，激光扫描传感器安装在离地若干米的装卸装置上，使其感知范围覆盖废物包的装卸区域，以此采集整个场景点云。

进一步，步骤s2中，通过欧式聚类或dbscan聚类方法获得各个物体点云，并根据聚类尺寸或计算特征分布直方图筛选出待匹配的车辆点云及屏蔽容器点云。

进一步，两阶段匹配算法是指：利用环境约束特征向量粗配准和icp精配准的两步匹配法求解得到位姿。

进一步，步骤s3中，当屏蔽容器的位姿识别异常时，则上报中控系统，在中控系统进行检查排除异常之后重新识别屏蔽容器的位姿。中控系统是指用于统一调度控制的后台控制端。

进一步，步骤s3中，对屏蔽容器点云模板和屏蔽容器点云进行降采样，使其点云密度统一。

本发明还提供一种放射性废物包识别定位装置，包括应用上述的放射性废物包识别定位方法。

本发明的有益效果为：

第一、识别与定位精度高。一方面，基于tof原理的激光扫描传感器(激光雷达)相比摄像头有更高的空间测量精度，而且点云数据具有空间旋转不变性，基于点云的空间几何特征定位精度远高于基于图像的定位精度。另一方面，单一传感器的测量精度相比于多传感器融合系统有先天的优势，在避免了数据标定的情况下可以获得更准确的原始数据，从而得到更准确的三维位姿。

第二、对象与环境适应性好。不受光线材质等影响，识别范围广，鲁棒性强。相比于摄像头等二维视觉传感器只有在正视角度才能获得较好的识别定位效果，由于点云的空间旋转不变性，激光扫描传感器在视野范围内，只要没有明显遮挡都可以正常工作，识别范围有很大的提升，提升了无人作业的灵活度与鲁棒性。

第三、可以识别屏蔽容器内的非固定位置废物包，由于废物包和屏蔽容器外形相似但在空间上是分离的，基于点云数据的空间分割聚类对比其他方法有先天性的优势，并且可以自适应区分卸载工况和装载工况，实现了屏蔽容器的姿态精确检测(是否倾斜具备装载条件)与容器内部状态精确判断(是否有异物)。

第四、废物包识别定位简单，硬件方面除去计算必要的传输单元，仅使用一个激光扫描云台作为传感器，相比目前货包识别装置，在设备选型采购、设备安装标定、设备维护检修等方面简化很多，减少了传感器之间的标定误差。

附图说明

图1为本发明的放射性废物包识别定位方法流程示意图；

图2为卸载过程下废物包识别示意图；

图3为装载过程下废物包识别示意图；

图中：激光扫描传感器1、装卸装置2、运输车辆3、屏蔽容器4、废物包5。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

实施例1：

如图1所示，一种放射性废物包识别定位方法，制作废物包点云模板及制作屏蔽容器点云模板，利用两阶段匹配算法识别出屏蔽容器的位姿及废物包的位姿，以此进行识别定位，根据识别定位的结果对废物包进行装载或卸载，具体步骤如下：

s1、通过激光扫描传感器采集场景点云，提取标定好的车道有效区域点云，进行降噪滤波去除杂乱随机离群点，并提取地面点云；

s2、去除地面点云，并对场景内物体点云进行聚类，获得各个物体点云，并通过筛选获取聚类的车辆点云及屏蔽容器点云；

s3、应用屏蔽容器点云模板对获取的屏蔽容器点云进行两阶段位姿匹配，识别得到屏蔽容器的位姿，如位姿正常，则根据位姿指导装卸装置卸载屏蔽容器的顶盖，如位姿异常，则重新识别屏蔽容器的位姿；

s4、根据识别得到的屏蔽容器位姿，对屏蔽容器内点云进行分析，设定点云垂向密度最小阈值和最大阈值，检测屏蔽容器内是否装载废物包及屏蔽容器内部状态是否正常；

s41、如点云垂向密度大于最大阈值，则屏蔽容器内装载有废物包，当屏蔽容器内装载有废物包时，要进行卸载，卸载过程如下：

s411、提取屏蔽容器内的废物包点云并提取边界；

s412、应用废物包点云模板对提取的废物包点云进行两阶段位姿匹配，获得废物包的位姿并传输给卸载系统，卸载系统发送操作指令给装卸装置抓取废物包进行卸载；

s413、在卸载过程中实时监测废物包的位姿并反馈给卸载系统；

s42、如点云垂向密度小于最小阈值，则屏蔽容器内为空，当屏蔽容器内为空时，要进行装载，装载过程如下：

s421、直接将屏蔽容器的内部空间位姿传输给装载系统，装载系统发送操作指令给装卸装置抓取废物包进行装载；

s422、在装载完成后检测废物包的位姿并反馈给装载系统；

s43、其他状态则表明容器内存在异物，当屏蔽容器内存在异物时，则上报中控系统，在中控系统进行检查排除异常之后重新识别屏蔽容器的位姿。

在本实施例中，制作废物包点云模板包括以下过程：

采集点云，包括从1:1三维模型采集点云或者使用激光雷达扫描采集拼接点云，因实际情况激光扫描到的废物包点云存在遮挡情况，所以仅对其顶面进行定位即能获得精确的位姿，即只需采集废物包的顶面点云；

之后将采集到的顶面点云对齐到原点并均匀采样，方便后续求解精确三维位姿并避免点云密度不均带来的干扰，因为废物包的顶部几何特征集中在边缘处，并且有四处方孔被装卸装置抓取，故还需提取边界点云，最大化保留废物包的几何特征并剔除冗余特征点。

在本实施例中，制作屏蔽容器点云模板包括以下过程：

采集点云，包括从1:1三维模型采集点云或者使用激光雷达扫描采集拼接点云，因实际情况下激光扫描到的屏蔽容器点云存在自身遮挡的情况，只能扫描到屏蔽容器的内外侧和顶部点云，所以只采集屏蔽容器的内外侧和顶部点云；

之后将采集到的点云对齐到原点并均匀采样，以便后续求解精确三维位姿并避免点云密度不均带来的干扰。

在本实施例的步骤s1中，激光扫描传感器安装在离地若干米的装卸装置上，使其感知范围覆盖废物包的装卸区域，以此采集整个场景点云；并通过随机采样一致性算法提取地面点云。

在本实施例的步骤s2中，通过欧式聚类或dbscan聚类方法获得各个物体点云；因为运输车辆和屏蔽容器相比其他场景物体的体积差异明显，所以根据聚类尺寸或计算特征分布直方图即可快速筛选出待匹配的车辆点云及屏蔽容器点云。

在本实施例的步骤s3中，对屏蔽容器点云模板和屏蔽容器点云进行降采样，使其点云密度统一；并且当屏蔽容器的位姿识别异常时，则上报中控系统，在中控系统进行检查排除异常之后重新识别屏蔽容器的位姿，其中，中控系统是指用于统一调度控制的后台控制端。

在本实施例中，两阶段匹配算法是指：利用环境约束特征向量粗配准和icp精配准的两步匹配法求解得到位姿。

在本实施例中，装卸装置是指具有抓取功能、且具有移位功能的设备。

在本实施例的步骤s4中，点云垂向密度的最小阈值和最大阈值可以事先设定，根据屏蔽容器内为空时的样本点云密度和屏蔽容器内装载有废物包时的样本点云密度进行制定。

在本实施例中，还包括一种放射性废物包识别定位装置，包括应用上述的放射性废物包识别定位方法。

为了进一步举例说明，绘制了识别定位的应用场景，卸载过程如图2所示，装载过程如图3所示。激光扫描传感器1安装在离地数米的装卸装置2上，使其感知范围能覆盖废物包装卸区域即可，布置简单，维护方便。

在上述识别定位过程中，涉及坐标系的相互转换，整个过程涉及世界坐标系w、激光扫描传感器坐标系l、点云模板坐标系p三种，制作点云模板时将点云对齐到坐标系原点，即可简化计算，使激光扫描传感器坐标系l到点云模板坐标系p的变换矩阵mlp为单位矩阵,通过激光扫描传感器外参和扫描点云地面参数联合标定，可以获取从激光扫描传感器坐标系l到世界坐标系w的变换矩阵mlw，假设识别定位过程获得的位置信息(x,y,z)，则其在世界坐标系的位置(x′,y′,z′)可表示为：

(x′,y′,z′,1)＝mlw*(x,y,z,1)^t

从而将计算的位姿统一变换到世界坐标系下。

在开阔仿真环境下进行测试，具体布置如图2所示，运输车辆3的尺寸为10.5m*2.7m*3.3m(车净高1.6m)，屏蔽容器4的尺寸4.2m*2.2m*1.7m，壁厚20mm，位于车辆顶部正中，废物包5的尺寸为3.5m*2m*1.5m，四角有夹取用的夹具孔，孔弧度直径为124mm，宽度为64mm，激光扫描传感器1的视场角为45°*90°，角分辨率为0.3°，激光扫描传感器1布置在装卸装置2上，离地面9.5m处。其中，装卸装置2上的用于抓取废物包5的抓取机械没有示出。

制作点云模板时，点云模板应是所有情况下被识别物体的点云之和，实际采用等比例的三维模型均匀采样点云，并剔除扫描不到区域的点云，将废物包5和屏蔽容器4的点云分别对齐到坐标系原点，将点云中心对齐原点，分别将长宽高方向对齐x、y、z轴方向，完成点云模板制作。在废物包点云模板制作过程中，从1:1三维模型采集点云，截取可见区域的顶面点云，并提取边界点云得到的废物包点云模板，并将点云对齐到坐标系原点。在屏蔽容器点云模板制作过程中，从1:1三维模型采样得到点云，去除不可见的点云数据，剩下的包括运输车辆3车身部分的点云数据，及屏蔽容器4的部分点云数据，并将其对齐到坐标系原点。

建立真实尺寸的仿真场景，将废物包5放置在屏蔽容器4内随机位置，将运输车辆3放置于工作区域任意位置，激光扫描传感器1进行激光扫描采集场景点云。例如在如图2所示位置关系时，在顶盖已经卸载的情况下，得到当时的场景点云。假设点云集合为c：

c＝{p1,p2,…,pn}

其中pi(i＝1,2,…,n)为点云中的某点，包含x,y,z三个坐标值，n为点云中点的数目。

根据事先标定的数据，提取车道区域的有效数据，删除无效点云数据，之后对采集到的点云进行平面(地面)提取，利用ransac方法，首先随机选择c中的一些点，利用最小二乘法拟合得到最佳拟合平面，再次随机选择c中的同样数目的点，计算每个点到平面的距离，如果小于阈值10cm则视为内点，如果内点比例较小则根据新的点云集合重新拟合平面，如果内点比例超过阈值，则循环结束，得到平面点云(地面点云)及平面法向量(xp,yp,zp)，去除平面点云得到平面外的物体点云c1：

c1＝{p1,p2,…,pn′}

其中n’为平面外的物体点云数目。

基于欧式聚类对平面外的物体点云c1进行聚类，对点云c1建立八叉树，随机选择某一点pi，搜索最近邻点pi0，若pi与pi0的欧式距离小于阈值，则将pi0保存，下一步搜索pi0的最近邻点，以此类推，直到某次两点距离大于阈值结束，则获得了某聚类点云，继续在剩余点中随机选择某一点进行聚类，以此类推获得所有聚类点云。

根据聚类尺寸筛选出待匹配运输车辆3的点云及屏蔽容器4的点云，对屏蔽容器4的点云和屏蔽容器点云模板进行降采样使其点云密度统一。降采样采用体素滤波，选定体素大小，对于每个体素内的点数据矩阵，

其中xi,yi,zi(i＝1,2,…,n)分别为某点i的xyz坐标值，n为点云规模。

体素滤波处理后的点的坐标为：

假设降采样为点云c2，其数据矩阵mc2为：

其中xi,yi,zi(i＝1,2,…,m)分别为某点i的xyz坐标值，m为点云规模，最后一行1为缩放系数。

计算屏蔽容器点云c2的中心:

其中xmin和xmax分别为矩阵mc2中xi(i＝1,2,…,m)中的最小值和最大值，ymin和ymax分别为yi(i＝1,2,…,m)中的最小值和最大值，zmin和zmax分别为zi(i＝1,2,…,m)中的最小值和最大值。

进行环境约束特征向量粗配准：计算c2的最大主向量，即mc2的协方差矩阵的最大特征值对应的特征向量，由于车辆一般正常行驶在路面上，将(xp,yp,zp)作为第二主向量，第三主向量根据第一、二主向量叉乘获得并做标准化，将屏蔽容器点云模板的中心(0,0,0)对齐到点云中心center，x、y、z轴分别对齐到点云第一、第二、第三主向量方向，完成初步配准，配准后的点云数据矩阵mc3为：

ttr1＝-mtr1·[cx,cy,cz]^t

其中mt1为第一次配准的刚体变换矩阵，mtr1为旋转矩阵，每一列分别为前面求得的三个主向量，ttr1为平移变换矩阵。

进行icp精配准：二次精配准以屏蔽容器4的点云为目标位置，上述初步配准的屏蔽容器点云模板为初始位置，进行icp迭代配准，每次迭代中，对于初始位置点云q＝{q1,q2,…,ql}(对应点云数据矩阵mc3)的对应点对组成的目标点云p＝{p1,p2,…,p1}，最小化其均方根误差e：

其中r为旋转矩阵，t为平移矩阵，pi、qi为某点，l为点云规模，每次运算寻找r和t使e最小，重复计算直至收敛，完成第二部精配准。计算整个过程的刚体变换矩阵mt2，应用于模板点云上，可以获得屏蔽容器4的精确空间位姿。二次配准后的点云数据矩阵为

mc4＝mt2*mc3

屏蔽容器的空间位姿r为：

其中(xl,yl,zl)为位置信息，(rx,ry,rz)为姿态信息。

根据屏蔽容器4的尺寸，制定屏蔽容器4的内部空间约束g，则屏蔽容器4的实际空间位置约束g′＝mt2*mt1*g，根据约束g′提取屏蔽容器4的内部空间点云，投影至以center为原点，(xp,yp,zp)为方向的向量上计算密度分布集合d＝{d1,d2,…,dn}，其中的每一个元素：

其中xi，yi，zi为内部空间点云的某一点坐标。分析d中的数值分布规律，可以判断屏蔽容器内是否装载有废物包及异物，此场景中，d的大部分元素分布(1300,1600)区间内(即最大阈值设置为1300)，经判断屏蔽容器内装载有废物包，无异物。

参阅图2，卸载工况下，对屏蔽容器4的内部点云依照与屏蔽容器4定位相同思路的两阶段配准，以屏蔽容器4内的废物包5的顶面点云为目标位置，以废物包点云模板为初始位置，进行匹配，获取刚体变换矩阵mt1′和mt2′，则废物包5的空间位姿为：

其中(xl′,yl′,zl′)为位置信息，(rx′,ry′,rz′)为姿态信息。并且在卸载过程中实施实时监控。

实际测试结果：经计算废物包5和屏蔽容器4的位姿识别误差在10mm左右。在激光扫描传感器1可见范围内都可以较好的识别定位屏蔽容器4，在传感器周围较大区域都可识别定位屏蔽容器4中的废物包5。

如在装载情况下，则布置场景如图3所示，屏蔽容器4识别定位和之前步骤与上文相同，不同点在于将屏蔽容器4的内部点云投影到以center为原点,(xp,yp,zp)为方向的向量上时，获得的密度分布集合d'中，大部分元素分布(-100,100)区间内(即最小阈值设置为100)，说明容器内部为空，且没有异物。则把屏蔽容器4的内部空间位姿反馈装载系统并实时更新，在装载完成后，提取容器内部点云进行废物包5的位姿识别，反馈给装载系统进行判断。

本发明的放射性废物包识别定位方法，主要用于无人场景下需要对放射性废物包运输进行装卸操作的堆场或仓库等，可以通过简单的识别过程获得废物包的精确空间位置信息，从而引导装卸装置执行高效高精度的抓取放置操作。不同于一般的装卸与抓取场景，废物包放置在特制转运的屏蔽容器中，废物包与屏蔽容器相对位置不固定，抓取时需要准确定位屏蔽容器中的废物包，装载时则需要识别屏蔽容器的姿态与容器内部状态以判断是否具备装载条件与精确引导装载。相较于传统定位装置和技术，本发明的识别方法布置简单，覆盖范围广，识别准确度高，可以对屏蔽容器内货包精确定位，引导控制抓具精确抓取与装载，在对准废物包的同时避免与屏蔽容器的干涉。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。