CN115063475A - 一种基于3d传感器的面形状识别及定位方法 - Google Patents

Abstract

本发明提供了一种基于3D传感器的面形状识别及定位方法，其识别方法步骤包括：S1获取目标物体参数，构建目标物体端面的栅格模板；S2基于3D传感器采集的传感数据建立目标点云；S3剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算；S4根据目标物体的端面参数设置滑窗，并根据预设步长滑动滑窗，每次滑动判断滑窗内的点云数量及其凸包面积是否符合预设条件；S5当判断结果符合预设条件时，提取滑窗内的点云与栅格模板进行匹配，步骤包括：将滑窗内的点云按照栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，计算其与栅格模板之间的匹配度，直至符合匹配度阈值时停止，以获取识别结果。

Description

一种基于3D传感器的面形状识别及定位方法

技术领域

本发明涉及机器人定位技术，尤其涉及基于传统3D传感器采集的数据来进行的目标物体面形状识别及定位方法。

背景技术

本发明所称的机器人是一种能够自动运行的移动机器人，如轮式机器人，其根据工作属性的不同可以分为：清扫机器人、消毒机器人、巡检机器人、搬运机器人等。

在工业应用领域，具有移动和搬运功能的自动化机器人正逐步替代人工介入到现有的生产体系之中，以实现物料搬运、插取等任务，然而这机器人之所以能够实现自动化控制，主要仰赖于不断发展的机器人的识别及定位技术。

比如自动叉车机器人通过插取栈板来进行货物转运，如果事先不知道栈板的具体位姿，那么在机器人插取过程中，很容易出现插取失败的情况，因此如何快速识别及定位此类栈板的位置，则是目前本领域一直在迭代改进的目标。

目前现有技术中，比较成熟的技术通常会使用RGB-D相机根据特征来确定栈板的位置，这种技术也是目前应用比较广泛的技术，但是该技术的问题在于通用性不强，依赖于特定的深度相机设备。

另一方面，基于样本学习的技术也是目前比较热门的技术，也叫深度学习，深度学习可以使识别达到很高的准确度，但是对计算性能要求较高。

发明内容

本发明的主要目的在于提供一种基于3D传感器的面形状识别及定位方法，以普适多种3D传感器提高通用性，同时降低计算性能要求。

为了实现上述目的，根据本发明的第一个方面，提供了一种基于3D传感器的面形状识别方法，其步骤包括：

S1获取目标物体参数，构建目标物体端面的栅格模板；

S2 基于3D传感器采集的传感数据建立目标点云；

S3剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算；

S4根据目标物体的端面参数设置滑窗，并根据预设步长滑动滑窗，其中每次滑动判断滑窗内的面状点云块的点云数量及其凸包面积是否符合预设条件；

S5当步骤S4判断结果符合预设条件时，提取滑窗内的点云与栅格模板进行匹配，步骤包括：将滑窗内的点云按照栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，计算其与栅格模板之间的匹配度，直至符合匹配度阈值时停止，以获取识别结果。

为了实现上述目的，根据本发明的第二个方面，还提供了一种基于3D传感器的面形状识别方法，其步骤包括：

S1获取目标物体参数，构建目标物体端面的粗栅格模板和细栅格模板；

S2 基于3D传感器采集的传感数据建立目标点云；

S3剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算；

S4根据目标物体的端面参数设置滑窗，并根据预设步长来滑动滑窗，其中每次滑动需判断滑窗内的面状点云块的点云数量及其凸包面积是否符合预设条件；

S5当步骤S4判断结果符合预设条件时，提取滑窗内的点云与粗栅格模板进行匹配，步骤包括：将滑窗内的点云按照粗栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，并计算其与粗栅格模板之间的匹配度，直至符合匹配度阈值时停止，并记录匹配区域；

S6在匹配区域附近建立包围盒，将滑窗栅格中的点云按照细栅格模板的栅格尺寸进行栅格化，以构建细滑窗栅格，并根据预设步长在包围盒内与细栅格模板进行滑窗匹配，直至获取匹配度最高的一次作为识别结果。

在可能的优选实施方式中，步骤S2中还包括对目标点云进行预处理，其步骤包括：

S21对目标点云进行滤波处理，以将目标点云数据赋予为对应于自然三轴坐标系的点云坐标，并根据目标物体的高度参数，滤除不匹配的目标点云；

S22 对步骤S21处理后的目标点云采用统计滤波处理，除去离群点。

在可能的优选实施方式中，步骤S3中剔除目标点云中的地面点云的步骤包括：

S31选择目标点云数据下所有的点距离基准平面的在容差距离范围内的点的数量最多的基准平面作为地面，并将该面上的所有的点归属于地面成分，而其余点归属于物体成分。

在可能的优选实施方式中，步骤S3中提取出垂直于地面的面状点云块的步骤包括：

S32从物体成分点云数据中随机选取种子点，并判断种子点与作为种子点周围的非种子点是否处于同一平面内，其中种子点的法向量与地面法向量垂直，当确定种子点和非种子点处于同一平面内时，确定将该非种子点作为新的种子点；

S33迭代判断新的种子点与其周围的非种子点是否处于同一平面内，以通过点云区域生长的方式统计出所有的种子点;

S34基于所统计的种子点，构建垂直于地面的面状点云块。

在可能的优选实施方式中，步骤S34中基于所统计的种子点，构建垂直于地面的面状点云块的步骤包括：判断所统计的种子点的数量是否处于预设定的数量范围内，并当种子点数量在数量范围内时，基于所统计的种子点构建所述面状点云块。

在可能的优选实施方式中，构建垂直于地面的面状点云块的步骤还包括：对判断为同一物体的同一面的所有面状点云块做合并处理。

在可能的优选实施方式中，所述判断同一物体的同一面的步骤包括：将各面状点云块转为平面方程ax+by+cz=1，判断各个面状点云块的平面方程式中a、b、c三个因子是否相似且因子之差的绝对值小于预设阈值，若符合的则判断为同一物体的同一面。

在可能的优选实施方式中，所述步骤S5及S6中的匹配步骤包括：将滑窗栅格/细滑窗栅格中存在点云的栅格进行标记，并计算 X=(p-q)/(1-q)，其中X表示当前滑窗栅格/细滑窗栅格与粗栅格模板/细栅格模板之间的匹配度，p表示当前滑窗栅格/细滑窗栅格与粗栅格模板/细栅格模板之间相同栅格与粗栅格模板/细栅格模板栅格个数的比，q表示滑窗栅格/细滑窗栅格中未标记栅格的个数与滑窗栅格/细滑窗栅格中栅格总数之比。

为了实现上述目的，根据本发明的第三个方面，还提供了一种基于3D传感器的定位方法，其步骤包括：

S1根据如上述任一所述基于3D传感器的面形状识别方法获取匹配度最高的一次的细滑窗栅格的中心点坐标作为6Dpose的x、y、z参数；

S2 将roll和pitch的角度设为0；

S3 根据面状点云块建立平面方程ax+by+cz=1，依据该平面方程及其a、b、c三个因子，计算出yaw参数值

，以获取完整6D pose。

通过本发明提供的该基于3D传感器的面形状识别及定位方法，能够普适多种3D传感器技术通用性较强，无论是深度相机、多线激光雷达还是固态激光雷达，都能适用；其次，本发明的方案不需要预先训练样本，同时相比深度学习方案占用的计算性能更低。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明的基于3D传感器的面形状识别方法中目标物体栈板端面示意图；

图2为本发明的基于3D传感器的面形状识别方法中粗栅格模板示意图；

图3为本发明的基于3D传感器的面形状识别方法中细栅格模板示意图；

图4为本发明的基于3D传感器的面形状识别方法中滑窗示意图；

图5为本发明的基于3D传感器的面形状识别方法中滑窗在栈板端面面状点云块上滑动的示意图。

图6为本发明的基于3D传感器的面形状识别方法中将滑窗内的点云与粗栅格模板进行匹配的示意图。

图7为本发明的基于3D传感器的面形状识别方法中将滑窗内的点云与细栅格模板进行匹配的示意图。

图8为本发明的基于3D传感器的面形状识别及定位方法的步骤示意图。

具体实施方式

为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本申请中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。

此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“S1”、“S2”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。除非另有明确的规定和限定，术语“设置”、“布设”、“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况，结合现有技术来理解上述术语在本案中的具体含义。

其中需要说明的是，本发明的示例中，所述3D传感器可以是3D激光传感器，也可以使用3D相机传感器并且只使用深度信息。具体来说，本发明的方案可适用于非重复式扫描模式的3D激光雷达，如大疆LivoxM70传感器，但对于机械式扫描的3D激光雷达以及深度相机同样适用。对于机械式扫描的3D激光雷达，不需要对点云进行拼接即可直接进行计算；对于深度相机，需要先把深度图转换为3D点云，也不需要进行点云拼接即可进行计算。

此外，下述示例中虽然示例了一种栈板的识别及定位处理过程，但本领域技术人员可以根据下述实施例理解到，本发明的方案也可适用于识别多个不同种类的栈板，甚至可以拓展为非标准的栈板。从而本发明可通过直接输入CAD图纸即可取得模板参数，以将模板和面点云进行栅格化，并通过粗细两种不同分辨率的栅格进行快速定位以及精准识别。

以下本发明将以叉车机器人插取栈板为例，示例在叉车机器人上安装3D传感器去识别栈板及其位置的过程。

为此，请参阅图1至图8所示，本发明提供的该基于3D传感器的面形状识别方法，其步骤包括：

步骤S1

获取目标物体参数，构建目标物体端面的粗栅格模板（图2所示）和细栅格模板（图3所示）。具体来说，如图1所示，本本示例中该目标物体（如：栈板）可通过软件进行模型构建（如：CAD软件），为此该栈板的各个端面的参数可以预先获得，其次栈板的端面参数配置后，即可形成目标物体的模板，之后即可将该模板对应转换为粗栅格模板和细栅格模板，以备后续匹配之用。

其中本示例中的粗栅格模板和细栅格模板的“粗/细”为两者相对尺寸比较关系，因此可以理解为粗/细栅格模板分别需要采用不同的分辨率构建，即粗栅格模板的分辨率要小于细栅格模板，而具体分辨率可根据实际目标物体端面大小适应性设置，此外在本示例中任一栅格化后的特征，皆可理解为，如果占据了一个栅格方框面积的1/2，即认为该栅格是被占据的，从而便于后续模板匹配判断。

步骤S2

基于3D传感器采集的传感数据建立栈板点云，并对栈板点云进行预处理，步骤包括：

步骤S21对栈板点云数据进行滤波的预处理操作，然后再经滤波的栈板点云数据中分离出地面成分和物体成分。其中关于滤波的处理操作，其具体可以是将所获取的栈板点云数据赋予为对应于自然三轴坐标系的点云坐标，然后获取待识别的栈板的需求高度，进而根据需求高度，滤除点云坐标下与需求高度不匹配的栈板点云数据。

例如示例中，当栈板的高度为15cm时，若栈板放在水平地面上，则可以是使用直通滤波器仅获取距离地面0~15cm的点云，而若栈板堆叠在两层同样高度的栈板上，则用直通滤波器获取距离地面30~45cm的点云。

步骤S22 然后对步骤S21处理后获取的点云采用统计滤波器，除去离群点。

由此，便可通过多次滤波的预处理方式，有效减少噪音和冗余的数据，以提高后续算法的准确率和运算速度，以及算法的鲁棒性和实时性

步骤S3

剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算。

其中剔除目标点云中的地面点云的步骤包括：

步骤S31选择目标点云数据下所有的点距离基准平面的在容差距离范围内的点的数量最多的基准平面作为地面，并将该面上的所有的点归属于地面成分，而其余点归属于物体成分。

具体来说，要对步骤S2预处理之后的点云进行地面点云剔除，首先需要分别提取出栈板点云数据中地面成分和物体成分，为此本示例中，以随机抽样一致算法与平面模型匹配方式，来分离点云数据中的地面成分和物体成分。具体的，可以是分别多次随机从栈板点云数据中抽取多个点，并相应地拟合出多个基准平面；之后统计点云数据下所有的点分别与多个基准平面的距离在容差距离范围内的相应的点的数量；然后比较所统计的点的数量，以从多个基准平面确定地面成分。

例如，可以是选择点云数据下所有的点距离基准平面的在容差距离范围内的点的数量最多的基准平面作为地面，并将该面上的所有的点归属于地面成分，从而分离出栈板点云数据中地面成分和物体成分。

其中提取出垂直于地面的面状点云块的步骤包括：

步骤S32从物体成分点云数据中随机选取种子点，并判断种子点与作为种子点周围的非种子点是否处于同一平面内，其中种子点的法向量与地面法向量垂直，当确定种子点和非种子点处于同一平面内时，确定将该非种子点作为新的种子点。

步骤S33迭代判断新的种子点与其周围的非种子点是否处于同一平面内，以通过点云区域生长的方式统计出所有的种子点。

步骤S34基于步骤S33所统计的种子点，构建垂直于地面的面状点云块。具体来说，即判断所统计的种子点的数量是否处于预设定的数量范围内，并当该种子点数量在数量范围内时，可以基于所统计的种子点构建垂直于地面的面状点云块。

同时若该种子点数量过高或过低时，则可以认定所统计的种子点所对应的面不属于面状点云块，该做法同时可以得出平面方程参数。

之后，在获取所有的面状点云块之后，为了保证同一物体的同一面的完整性，本示例中还进一步对所有垂直于地面的面状点云块做合并处理。如通过前述平面方程以及距离做判断，如果平面方程的因子相似且距离满足阈值时，则将两个点云块合并为同一点云块。

如示例中，任何平面方程都可以写成ax+by+cz=1，如果两个面状点云块的平面方程式中a、b、c三个因子相似并且因子之差的绝对值小于阈值，则可将两个面状点云块合并为同一点云块。籍此保证同一物体同一面的完整性。

此后，由于上述步骤已经剔除了点数过少的点云块，因此便可对每个点云块计算其凸包的面积，以剔除面积过小的点云块。即本示例中，所称凸包面积计算，实际是寻找一个多边形，以能够将面状点云块完整的包围起来，并计算这个多边形的面积。

步骤S4

根据栈板的端面参数设置滑窗，并根据预设步长来滑动滑窗，其中每次滑动需判断滑窗内的面状点云块的点云数量及其凸包面积是否符合预设条件。

具体来说，建立该滑窗的端面参数在步骤S1中已经获得，如栈板端面的长宽以及中间孔洞的长宽参数，因此如图4所示可直接根据该参数设置滑窗，之后根据预设步长来滑动该滑窗，其中该预设步长根据单位栅格的大小而定，例如为粗栅格模板单位栅格长的整数倍。

之后，如图5所示，每滑动一次，需要判断窗口内的点数以及凸包的面积，符合条件即可进行下一步，其中凸包中的点数需要根据不同的3D传感器的分辨率以及扫描栈板的距离而定，示例中一般不少于100个点，此外根据已知的栈板参数长宽可求得其面积，以此面积的2/3作为阈值，大于该阈值则视为有效点云块，即符合条件，否则继续滑动窗口。

步骤S5

当步骤S4判断结果符合预设条件时，提取滑窗内的点云与粗栅格模板进行匹配，步骤包括：将滑窗内的点云按照粗栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，并计算其与粗栅格模板之间的匹配度，直至符合匹配度阈值时停止，并记录匹配区域；

具体来说，如图6所示，将滑窗内的点云与构建出来的粗栅格模板进行匹配，首先将滑窗内的点云按照粗栅格模板的栅格大小进行栅格化，以获得滑窗栅格，其中栅格的大小决定着识别定位的精度以及计算速度，本示例中将粗栅格模板的resolution设置为3cm，将细栅格的resolution设置为0.5cm，需要注意的是，栅格大小的具体划分和3D传感器的分辨率以及栈板的大小相关，实施过程中可以自行进行调节。如对于每个面状点云块，首先计算点云块中最大和最小的点，通过最大最小点x和y坐标相减来得到真实尺寸的height和width，接着可以得到栅格图的大小为：

接着将滑窗栅格与粗栅格进行匹配判断，如图6所示，灰色栅格为占据栅格，白色栅格为空闲栅格，则匹配计算公式为：

式中，X表示当前滑窗与粗栅格模板之间的匹配度，p表示当前滑窗栅格与粗栅格之间相同栅格与粗栅格栅格个数的比，q表示窗口栅格中白色栅格的个数与粗栅格中栅格总数之比。

之后，根据预设步长逐步滑动滑窗栅格，判断滑窗栅格与粗栅格模板的匹配度是否大于设定的阈值，如果大于阈值则意味着获得了识别结果，并且此时滑窗栅格处于匹配区，此时根据匹配的粗栅格模板即可知晓当前检测输入的的点云是栈板的哪个端面。

进一步的，当完成粗识别后，也可以直接进行识别后的定位计算，即计算当前3D传感器获取的栈板点云在空间中的位姿即6Dpose，

如本发明另一方面，还提供了一种基于3D传感器的栈板定位方法，其中由于栈板的位姿在空间中存在6个自由度，即平移量x，y，z和旋转量roll，pitch，yaw，即6d pose。为此如图8所示，该栈板定位方法的步骤包括：

步骤S1

根据上述实施例中基于3D传感器的面形状识别方法获取匹配度最高的一次的滑窗栅格的中心点坐标作为6Dpose的x、y、z参数。

步骤S2

其中由于栈板和机器人都是默认在同一空间，且地面是水平的，那么此时可以认为栈板相对于叉车叉齿是平行的，即不存在roll和pitch的角度，为此可将roll和pitch的角度设为0。

步骤S3

此时6Dpose还需要计算yaw角，为此本示例中，优选根据上述实施例中步骤S34中建立的栈板面状点云块平面方程ax+by+cz=1，依据该平面方程及其a、b、c三个因子，计算出yaw参数值

，即可获取该栈板端面完整的6D pose[x，y，z，roll，pitch，yaw]参数，至此完成对其定位。

另一方面，在步骤S5的基础上，若为了获取更高精准的6Dpose数据，还可执行精匹配步骤。

步骤S6

在步骤S5的匹配区附近建立包围盒，将滑窗栅格中的点云按照细栅格模板的栅格尺寸进行栅格化，以构建细滑窗栅格，并根据预设步长在包围盒内与细栅格模板进行滑窗匹配，直至获取匹配度最高的一次作为识别结果。

具体来说，如图7所示，为了得到栈板点云更为精准的6Dpose，需要对滑窗栅格中的点云进行更精细的栅格化，此时在步骤S5的匹配区周围建立包围盒，如以当前滑窗向四周扩散一个或多个单位栅格所围设组成的区域，然后根据细栅格模板的栅格大小对滑窗栅格进行转化，以获取细滑窗栅格，（具体步骤可参考步骤S5），并设置步长。

此后细滑窗栅格从包围盒的左上角开始滑动，每滑动一次计算一下匹配度，直到满足匹配度后滑动结束，选取匹配度（具体步骤可参考步骤S5）最高的那次滑窗结果作为识别结果。当然该结果此时根据匹配的细栅格模板可以更进一步准确判断当前检测输入的的点云是栈板的哪个端面。

但另一方面，基于该识别结果，还可以获取栈板识别端面处更高精准的6Dpose数据。具体来说，其中由于栈板的位姿在空间中存在6个自由度，即平移量x，y，z和旋转量roll，pitch，yaw，即6d pose。为此本发明另一方面还提供了一种基于3D传感器的栈板定位方法，如图8所示，其步骤包括：

步骤S1

根据上述实施例中基于3D传感器的面形状识别方法获取匹配度最高的一次的细滑窗栅格的中心点坐标作为6Dpose的x、y、z参数。

步骤S2

其中由于栈板和机器人都是默认在同一空间，且地面是水平的，那么此时可以认为栈板相对于叉车叉齿是平行的，即不存在roll和pitch的角度，为此可将roll和pitch的角度设为0。

步骤S3

此时6Dpose还需要计算yaw角，为此本示例中，优选根据上述实施例中步骤S34中建立的栈板面状点云块平面方程ax+by+cz=1，依据该平面方程及其a、b、c三个因子，以及计算出yaw参数值

，即可获取该栈板端面完整的6D pose[x，y，z，roll，pitch，yaw]参数，至此完成对其定位。

综上所述，本发明的该基于3D传感器的面形状识别及定位方法，对于需要与机器人交互的物体，只要物体在传感器能够扫描的角度存在一定的、连续的面特征，即可准确、快速的识别该物体及其位置。并且拓展性非常强，例如本案示例中的栈板，无论是标准的栈板还是非标准的栈板，只需提供栈板的各个结构参数即可构建模板。此外本发明还能够普适多种3D传感器，通用性较强，无论是深度相机、多线激光雷达还是固态激光雷达，都能适用；并且本发明的方案不需要预先训练样本，因此相比深度学习方案占用的计算性能更低。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，RandomAccess Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。

Claims (10)

1.一种基于3D传感器的面形状识别方法，其特征在于步骤包括：

S1获取目标物体参数，构建目标物体端面的栅格模板；

S2 基于3D传感器采集的传感数据建立目标点云；

S3剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算；

S4根据目标物体的端面参数设置滑窗，并根据预设步长滑动滑窗，其中每次滑动判断滑窗内的面状点云块的点云数量及其凸包面积是否符合预设条件；

S5当步骤S4判断结果符合预设条件时，提取滑窗内的点云与栅格模板进行匹配，步骤包括：将滑窗内的点云按照栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，计算其与栅格模板之间的匹配度，直至符合匹配度阈值时停止，以获取识别结果。

2.根据权利要求1所述的基于3D传感器的面形状识别方法，其特征在于，步骤包括：

S1获取目标物体参数，构建目标物体端面的粗栅格模板和细栅格模板；

S2 基于3D传感器采集的传感数据建立目标点云；

S3剔除目标点云中的地面点云，以从中提取出垂直于地面的面状点云块进行凸包面积计算；

S4根据目标物体的端面参数设置滑窗，并根据预设步长来滑动滑窗，其中每次滑动需判断滑窗内的面状点云块的点云数量及其凸包面积是否符合预设条件；

S5当步骤S4判断结果符合预设条件时，提取滑窗内的点云与粗栅格模板进行匹配，步骤包括：将滑窗内的点云按照粗栅格模板的栅格尺寸进行栅格化，以构建滑窗栅格；根据预设步长逐步滑动滑窗栅格，并计算其与粗栅格模板之间的匹配度，直至符合匹配度阈值时停止，并记录匹配区域；

S6在匹配区域附近建立包围盒，将滑窗栅格中的点云按照细栅格模板的栅格尺寸进行栅格化，以构建细滑窗栅格，并根据预设步长在包围盒内与细栅格模板进行滑窗匹配，直至获取匹配度最高的一次作为识别结果。

3.根据权利要求2所述的基于3D传感器的面形状识别方法，其特征在于，步骤S2中还包括对目标点云进行预处理，其步骤包括：

S21对目标点云进行滤波处理，以将目标点云数据赋予为对应于自然三轴坐标系的点云坐标，并根据目标物体的高度参数，滤除不匹配的目标点云；

S22 对步骤S21处理后的目标点云采用统计滤波处理，除去离群点。

4.根据权利要求2所述的基于3D传感器的面形状识别方法，其特征在于，步骤S3中剔除目标点云中的地面点云的步骤包括：

S31选择目标点云数据下所有的点距离基准平面的在容差距离范围内的点的数量最多的基准平面作为地面，并将该面上的所有的点归属于地面成分，而其余点归属于物体成分。

5.根据权利要求4所述的基于3D传感器的面形状识别方法，其特征在于，步骤S3中提取出垂直于地面的面状点云块的步骤包括：

S32从物体成分点云数据中随机选取种子点，并判断种子点与作为种子点周围的非种子点是否处于同一平面内，其中种子点的法向量与地面法向量垂直，当确定种子点和非种子点处于同一平面内时，确定将该非种子点作为新的种子点；

S33迭代判断新的种子点与其周围的非种子点是否处于同一平面内，以通过点云区域生长的方式统计出所有的种子点;

S34基于所统计的种子点，构建垂直于地面的面状点云块。

6.根据权利要求5所述的基于3D传感器的面形状识别方法，其特征在于，步骤S34中基于所统计的种子点，构建垂直于地面的面状点云块的步骤包括：判断所统计的种子点的数量是否处于预设定的数量范围内，并当种子点数量在数量范围内时，基于所统计的种子点构建所述面状点云块。

7.根据权利要求6所述的基于3D传感器的面形状识别方法，其特征在于，构建垂直于地面的面状点云块的步骤还包括：对判断为同一物体的同一面的所有面状点云块做合并处理。

8.根据权利要求7所述的基于3D传感器的面形状识别方法，其特征在于，所述判断同一物体的同一面的步骤包括：将各面状点云块转为平面方程ax+by+cz=1，判断各个面状点云块的平面方程式中a、b、c三个因子是否相似且因子之差的绝对值小于预设阈值，若符合的则判断为同一物体的同一面。

9.根据权利要求2所述的基于3D传感器的面形状识别方法，其特征在于，所述步骤S5及S6中的匹配步骤包括：将滑窗栅格/细滑窗栅格中存在点云的栅格进行标记，并计算 X=(p-q)/(1-q)，其中X表示当前滑窗栅格/细滑窗栅格与粗栅格模板/细栅格模板之间的匹配度，p表示当前滑窗栅格/细滑窗栅格与粗栅格模板/细栅格模板之间相同栅格与粗栅格模板/细栅格模板栅格个数的比，q表示滑窗栅格/细滑窗栅格中未标记栅格的个数与滑窗栅格/细滑窗栅格中栅格总数之比。

10.一种基于3D传感器的定位方法，其特征在于，步骤包括：

S1根据如权利要求1至9任一所述基于3D传感器的面形状识别方法获取匹配度最高的一次的细滑窗栅格的中心点坐标作为6Dpose的x、y、z参数；

S2 将roll和pitch的角度设为0；

S3 根据面状点云块建立平面方程ax+by+cz=1，依据该平面方程及其a、b、c三个因子，计算出yaw参数值

，以获取完整6D pose。

Images