CN114859319A - 一种牵引车与挂车的定位方法、系统、存储介质及车辆 - Google Patents

Abstract

本发明公开了一种牵引车与挂车的定位方法、系统、存储介质及车辆，包括以下步骤：在半挂车前表面固定有标识板底座，标识板底座上设置有标识板，采集半挂车与牵引车脱挂状态下的点云数据，建立最小包围盒，对点云数据进行计算，得到标识板第一次中心坐标；根据得到的标识板第一次中心坐标设定小邻域搜索，对小邻域搜索得到的点云数据进行平面方程拟合，计算挂车前表面法向量；根据得到的标识板第一次中心坐标设定大邻域搜索，将大邻域搜索得到的点云数据分类排序，筛选平面上的点云数据及其所在线束；对应标识板底座的宽度，查找各线束的边界点，筛选位于平面上的线束，计算得到标识板第二次中心坐标。以此实现牵引车与挂车相对位置的快速精准确定。

Description

一种牵引车与挂车的定位方法、系统、存储介质及车辆

技术领域

本发明涉及定位领域，具体涉及一种牵引车与挂车的定位方法、系统、存储介质及车辆。

背景技术

通常所说的“港口无人驾驶”，指从货船到仓库之间(卸货和装货)的往返运输。传统的人工作业方式存在着巨大的安全隐患，码头的各种大型设备会对工人的人身安全造成威胁。并且，随着港口吞吐量的不断增大，装卸运输工作越来越复杂，传统的半挂牵引车搬运货物成为制约装卸效率的瓶颈，因此迫切需要通过自动化技术来解决问题。牵引车与半挂车的接挂过程是至关重要的一个环节，只有二者的相对位置精准确定，才能保证自动化接挂的顺利完成。常用的定位方法有GPS定位、视觉定位、激光雷达定位。GPS定位更新频率较低、易受信号强弱影响且精度不够高，难以满足接挂的要求，视觉定位受环境光影响大，光线暗时无法工作，而激光雷达定位精度高、可靠性高且技术成熟，可以应用到港口牵引车与半挂车的接挂定位中。

发明内容

本发明的目的是解决现有的无人驾驶牵引车鞍座与半挂车牵引销连接时位置难以确定的问题，提供了一种牵引车与挂车的定位方法、系统、存储介质及车辆。

为了达到以上目的，本发明的第一方面，提供了一种牵引车与挂车的定位方法，具体包括以下步骤：

步骤101：在挂车前表面固定有标识板底座，所述标识板底座上设置有标识板，采集挂车与牵引车脱挂状态下的点云数据，建立最小包围盒，对所述点云数据进行计算，得到标识板第一次中心坐标；

步骤102：根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索，对所述小邻域搜索得到的点云数据进行平面方程拟合，计算挂车前表面法向量；

步骤103：根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索，将所述大邻域搜索得到的点云数据分类排序，筛选平面上的点云数据及其所在雷达上的线束；

步骤104：对应所述标识板底座的宽度，查找雷达上各线束的边界点，筛选位于平面上的线束，计算得到标识板第二次中心坐标。

在一些可能的实施方式，步骤101中，所述“采集挂车与牵引车脱挂状态下的点云数据”具体包括：

在挂车与牵引车脱挂状态下，运用雷达，采集所述脱挂状态下的位置下的点云数据，所述雷达的坐标系为O_LX_LY_LZ_L，其中O_L为坐标系原点，X_L轴与多线雷达激光发出中心轴重合，Y_L轴与Z_L轴分别垂直于X_L轴，且坐标系O_LX_LY_LZ_L满足右手准则。

在一些可能的实施方式中，步骤101中，所述“建立最小包围盒”具体包括：

对采集到的点云进行直通滤波，选定三轴坐标范围，剔除不在需求范围内的离群点，调用体素网格滤波器对点云进行缩减采样；

预处理后的剩余点云通过欧式聚类被分割成为一个个单独的点云簇，检测出障碍物的各边位置并建立最小包围盒，对应实际标识板底座的尺寸，从中筛选出标识板底座的最小包围盒。

在一些可能的实施方式中，步骤101中，所述“对所述点云数据进行计算，得到标识板第一次中心坐标”具体包括：

输出标识板最小包围盒的各顶点位置，靠近雷达一侧的四个顶点分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)和D(x₄,y₄,z₄)，通过计算得到标识板第一次中心坐标A＝(x_A,y_A,z_A)，所述标识板第一次中心坐标A＝(x_A,y_A,z_A)是多线激光雷达坐标系下的坐标；

其中，x_A＝x₁＝x₂＝x₃＝x₄，y_A＝(x₁+x₂)/2＝(x₃+x₄)/2，z_A＝(x₁+x₃)/2＝(x₂+x₄)/2。

在一些可能的实施方式中，步骤102中，所述“根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索”具体包括：

设定平面拟合的邻域搜索半径radius＝r₁，创建KdTree结构，采用基于KdTree的最近邻搜索算法进行计算，所述基于KdTree的最近邻搜索算法中所构造的k维树是三维树，其各个节点都为三维的二叉树，对每一个数据点从KdTree的根节点开始与设定阈值r₁进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得小邻域。

在一些可能的实施方式中，步骤102中，所述“对所述小邻域搜索得到的点云进行平面方程拟合”具体包括：

运用RANSAC算法，对小邻域搜索得到的点云进行平面方程拟合，具体为：假定点云由“内点”和“外点”组成，先由一组内点得到初步平面模型，计算内点数目，迭代平面模型，直到达到最优，最终输出平面方程为：ax+by+cz+d＝0；

其中，a，b，c和d为所述平面方程ax+by+cz+d＝0的四个系数。

在一些可能的实施方式中，步骤102中，所述“计算挂车前表面法向量”具体包括：

由平面方程ax+by+cz+d＝0得到标识板平面法向量，即半挂车前表面法向量(a，b，c)，计算输出牵引车与挂车相对角度为：

其中，a和b为平面方程ax+by+cz+d＝0的系数。

在一些可能的实施方式中，步骤103中，所述“根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索”具体包括：

设定平面拟合的邻域搜索半径radiusplus＝r₂，创建KdTree结构，创建KdTree结构，采用基于KdTree的最近邻搜索算法来进行计算，所述基于KdTree的最近邻搜索算法所构造的k维树是三维树，其各个节点都为三维的二叉树，对每一个数据点从KdTree的根节点开始与设定阈值r₂进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得大邻域。

在一些可能的实施方式中，步骤103中，所述“将所述大邻域搜索得到的点云数据分类排序”具体包括：

定义雷达线束N_SCANS＝N，根据雷达自带点云坐标输出的Ring值，将搜索到的点云放入对应的雷达帧通道内，对每个雷达帧通道内的点云，按照点云的水平旋转角度从小到大排序。

在一些可能的实施方式中，步骤103中，所述“筛选平面上的点云数据及其所在线束”具体包括：

通过公式计算各线束中的点(x₀,y₀,z₀)到标识板平面ax+by+cz+d＝0的距离为：

其中，a，b，c和d为平面方程ax+by+cz+d＝0的四个系数；设定点到平面的距离阈值d₁，判断条件为：d_0-plane<d₁,符合距离阈值d₁要求的点，即为标识板平面上的点云，从而得到各通道中位于平面上的点云数。

在一些可能的实施方式中，步骤104中，所述“查找雷达上各线束的边界点，筛选位于平面上的线束”具体包括：

根据标识板底座的宽度设定边界阈值d₂，查找各线束位于平面上的两边界点P(x_p,y_p,z_p)和Q(x_q,y_q,z_q)，由两点间欧式距离进行判断，判断条件为：

符合边界阈值d₂要求的即为标识板平面上的线束。

在一些可能的实施方式中，步骤104中，所述“计算得到标识板第二次中心坐标”具体包括：

通过查找得到的位于平面上的雷达线束条数为N，由各对边界点坐标的平均值，由如下公式计算得到标识板第二次中心坐标：

其中，x和y分别为标识板中心X_L轴、Y_L轴的坐标值，x_p、x_q、y_p和y_q分别为各边界点X_L轴、Y_L轴的坐标值。

本发明的第三方面，提供了一种基于雷达的牵引车与挂车定位系统，包括处理器以及存储器：

所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；

所述处理器用于根据所述程序代码中的指令执行上述一种牵引车与挂车的定位方法。

本发明的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行上述一种牵引车与挂车的定位方法。

本发明的第五方面，提供了一种车辆，包括上述一种牵引车与挂车的定位方法或上述一种牵引车与挂车的定位系统。

本发明的有益效果在于：

1.通过在挂车前表面粘贴一标识板底座，利用多线激光雷达，采集挂车与牵引车脱挂时的点云数据，通过建立最小包围盒，得到粗计算的标识板第一次中心坐标，将标识板第一次中心坐标通过最近邻搜索算法和平面方程拟合处理，得到经过搜索、拟合和查找等步骤得到的精确的标识板的第二中心坐标，实现牵引车与挂车相对位置的快速精准确定。

2.通过平面方程拟合和标识板边界点查找实现标识板中心坐标的确定，不需要在前期进行大量设备和基础设施的投入，且适用于码头的定位场景，解决了现有的人工接挂准确性低、效率低等问题。

3.在已有的PCL库封装算法的基础上，引入按照雷达线束对点云分类排序的算法，有效提高了边界点查找的精度。

4.与已有的GPS定位、视觉定位方法相比，本发明应用的激光雷达定位技术不易受环境因素影响，具有更高的定位精度。

附图说明

图1为本发明实施例一种牵引车与挂车的定位方法中标识板与激光雷达位置的示意图；

图2为本发明实施例一种牵引车与挂车的定位方法中建立的的最小包围盒的效果图；

图3为本发明实施例一种牵引车与挂车的定位方法中点云处理效果图；

图4为本发明实施例一种牵引车与挂车的定位方法的的步骤流程图；

图5为本发明实施例一种牵引车与挂车的定位方法中步骤101的步骤流程图；

图6为本发明实施例一种牵引车与挂车的定位方法中步骤102的步骤流程图；

图7为本发明实施例一种牵引车与挂车的定位方法中步骤103的步骤流程图；

图8为本发明实施例一种牵引车与挂车的定位方法中步骤104的步骤流程图；

图9为本发明实施例一种牵引车与挂车的定位方法中的算法流程图；

图10为本发明实施例一种牵引车与挂车的定位系统的结构图。

图中：51、处理器；52、存储器。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征能更易于被本领域技术人员理解，从而对本发明的保护范围做出更为清楚明确的界定。

本实施例的第一方面，参照附图9所示，提供一种牵引车与挂车的定位方法，具体包括以下步骤：

步骤101：参照附图1所示，在挂车前表面固定有一个尺寸为38×38×20(cm)的标识板底座，所述标识板底座上设置有一个尺寸为38×38(cm)标识板，参照附图5所示，利用速腾RS-Ruby_Lite多线激光雷达采集挂车与牵引车脱挂状态下的点云数据，建立最小包围盒，对所述点云数据进行计算，得到标识板第一次中心坐标。所述标识板的中心坐标与雷达位置有关，当雷达位置改变时，中心坐标也会改变。

所述“采集半挂车与牵引车脱挂状态下的点云数据”具体包括：

在半挂车与牵引车脱挂状态下，运用一个80线激光雷达，采集所述脱挂状态下的位置下的点云数据，所述80线激光雷达的坐标系为O_LX_LY_LZ_L，其中O_L为坐标系原点，X_L轴与80线雷达激光发出中心轴重合，Y_L轴与Z_L轴分别垂直于X_L轴，且坐标系O_LX_LY_LZ_L满足右手准则。

所述“建立最小包围盒”具体包括：

对采集到的点云进行直通滤波，选定三轴坐标范围，快速剔除不在需求范围内的离群点，调用体素网格滤波器对点云进行缩减采样，在保证原本的点云结构不变的基础上降低点云密度，便于后续分割聚类；预处理后的剩余点云通过欧式聚类被分割成为一个个单独的点云簇，检测出障碍物的各边位置并建立最小包围盒，对应实际标识板底座的尺寸，从中筛选出标识板底座的最小包围盒。建立最小包围盒的目的是筛选出对应于标识板的点云，参照附图2所示，图2为最小包围盒的效果图。

应用到本实施例：对采集到的点云进行直通滤波，根据标识板与雷达的相对位置，确定三轴坐标范围，X轴方向即雷达深度方向上，去除过远距离的点云，Y轴、Z轴方向上，对应标识板的大致位置，去除上下及两侧不在需求范围内的离群点；调用体素网格滤波器，设置单个体素的大小为2cm×2cm×2cm，对点云进行缩减采样，在保证原本的点云结构不变的基础上降低点云密度，便于后续分割聚类；预处理后的剩余点云通过欧式聚类被分割成为一个个单独的点云簇，并建立最小包围盒，检测出障碍物的各边位置，对应实际标识板底座的尺寸，从中筛选出标识板底座的最小包围盒。

所述“对所述点云数据进行计算，得到标识板第一次中心坐标”具体包括：

输出标识板最小包围盒的各顶点位置，靠近80线激光雷达一侧的四个顶点分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)和D(x₄,y₄,z₄)，通过计算得到标识板第一次中心坐标A＝(x_A,y_A,z_A)，所述标识板第一次中心坐标A＝(x_A,y_A,z_A)是80线激光雷达坐标系下的坐标，其中，x_A＝x₁＝x₂＝x₃＝x₄，y_A＝(x₁+x₂)/2＝(x₃+x₄)/2，z_A＝(x₁+x₃)/2＝(x₂+x₄)/2。参照附3所示，图3为本发明实施例中点云处理效果图。

步骤102：参照附图6所示，根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索，对所述小邻域搜索得到的点云数据进行平面方程拟合，计算挂车前表面法向量。设定小邻域搜索的目的是为了得到标识板所在平面的平面方程，通过标识板所在平面的平面方程可以计算出挂车前表面法向量。

所述“根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索”具体包括：

以标识板中心为圆心，设定平面拟合的邻域搜索半径radius＝r₁，创建KdTree结构，KdTree即为kd-tree(k-dimensional树的简称)，是一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。主要应用于多维空间关键数据的搜索(如：范围搜索和最近邻搜索)。在计算机科学里，k-d树(k-维树的缩写)是在k维欧几里德空间组织点的数据结构。k-d树可以使用在多种应用场合，如多维键值搜索(例：范围搜寻及最邻近搜索)。k-d树是空间二分树(Binary space partitioning)的一种特殊情况。

采用基于KdTree的最近邻搜索算法进行计算，用来找出在树中与输入点最接近的点。所述基于KdTree的最近邻搜索算法中所构造的k维树是三维树，其各个节点都为三维的二叉树，

k-d树最邻近搜索的过程如下：对每一个数据点从KdTree的根节点开始与设定阈值r₁进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得小邻域。

所述“对所述小邻域搜索得到的点云进行平面方程拟合”具体包括：

RANSAC为Random Sample Consensus的缩写，它是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数，得到有效样本数据的算法。运用RANSAC算法，对小邻域搜索得到的点云进行平面方程拟合；具体为：假定点云由“内点”和“外点”组成，先由一组内点得到初步平面模型，计算内点数目，迭代平面模型，直到达到最优，最终输出平面方程为：ax+by+cz+d＝0；

其中，a，b，c和d为平面方程ax+by+cz+d＝0的四个系数。

所述“计算挂车前表面法向量”具体包括：

由平面方程ax+by+cz+d＝0得到标识板平面法向量，即挂车前表面法向量(a，b，c)，计算输出牵引车与挂车的相对角度为：

其中，a和b为平面方程ax+by+cz+d＝0的系数。通过平面方程拟合和标识板边界点查找实现标识板中心坐标的确定，不需要在前期进行大量设备和基础设施的投入，且适用于码头的定位场景，解决了现有的人工接挂准确性低、效率低等问题。

步骤103：参照附图7所示，根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索，将所述大邻域搜索得到的点云数据分类排序，筛选平面上的点云数据及其所在线束。设定大邻域搜索的目的是为了对搜索得到的点云进行分类排序，筛选位于平面上的点云及其对应线束，为了后续查找标识板平面上线束的边界点，所设邻域范围必须能完全包括标识板。此外，分别设置大、小邻域搜索，在可视化时也能便于区分。

所述“根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索”具体包括：

以标识板中心为圆心，设定平面拟合的邻域搜索半径radiusplus＝r₂，创建KdTree结构，kd-tree(k-dimensional树的简称)，是一种对k维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。主要应用于多维空间关键数据的搜索(如：范围搜索和最近邻搜索)。在计算机科学里，k-d树(k-维树的缩写)是在k维欧几里德空间组织点的数据结构。k-d树可以使用在多种应用场合，如多维键值搜索(例：范围搜寻及最邻近搜索)。k-d树是空间二分树(Binary space partitioning)的一种特殊情况。

采用基于KdTree的最近邻搜索算法进行计算，用来找出在树中与输入点最接近的点。所述基于KdTree的最近邻搜索算法中所构造的k维树是三维树，其各个节点都为三维的二叉树，

k-d树最邻近搜索的过程如下：对每一个数据点从KdTree的根节点开始与设定阈值r₂进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得大邻域。

所述“将所述大邻域搜索得到的点云数据分类排序”具体包括：

采用速腾RS-Ruby_Lite 80线雷达，定义80线激光雷达线束N_SCANS＝80，根据多线激光雷达自带点云坐标输出的Ring值，所谓Ring的值,就是每个点是几束光发出来的。将搜索到的点云放入对应的雷达帧通道内，对每个雷达帧通道内的点云，按照点云的水平旋转角度从小到大排序。

所述“筛选平面上的点云数据及其所在雷达上的线束”具体包括：

通过公式计算各线束中的点(x₀,y₀,z₀)到标识板平面ax+by+cz+d＝0的距离为：

其中，a，b，c和d为平面方程ax+by+cz+d＝0的四个系数；设定点到平面的距离阈值d₁，判断条件为：d_0-plane<d₁,符合距离阈值d₁要求的点，即为标识板平面上的点云，从而得到各通道中位于平面上的点云数。引入按照雷达线束对点云分类排序的算法，有效提高了雷达上线束边界点查找的精度。

步骤104：参照附图8所示，对应所述标识板底座的宽度，查找雷达上各线束的边界点，筛选位于平面上的线束，计算得到标识板第二次中心坐标。

所述“查找雷达上各线束的边界点，筛选位于平面上的线束”具体包括：

根据标识板底座的宽度设定边界阈值d₂，查找雷达上各线束位于平面上的两边界点P(x_p,y_p,z_p)和Q(x_q,y_q,z_q)，由两点间欧式距离进行判断，判断条件为：

符合边界阈值d₂要求的即为标识板平面上的线束。

所述“计算得到标识板第二次中心坐标”具体包括：

通过查找得到的位于平面上的激光雷达线束条数为N，由各对边界点坐标的平均值，由公式计算得到标识板第二次中心坐标：

其中，x和y分别为标识板中心X_L轴、Y_L轴的坐标值，x_p、x_q、y_p和y_q分别为各边界点X_L轴、Y_L轴的坐标值。

本实施例的第三方面，参照附图10所示，提供了一种基于雷达的牵引车与挂车定位系统，包括处理器51以及存储器52：

所述存储器52用于存储程序代码，并将所述程序代码传输给所述处理器；

所述处理器51用于根据所述程序代码中的指令执行上述一种牵引车与挂车的定位方法。

其中，处理器51还可以称为CPU(Central Processing Unit，中央处理单元)。处理器51可能是一种集成电路芯片，具有信号的处理能力。处理器51还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

本实施例的第四方面，提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行上述一种牵引车与挂车的定位方法。

存储介质存储有能够实现上述所有方法的程序指令，其中，该程序指令可以以软件产品的形式存储在上述存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质，或者是计算机、服务器、手机、平板等终端设备。

本实施例的第五方面，提供了一种车辆，包括上述一种牵引车与挂车的定位方法或上述一种牵引车与挂车的定位系统。

以上实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人了解本发明的内容并加以实施，并不能以此限制本发明的保护范围，凡根据本发明精神实质所做的等效变化或修饰，都应涵盖在本发明的保护范围内。

Claims (15)

1.一种牵引车与挂车的定位方法，其特征在于，具体包括以下步骤：

步骤101：在挂车前表面固定有标识板底座，所述标识板底座上设置有标识板，采集挂车与牵引车脱挂状态下的点云数据，建立最小包围盒，对所述点云数据进行计算，得到标识板第一次中心坐标；

步骤102：根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索，对所述小邻域搜索得到的点云数据进行平面方程拟合，计算挂车前表面法向量；

步骤103：根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索，将所述大邻域搜索得到的点云数据分类排序，筛选平面上的点云数据及其所在雷达上的线束；

步骤104：对应所述标识板底座的宽度，查找雷达上各线束的边界点，筛选位于平面上的线束，计算得到标识板第二次中心坐标。

2.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤101中，所述“采集挂车与牵引车脱挂状态下的点云数据”具体包括：

在挂车与牵引车脱挂状态下，运用雷达，采集所述脱挂状态下的位置下的点云数据，所述雷达的坐标系为O_LX_LY_LZ_L，其中O_L为坐标系原点，X_L轴与多线雷达激光发出中心轴重合，Y_L轴与Z_L轴分别垂直于X_L轴，且坐标系O_LX_LY_LZ_L满足右手准则。

3.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤101中，所述“建立最小包围盒”具体包括：

对采集到的点云进行直通滤波，选定三轴坐标范围，剔除不在需求范围内的离群点，调用体素网格滤波器对点云进行缩减采样；

预处理后的剩余点云通过欧式聚类被分割成为一个个单独的点云簇，检测出障碍物的各边位置并建立最小包围盒，对应实际标识板底座的尺寸，从中筛选出标识板底座的最小包围盒。

4.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤101中，所述“对所述点云数据进行计算，得到标识板第一次中心坐标”具体包括：

输出标识板最小包围盒的各顶点位置，靠近雷达一侧的四个顶点分别为A(x₁,y₁,z₁)、B(x₂,y₂,z₂)、C(x₃,y₃,z₃)和D(x₄,y₄,z₄)，通过计算得到标识板第一次中心坐标A＝(x_A,y_A,z_A)，所述标识板第一次中心坐标A＝(x_A,y_A,z_A)是多线激光雷达坐标系下的坐标；

其中，x_A＝x₁＝x₂＝x₃＝x₄，y_A＝(x₁+x₂)/2＝(x₃+x₄)/2，z_A＝(x₁+x₃)/2＝(x₂+x₄)/2。

5.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤102中，所述“根据步骤101中得到的标识板第一次中心坐标设定小邻域搜索”具体包括：

设定平面拟合的邻域搜索半径radius＝r₁，创建KdTree结构，采用基于KdTree的最近邻搜索算法进行计算，所述基于KdTree的最近邻搜索算法中所构造的k维树是三维树，其各个节点都为三维的二叉树，对每一个数据点从KdTree的根节点开始与设定阈值r₁进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得小邻域。

6.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤102中，所述“对所述小邻域搜索得到的点云进行平面方程拟合”具体包括：

运用RANSAC算法，对小邻域搜索得到的点云进行平面方程拟合，具体为：假定点云由“内点”和“外点”组成，先由一组内点得到初步平面模型，计算内点数目，迭代平面模型，直到达到最优，最终输出平面方程为：ax+by+cz+d＝0；

其中，a，b，c和d为所述平面方程ax+by+cz+d＝0的四个系数。

7.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤102中，所述“计算挂车前表面法向量”具体包括：

由平面方程ax+by+cz+d＝0得到标识板平面法向量，即半挂车前表面法向量(a，b，c)，计算输出牵引车与挂车相对角度为：

其中，a和b为平面方程ax+by+cz+d＝0的系数。

8.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤103中，所述“根据步骤101中得到的标识板第一次中心坐标设定大邻域搜索”具体包括：

设定平面拟合的邻域搜索半径radiusplus＝r₂，创建KdTree结构，创建KdTree结构，采用基于KdTree的最近邻搜索算法来进行计算，所述基于KdTree的最近邻搜索算法所构造的k维树是三维树，其各个节点都为三维的二叉树，对每一个数据点从KdTree的根节点开始与设定阈值r₂进行比较，不断向下直至到达某一子节点，则该节点为最近邻点，之后搜索其余子节点，将其距离值与当前最近邻点距离值进行比较并不断更新最近邻点，直至全部搜索完毕，得到最终的最近邻点，组成搜索所得大邻域。

9.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤103中，所述“将所述大邻域搜索得到的点云数据分类排序”具体包括：

定义雷达线束N_SCANS＝N，根据雷达自带点云坐标输出的Ring值，将搜索到的点云放入对应的雷达帧通道内，对每个雷达帧通道内的点云，按照点云的水平旋转角度从小到大排序。

10.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤103中，所述“筛选平面上的点云数据及其所在线束”具体包括：

通过公式计算各线束中的点(x₀,y₀,z₀)到标识板平面ax+by+cz+d＝0的距离为：

其中，a，b，c和d为平面方程ax+by+cz+d＝0的四个系数；设定点到平面的距离阈值d₁，判断条件为：d_0-plane<d₁,符合距离阈值d₁要求的点，即为标识板平面上的点云，从而得到各通道中位于平面上的点云数。

11.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤104中，所述“查找雷达上各线束的边界点，筛选位于平面上的线束”具体包括：

根据标识板底座的宽度设定边界阈值d₂，查找各线束位于平面上的两边界点P(x_p,y_p,z_p)和Q(x_q,y_q,z_q)，由两点间欧式距离进行判断，判断条件为：

符合边界阈值d₂要求的即为标识板平面上的线束。

12.根据权利要求1所述的一种牵引车与挂车的定位方法，其特征在于，步骤104中，所述“计算得到标识板第二次中心坐标”具体包括：

通过查找得到的位于平面上的雷达线束条数为N，由各对边界点坐标的平均值，由如下公式计算得到标识板第二次中心坐标：

其中，x和y分别为标识板中心X_L轴、Y_L轴的坐标值，x_p、x_q、y_p和y_q分别为各边界点X_L轴、Y_L轴的坐标值。

13.一种牵引车与挂车的定位系统，其特征在于，包括处理器(51)以及存储器(52)：

所述存储器(52)用于存储程序代码，并将所述程序代码传输给所述处理器；

所述处理器(51)用于根据所述程序代码中的指令执行权利要求1-12中任一项所述的一种牵引车与挂车的定位方法。

14.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行权利要求1-12中任一项所述的一种牵引车与挂车的定位方法。

15.一种车辆，其特征在于，包括控制装置，所述控制装置包括权利要求1-12中任一项所述的一种牵引车与挂车的定位方法或权利要求13所述的一种牵引车与挂车的定位系统。

Images