一种基于工业相机和多线激光雷达的半挂车定位方法及系统技术方案

一种基于工业相机和多线激光雷达的半挂车定位方法及系统，涉及牵引车与半挂车定位技术领域，用以解决现有定位方法对于小型码头上牵引车与半挂车相对位置的确定精确度不高的问题。本发明专利技术的技术要点包括：设置二维码、工业相机与激光雷达的位置，利用工业相机检测二维码，提取中心坐标；利用激光雷达采集点云，在指定半径范围内查询，由搜索出的点云拟合二维码平面方程，得到半挂车前表面法向量及夹角；创建K

【技术实现步骤摘要】
一种基于工业相机和多线激光雷达的半挂车定位方法及系统


[0001]本专利技术涉及牵引车与半挂车定位
，具体涉及一种基于工业相机和多线激光雷达的半挂车定位方法及系统。

技术介绍

[0002]随着码头吞吐量的不断增大，装卸运输工作越来越复杂，传统的拖挂车搬运货物成为制约装卸效率的瓶颈，因此迫切需要通过自动化技术来解决问题。码头通常采用拖挂车来运输货物，需要司机驾驶，与码头其它作业设备衔接差，不够灵活，但码头的行驶环境具有平坦、宽阔、车流规则度高的特点，实现自动驾驶难度相对较低。现有的无人码头运输多使用AGV，是一种装备有电磁或光学等自动导航装置，能够沿规定的导航路径行驶，具有安全保护以及各种移载功能的运输车，但AGV多应用于大型港口集装箱运输枢纽，不适用于场地有限、运输货物不规则的小型码头。
[0003]华菱湘钢无人码头自动驾驶车，是一款偏置运输车，由牵引车与半挂车两部分组成，即挂即运，适用于小型码头的应用场景。牵引车与半挂车的接挂过程是至关重要的一个环节，只有二者的相对位置精准确定，才能保证自动化接挂的顺利完成。常用的定位方法有GPS定位、视觉定位、激光雷达定位。GPS定位更新频率较低、易受信号强弱影响且精度不够高，难以满足接挂的要求，视觉定位受环境光影响大，光线暗时无法工作，而激光雷达定位精度高、可靠性高且技术成熟。

技术实现思路

[0004]鉴于以上问题，本专利技术提出一种基于工业相机和多线激光雷达的半挂车定位方法及系统，用以解决现有定位方法对于小型码头上牵引车与半挂车相对位置的确定精确度不高的问题。
[0005]根据本专利技术的一方面，提供一种基于工业相机和多线激光雷达的半挂车定位方法，该方法包括以下步骤：
[0006]步骤一、将二维码图片放置在半挂车前表面，将工业相机和多线激光雷达放置在牵引车后表面，在半挂车与牵引车脱挂的状态下，利用工业相机采集包含二维码图片的图像，并对图像进行处理，获得二维码图片中心在相机坐标下的坐标；
[0007]步骤二、将二维码图片中心在相机坐标下的坐标转换至雷达坐标系下，获得二维码图片中心在雷达坐标系下的坐标；
[0008]步骤三、利用多线激光雷达采集原始点云数据，并以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第一搜索半径，在所述第一搜索半径范围内，对所述原始点云数据进行搜索，获取小邻域点云；
[0009]步骤四、利用小邻域点云拟合二维码图片的平面方程，根据所述平面方程获得半挂车与牵引车的相对位置夹角，以获得半挂车与牵引车的相对姿态；
[0010]步骤五、以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第二
搜索半径，在所述第二搜索半径范围内，对所述原始点云数据进行搜索，获取大邻域点云；
[0011]步骤六、根据预设的第一距离阈值和二维码图片的平面方程，在所述大邻域点云中筛选出到二维码图片平面小于第一距离阈值的所有点云，即与二维码图片同平面的点云及其所在线束；
[0012]步骤七、根据预设的第二距离阈值，在筛选得到的与二维码图片同平面的点云及其所在线束中，查找各线束中两点距离小于第二距离阈值的两个点即为二维码图片平面上的边界点；
[0013]步骤八、根据获得的所有边界点坐标计算获得二维码图片中心在雷达坐标系下的精确坐标，以获得半挂车与牵引车的相对位置。
[0014]进一步地，步骤一中对图像进行如下处理：进行平滑滤波和二值化，确定二维码的三个角点位置；根据三个角点位置确定二维码中心坐标。
[0015]进一步地，步骤三中获取小邻域点云的具体步骤包括：创建K
‑
D树结构，采用基于K
‑
D树的最近邻搜索算法在所述第一搜索半径范围内查找所有近邻点，组成小邻域点云；步骤五中获取大邻域点云的具体步骤包括：创建K
‑
D树结构，采用基于K
‑
D树的最近邻搜索算法在所述第二搜索半径范围内查找所有近邻点，组成大邻域点云；其中，所述第二搜索半径大于所述第一搜索半径。
[0016]进一步地，步骤四中半挂车与牵引车的相对位置夹角为：
[0017][0018]式中，A,B为二维码图片的平面方程的系数。
[0019]进一步地，步骤六中对与二维码图片同平面的点云线束内的点云按照其水平旋转角度从小到大排序，以在步骤七中查找二维码图片平面上的边界点时按照所述顺序进行查找。
[0020]进一步地，步骤八中二维码图片中心在雷达坐标系下的精确坐标(x,y)计算公式如下：
[0021][0022]式中，(x
p
,y
p
)、(x
q
,y
q
)表示两个边界点在雷达坐标系下的坐标。
[0023]根据本专利技术另一方面，提供一种基于工业相机和多线激光雷达的半挂车定位系统，将二维码图片放置在半挂车前表面，将工业相机和多线激光雷达放置在牵引车后表面，所述定位系统包括：
[0024]相机坐标获取模块，其配置成在半挂车与牵引车脱挂的状态下，利用工业相机采集包含二维码图片的图像，并对图像进行处理，获得二维码图片中心在相机坐标下的坐标；
[0025]坐标转换模块，其配置成将二维码图片中心在相机坐标下的坐标转换至雷达坐标系下，获得二维码图片中心在雷达坐标系下的坐标；
[0026]小邻域点云获取模块，其配置成利用多线激光雷达采集原始点云数据，并以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第一搜索半径，在所述第一搜索半径范围内，对所述原始点云数据进行搜索，获取小邻域点云；
[0027]相对姿态获取模块，其配置成利用小邻域点云拟合二维码图片的平面方程，根据
所述平面方程获得半挂车与牵引车的相对位置夹角，以获得半挂车与牵引车的相对姿态；
[0028]大邻域点云获取模块，其配置成以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第二搜索半径，在所述第二搜索半径范围内，对所述原始点云数据进行搜索，获取大邻域点云；
[0029]点云筛选模块，其配置成根据预设的第一距离阈值和二维码图片的平面方程，在所述大邻域点云中筛选出到二维码图片平面小于第一距离阈值的所有点云，即与二维码图片同平面的点云及其所在线束；
[0030]边界点查找模块，其配置成根据预设的第二距离阈值，在筛选得到的与二维码图片同平面的点云及其所在线束中，查找各线束中两点距离小于第二距离阈值的两个点即为二维码图片平面上的边界点；
[0031]相对位置获取模块，其配置成根据获得的所有边界点坐标计算获得二维码图片中心在雷达坐标系下的精确坐标，以获得半挂车与牵引车的相对位置。
[0032]进一步地，所述相机坐标获取模块中对图像进行如下处理：进行平滑滤波和二值化，确定二维码的三个角点位置；根据三个角点位置确定二维码中心坐标。
[0033]进一步地，所述小邻域点云获取模块中获取小邻域点云的具体步骤包括：创建K
‑<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于工业相机和多线激光雷达的半挂车定位方法，其特征在于，包括以下步骤：步骤一、将二维码图片放置在半挂车前表面，将工业相机和多线激光雷达放置在牵引车后表面，在半挂车与牵引车脱挂的状态下，利用工业相机采集包含二维码图片的图像，并对图像进行处理，获得二维码图片中心在相机坐标下的坐标；步骤二、将二维码图片中心在相机坐标下的坐标转换至雷达坐标系下，获得二维码图片中心在雷达坐标系下的坐标；步骤三、利用多线激光雷达采集原始点云数据，并以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第一搜索半径，在所述第一搜索半径范围内，对所述原始点云数据进行搜索，获取小邻域点云；步骤四、利用小邻域点云拟合二维码图片的平面方程，根据所述平面方程获得半挂车与牵引车的相对位置夹角，以获得半挂车与牵引车的相对姿态；步骤五、以已经获得的二维码图片中心在雷达坐标系下的坐标为圆心，预设第二搜索半径，在所述第二搜索半径范围内，对所述原始点云数据进行搜索，获取大邻域点云；步骤六、根据预设的第一距离阈值和二维码图片的平面方程，在所述大邻域点云中筛选出到二维码图片平面小于第一距离阈值的所有点云，即与二维码图片同平面的点云及其所在线束；步骤七、根据预设的第二距离阈值，在筛选得到的与二维码图片同平面的点云及其所在线束中，查找各线束中两点距离小于第二距离阈值的两个点即为二维码图片平面上的边界点；步骤八、根据获得的所有边界点坐标计算获得二维码图片中心在雷达坐标系下的精确坐标，以获得半挂车与牵引车的相对位置。2.根据权利要求1所述的一种基于工业相机和多线激光雷达的半挂车定位方法，其特征在于，步骤一中对图像进行如下处理：进行平滑滤波和二值化，确定二维码的三个角点位置；根据三个角点位置确定二维码中心坐标。3.根据权利要求1或2所述的一种基于工业相机和多线激光雷达的半挂车定位方法，其特征在于，步骤三中获取小邻域点云的具体步骤包括：创建K
‑
D树结构，采用基于K
‑
D树的最近邻搜索算法在所述第一搜索半径范围内查找所有近邻点，组成小邻域点云；步骤五中获取大邻域点云的具体步骤包括：创建K
‑
D树结构，采用基于K
‑
D树的最近邻搜索算法在所述第二搜索半径范围内查找所有近邻点，组成大邻域点云；其中，所述第二搜索半径大于所述第一搜索半径。4.根据权利要求3所述的一种基于工业相机和多线激光雷达的半挂车定位方法，其特征在于，步骤四中半挂车与牵引车的相对位置夹角为：式中，A,B为二维码图片的平面方程的系数。5.根据权利要求4所述的一种基于工业相机和多线激光雷达的半挂车定位方法，其特征在于，步骤六中对与二维码图片同平面的点云线束内的点云按照其水平旋转角度从小到大排序，以在步骤七中查找二维码图片平面上的边界点时按照所述顺序进行查找。6.根据权利要求5所述的一种基于工业相机和多线激光雷达的半挂车定位方法，其特
征在于，步骤八中二维码图片中心在雷达坐标系下的精确坐标(x,y)计算公式如下：式中，(x
p
,y
q
)、(x
q
,y
q
...

【专利技术属性】
技术研发人员：李卫华，王艳桃，陈栋磊，陈博文，吴雯珺，王剑锋，
申请(专利权)人：国网智慧能源交通技术创新中心苏州有限公司，
类型：发明
国别省市：