挂车位姿检测方法、装置、目标拖车及存储介质与流程

1.本发明涉及智能驾驶技术领域，具体涉及一种挂车位姿检测方法、装置、目标拖车及存储介质。


背景技术：

2.无人驾驶拖挂车在封闭园区、干线物流、港口等场景广泛应用。无人驾驶拖车与挂车进行协同对中及姿态检测是无人驾驶拖挂车辆实现全无人化作业的核心场景之一。
3.无人驾驶拖车与挂车对中协同需要车辆行驶到一个特定区域，如停车场或者装/卸载场。进行泊车检测挂车，通过检测挂车的位置和姿态，控制车辆泊车行驶到挂车位置进行对中。同时在拖车牵引挂车行驶的过程中，由于车辆整体长度较大，需要实时的检测挂车的姿态(航向)，感知、规划和决策模块进行车辆路径规划和车辆周围障碍物检测。控制模块基于规划的路径和挂车的位置和姿态进行车辆平稳安全控制。
4.因此，无人驾驶拖车如何准确且快速检测到挂车的位姿成为了亟待解决的问题。


技术实现要素：

5.有鉴于此，本发明实施例提供了一种挂车位姿检测方法、装置、目标拖车及存储介质，旨在解决如何准确且快速检测到挂车的位姿成为了亟待解决的问题。
6.根据第一方面，本发明实施例提供了一种挂车位姿检测方法，应用于目标拖车，目标拖车用于对目标挂车进行拖挂，包括：
7.利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧各粘贴一个反光条的区域；
8.利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；
9.根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息；
10.根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
11.本发明实施例提供的挂车位姿检测方法，利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，保证了得到的初始点云数据的准确性。然后，利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据，保证了得到的第一目标点云数据和/或第二目标点云数据的准确性。根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息，保证了确定的第一目标中心点的第一位置信息和第二目标中心点的第二位置信息的准确性。然后，根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿，保证了确定的目标挂车的位姿的准确性。从而在目标拖车与目标挂车对中过
程中，可以快速且准确地实现目标拖车与目标挂车的对中。此外，在目标拖车拖挂着目标挂车行驶时，目标拖车还可以根据目标挂车的位姿，准确且安全地拖挂着目标挂车行驶，保证了行驶的安全性，避免了事故的发生。
12.结合第一方面，在第一方面第一实施方式中，利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据，包括：获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据；获取第一候选点云数据对应的反射率阈值；从第一候选点云数据中剔除反射率小于反射率阈值的点云数据，得到第二候选点云数据；对第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，并计算各簇第三候选点云数据对应的初始中心点的初始位置信息；获取各个初始中心点对应的第二预设范围；以各个初始中心点为中心，分别保留初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据；根据第四候选点云数据，得到第一目标点云数据。
13.本发明实施例提供的挂车位姿检测方法，获取初始点云数据对应的第一预设范围，然后，从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据，保证了得到的第一候选点云数据的准确性，避免了第一候选点云数据范围太大。然后，获取第一候选点云数据对应的反射率阈值，从第一候选点云数据中剔除反射率小于反射率阈值的点云数据，得到第二候选点云数据，保证了得到的第二候选点云数据的准确性，剔除了左侧特征区域和右侧特征区域之外的点云数据。对第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，保证了得到的两簇第三候选点云数据的准确性，从而使得两簇第三候选点云数据分别表征左侧特征区域和右侧特征区域。然后，计算各簇第三候选点云数据对应的初始中心点的初始位置信息，保证了计算得到的初始中心点的初始位置信息的准确性。然后，获取各个初始中心点对应的第二预设范围，以各个初始中心点为中心，分别保留初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据，保证了得到的第四候选点云数据的准确性。根据第四候选点云数据，得到第一目标点云数据，保证了得到的第一目标点云数据的准确性。
14.结合第一方面第一实施例方式，在第一方面第二实施方式中，根据第四候选点云数据，得到第一目标点云数据，包括：使用法向估计和网格占有方法对第四候选点云数据进行边沿检测，得到第一边沿点云数据；对第一边沿点云数据进行识别，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据。
15.本发明实施例提供的挂车位姿检测方法，使用法向估计和网格占有方法对第四候选点云数据进行边沿检测，得到第一边沿点云数据，保证了得到的第一边沿点云数据的准确性。然后，对第一边沿点云数据进行识别，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据，保证了得到的第一目标点云数据的准确性，也使得第一目标点云数据可以更加准确地表征左侧特征区域和右侧特征区域对应的点云数据。
16.结合第一方面，在第一方面第三实施方式中，利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第二目标点云数据，还包括：获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据；使用法向估计和网格占有方法对第一候选点云数据进行边沿检测，得到第二边沿点云数据；对第二边沿点云数据进行识别，提取第二边沿点云数据对应的x轴、y轴以
及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围；获取第二边沿点云数据对应的z轴的第三预设范围；从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据；对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据，将两簇第四边沿点云数据，确定为第二目标点云数据。
17.本发明实施例提供的挂车位姿检测方法，获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据，保证了得到的第一候选点云数据的准确性。使用法向估计和网格占有方法对第一候选点云数据进行边沿检测，得到第二边沿点云数据，保证了得到的第二边沿点云数据的准确性。对第二边沿点云数据进行识别，提取第二边沿点云数据对应的x轴、y轴以及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围，保证了得到的第二边沿点云数据对应的轮廓范围的准确性。然后，获取第二边沿点云数据对应的z轴的第三预设范围；从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据，保证了得到的第三边沿点云数据的准确性。对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据，保证了得到的两簇第四边沿点云数据的准确性。将两簇第四边沿点云数据，确定为第二目标点云数据，保证了得到的第二目标点云数据的准确性。也使得第二目标点云数据可以更加准确地表征左侧特征区域和右侧特征区域对应的点云数据。
18.结合第一方面，在第一方面第四实施方式中，根据第一目标点云数据和第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息，包括：从第一目标点云数据中提取左侧特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息；从第二目标点云数据中提取左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息；获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息。
19.本发明实施例提供的挂车位姿检测方法，从第一目标点云数据中提取左侧特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息，保证了提取到的左侧第一备用位置信息以及右侧第一备用位置信的准确性。然后，从第二目标点云数据中提取左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息，保证了提取到的左侧第二备用位置信息以及右侧第二备用位置信息的准确性。获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息，保证了得到的第一目标中心点的第一位置信息以及第二目标中心点的第二位置信息的准确性。
20.结合第一方面，在第一方面第五实施方式中，根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿，包括：以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系；获取第一目标中心点在目标坐标系的第一位置坐标，以及第二目标中心点在目标坐标的第二位置坐标；根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角；获取目标挂车的后轴中心点到预设传感器之间的距离；根据目标挂车的后轴中心点到预设传感器之间的距离与备用航向角之间的关系，确定目标挂车在目标坐标系下的备用位置信息；根据目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息。
21.本发明实施例提供的挂车位姿检测方法，以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系，保证了建立的目标坐标系的准确性。获取第一目标中心点在目标坐标系的第一位置坐标，以及第二目标中心点在目标坐标的第二位置坐标，根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角，保证了计算得到的目标挂车在目标坐标下的备用航向角的准确性。然后，获取目标挂车的后轴中心点到预设传感器之间的距离，根据目标挂车的后轴中心点到预设传感器之间的距离与备用航向角之间的关系，确定目标挂车在目标坐标系下的备用位置信息，保证了确定的目标挂车在目标坐标系下的备用位置信息的准确性。然后，根据目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息，保证了得到的目标挂车的目标航向角以及目标位置信息的准确性，从而保证了确定的目标挂车在大地坐标系下的位姿的准确性。从而在目标拖车与目标挂车对中过程中，可以快速且准确地实现目标拖车与目标挂车的对中。此外，在目标拖车拖挂着目标挂车行驶时，目标拖车还可以根据目标挂车的位姿，准确且安全地拖挂着目标挂车行驶，保证了行驶的安全性，避免了事故的发生。
22.结合第一方面第五实施方，在第一方面第六实施方式中，根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角，包括：根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线与y轴之间的第一夹角；根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标；确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角；利用第一夹角减去第二夹角，得到备用航向角。
23.本发明实施例提供的挂车位姿检测方法，根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线与y轴之间的第一夹角，保证了确定的第一夹角的准确性。根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标，保证了确定的第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标的准确性。确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角，保证了确定的第二夹角的准确性。利用第一夹角减去第二夹角，得到备用航向角，保证了得到的备用航向角的准确性，进而可以保证确定的目标挂车的位姿的准确性。
24.根据第二方面，本发明实施例还提供了一种挂车位姿检测装置，应用于目标拖车，目标拖车用于对目标挂车进行拖挂，包括：
25.获取模块，用于利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧各粘贴一个反光条的区域；
26.滤波处理模块，用于利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；
27.第一确定模块，用于根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息；
28.第二确定模块，用于根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
29.本发明实施例提供的挂车位姿检测装置，利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，保证了得到的初始点云数据的准确性。然后，利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据，保证了得到的第一目标点云数据和/或第二目标点云数据的准确性。根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息，保证了确定的第一目标中心点的第一位置信息和第二目标中心点的第二位置信息的准确性。然后，根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿，保证了确定的目标挂车的位姿的准确性。从而在目标拖车与目标挂车对中过程中，可以快速且准确地实现目标拖车与目标挂车的对中。此外，在目标拖车拖挂着目标挂车行驶时，目标拖车还可以根据目标挂车的位姿，准确且安全地拖挂着目标挂车行驶，保证了行驶的安全性，避免了事故的发生。
30.根据第三方面，本发明实施例提供了一种目标拖车，包括电子设备和拖车本体，电子设备包括存储器和处理器，存储器和处理器之间互相通信连接，存储器中存储有计算机指令，处理器通过执行计算机指令，从而执行第一方面或者第一方面的任意一种实施方式中的挂车位姿检测方法。
31.根据第四方面，本发明实施例提供了一种计算机可读存储介质，计算机可读存储介质存储计算机指令，计算机指令用于使计算机执行第一方面或者第一方面的任意一种实施方式中的挂车位姿检测方法。
附图说明
32.为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
33.图1是应用本发明实施例提供的挂车位姿检测方法的流程图；
34.图2是应用本发明另一实施例提供的目标挂车的特征区域的示意图；
35.图3是应用本发明实施例提供的电子设备利用激光雷达传感器对目标挂车的特征区域进行扫描，并与目标挂车进行对中的示意图；
36.图4是应用本发明另实施例提供的挂车位姿检测方法的流程图；
37.图5是应用本发明另实施例提供的挂车位姿检测方法的流程图；
38.图6是应用本发明另实施例提供的挂车位姿检测方法的流程图；
39.图7是应用本发明另实施例提供的第一目标中心点的第一位置信息和第二目标中心点的第二位置信息的示意图；
40.图8是应用本发明另实施例提供的计算目标挂车的备用航向角的示意图；
41.图9是应用本发明另实施例提供的目标拖车和目标挂车夹角较大时，计算目标挂车的备用航向角的示意图；
42.图10是应用本发明另实施例提供的目标拖车和目标挂车夹角较大时，采集的初始点云数据的示意图；
43.图11是应用本发明实施例提供的挂车位姿检测装置的功能模块图；
44.图12是应用本发明实施例提供的电子设备的硬件结构示意图。
具体实施方式
45.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
46.需要说明的是，本技术实施例提供的挂车位姿检测的方法，其执行主体可以是挂车位姿检测的装置，该挂车位姿检测的装置可以通过软件、硬件或者软硬件结合的方式实现成为目标拖车对应的电子设备的部分或者全部，其中，该电子设备可以是目标拖车内部的处理器。也可以是独立与目标拖车的服务器或者终端，其中，本技术实施例中的服务器可以为一台服务器，也可以为由多台服务器组成的服务器集群，本技术实施例中的终端可以是智能手机、个人电脑、平板电脑、可穿戴设备以及智能机器人等其他智能硬件设备。下述方法实施例中，均以执行主体是电子设备为例来进行说明。
47.在本技术一个实施例中，如图1所示，提供了一种挂车位姿检测方法，以该方法应用于目标拖车对应的电子设备为例进行说明，其中，目标拖车用于对目标挂车进行拖挂，包括以下步骤：
48.s11、利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据。
49.其中，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧粘贴两个反光条的区域。示例性的，如图2所示，为目标挂车的特征区域的示意图。
50.具体地，如图3所示，电子设备可以利用安装在目标拖车后方的激光雷达传感器，对目标挂车的特征区域进行扫描，从而得到目标挂车的特征区域对应的初始点云数据。
51.s12、利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据。
52.具体地，电子设备在获取到目标挂车的特征区域对应的初始点云数据之后，电子
设备可以利用激光反射检测方法和激光边沿检测方法对初始点云数据进行至少两次滤波处理，得到第一目标点云数据和/或第二目标点云数据。
53.关于该步骤将在下文进行详细介绍。
54.s13、根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息。
55.在一种可选的实施方式中，电子设备可以对第一目标点云数据进行识别，从第一目标点云数据中提取左侧特征区域对应的第一目标中心点的第一位置信息以及右侧特征区域对应的第二目标中心点的第二位置信息。
56.在另一种可选的实施方式中，电子设备可以对第二目标点云数据进行识别，从第二目标点云数据中提取左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息。
57.在另一种可选的实施方式中，电子设备还可对根据第一目标点云数据和第二目标点云数据进行融合处理，确定第一目标中心点的第一位置信息和第二目标中心点的第二位置信息。
58.关于该步骤将在下文进行详细介绍。
59.s14、根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
60.具体地，在得到左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息之后，电子设备可以根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
61.关于该步骤将在下文进行详细介绍。
62.本发明实施例提供的挂车位姿检测方法，利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，保证了得到的初始点云数据的准确性。然后，利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据，保证了得到的第一目标点云数据和/或第二目标点云数据的准确性。根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息，保证了确定的第一目标中心点的第一位置信息和第二目标中心点的第二位置信息的准确性。然后，根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿，保证了确定的目标挂车的位姿的准确性。从而在目标拖车与目标挂车对中过程中，可以快速且准确地实现目标拖车与目标挂车的对中。此外，在目标拖车拖挂着目标挂车行驶时，目标拖车还可以根据目标挂车的位姿，准确且安全地拖挂着目标挂车行驶，保证了行驶的安全性，避免了事故的发生。
63.在本技术一个实施例中，如图4所示，提供了一种挂车位姿检测方法，以该方法应用于电子设备为例进行说明，其中，目标拖车用于对目标挂车进行拖挂，包括以下步骤：
64.s21、利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据。
65.其中，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧粘贴两个反光条的区域。
66.关于该步骤请参见图1对s11的介绍，在此不进行赘述。
67.s22、利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据。
68.在本技术一种可选的实施方式中，上述步骤s22“利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据”，可以包括如下步骤：
69.s221、获取初始点云数据对应的第一预设范围。
70.具体地，电子设备可以接收用户输入的初始点云数据对应的第一预设范围，也可以接收其他设备发送的初始点云数据对应的第一预设范围，电子设备还可以对目标挂车的特征区域进行图像采集，并对采集到的特征区域的图像进行识别，根据识别结果确定特征区域的尺寸范围。然后，根据特征区域的尺寸范围，确定初始点云数据对应的第一预设范围。
71.本技术实施例对电子设备获取初始点云数据对应的第一预设范围的方式不做具体限定。
72.s222、从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据。
73.具体地，电子设备可以对初始点云数据中的各个点云数据进行识别，确定各个点云数据的位置信息。电子设备可以根据各个点云数据的位置信息，从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据。
74.示例性的，第一预设范围可以是(0≤xi≤40)，(-30≤yi≤30)，(-0.5≤zi≤2)，电子设备可以利用如下公式计算得到第一候选点云数据：
[0075][0076]
s223、获取第一候选点云数据对应的反射率阈值。
[0077]
具体地，电子设备可以接收用户输入的第一候选点云数据对应的反射率阈值，也可以接收其他设备发送的第一候选点云数据对应的反射率阈值，电子设备还可以对第一候选点云数据中各个点云数据的反射率进行识别，并确定第一候选点云数据中各个点云数据的反射率，根据第一候选点云数据中各个点云数据的反射率，确定第一候选点云数据对应的反射率阈值。
[0078]
本技术实施例对电子设备获取第一候选点云数据对应的反射率阈值的方式不做具体限定。
[0079]
其中，反射率阈值可以是200，也可以是210，还可以是220，本技术实施例对反射率阈值不做具体限定。
[0080]
s224、从第一候选点云数据中剔除反射率小于反射率阈值的点云数据，得到第二候选点云数据。
[0081]
具体地，电子设备在获取到第一候选点云数据对应的反射率阈值之后，可以将第一候选点云数据中的各个点云数据的反射率与反射率阈值进行对比，将第一候选点云数据中反射率小于反射率阈值的点云数据从第一候选点云数据中剔除，得到第二候选点云数据，从而可以剔除掉左右两侧特征区域之外的第一候选点云数据。
[0082]
示例性的，当反射率阈值为200时，用i
l
表示，电子设备进行如下过滤检测：
[0083]
p(xi，yi，zi，ii)＝p(xi，yi，zi，ii)，(200≤ii)
ꢀꢀꢀꢀ
(2)
[0084]
s225、对第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，并计算各簇第三候选点云数据对应的初始中心点的初始位置信息。
[0085]
具体地，在得到第二候选点云数据之后，电子设备可以根据利用预设聚类方法，对第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，从而使得两簇第三候选点云数据可以分别表征左侧特征区域和右侧特征区域。
[0086]
其中，预设聚类方法可以是划分式聚类方法、基于密度的聚类方法、层次化聚类方法等，本技术实施例对预设聚类方法不做具体限定。
[0087]
在得到两簇第三候选点云数据之后，电子设备可以分别对两簇第三候选点云数据中的各个点云数据的位置进行识别，根据识别得到的各个点云数据的位置，计算各簇第三候选点云数据对应的初始中心点的初始位置信息。
[0088]
示例性的，可以分别使用p
l
和pr表示两簇第三候选点云数据，其中，p
l
表示左侧第三候选点云数据，pr表示右侧第三候选点云数据。p
l
和pr每个点云数据为(xi，yi，zi，ii)，对各簇第三候选点云数据进行质心坐标计算得到各簇第三候选点云数据对应的初始中心点的初始位置信息，如下：
[0089][0090]
其中，表示左侧第三候选点云数据的质心x值，即左侧第三候选点云数据的初始中心点的x轴位置信息，表示左侧第三候选点云数据的质心y值，即左侧第三候选点云数据的初始中心点的y轴位置信息。表示右侧第三候选点云数据的质心x值，即右侧第三候选点云数据的初始中心点的x轴位置信息，表示右侧第三候选点云数据的质心y值，即右侧第三候选点云数据的初始中心点的y轴位置信息。x
i，l
表示左侧第三候选点云数据的第i个点云数据x值，y
i，l
表示左侧第三候选点云数据的第i个点云数据y值，x
i，r
表示右侧第三候选点云数据的第i个点云数据x值，y
i，r
表示右侧第三候选点云数据的第i个点云数据y值。
[0091]
s226、获取各个初始中心点对应的第二预设范围。
[0092]
具体地，电子设备可以接收用户输入的各个初始中心点的初始位置信息对应的第二预设范围，也可以接收其他设备发送的各个初始中心点的初始位置信息对应的第二预设范围，电子设备还可以两簇第三候选点云数据中的各点云数据的位置进行识别，根据两簇第三候选点云数据中的各点云数据的位置，确定各个初始中心点的初始位置信息对应的第二预设范围。
[0093]
本技术实施例对电子设备获取各个初始中心点的初始位置信息对应的第二预设范围方式不做具体限定。
[0094]
s227、以各个初始中心点为中心，分别保留初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据。
[0095]
具体地，在获取到各个初始中心点的初始位置信息对应的第二预设范围，电子设备可以以各个初始中心点为中心，分别保留初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据。
[0096]
示例性的，第二预设范围可以是(δx，δy)，用q(xi，yi，zi，ii)表示提取优化后的
第四候选点云数据，p(xi，yi，zi，ii)表示第三候选点云数据：
[0097][0098]
同理，q(xi，yi，zi，ii)对y值处理方法相同。
[0099][0100]
s228、根据第四候选点云数据，得到第一目标点云数据。
[0101]
在本技术一种可选的实施方式中，电子设备可以将第四候选点云数据，确定为第一目标点云数据。
[0102]
在本技术一种可选的实施方式中，上述步骤s228“根据第四候选点云数据，得到第一目标点云数据”，可以包括如下步骤：
[0103]
(1)使用法向估计和网格占有方法对第四候选点云数据进行边沿检测，得到第一边沿点云数据。
[0104]
具体地，电子设备在获取到第四候选点云数据之后，可以分别获取第四候选点云数据中各个点云数据对应的法向量，然后根据各个点云数据对应的法向量确定处于同一平面内的点云数据。然后，对同一平面内的点云数据进行网格划分，根据各个网格中点云数据的数量以及位置信息，得到边缘网格。然后根据边缘网格中点云数据的位置信息，得到第一边沿点云数据。
[0105]
(2)对第一边沿点云数据进行识别，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据。
[0106]
具体地，在得到第一边沿点云数据之后，电子设备可以对第一边沿点云数据中各个点云数据的位置进行识别，根据识别到的第一边沿点云数据中各个点云数据的位置，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据。
[0107]
s23、根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息。
[0108]
关于该步骤请参见图1对s13的介绍，在此不进行赘述。
[0109]
s24、根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
[0110]
关于该步骤请参见图1对s14的介绍，在此不进行赘述。
[0111]
本发明实施例提供的挂车位姿检测方法，获取初始点云数据对应的第一预设范围，然后，从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据，保证了得到的第一候选点云数据的准确性，避免了第一候选点云数据范围太大。然后，获取第一候选点云数据对应的反射率阈值，从第一候选点云数据中剔除反射率小于反射率阈值的点云数据，得到第二候选点云数据，保证了得到的第二候选点云数据的准确性，剔除了左侧特征区域和右侧特征区域之外的点云数据。对第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，保证了得到的两簇第三候选点云数据的准确性，从而使得两簇第三候选点云数据分别表征左侧特征区域和右侧特征区域。然后，计算各簇第三候选点云数据对应的初始中心点的初始位置信息，保证了计算得到的初始中心点的初始位置信息的准确性。然后，获取各个初始中心点对应的第二预设范围，以各个初始中心点为中心，分别保留
初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据，保证了得到的第四候选点云数据的准确性。然后，使用法向估计和网格占有方法对第四候选点云数据进行边沿检测，得到第一边沿点云数据，保证了得到的第一边沿点云数据的准确性。然后，对第一边沿点云数据进行识别，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据，保证了得到的第一目标点云数据的准确性，也使得第一目标点云数据可以更加准确地表征左侧特征区域和右侧特征区域对应的点云数据。
[0112]
在本技术一个实施例中，如图5所示，提供了一种挂车位姿检测方法，以该方法应用于目标拖车对应的电子设备为例进行说明，其中，目标拖车用于对目标挂车进行拖挂，包括以下步骤：
[0113]
s31、利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据。
[0114]
其中，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧粘贴两个反光条的区域。
[0115]
关于该步骤请参见图4对s21的介绍，在此不进行赘述。
[0116]
s32、利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据。
[0117]
在本技术一种可选的实施方式中，上述步骤s32“利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据”，可以包括如下步骤：
[0118]
s321、获取初始点云数据对应的第一预设范围。
[0119]
具体地，电子设备可以接收用户输入的初始点云数据对应的第一预设范围，也可以接收其他设备发送的初始点云数据对应的第一预设范围，电子设备还可以对目标挂车的特征区域进行图像采集，并对采集到的特征区域的图像进行识别，根据识别结果确定特征区域的尺寸范围。然后，根据特征区域的尺寸范围，确定初始点云数据对应的第一预设范围。
[0120]
本技术实施例对电子设备获取初始点云数据对应的第一预设范围的方式不做具体限定。
[0121]
s322、从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据。
[0122]
具体地，电子设备可以对初始点云数据中各个点云数据进行识别，确定各个点云数据的位置信息。电子设备可以根据各个点云数据的位置信息，从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据。
[0123]
示例性的，第一预设范围可以是(0≤xi≤40)，(-30≤yi≤30)，(-0.5≤zi≤2)，电子设备可以利用如下公式计算得到第一候选点云数据：
[0124][0125]
s323、使用法向估计和网格占有方法对第一候选点云数据进行边沿检测，得到第二边沿点云数据。
[0126]
具体地，电子设备在获取到第一候选点云数据之后，可以分别获取第一候选点云
数据中各个点云数据对应的法向量，然后根据各个点云数据对应的法向量确定处于同一平面内的点云数据。然后，对同一平面内的点云数据进行网格划分，根据各个网格中点云数据的数量以及位置信息，得到边缘网格。然后根据边缘网格中点云数据的位置信息，得到第二边沿点云数据。
[0127]
s324、对第二边沿点云数据进行识别，提取第二边沿点云数据对应的x轴、y轴以及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围。
[0128]
具体地，在获取到第二边沿点云数据之后，电子设备可以对第二边沿点云数据中的各个点云数据的位置进行识别，根据识别结果，提取第二边沿点云数据对应的x轴、y轴以及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围。
[0129]
s325、获取第二边沿点云数据对应的z轴的第三预设范围。
[0130]
具体地，电子设备可以接收用户输入的第二边沿点云数据对应的z轴的第三预设范围，也可以接收其他设备发送的第二边沿点云数据对应的z轴的第三预设范围。
[0131]
本技术实施例对电子设备获取第二边沿点云数据对应的z轴的第三预设范围的方式不做具体限定。
[0132]
s326、从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据。
[0133]
具体地，在获取到第二边沿点云数据对应的z轴的第三预设范围之后，电子设备可以将第二边沿点云数据对应的z轴的真实范围与第二边沿点云数据对应的z轴的第三预设范围进行对比，根据对比结果从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据。
[0134]
s327、对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据。
[0135]
具体地，电子设备在获取到第三边沿点云数据之后，可以利用预设聚类分析方法，对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据。
[0136]
其中，预设聚类方法可以是划分式聚类方法、基于密度的聚类方法、层次化聚类方法等，本技术实施例对预设聚类方法不做具体限定。
[0137]
s328、将两簇第四边沿点云数据，确定为第二目标点云数据。
[0138]
具体地，在得到两簇第四边沿点云数据之后，电子设备可以将两簇第四边沿点云数据，确定为第二目标点云数据。
[0139]
s33、根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息。
[0140]
关于该步骤请参见图4对s23的介绍，在此不进行赘述。
[0141]
s34、根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
[0142]
关于该步骤请参见图4对s24的介绍，在此不进行赘述。
[0143]
本发明实施例提供的挂车位姿检测方法，获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据，保证了得到的第一候选点云数据的准确性。使用法向估计和网格占有方法对第一候选点云数据进行边沿检测，得到第二边沿点云数据，保证了得到的第二边沿点云数据的准确性。对第二边沿点云数据进行识别，提取第二边沿点云数据对应的x轴、y轴以及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围，保证了得到的第二边沿点云数据对应的轮廓范围的准确性。
然后，获取第二边沿点云数据对应的z轴的第三预设范围；从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据，保证了得到的第三边沿点云数据的准确性。对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据，保证了得到的两簇第四边沿点云数据的准确性。将两簇第四边沿点云数据，确定为第二目标点云数据，保证了得到的第二目标点云数据的准确性。也使得第二目标点云数据可以更加准确地表征左侧特征区域和右侧特征区域对应的点云数据。
[0144]
在本技术一个实施例中，如图6所示，提供了一种挂车位姿检测方法，以该方法应用于目标拖车对应的电子设备为例进行说明，其中，目标拖车用于对目标挂车进行拖挂，包括以下步骤：
[0145]
s41、利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据。
[0146]
其中，特征区域是指目标挂车上靠近目标拖车的侧面的左右两侧粘贴两个反光条的区域。
[0147]
关于该步骤请参见图5对s31的介绍，在此不进行赘述。
[0148]
s42、利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据。
[0149]
关于该步骤请参见图4对s22的介绍和5对s32的介绍，在此不进行赘述。
[0150]
s43、根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息。
[0151]
在本技术一种可选的实施方式中，上述s43“根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息”，可以包括如下步骤：
[0152]
s431、从第一目标点云数据中提取左侧特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息。
[0153]
具体地，在获取到第一目标点云数据之后，电子设备可以对第一目标点云数据中左侧特征区域对应的点云数据以及右侧特征区域对应的点云数据的位置进行识别，然后，根据识别结果，确定对左侧特征区域对应的点云数据和右侧特征区域对应的点云数据进行质心坐标计算，得到左侧特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息。
[0154]
s432、从第二目标点云数据中提取左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息。
[0155]
具体地，在获取到第二目标点云数据之后，电子设备可以对第二目标点云数据中左侧特征区域对应的点云数据以及右侧特征区域对应的点云数据的位置进行识别，然后，根据识别结果，确定对左侧特征区域对应的点云数据和右侧特征区域对应的点云数据进行质心坐标计算，得到左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息。
[0156]
示例性的，左侧第一备用位置信息可以表示为右侧第一备用位置信息
可以表示为左侧第二备用位置信息可以表示为右侧第二备用位置信息可以表示为
[0157]
s433、获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息。
[0158]
具体地，电子设备可以接收用户输入的左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；也可以接收其他设备发送的左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；电子设备还可以根据第一目标点云数据以及第二目标点云数据对应的准确性，确定左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息。
[0159]
本技术实施例对电子设备获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息的方式不做具体限定。
[0160]
s434、根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息。
[0161]
具体地，在获取到第一权重信息和第二权重信息之后，电子设备可以根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息。示例性的，如图7所示。
[0162]
在本技术一种可选的实施方式中，电子设备可以利用第一权重信息乘以左侧第一备用位置信息中的x轴坐标，然后利用第二权重信息乘以左侧第二备用位置信息的x轴坐标，然后相加得到第一目标中心点的第一位置信息中的x轴坐标；第一权重信息乘以左侧第一备用位置信息中的y轴坐标，然后利用第二权重信息乘以左侧第二备用位置信息的y轴坐标，然后相加得到第一目标中心点的第一位置信息中的y轴坐标；第一权重信息乘以右侧第一备用位置信息中的x轴坐标，然后利用第二权重信息乘以右侧第二备用位置信息的x轴坐标，然后相加得到第二目标中心点的第二位置信息中的x轴坐标；第一权重信息乘以右侧第一备用位置信息中的y轴坐标，然后利用第二权重信息乘以右侧第二备用位置信息的y轴坐标，然后相加得到第二目标中心点的第二位置信息中y轴坐标。
[0163]
在本技术另一种可选的实施方式中，电子设备可以利用如下公式计算第一目标中心点的第一位置信息和第二目标中心点的第二位置信息：
[0164][0165]
[0166]
其中，(x
′
l
，y
′
l
)为第一中心点的第一位置信息，(x
′r，y
′r)为第二目标中心点的第二位置信息，为左侧第一备用位置信息，为右侧第一备用位置信息，为左侧第二备用位置信息，为右侧第二备用位置信息，k1为第一权重信息，k2为第二权重信息。
[0167]
s44、根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
[0168]
在本技术一种可选的实施方式中，上述s34“根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿”，可以包括如下步骤：
[0169]
s441、以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系。
[0170]
具体地，电子设备可以以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系。
[0171]
s442、获取第一目标中心点在目标坐标系的第一位置坐标，以及第二目标中心点在目标坐标的第二位置坐标。
[0172]
具体地，在建立目标坐标系之后，电子设备可以获取第一目标中心点在目标坐标系的第一位置坐标，以及第二目标中心点在目标坐标的第二位置坐标。
[0173]
s443、根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角。
[0174]
在本技术一种可选的实施方式中，上述s443“根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角”，还可以包括如下步骤：
[0175]
(1)根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线与y轴之间的第一夹角。
[0176]
具体地，电子设备可以根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线，然后确定第一连线与y轴之间的第一夹角。
[0177]
(2)根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标；
[0178]
具体地，电子设备在获取到第一位置坐标和第二位置坐标之后，可以根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标。
[0179]
(3)确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角。
[0180]
具体地，电子设备可以根据第三位置坐标和原点坐标之间的关系，确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角。
[0181]
(4)利用第一夹角减去第二夹角，得到备用航向角。
[0182]
具体地，在确定了第一夹角和第二夹角之后，电子设备可以利用第一夹角减去第二夹角，得到备用航向角。
[0183]
示例性的，如图8所示，图中l点坐标为第一目标中心点的第一位置坐标(x
′r，y
′r)。从图中可知∠cof＝∠roh-∠rom。使用α表示∠roh，使用β为第一位置坐标与原点坐标之间
的第一连线与y轴之间的第一夹角，表示为∠rom，使用θ为确定第一连线与第二连线之间的第二夹角，表示为∠cof，即备用航向角。(x
′
l
，y
′
l
)为第一中心点的第一位置信息，(x
′r，y
′r)为第二目标中心点的第二位置信息。
[0184]
步骤一：求解β
[0185]
m点为第一位置坐标和第二位置坐标之间的中心点，xm＝(x
′
l
+x
′r)/2，ym＝(y
′
l
+y
′r)/2。用d
rm
表示rm的长度，d
or
表示or的长度，因此sinβ＝d
rm
/d
or
，解算出β。
[0186]
步骤二：求解α
[0187]
用d
rh
表示rh的长度，d
rh
＝||y
′r||，sinα＝d
rh
/d
or
，解算出α。
[0188]
步骤三：求解θ
[0189]
θ＝α-β。
[0190]
s444、获取目标挂车的后轴中心点到预设传感器之间的距离。
[0191]
具体地，电子设备可以获取目标挂车的后轴中心点到第一目标中心点与第二目标中心点之间的中心点之间的第一距离。然后，根据预设传感器扫描的得到的点云数据，确定预设传感器到第一目标中心点与第二目标中心点之间的中心点之间的第二距离，将第一距离和第二距离进行相加，得到目标挂车的后轴中心点到预设传感器之间的距离。
[0192]
s445、根据目标挂车的后轴中心点到预设传感器之间的距离与备用航向角之间的关系，确定目标挂车在目标坐标系下的备用位置信息。
[0193]
具体地，在获取到目标挂车的后轴中心点到预设传感器之间的距离之后，电子设备可以利用目标挂车的后轴中心点到预设传感器之间的距离乘以备用航向角的余弦值，确定目标挂车在目标坐标系下y轴的位置信息。
[0194]
然后，电子设备利用目标挂车的后轴中心点到预设传感器之间的距离乘以备用航向角的正弦值，确定目标挂车在目标坐标系下x轴的位置信息。
[0195]
示例性的，如图8所示，目标挂车后轴中心点与第一目标中心点c与第二目标中心点之间的中心点m点之间的第一距离已知，设为dv。用d
om
表示预设传感器o点到第一目标中心点与第二目标中心点之间的中心点m点之间的第二距离，表示为om的长度，用表示目标挂车的备用航向。因此，目标挂车在目标坐标系下的备用位置信息解算如下：
[0196]
xw＝(d
om
+dv)*cos(θ)
ꢀꢀꢀꢀ
(8)
[0197]yw
＝(d
om
+dv)*sin(θ)
ꢀꢀꢀꢀ
(9)
[0198]
其中，d
om
为预设传感器o点到第一目标中心点与第二目标中心点之间的中心点m点之间的第二距离；dv为目标挂车后轴中心点c与第一目标中心点与第二目标中心点之间的中心点m点之间的第一距离；θ为备用航向角。
[0199]
s446、根据目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息。
[0200]
具体地，电子设备可以根据预设传感器与目标挂车后轴中心点之间的位置关系，确定目标拖车的车身坐标下。然后，根据目标拖车的航向角，确定车身坐标系与大地坐标下之间的第二关系。然后，根据第一关系与第二关系，确定目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息。
[0201]
本发明实施例提供的挂车位姿检测方法，从第一目标点云数据中提取左侧特征区
域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息，保证了提取到的左侧第一备用位置信息以及右侧第一备用位置信的准确性。然后，从第二目标点云数据中提取左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息，保证了提取到的左侧第二备用位置信息以及右侧第二备用位置信息的准确性。获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息，保证了得到的第一目标中心点的第一位置信息以及第二目标中心点的第二位置信息的准确性。
[0202]
然后，以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系，保证了建立的目标坐标系的准确性。根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线与y轴之间的第一夹角，保证了确定的第一夹角的准确性。根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标，保证了确定的第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标的准确性。确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角，保证了确定的第二夹角的准确性。利用第一夹角减去第二夹角，得到备用航向角，保证了得到的备用航向角的准确性，进而可以保证确定的目标挂车的位姿的准确性。然后，获取目标挂车的后轴中心点到预设传感器之间的距离，根据目标挂车的后轴中心点到预设传感器之间的距离与备用航向角之间的关系，确定目标挂车在目标坐标系下的备用位置信息，保证了确定的目标挂车在目标坐标系下的备用位置信息的准确性。然后，根据目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息，保证了得到的目标挂车的目标航向角以及目标位置信息的准确性，从而保证了确定的目标挂车在大地坐标系下的位姿的准确性。从而在目标拖车与目标挂车对中过程中，可以快速且准确地实现目标拖车与目标挂车的对中。此外，在目标拖车拖挂着目标挂车行驶时，目标拖车还可以根据目标挂车的位姿，准确且安全地拖挂着目标挂车行驶，保证了行驶的安全性，避免了事故的发生。
[0203]
在本技术一种可选的实施方式中，当目标拖车牵引目标挂车在极大角度转弯的时候，存在目标拖车与目标挂车有极大转角差异情况下。判断条件为，当激光反射方法检测不到两条反光带特征区域的点云，只能检测到其中一个特征区域的点云。在该种情况下，使用获取所述初始点云数据对应的第一预设范围；从所述初始点云数据中剔除所述第一预设范围之外的点云数据，得到第一候选点云数据。然后，获取所述第一候选点云数据对应的反射率阈值，从所述第一候选点云数据中剔除反射率小于所述反射率阈值的点云数据，得到第二候选点云数据。对所述第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，并计算各簇所述第三候选点云数据对应的初始中心点的初始位置信息。然后，获取各个所述初始中心点对应的第二预设范围，以各个所述初始中心点为中心，分别保留所述初始中心点对应的所述第二预设范围内的点云数据，得到第四候选点云数据。使用法向估计和网格
占有方法对所述第四候选点云数据进行边沿检测，得到第一边沿点云数据，对所述第一边沿点云数据进行识别，提取所述第一边沿点云数据中的左右边沿的点云数据，得到所述第一目标点云数据。计算第一目标点云数据相对平面的法向量估计，根据法向量方向阈值区分两个截面，示例性的如图9和图10所示。对两个截面进行点云聚类和数量统计，根据两个截面聚类点云占比，基于上一时刻的挂车航向角进行角度估计。用n1
size
和n2
size
表示两个截面聚类的点云数量。θk表示当前时刻挂车的航向角，θ
k-1
表示上一时刻挂车的航向角。该情况下的挂车航向角计算估计方式为：
[0204][0205]
该种情况为低速转弯大转角特殊情况，对于车辆控制来说航向角的估计比挂车位置估计更重要。该种情况下不能根据s1-s9的方法计算挂车的位姿。使用车辆速度根据挂车角度增量在挂车前进方向和侧移方向进行位移积分。用v
m，k-1
表示车辆上一时刻的挂车行驶速度。用x
w，k-1
和y
w，k-1
表示上一时刻挂车的位置。δθk表示k时刻挂车航向角增量。对于该情况下的位置估计近似如下：
[0206]
x
w，k
＝x
w，k-1
+v
m，k-1
*cos(δθk)*(t
k-t
k-1
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0207]yw，k
＝y
w，k-1
+v
m，k-1
*sin(δθk)*(t
k-t
k-1
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0208]
其中，tk和t
k-1
为表示k-1和k时刻。
[0209]
应该理解的是，虽然图1以及图4-6的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1以及图4-6中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
[0210]
如图11所示，本实施例提供一种挂车位姿检测装置，包括：
[0211]
获取模块51，用于利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据，特征区域是指目标挂车上靠目标拖车的侧面的左右两侧各粘贴一个反光条的区域；
[0212]
滤波处理模块52，用于利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；
[0213]
第一确定模块53，用于根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息；
[0214]
第二确定模块54，用于根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。
[0215]
在本技术一个实施例中，上述滤波处理模块52，具体用于获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据；获取第一候选点云数据对应的反射率阈值；从第一候选点云数据中剔除反射率小于反射率阈值的点云数据，得到第二候选点云数据；对第二候选点云数据进行聚类分析，得
到两簇第三候选点云数据，并计算各簇第三候选点云数据对应的初始中心点的初始位置信息；获取各个初始中心点对应的第二预设范围；以各个初始中心点为中心，分别保留初始中心点对应的第二预设范围内的点云数据，得到第四候选点云数据；根据第四候选点云数据，得到第一目标点云数据。
[0216]
在本技术一个实施例中，上述滤波处理模块52，具体用于使用法向估计和网格占有方法对第四候选点云数据进行边沿检测，得到第一边沿点云数据；对第一边沿点云数据进行识别，提取第一边沿点云数据中的左右边沿的点云数据，得到第一目标点云数据。
[0217]
在本技术一个实施例中，上述滤波处理模块52，具体用于获取初始点云数据对应的第一预设范围；从初始点云数据中剔除第一预设范围之外的点云数据，得到第一候选点云数据；使用法向估计和网格占有方法对第一候选点云数据进行边沿检测，得到第二边沿点云数据；对第二边沿点云数据进行识别，提取第二边沿点云数据对应的x轴、y轴以及z轴的轮廓，得到第二边沿点云数据对应的轮廓范围；获取第二边沿点云数据对应的z轴的第三预设范围；从第二边沿点云数据中剔除z轴范围在第三预设范围外的点云数据，得到第三边沿点云数据；对第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据；将两簇第四边沿点云数据，确定为第二目标点云数据。
[0218]
在本技术一个实施例中，上述第一确定模块53，具体用于从第一目标点云数据中提取左侧特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧特征区域对应的右侧第一备用中心点的右侧第一备用位置信息；从第二目标点云数据中提取左侧特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧特征区域对应的右侧第二备用中心点的右侧第二备用位置信息；获取左侧第一备用位置信息和右侧第一备用位置信息对应的第一权重信息，以及左侧第二备用位置信息和右侧第二备用位置信息对应的第二权重信息；根据第一权重信息和第二权重信息，对左侧第一备用位置信息、右侧第一备用位置信息、左侧第二备用位置信息以及右侧第二备用位置信息进行加权计算，得到第一目标中心点的第一位置信息和第二目标中心点的第二位置信息。
[0219]
在本技术一个实施例中，上述第二确定模块54，具体用于以预设传感器为原点，以平行于地面指向目标拖车前方为x轴，垂直于x轴指向左侧为y轴，建立目标坐标系；获取第一目标中心点在目标坐标系的第一位置坐标，以及第二目标中心点在目标坐标的第二位置坐标；根据第一位置坐标、第二位置坐标以及原点坐标之间的关系，计算目标挂车在目标坐标下的备用航向角；获取目标挂车的后轴中心点到预设传感器之间的距离；根据目标挂车的后轴中心点到预设传感器之间的距离与备用航向角之间的关系，确定目标挂车在目标坐标系下的备用位置信息；根据目标坐标系与大地坐标系之间的关系转换，对备用航向角以及备用位置信息进行转换，得到目标挂车在大地坐标系下的目标航向角以及目标位置信息。
[0220]
在本技术一个实施例中，上述第二确定模块54，具体用于根据第一位置坐标与原点坐标之间的关系，确定第一位置坐标与原点坐标之间的第一连线与y轴之间的第一夹角；根据第一位置坐标和第二位置坐标之间的关系，确定第一位置坐标和第二位置坐标之间的中心点对应的第三位置坐标；确定原点坐标与第三位置坐标之间的第二连线，并确定第一连线与第二连线之间的第二夹角；利用第一夹角减去第二夹角，得到备用航向角。
[0221]
关于挂车位姿检测装置的具体限定以及有益效果可以参见上文中对于挂车位姿
drive，缩写：hdd)或固态硬盘(solid-state drive，ssd)等；所述存储介质还可以包括上述种类的存储器的组合。
[0230]
本发明实施例还提供了一种拖车，包括电子设备和拖车本体，电子设备用于执行如本技术图1以及图4-6实施例中所示的挂车位姿检测方法。
[0231]
虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下做出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

技术特征：
1.一种挂车位姿检测方法，其特征在于，应用于目标拖车，所述目标拖车用于对目标挂车进行拖挂，包括：利用安装在所述目标拖车上的预设传感器对所述目标挂车的特征区域进行扫描，得到所述特征区域对应的初始点云数据，所述特征区域是指所述目标挂车上靠近所述目标拖车的侧面的左右两侧各粘贴一个反光条的区域；利用激光反射检测方法和激光边沿检测方法对所述初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；根据所述第一目标点云数据和/或所述第二目标点云数据，确定左侧所述特征区域对应的第一目标中心点的第一位置信息和右侧所述特征区域对应的第二目标中心点的第二位置信息；根据所述第一位置信息与所述第二位置信息之间的关系，确定所述目标挂车的位姿。2.根据权利要求1所述的方法，其特征在于，所述利用激光反射检测方法和激光边沿检测方法对所述初始点云数据进行滤波处理，得到第一目标点云数据，包括：获取所述初始点云数据对应的第一预设范围；从所述初始点云数据中剔除所述第一预设范围之外的点云数据，得到第一候选点云数据；获取所述第一候选点云数据对应的反射率阈值；从所述第一候选点云数据中剔除反射率小于所述反射率阈值的点云数据，得到第二候选点云数据；对所述第二候选点云数据进行聚类分析，得到两簇第三候选点云数据，并计算各簇所述第三候选点云数据对应的初始中心点的初始位置信息；获取各个所述初始中心点对应的第二预设范围；以各个所述初始中心点为中心，分别保留所述初始中心点对应的所述第二预设范围内的点云数据，得到第四候选点云数据；根据所述第四候选点云数据，得到所述第一目标点云数据。3.根据权利要求2所述的方法，其特征在于，所述根据所述第四候选点云数据，得到第一目标点云数据，包括：使用法向估计和网格占有方法对所述第四候选点云数据进行边沿检测，得到第一边沿点云数据；对所述第一边沿点云数据进行识别，提取所述第一边沿点云数据中的左右边沿的点云数据，得到所述第一目标点云数据。4.根据权利要求1所述的方法，其特征在于，所述利用激光反射检测方法和激光边沿检测方法对所述初始点云数据进行滤波处理，得到第二目标点云数据，还包括：获取所述初始点云数据对应的第一预设范围；从所述初始点云数据中剔除所述第一预设范围之外的点云数据，得到第一候选点云数据；使用法向估计和网格占有方法对所述第一候选点云数据进行边沿检测，得到第二边沿点云数据；对所述第二边沿点云数据进行识别，提取所述第二边沿点云数据对应的x轴、y轴以及z
轴的轮廓，得到所述第二边沿点云数据对应的轮廓范围；获取所述第二边沿点云数据对应的z轴的第三预设范围；从所述第二边沿点云数据中剔除z轴范围在所述第三预设范围外的点云数据，得到第三边沿点云数据；对所述第三边沿点云数据进行聚类分析，得到两簇第四边沿点云数据；将两簇所述第四边沿点云数据，确定为所述第二目标点云数据。5.根据权利要求1所述的方法，其特征在于，根据所述第一目标点云数据和所述第二目标点云数据，确定左侧所述特征区域对应的第一目标中心点的第一位置信息和右侧所述特征区域对应的第二目标中心点的第二位置信息，包括：从所述第一目标点云数据中提取左侧所述特征区域对应的左侧第一备用中心点的左侧第一备用位置信息以及右侧所述特征区域对应的右侧第一备用中心点的右侧第一备用位置信息；从所述第二目标点云数据中提取左侧所述特征区域对应的左侧第二备用中心点的左侧第二备用位置信息以及右侧所述特征区域对应的右侧第二备用中心点的右侧第二备用位置信息；获取所述左侧第一备用位置信息和所述右侧第一备用位置信息对应的第一权重信息，以及所述左侧第二备用位置信息和所述右侧第二备用位置信息对应的第二权重信息；根据所述第一权重信息和所述第二权重信息，对所述左侧第一备用位置信息、所述右侧第一备用位置信息、所述左侧第二备用位置信息以及所述右侧第二备用位置信息进行加权计算，得到所述第一目标中心点的第一位置信息和所述第二目标中心点的第二位置信息。6.根据权利要求1所述的方法，其特征在于，所述根据所述第一位置信息与所述第二位置信息之间的关系，确定所述目标挂车的位姿，包括：以所述预设传感器为原点，所述以平行于地面指向目标拖车前方为x轴，垂直于所述x轴指向左侧为y轴，建立目标坐标系；获取所述第一目标中心点在所述目标坐标系的第一位置坐标，以及所述第二目标中心点在所述目标坐标的第二位置坐标；根据所述第一位置坐标、所述第二位置坐标以及原点坐标之间的关系，计算所述目标挂车在所述目标坐标下的备用航向角；获取所述目标挂车的后轴中心点到所述预设传感器之间的距离；根据所述目标挂车的后轴中心点到所述预设传感器之间的距离与所述备用航向角之间的关系，确定所述目标挂车在所述目标坐标系下的备用位置信息；根据所述目标坐标系与大地坐标系之间的关系转换，对所述备用航向角以及所述备用位置信息进行转换，得到所述目标挂车在所述大地坐标系下的目标航向角以及目标位置信息。7.根据权利要求6所述的方法，其特征在于，所述根据所述第一位置坐标、所述第二位置坐标以及原点坐标之间的关系，计算所述目标挂车在所述目标坐标下的备用航向角，包括：根据所述第一位置坐标与所述原点坐标之间的关系，确定所述第一位置坐标与所述原
点坐标之间的第一连线与所述y轴之间的第一夹角；根据所述第一位置坐标和所述第二位置坐标之间的关系，确定所述第一位置坐标和所述第二位置坐标之间的中心点对应的第三位置坐标；确定所述原点坐标与所述第三位置坐标之间的第二连线，并确定所述第一连线与所述第二连线之间的第二夹角；利用所述第一夹角减去所述第二夹角，得到所述备用航向角。8.一种挂车位姿检测装置，其特征在于，应用于目标拖车，所述目标拖车用于对目标挂车进行拖挂，包括：获取模块，用于利用安装在所述目标拖车上的预设传感器对所述目标挂车的特征区域进行扫描，得到所述特征区域对应的初始点云数据，所述特征区域是指所述目标挂车上靠近所述目标拖车的侧面的左右两侧各粘贴一个反光条的区域；滤波处理模块，用于利用激光反射检测方法和激光边沿检测方法对所述初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；第一确定模块，用于根据所述第一目标点云数据和/或所述第二目标点云数据，确定左侧所述特征区域对应的第一目标中心点的第一位置信息和右侧所述特征区域对应的第二目标中心点的第二位置信息；第二确定模块，用于根据所述第一位置信息与所述第二位置信息之间的关系，确定所述目标挂车的位姿。9.一种目标拖车，其特征在于，包括电子设备和拖车本体，所述电子设备中包括存储器和处理器，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行权利要求1-7中任一项所述的挂车位姿检测方法。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使计算机执行权利要求1-7中任一项所述的挂车位姿检测方法。

技术总结
本发明涉及智能驾驶技术领域，具体涉及一种挂车位姿检测方法、装置、目标拖车及存储介质。包括：利用安装在目标拖车上的预设传感器对目标挂车的特征区域进行扫描，得到特征区域对应的初始点云数据；利用激光反射检测方法和激光边沿检测方法对初始点云数据进行滤波处理，得到第一目标点云数据和/或第二目标点云数据；根据第一目标点云数据和/或第二目标点云数据，确定左侧特征区域对应的第一目标中心点的第一位置信息和右侧特征区域对应的第二目标中心点的第二位置信息；根据第一位置信息与第二位置信息之间的关系，确定目标挂车的位姿。保证了确定的目标挂车的位姿的准确性，进而保证了行驶的安全性，避免了事故的发生。避免了事故的发生。避免了事故的发生。


技术研发人员：欧敏辉 谢聪
受保护的技术使用者：深圳海星智驾科技有限公司
技术研发日：2023.04.11
技术公布日：2023/8/5