CN111652936A - 开放式货箱装车三维感知和码垛规划方法及*** - Google Patents

Abstract

本发明提出一种开放式货箱装车三维感知和码垛规划方法及***，主要包括三维激光点云数据的采集，开放式货箱的尺寸、位置和姿态估计，码垛规划方法，感知单元和自动化装车单元的自动化标定，装车三维可视化等技术方案。本发明的三维感知具备精度高、速度快的特点，码垛规划支持花样码垛等多种码垛方式，极大地提高了开放式货箱装车的自动化水平和效率。

Description

开放式货箱装车三维感知和码垛规划方法及***

技术领域

本发明属于机器视觉感知领域，更进一步涉及开放式货箱装车三维感知和码垛规划方法及***。

背景技术

开放式货箱货车是一种广泛应用的运输工具，日常生活中的许多用品都需要用它来运输，例如白砂糖、化肥、水泥、面粉等等。此类物体一般单袋重量比较大，单次装车的运输量也很大，传统的人工装车方法工人工作量很大，装车效率很低，又加上高温、高湿、高扬尘的恶劣作业环境，近几年来逐渐出现了用工难的情况，厂家每年也为此付出了高额的装车费用。

随着我国自动化技术的进步，越来越多的自动化装车设备面世，用机械化方法完成开放式货箱货车的自动装车。此类设备在一定程度上解决了单袋大重量物体的自动化装车需求，提升了装车的质量和效率。但是目前的自动化装车设备缺乏对开放式货箱货车的三维感知，或者需要货车司机准确地将货车停放在特定区域位置，或者需要针对货车停放的位姿人工手动调整自动装车设备的姿态，因此此类自动化装车设备自动化水平不高，对货车的类型也不能很好的适应，使用起来不是特别方便。

本专利针对以上问题和需求，提出了一种开放式货箱的位置、姿态和尺寸的三维感知以及袋装物体码垛规划方法。本方法可使自动化装车设备自动感知货车停放的不同位姿以及不同的货车类型，根据装货量自动规划码垛装车方法，并视觉伺服控制装车设备完成自动化装车。整个装车过程完全不需要人工介入，极大地提高了开放式货箱装车的自动化水平和效率，为厂家节省了成本。

发明内容

本发明针对以上问题和需求，提出了一种开放式货箱的位置、姿态和尺寸的三维感知以及袋装物体码垛规划方法，极大地提高了开放式货箱装车的自动化水平和效率。

为达到上述目的，本发明的技术方案是这样实现的：

一种开放式货箱装车三维感知和码垛规划方法，将2D激光扫描雷达搭载在横移同步带上随同步带移动作为感知单元采集三维激光点云数据，还包括：

S1、自动化标定，得到感知单元坐标系和装车单元坐标系的坐标转换关系；

S2、背景建模，基于背景模型进行滤波，滤除背景点；

S3、数据预处理，对步骤S2后的点云数据进行降噪处理；

S4、根据预处理后的点云数据，进行货箱尺寸检测和位姿估计；

S5、根据S4得到的货箱尺寸及位姿，并根据已知货物的尺寸信息，进行码垛规划。

进一步的，步骤S1所述自动化标定的方法包括：

S11、在装车单元运动范围内的固定物上安装人工标记物；

S12、指令控制装车单元在装车作业空间内运动到人工标记物处，分别采集人工标记物在感知单元坐标系下的坐标P_sense和在装车单元坐标系下的坐标 P_load；

S13、构建P_sense的齐次坐标矩阵P_s_matrix；构建P_load的坐标矩阵P_l_matrix，并计算P_s_matrix的广义逆矩阵P_s_matrix_inv，感知单元坐标系与装车单元坐标系的转换矩阵为[R t]=P_l_matrix*P_s_matrix_inv，利用最小二乘法计算出最优的坐标系转换矩阵，完成自动化标定。

进一步的，步骤S2中，具体方法包括：

S21、背景离线建模：采集背景的三维激光点云数据，计算点云每个点的垂直法线向量和曲率，利用区域增长算法将背景点云数据分割为多个面元，每个面元均具备相似的垂直法线和曲率，计算每个面元的中心点、法线方向和最大半径作为特征，依次存储到背景模型配置文件中，完成背景建模；

S22、背景在线滤波：载入背景模型；在获取到包含开放式货箱货车的点云数据之后，计算当前点云中每个点到每个背景面元的距离，以判断该点是否在背景面元的尺寸范围内，然后根据特定的阈值判断当前点是否属于背景，若属于则将该点滤除，循环直至所有点都完成背景滤波。

进一步的，步骤S4中，货箱检测和位姿估计的具体方法包括：

S41、使用区域增长算法对货箱点云进行分割，得到开放式货箱的各个面元；

S42、利用最小包围盒算法计算每个面元的几何尺寸信息；

S43、根据开放式货箱的尺寸确定货箱各个顶点在感知单元坐标系坐标，以此计算开放式货箱的位姿。

进一步的，步骤S5中所述码垛规划的具体方法包括：

a. 根据装车的总重量和袋装粉状物品的单袋重量计算装车的总袋数：N=Weight/weight;

b. 确定物品袋重叠的距离overlay和物品袋侧边可探出车厢的距离extend;

c. 码垛规划自下而上分为货箱内部分、货箱上部分和顶层部分，货箱内部分物品袋无法探出，因此extend=0；顶层部分为保证稳定性，需要骑缝码垛；

d. 码垛共分为三个模块：货箱内，相关参数用in表示；货箱上部分，相关参数用out表示；货箱顶层，相关参数用top表示；rows代表行，cols代表列，layers代表码垛层数；

N代表需要装车的总袋数，N_in代表货箱内可以码垛的袋数；

码垛规划采用整数的线性规划，首先规划货箱内部分，计算货箱内每一列可以码垛的物品袋数cols_in=round(L/(w-overlay/2)), L为车厢长度，w为物品袋宽度，round（*）为四舍五入取整函数；

计算货箱内每一行可以码垛的物品袋数rows_in=round(W/(l-overlay)),W为车厢的宽度，l为物品袋的长度；

计算货箱内可以码垛的层数，layers_in=round(H/h),H为车厢的高度，h为物品袋的高度；

计算货箱内可以码垛的总袋数，N_in = layers_in*cols_in*rows_in；

e. 如果N_in < N,表示货物没有装完，还要继续往上码垛，此时开始进行货箱上部分码垛规划；

由于货箱上部分物品袋可以探出车厢，因此货箱上部分每一行可以码垛的物品袋数rows_out=round((W+2*extend)/(l-overlay))，每一列可以码垛的物品袋数cols_out=round(L/(w-overlay/2))，货箱上部分可以码垛的层数layers_out=floor((N-N_in)/(rows_out*cols_out))，floor为向下取整函数，余数用c表示。货箱上部分每层码垛袋数为rows_out*cols_out，设定layer_out的余数c阈值范围为[α*（rows_out*cols_out），β*（rows_out*cols_out）]，若余数c落在区间[α*（rows_out*cols_out）和β*（rows_out*cols_out）]内，则余数可以实现顶层骑缝码垛，即余数层即为顶层；若余数c小于α*（rows_out*cols_out）或大于β*（rows_out*cols_out），说明此时货物最上层码垛袋数太多或太少，为保证码垛稳定性，此时会调整货箱上部分extend和overlay数值，从而调整余数值，使之落入区间[α*（rows_out*cols_out），β*（rows_out*cols_out）]内；

货箱上部分码垛的总袋数，N_out= layers_out*cols_out*rows_out；

余数c即为货箱顶层码垛袋数N_top；

f. 顶层部分为保证码垛稳定性，需要骑缝码垛，所以顶层部分每一行可以码垛的袋数为rows_top=round((W+2*extend-l)/(l-overlay))，每一列可以码垛的袋数为cols_top=round(L/(w-overlay/2))；

g. 已知每一层的码垛规划结果，根据袋装物品的尺寸，以及货车的坐标依次提取每一袋的坐标，完成码垛规划。

本发明的另一方面，还提供了一种开放式货箱装车三维感知和码垛规划***，包括；

感知单元：将2D激光扫描雷达搭载在横移同步带上随同步带移动作为感知单元，采集三维激光点云数据；

自动化标定单元，用于自动化标定，得到感知单元坐标系和装车单元坐标系的坐标转换关系；

背景建模及滤除单元，用于背景建模，基于背景模型进行滤波，滤除背景点；

数据预处理单元，用于对背景建模及滤除单元处理后的点云数据进行降噪处理；

货箱检测和位姿估计单元，用于根据数据预处理单元预处理后的点云数据，进行货箱检测和位姿估计；

码垛规划单元，用于根据货箱检测和位姿估计单元得到的货箱尺寸及位姿，并根据已知货物的尺寸信息，进行码垛规划。

进一步的，所述自动化标定单元包括：

采集模块：在装车单元运动范围内的固定物上安装人工标记物后，用于指令控制装车单元在装车作业空间内运动到人工标记物处，分别采集人工标记物在感知单元坐标系下的坐标P_sense和装车单元坐标系下的坐标 P_load；

标定模块：构建P_sense的齐次坐标矩阵P_s_matrix；构建P_load的坐标矩阵P_l_matrix，并计算P_s_matrix的广义逆矩阵P_s_matrix_inv，感知单元坐标系与装车单元坐标系的转换矩阵为[R t]=P_l_matrix*P_s_matrix_inv，计算出最小二乘最优的坐标系转换矩阵，完成自动化标定。

进一步的，所述背景建模及滤除单元包括：

背景离线建模模块：用于采集背景的三维激光点云数据，计算点云每个点的垂直法线向量和曲率，利用区域增长算法将背景点云数据分割为多个面元，每个面元均具备相似的垂直法线和曲率，计算每个面元的中心点、法线方向和最大半径作为特征，依次存储到背景模型配置文件中，完成背景建模；

背景在线滤波模块，用于载入背景模型，在获取到包含开放式货箱货车的点云数据之后，计算当前点云中每个点到每个背景面元的距离，以判断该点是否在背景面元的尺寸范围内，然后根据特定的阈值判断当前点是否属于背景，若属于则将该点滤除，循环直至所有点都完成背景滤波。

进一步的，所述货箱检测和位姿估计单元包括：

分割模块，使用区域增长算法对货箱点云进行分割，得到开放式货箱的各个面元；

尺寸计算模块，利用最小包围盒算法计算每个面元的几何尺寸信息；

位姿计算模块，根据开放式货箱的尺寸确定货箱各个顶点在感知单元坐标系坐标，以此计算开放式货箱的位姿。

进一步的，所述码垛规划单元包括：

袋数确定模块：用于根据装车的总重量和袋装粉状物品的单袋重量确定装车的总袋数：N=Weight/weight，在码垛规划之前，N即为已知确定值；

距离确定模块：用于设定物品袋重叠的距离overlay和物品袋侧边可探出车厢的距离extend；

分层模块：用于将码垛规划自下而上分为货箱内部分、货箱上部分和顶层部分，货箱内部分物品袋无法探出，因此extend=0；顶层部分为保证稳定性，需要骑缝码垛；

货箱内部规划模块：码垛共分为三个模块：货箱内，相关参数用in表示；货箱上部分，相关参数用out表示；货箱顶层，相关参数用top表示；rows代表行，cols代表列，layers代表码垛层数；

N代表需要装车的总袋数，N_in代表货箱内可以码垛的袋数；货箱内部规划模块用于将码垛规划采用整数的线性规划，首先规划货箱内部分，计算每一列可以码垛的物品袋数cols_in=round(L/(w-overlay/2)), L为车厢长度，w为物品袋宽度，round（*）为四舍五入取整函数；

计算货箱内每一行可以码垛的物品袋数rows_in=round(W/(l-overlay)),W为车厢的宽度，l为物品袋的长度；

计算货箱内可以码垛的层数，layers_in=round(H/h),H为车厢的高度，h为物品袋的高度；

计算货箱内可以码垛的总袋数，N_in = layers_in*cols_in*rows_in；

货箱上部规划模块：用于如果N_in < N,表示货物没有装完，还要继续往上码垛，此时开始进行货箱上部分码垛规划；

由于货箱上部分物品袋可以探出车厢，因此货箱上部分每一行可以码垛的物品袋数rows_out=round((W+2*extend)/(l-overlay))，每一列可以码垛的物品袋数cols_out=round(L/(w-overlay/2))，货箱上部分可以码垛的层数layers_out=floor((N-N_in)/(rows_out*cols_out))，floor为向下取整函数，余数用c表示。货箱上部分每层码垛袋数为rows_out*cols_out，设定layer_out的余数c阈值范围为[α*（rows_out*cols_out），β*（rows_out*cols_out）]，若余数c落在区间[α*（rows_out*cols_out）和β*（rows_out*cols_out）]内，则余数可以实现顶层骑缝码垛，即余数层即为顶层；若余数c小于α*（rows_out*cols_out）或大于β*（rows_out*cols_out），说明此时货物最上层码垛袋数太多或太少，为保证码垛稳定性，此时会调整货箱上部分extend和overlay数值，从而调整余数值，使之落入区间[α*（rows_out*cols_out），β*（rows_out*cols_out）]内；

货箱上部分码垛的总袋数，N_out= layers_out*cols_out*rows_out；

余数c即为货箱顶层码垛袋数N_top；

货箱顶层规划模块：用于保证码垛稳定性，需要骑缝码垛，所以顶层部分每一行可以码垛的袋数为rows_top=round((W+2*extend-l)/(l-overlay))，每一列可以码垛的袋数为cols_top=round(L/(w-overlay/2))；

坐标提取模块：用于已知每一层的码垛规划结果，根据袋装物品的尺寸，以及货车的坐标依次提取每一袋的坐标，完成码垛规划。

与现有技术相比，本发明具有如下的有益效果：

（1）本发明使用同步带电机和2D激光扫描雷达构建三维感知单元，既节省了硬件成本，又提高了测量精度和分辨率；

（2）本发明采用基于背景滤波的开放式货箱检测方法，无需构建开放式货箱的模型，因此对不同型号和尺寸的开放式货箱都具有很好的适应能力；

（3）本发明基于最优化原理和稳定装车经验实现了自动码垛规划，进一步提升了自动化装车设备的智能化和自动化水平，提升作业效率和质量，并节省厂家装车的成本；

（4）本发明可广泛应用于开放式货箱的自动化装车设备。

具体实施方式

本发明提出的三维感知和码垛规划方法针对不同类型的开放式货箱货车，主要包括三维激光点云数据的采集，开放式货箱的尺寸、位置和姿态估计，码垛规划方法，感知单元和自动化装车单元的自动化标定，装车三维可视化等技术方案。本发明的三维感知具备精度高、速度快的特点，码垛规划支持花样码垛等多种码垛方式，二者均可以在线运行。

本发明出于成本和感知精度的综合考虑，采用2D激光扫描雷达搭载在横移同步带上随同步带移动的三维感知方案，相对于2D激光扫描雷达旋转扫描的方法，本方法在横向移动方向的测量误差为线性误差，不存在测量误差随着观测距离增大而增大的现象，能有效提高测量的精度；相对于3D激光扫描雷达直接测量的方法，本方法在横向移动方向的分辨率（800线）和精度远高于现有的3D激光扫描雷达（16/32线），而且成本低很多。

在实际运行过程中，电脑通过串口向电机控制器发送运动控制指令，电机同步带带动2D激光扫描雷达沿着同步带方向运动，与此同时，2D激光扫描雷达沿着垂直于同步带的方向扫描。对同步带位置数据和2D激光扫描雷达扫描数据进行时间同步，融合后即可得到三维点云。

本次实施采用的激光传感器型号为Sick LMS11系列2D激光扫描雷达，激光的角分辨率设置为0.25°，扫描范围为-45°~225°，激光扫描频率为25HZ;横向导轨的长度为3400mm，横移速度为80.0mm/s;单次扫描时间为42.229s,可以获取包含货车和背景的三维点云数据，共计1099377个点云数据；

本发明中，开放式货箱位姿、尺寸三维感知和码垛规划包含以下步骤：

1、自动化标定。为了完成精准的视觉伺服运动控制，感知单元坐标系和装车单元坐标系之间需要准确的转换关系，为此本发明提出了一种自动化标定方法。首先在装车单元运动范围内的固定物上安装人工标记物，然后指令控制装车单元在装车作业空间内运动到人工标记物处，分别采集人工标记物在感知单元坐标系和在装车单元坐标系下的坐标，P_sense和P_load，采集M组数据（M>3）。感知单元坐标系和装车单元坐标系的坐标转换关系为P_load=[R t]P_sense。构建P_sense的齐次坐标矩阵P_s_matrix；构建P_load的坐标矩阵P_l_matrix，并计算P_s_matrix的广义逆矩阵P_s_matrix_inv，感知单元坐标系与装车单元坐标系的转换矩阵为[R t]=P_l_matrix*P_s_matrix_inv，利用最小二乘法计算出最优的坐标系转换矩阵，完成自动化标定。

具体为：利用激光采集4组标记物的点云数据,得到标记物在感知单元坐标系下的齐次坐标矩阵为P_s_matrix为:

，利用指令控制装车单元采集4组不同位置下标记物的数据得到标记物在装车单元坐标系下的坐标矩阵P_l_matrix为：

，

求取P_s_matrix的广义逆矩阵P_s_matrix_inv为:

，

根据公式[R t]=P_l_matrix*P_s_matrix_inv，求取感知单元坐标系与装车单元坐标系之间的转换矩阵[R t]为：

，

以上完成了自动化标定。

2、背景建模与滤波。基于三维点云的物体检测、识别和分割算法有很多。直接检测的方法需要实现对被检测物体进行建模，并要求被检测物体有明显的特征。在本应用场景下无法获取所有开放式货箱货车模型并且开放式货箱货车一般比较类似，无明显的可区分特征，所以不适合用直接检测的方法。本发明针对特定的应用场景独创性的提出了基于背景模型滤波的开放式货箱检测方法，具体方法如下所示：

2.1、背景离线建模

首先采集背景的三维激光点云数据，可采集多张融合为一个稠密的激光点云背景数据。然后计算点云每个点的垂直法线向量和曲率，利用区域增长算法将背景点云数据分割为多个面元，每个面元均具备相似的垂直发现和曲率。最后计算每个面元的中心点、法线方向和最大半径作为特征，依次存储到背景模型配置文件中，完成背景建模；

2.2、背景在线滤波

首先载入背景模型，在获取到包含开放式货箱货车的点云数据之后，计算当前点云中每个点到每个背景面元的距离以及判断改点是否在背景面元的尺寸范围内，然后根据特定的阈值判断当前点是否属于背景，若属于则将该点滤除，循环直至所有点均完成背景滤波。

3、数据预处理。完成背景滤波之后的点云包含开放式货箱的点云和其他噪声点云，为了提升后续感知算法的效率和精度，进行降噪处理。首先用混合高斯模型描述点云的空间分布状况，计算反应点云稀疏程度的方差Sigma；然后设定阈值，将大于阈值的点剔除；最后对新的点云进行降采样，完成数据预处理。

4、货箱检测和位姿估计。同背景离线建模类似，本发明首先使用区域增长算法对货箱点云进行分割，得到开放式货箱的各个面元；然后利用最小包围盒算法计算每个面元的几何尺寸信息，最小包围盒算法的主体思路是对点云的协方差矩阵进行主成分分析（PCA）确定点云在三维方向的主成分，由此生成最小包围盒；最后根据开放式货箱面元的几何关系，例如底面面积最大、底面的长边与左右侧面共线、底面的短边与前后侧面共线等约束，推算集合面元的相互位置关系；然后根据各个面元的点云稀疏程度和观测状态确定各个面元的权重，以此推断开放式货箱的长宽高等尺寸。最后根据开放式货箱的尺寸确定货箱各个顶点在感知单元坐标系坐标，以此计算货箱的位姿。将上述中滤波后的点云数据带入三维感知算法，可以估计出货车车厢的尺寸为[4202,2365，429]，货车车厢的真实尺寸[4200,2350,410]mm；三个维度的误差均小于20mm；货车车辆的位置为[-527,363,-3109]mm,位姿三个轴分别为[0.9999,-0.0044,0.0013]、[0.0043,0.9999,-0.0009]、[-0.0014,0.0008,0.9999]。

5、码垛规划。一般待装箱货物都是符合行业标准的标准尺寸，例如水泥、白砂糖、面粉等等，所以货物的尺寸信息为已知的信息。将码垛规划自下而上分为货箱内部分、货箱上部分和顶层部分，货箱内部分和货箱上部分则采用整数的线性规划，顶层部分为保证稳定性，需要骑缝码垛；

具体实施方式为：假设要装载10吨物品，每袋物品的重量为50kg,物品袋的尺寸为[600,400,200]mm，设定码垛时物品袋重叠的距离overlay为100mm和物品袋侧边可探出车厢的距离extend为100mm;感知出来车体的尺寸为[4202,2365，429]mm,根据公式确定装车的总袋数N为10000/50=200袋，然后进行码垛规划，首先规划货箱内部分，计算每一列可以码垛的物品袋数cols_in=round(L/(w-overlay/2)), L为车厢长度，w为物品袋宽度，round（*）为四舍五入取整函数，代入数值为cols_in=(round(4202/(400-50)))=12；计算每一行可以码垛的物品袋数rows_in=round(W/(l-overlay)),W为车厢的宽度，l为物品袋的长度，代入数值为rows_in=(round(2365/(600-100)))=5；计算货箱内可以码垛的层数，layers_in=round(H/h),H为车厢的高度，h为物品袋的高度，代入数值为layers_in=round(429/200)=2；计算货箱内可以码垛的总袋数，N_in = layers_in*cols_in*rows_in=2*12*5=120；由于N_in<N,所以继续规划车箱上层部分，由于货箱上部分物品袋可以探出车厢，因此每一行可以码垛的物品袋数rows_out=round((W+2*extend)/(l-overlay))=round((2365+2*100)/(600-100))=5;cols_out=round(L/(w-overlay/2))=(round(4202/(400-50)))=12;layers_out=floor((N-N_in)/(rows_out*cols_out))=(200-120)/(12*5)=1…20；计算货箱上部分可以码垛的总袋数，N_out = layers_out*cols_out*rows_out=1*12*5=60;余数c=20,N_top=20,顶层部分为保证码垛稳定性，需要骑缝码垛，所以每一行可以码垛的袋数为cols_top=(W+2*extend-l)/(l-overlay)=(2365+2*100-600)/(600-100)=4,rows_top=round((W+2*extend)/(l-overlay))=round((2365+2*100)/(600-100))=5;；则码垛规划结果为：

a.车厢内装载120袋，2层12列5排

b.车厢外装载60袋，1层12列5排

c.顶层装载20袋，1层4列5排

6、三维可视化。得到开放式货箱的尺寸、位姿信息和码垛规划结构后，本发明可在三维可视化界面中按照真实尺寸展示开放式货箱，并按照码垛规划结果按照顺序依次显示货物的装车结果。

7、视觉伺服装车作业。物品装车单元的运动控制由PLC控制，三维感知和码垛规划的结果转换到装车单元坐标系下面并通过PLC特定的通信协议发送给PLC控制器，PLC控制器在收到结果后控制装车机构开始码垛，在此过程中可实时查询PLC控制器的状态。

本发明提出了一种开放式货箱的位置、姿态和尺寸的三维感知以及袋装物体码垛规划方法，本方法可使自动化装车设备自动感知货车停放的不同位姿以及不同的货车类型，根据装货量自动规划码垛装车方法，并视觉伺服控制装车设备完成自动化装车，整个装车过程完全不需要人工介入，极大地提高了开放式货箱装车的自动化水平和效率，为厂家节省了成本。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的技术人员，在不脱离本发明构思的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明保护范围内。

Claims (10)

1.一种开放式货箱装车三维感知和码垛规划方法，其特征在于，将2D激光扫描雷达搭载在横移同步带上随同步带移动作为感知单元采集三维激光点云数据，还包括：

S1、自动化标定，得到感知单元坐标系和装车单元坐标系的坐标转换关系；

S2、背景建模，基于背景模型进行滤波，滤除背景点；

S3、数据预处理，对步骤S2后的点云数据进行降噪处理；

S4、根据预处理后的点云数据，进行货箱尺寸检测和位姿估计；

S5、根据S4得到的货箱尺寸及位姿，并根据已知货物的尺寸信息，进行码垛规划。

2.根据权利要求1所述的方法，其特征在于，步骤S1所述自动化标定的方法包括：

S11、在装车单元运动范围内的固定物上安装人工标记物；

S12、指令控制装车单元在装车作业空间内运动到人工标记物处，分别采集人工标记物在感知单元坐标系下的坐标P_sense和在装车单元坐标系下的坐标 P_load；

S13、构建P_sense的齐次坐标矩阵P_s_matrix；构建P_load的坐标矩阵P_l_matrix，并计算P_s_matrix的广义逆矩阵P_s_matrix_inv，感知单元坐标系与装车单元坐标系的转换矩阵为[R t]=P_l_matrix*P_s_matrix_inv，利用最小二乘法计算出最优的坐标系转换矩阵，完成自动化标定。

3.根据权利要求1所述的方法，其特征在于，步骤S2中，具体方法包括：

S21、背景离线建模：采集背景的三维激光点云数据，计算点云每个点的垂直法线向量和曲率，利用区域增长算法将背景点云数据分割为多个面元，每个面元均具备相似的垂直法线和曲率，计算每个面元的中心点、法线方向和最大半径作为特征，依次存储到背景模型配置文件中，完成背景建模；

S22、背景在线滤波：载入背景模型；在获取到包含开放式货箱货车的点云数据之后，计算当前点云中每个点到每个背景面元的距离，以判断该点是否在背景面元的尺寸范围内，然后根据特定的阈值判断当前点是否属于背景，若属于则将该点滤除，循环直至所有点都完成背景滤波。

4.根据权利要求1所述的方法，其特征在于，步骤S4中，货箱检测和位姿估计的具体方法包括：

S41、使用区域增长算法对货箱点云进行分割，得到开放式货箱的各个面元；

S42、利用最小包围盒算法计算每个面元的几何尺寸信息；

S43、根据开放式货箱的尺寸确定货箱各个顶点在感知单元坐标系坐标，以此计算开放式货箱的位姿。

5.根据权利要求1所述的方法，其特征在于，步骤S5中所述码垛规划的具体方法包括：

a. 根据装车的总重量和袋装粉状物品的单袋重量计算装车的总袋数：N=Weight/weight；

b. 确定物品袋重叠的距离overlay和物品袋侧边可探出车厢的距离extend；

c. 码垛规划自下而上分为货箱内部分、货箱上部分和顶层部分，货箱内部分物品袋无法探出，因此extend=0；顶层部分为保证稳定性，需要骑缝码垛；

d. 码垛共分为三个模块：货箱内，相关参数用in表示；货箱上部分，相关参数用out表示；货箱顶层，相关参数用top表示；rows代表行，cols代表列，layers代表码垛层数；

N代表需要装车的总袋数，N_in代表货箱内可以码垛的袋数；

码垛规划采用整数的线性规划，首先规划货箱内部分，计算货箱内每一列可以码垛的物品袋数cols_in=round(L/(w-overlay/2)), L为车厢长度，w为物品袋宽度，round（*）为四舍五入取整函数；

计算货箱内每一行可以码垛的物品袋数rows_in=round(W/(l-overlay)),W为车厢的宽度，l为物品袋的长度；

计算货箱内可以码垛的层数，layers_in=round(H/h),H为车厢的高度，h为物品袋的高度；

计算货箱内可以码垛的总袋数，N_in = layers_in*cols_in*rows_in；

e. 如果N_in < N,表示货物没有装完，还要继续往上码垛，此时开始进行货箱上部分码垛规划；

由于货箱上部分物品袋可以探出车厢，因此货箱上部分每一行可以码垛的物品袋数rows_out=round((W+2*extend)/(l-overlay))，每一列可以码垛的物品袋数cols_out=round(L/(w-overlay/2))，货箱上部分可以码垛的层数layers_out=floor((N-N_in)/(rows_out*cols_out))，floor为向下取整函数，余数用c表示；

货箱上部分每层码垛袋数为rows_out*cols_out，设定layer_out的余数c阈值范围为[α*（rows_out*cols_out），β*（rows_out*cols_out）]，若余数c落在区间[α*（rows_out*cols_out）和β*（rows_out*cols_out）]内，则余数可以实现顶层骑缝码垛，即余数层即为顶层；若余数c小于α*（rows_out*cols_out）或大于β*（rows_out*cols_out），说明此时货物最上层码垛袋数太多或太少，为保证码垛稳定性，此时会调整货箱上部分extend和overlay数值，从而调整余数值，使之落入区间[α*（rows_out*cols_out），β*（rows_out*cols_out）]内；

货箱上部分码垛的总袋数，N_out= layers_out*cols_out*rows_out；

余数c即为货箱顶层码垛袋数N_top；

f. 顶层部分为保证码垛稳定性，需要骑缝码垛，所以顶层部分每一行可以码垛的袋数为rows_top=round((W+2*extend-l)/(l-overlay))，每一列可以码垛的袋数为cols_top=round(L/(w-overlay/2))；

g. 已知每一层的码垛规划结果，根据袋装物品的尺寸，以及货车的坐标依次提取每一袋的坐标，完成码垛规划。

6.一种开放式货箱装车三维感知和码垛规划***，其特征在于，包括：

感知单元：将2D激光扫描雷达搭载在横移同步带上随同步带移动作为感知单元，采集三维激光点云数据；

自动化标定单元，用于自动化标定，得到感知单元坐标系和装车单元坐标系的坐标转换关系；

背景建模及滤除单元，用于背景建模，基于背景模型进行滤波，滤除背景点；

数据预处理单元，用于对背景建模及滤除单元处理后的点云数据进行降噪处理；

货箱检测和位姿估计单元，用于根据数据预处理单元预处理后的点云数据，进行货箱检测和位姿估计；

码垛规划单元，用于根据货箱检测和位姿估计单元得到的货箱尺寸及位姿，并根据已知货物的尺寸信息，进行码垛规划。

7.根据权利要求6所述的***，其特征在于，所述自动化标定单元包括：

采集模块：在装车单元运动范围内的固定物上安装人工标记物后，用于指令控制装车单元在装车作业空间内运动到人工标记物处，分别采集人工标记物在感知单元坐标系下的坐标P_sense和装车单元坐标系下的坐标 P_load；

标定模块：构建P_sense的齐次坐标矩阵P_s_matrix；构建P_load的坐标矩阵P_l_matrix，并计算P_s_matrix的广义逆矩阵P_s_matrix_inv，感知单元坐标系与装车单元坐标系的转换矩阵为[R t]=P_l_matrix*P_s_matrix_inv，计算出最小二乘最优的坐标系转换矩阵，完成自动化标定。

8.根据权利要求6所述的***，其特征在于，所述背景建模及滤除单元包括：

背景离线建模模块：用于采集背景的三维激光点云数据，计算点云每个点的垂直法线向量和曲率，利用区域增长算法将背景点云数据分割为多个面元，每个面元均具备相似的垂直法线和曲率，计算每个面元的中心点、法线方向和最大半径作为特征，依次存储到背景模型配置文件中，完成背景建模；

背景在线滤波模块，用于载入背景模型，在获取到包含开放式货箱货车的点云数据之后，计算当前点云中每个点到每个背景面元的距离，以判断该点是否在背景面元的尺寸范围内，然后根据特定的阈值判断当前点是否属于背景，若属于则将该点滤除，循环直至所有点都完成背景滤波。

9.根据权利要求6所述的***，其特征在于，所述货箱检测和位姿估计单元包括：

分割模块，使用区域增长算法对货箱点云进行分割，得到开放式货箱的各个面元；

尺寸计算模块，利用最小包围盒算法计算每个面元的几何尺寸信息；

位姿计算模块，根据开放式货箱的尺寸确定货箱各个顶点在感知单元坐标系坐标，以此计算开放式货箱的位姿。

10.根据权利要求6所述的***，其特征在于，所述码垛规划单元包括：

袋数确定模块：用于根据装车的总重量和袋装粉状物品的单袋重量确定装车的总袋数：N=Weight/weight，在码垛规划之前，N即为已知确定值；

距离确定模块：用于设定物品袋重叠的距离overlay和物品袋侧边可探出车厢的距离extend；

分层模块：用于将码垛规划自下而上分为货箱内部分、货箱上部分和顶层部分，货箱内部分物品袋无法探出，因此extend=0；顶层部分为保证稳定性，需要骑缝码垛；

货箱内部规划模块：码垛共分为三个模块：货箱内，相关参数用in表示；货箱上部分，相关参数用out表示；货箱顶层，相关参数用top表示；rows代表行，cols代表列，layers代表码垛层数；

N代表需要装车的总袋数，N_in代表货箱内可以码垛的袋数；货箱内部规划模块用于将码垛规划采用整数的线性规划，首先规划货箱内部分，计算每一列可以码垛的物品袋数cols_in=round(L/(w-overlay/2)), L为车厢长度，w为物品袋宽度，round（*）为四舍五入取整函数；

计算货箱内每一行可以码垛的物品袋数rows_in=round(W/(l-overlay)),W为车厢的宽度，l为物品袋的长度；

计算货箱内可以码垛的层数，layers_in=round(H/h),H为车厢的高度，h为物品袋的高度；

计算货箱内可以码垛的总袋数，N_in = layers_in*cols_in*rows_in；

货箱上部规划模块：用于如果N_in < N,表示货物没有装完，还要继续往上码垛，此时开始进行货箱上部分码垛规划；

由于货箱上部分物品袋可以探出车厢，因此货箱上部分每一行可以码垛的物品袋数rows_out=round((W+2*extend)/(l-overlay))，每一列可以码垛的物品袋数cols_out=round(L/(w-overlay/2))，货箱上部分可以码垛的层数layers_out=floor((N-N_in)/(rows_out*cols_out))，floor为向下取整函数，余数用c表示；

货箱上部分每层码垛袋数为rows_out*cols_out，设定layer_out的余数c阈值范围为[α*（rows_out*cols_out），β*（rows_out*cols_out）]，若余数c落在区间[α*（rows_out*cols_out）和β*（rows_out*cols_out）]内，则余数可以实现顶层骑缝码垛，即余数层即为顶层；若余数c小于α*（rows_out*cols_out）或大于β*（rows_out*cols_out），说明此时货物最上层码垛袋数太多或太少，为保证码垛稳定性，此时会调整货箱上部分extend和overlay数值，从而调整余数值，使之落入区间[α*（rows_out*cols_out），β*（rows_out*cols_out）]内；

货箱上部分码垛的总袋数，N_out= layers_out*cols_out*rows_out；

余数c即为货箱顶层码垛袋数N_top；

货箱顶层规划模块：用于保证码垛稳定性，需要骑缝码垛，所以顶层部分每一行可以码垛的袋数为rows_top=round((W+2*extend-l)/(l-overlay))，每一列可以码垛的袋数为cols_top=round(L/(w-overlay/2))；

坐标提取模块：用于已知每一层的码垛规划结果，根据袋装物品的尺寸，以及货车的坐标依次提取每一袋的坐标，完成码垛规划。