[发明专利]一种牵引车组位姿检测方法及系统

权利要求书

1.一种牵引车组位姿检测方法，步骤包括：

S1设置扫描单元扫描范围，使其至少部分覆盖牵引车头后方挂载车车特征点；其中所述车特征点包括：车轮或车厢中的至少一种；

S2设置以牵引车头预设位置为参照点，建立全局坐标系；

S3计算单元获取扫描单元采集的扫描数据，经数据分割处理后获取数据块；

S4计算单元调取特征库比对数据块，识别数据块中的特征点数据；

S5计算单元依据全局坐标系，计算特征点数据所处坐标系方位，获取其位姿。

2.根据权利要求1所述的牵引车组位姿检测方法，其中扫描数据为坐标点数据，所述数据分割处理步骤包括：

S3.1采用聚类算法，相对参照点由近及远的对坐标点数据进行分析；

S3.2当判断坐标点数据近似位于一条直线上时，聚类形成数据块。

3.根据权利要求2所述的牵引车组位姿检测方法，其中步骤S4中的特征库比对步骤包括：

S4.1数据块生成点云，以与所述特征库中存储的特征模型的轮廓进行拟合，获取特征点数据；

S4.2对步骤S4.1拟合判断出的特征点数据标记出，拖车数据块、平台车数据块、障碍物数据块中的任一种。

4.根据权利要求3所述的牵引车组位姿检测方法，其中步骤S5中的数据块所处相对坐标方位计算步骤包括：

S5.1计算拖车数据块的坐标点与参照点的坐标距离，判断其为第几节拖车；

S5.2计算拖车数据块的坐标点所处全局坐标系中的坐标位置，判断拖车行驶位姿。

5.根据权利要求4所述的牵引车组位姿检测方法，其中步骤还包括：

S5.3计算平台车数据块的坐标点与参照点的坐标距离，判断其与拖车的距离；

S5.4计算平台车数据块的坐标点所处全局坐标系中的坐标位置，判断平台车与拖车的相对位姿。

6.根据权利要求4所述的牵引车组位姿检测方法，其中步骤还包括：

S5.5计算障碍物数据块的坐标点与参照点的坐标距离，判断其与拖车的距离；

S5.6计算障碍物数据块的坐标点所处全局坐标系中的坐标位置，判断障碍物与拖车的相对位姿。

7.根据权利要求1所述的牵引车组位姿检测方法，其中所述扫描单元设置在牵引车头下方，并配置扫描基线覆盖至少部分的拖车车轮或平台车车轮中的至少一项区域。

8.根据权利要求1所述的牵引车组位姿检测方法，其中所述特征库中包括：拖车车轮特征数据或平台车车轮特征数据中的至少一项。

9.根据权利要求1所述的牵引车组位姿检测方法，其中步骤S5为：计算单元依据全局坐标系，计算特征点数据之间所处坐标系中的相对方位，获取其位姿。

10.一种牵引车组位姿检测系统，其包括：扫描单元，计算单元，存储单元，电源模块，其特征在于，所述存储单元存储包括：如权利要求1-9任一所述牵引车组位姿检测方法及特征库，所述扫描单元固定在牵引车头预设位置处，并与所述计算单元通信连接，传输扫描数据，以供所述计算单元调取所述存储单元储存的所述牵引车组位姿检测方法及特征库，执行对所述扫描数据的处理。