确定工程车辆臂架与障碍物距离的方法、处理器及系统技术方案

本申请涉及工程车辆技术领域，具体涉及一种确定工程车辆臂架与障碍物距离的方法、处理器及系统。方法包括：建立第一坐标系；实时获取每个臂架在第一坐标系上的多个第一点云坐标和每个臂架的实时倾角；确定每个臂架的转轴中心并建立多个第二坐标系；根据每个臂架的转轴中心和实时倾角确定第一坐标系与每个臂架的第二坐标系之间的转换矩阵；针对每个臂架，根据转换矩阵确定臂架的每个第一点云坐标在臂架的第二坐标系中对应的第二点云坐标；根据每个臂架的外形参数和其对应的第二点云坐标确定每个臂架的包围盒模型；根据包围盒模型与障碍物的点云坐标确定每个臂架与障碍物之间的距离。本申请方法提高了工程车辆臂架与障碍物距离的计算速度和精度。距离的计算速度和精度。距离的计算速度和精度。

【技术实现步骤摘要】
确定工程车辆臂架与障碍物距离的方法、处理器及系统


[0001]本申请涉及工程车辆
，具体涉及一种确定工程车辆臂架与障碍物距离的方法、处理器、系统及存储介质。

技术介绍

[0002]很多工程车辆包括有可以旋转的臂架，例如泵车，高空作业平台车辆等，在臂架运动的过程中，可能会与建筑物、高压线等障碍物发生碰撞，引发臂架受损，因此需要确定臂架在运动过程中与障碍物之间的距离。
[0003]现有技术中，确定距离的方法有建立臂架和障碍物的包围盒模型，并通过确定两个包围盒模型之间的距离的方式确定臂架与障碍物的距离。但是，障碍物为不规则形状，通常为障碍物建立的球形包围盒模型与实际障碍物实体区别较大，即球形包围盒紧致性较差，导致确定的距离与实际距离相差较大。若同时建立臂架和障碍物紧致性较高的包围盒模型，计算量过大，难以实现实时确定臂架与障碍物之间的距离。

技术实现思路

[0004]本申请实施例的目的是提供一种确定工程车辆臂架与障碍物距离的方法、处理器、系统、工程车辆及存储介质。
[0005]为了实现上述目的，本申请第一方面提供一种确定工程车辆臂架与障碍物距离的方法，工程车辆包括多个可旋转臂架，激光雷达装置安装在任意一个臂架上，方法包括：
[0006]建立第一坐标系，其中，第一坐标系是指激光雷达装置以其采集点位置为原点建立的平面直角坐标系；
[0007]实时获取每个臂架在第一坐标系上的多个第一点云坐标和每个臂架的实时倾角；
[0008]根据每个臂架的多个第一点云坐标确定每个臂架的转轴中心并建立多个第二坐标系，其中，第二坐标系是以每个臂架的转轴中心为原点建立的平面直角坐标系，针对每个臂架对应的第二坐标系，臂架的延伸方向为第二坐标系X轴的正方向，与臂架底平面垂直向上的方向为第二坐标系Z轴的正方向，Y轴的正方向基于右手定则确定；
[0009]根据每个臂架的转轴中心和实时倾角确定第一坐标系与每个臂架的第二坐标系之间的转换矩阵；
[0010]针对每个臂架，根据转换矩阵确定臂架的每个第一点云坐标在臂架的第二坐标系中对应的第二点云坐标；
[0011]根据每个臂架的外形参数和其对应的第二点云坐标确定每个臂架的包围盒模型；
[0012]在工程车辆的臂架周围存在障碍物的情况下，获取每个臂架以及障碍物在第一坐标系上的多个第三点云坐标；
[0013]根据转换矩阵确定每个第三点云坐标在每个第二坐标系中对应的多个第四点云坐标；
[0014]根据包围盒模型与多个第四点云坐标确定每个臂架与障碍物之间的距离。
[0015]在本申请实施例中，根据包围盒模型与多个第四点云坐标确定每个臂架与障碍物之间的距离包括：在获取多个第四点云坐标之后，剔除第四点云坐标中与包围盒模型重合的点云坐标，以得到障碍物的第五点云坐标；确定第五点云坐标投射于二维有序坐标空间中所对应的第六点云坐标；针对每个包围盒模型，确定包围盒模型在二维有序坐标空间中对应的二维包围盒模型；根据二维包围盒模型与第六点云坐标确定每个臂架与障碍物之间的距离。
[0016]在本申请实施例中，根据二维包围盒模型与第六点云坐标确定每个臂架与障碍物之间的距离包括：在任意一个第六点云坐标对应的位置与二维包围盒模型之间的距离为零的情况下，确定包围盒模型对应的臂架与障碍物发生了碰撞；在确定臂架与障碍物未发生碰撞的情况下，控制二维包围盒模型多次移动，以确定二维包围盒模型在每个方向与第六点云坐标之间的距离，进而确定每个臂架与障碍物之间的距离。
[0017]在本申请实施例中，在确定臂架与障碍物没有发生碰撞的情况下，控制二维包围盒模型多次移动，以确定二维包围盒模型在每个方向与第六点云坐标之间的距离，进而确定每个臂架与障碍物之间的距离包括：控制二维包围盒模型向预设方向移动预设距离；在二维包围盒模型与任意一个第六点云坐标未重合的情况下，再次向预设方向移动预设距离；在二维包围盒模型与任意一个第六点云坐标重合的情况下，通过二分法控制二维包围盒模型多次移动，直至二维包围盒模型内部与任意一个第六点云坐标未重合，且二维包围盒模型的边界与任意一个或多个第六点云坐标重合；根据多次移动的位移量确定二维包围盒模型在预设方向与第六点云坐标之间的距离；确定二维包围盒模型在所有方向与第六点云坐标之间的距离，以确定每个臂架与障碍物之间的距离。
[0018]在本申请实施例中，激光雷达装置安装在第一臂架上，根据每个臂架的转轴中心和实时倾角确定第一坐标系与每个臂架的第二坐标系之间的转换矩阵包括：确定第一臂架的转轴中心与第二臂架的转轴中心之间的第一平移量，其中，第二臂架为与第一臂架相邻的臂架；根据第一平移量确定分别与第一臂架和第二臂架对应的第二坐标系之间的第一平移矩阵；根据第一臂架与第二臂架的实时倾角确定第一臂架与第二臂架分别对应的第二坐标系之间的第一旋转矩阵；根据第一臂架的实时倾角确定第一臂架对应的第二坐标系与第一坐标系之间的第二旋转矩阵；根据第一平移矩阵、第一旋转矩阵以及第二旋转矩阵确定第一坐标系与第二臂架的第二坐标系之间的第一转换矩阵。
[0019]在本申请实施例中，根据每个臂架的转轴中心和实时倾角确定第一坐标系与每个臂架的第二坐标系之间的转换矩阵还包括：确定第三臂架的转轴中心与上一相邻臂架的转轴中心之间的第二平移量，其中，上一相邻臂架位于第三臂架与第一臂架之间；根据第二平移量确定分别与第三臂架和上一相邻臂架对应的第二坐标系之间的第二平移矩阵；根据第三臂架与上一相邻臂架的实时倾角确定第三臂架与上一相邻臂架分别对应的第二坐标系之间的第三旋转矩阵；根据第二平移矩阵、第三旋转矩阵以及上一相邻臂架与第一臂架之间的转换矩阵确定第一坐标系与第三臂架的第二坐标系之间的第二转换矩阵。
[0020]在本申请实施例中，激光雷达装置安装在第一臂架上，根据每个臂架的多个第一点云坐标确定每个臂架的转轴中心并建立多个第二坐标系包括：针对除第一臂架的目标臂架，控制目标臂架多次旋转；在每次旋转之后，通过激光雷达装置采集目标臂架不同倾角对应的点云图；确定每个点云图在上一个臂架的第二坐标系的X
‑
Z坐标平面的多个二维点云
图，其中，上一个臂架为第一臂架或位于目标臂架与第一臂架之间的臂架；通过霍夫直线变换确定多个二维点云图对应的多个直线方程；根据直线方程确定多个二维点云图形成的公共交点，以确定为目标臂架的转轴中心；建立目标臂架对应的第二坐标系；控制目标臂架的下一个臂架多次旋转；根据下一个臂架在目标臂架对应的第二坐标系的X
‑
Z坐标平面的多个二维点云图确定下一个臂架的转轴中心。
[0021]在本申请实施例中，确定多个二维点云图形成的公共交点，以确定为目标臂架的转轴中心包括：确定多个二维点云图形成的公共交点；对公共交点附近预设范围内遍历，以确定目标臂架的转轴中心。
[0022]本申请第二方面提供一种处理器，被配置为执行为执行上述的确定工程车辆臂架与障碍物距离的方法。
[0023]本申请第三方面提供一种确定工程车辆臂架与障碍物距离的本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种确定工程车辆臂架与障碍物距离的方法，其特征在于，所述工程车辆包括多个可旋转臂架，激光雷达装置安装在任意一个臂架上，所述方法包括：建立第一坐标系，其中，所述第一坐标系是指所述激光雷达装置以其采集点位置为原点建立的平面直角坐标系；实时获取每个臂架在所述第一坐标系上的多个第一点云坐标和每个臂架的实时倾角；根据每个臂架的多个第一点云坐标确定每个臂架的转轴中心并建立多个第二坐标系，其中，所述第二坐标系是以每个臂架的转轴中心为原点建立的平面直角坐标系，针对每个臂架对应的第二坐标系，所述臂架的延伸方向为所述第二坐标系X轴的正方向，与所述臂架底平面垂直向上的方向为所述第二坐标系Z轴的正方向，Y轴的正方向基于右手定则确定；根据每个臂架的转轴中心和所述实时倾角确定所述第一坐标系与每个臂架的第二坐标系之间的转换矩阵；针对每个臂架，根据所述转换矩阵确定所述臂架的每个第一点云坐标在所述臂架的第二坐标系中对应的第二点云坐标；根据每个臂架的外形参数和其对应的第二点云坐标确定每个臂架的包围盒模型；在所述工程车辆的臂架周围存在障碍物的情况下，获取每个臂架以及所述障碍物在所述第一坐标系上的多个第三点云坐标；根据所述转换矩阵确定每个第三点云坐标在每个第二坐标系中对应的多个第四点云坐标；根据所述包围盒模型与所述多个第四点云坐标确定每个臂架与所述障碍物之间的距离。2.根据权利要求1所述的确定工程车辆臂架与障碍物距离的方法，其特征在于，所述根据所述包围盒模型与所述多个第四点云坐标确定所述每个臂架与所述障碍物之间的距离包括：在获取所述多个第四点云坐标之后，剔除所述第四点云坐标中与所述包围盒模型重合的点云坐标，以得到所述障碍物的第五点云坐标；确定所述第五点云坐标投射于二维有序坐标空间中所对应的第六点云坐标；针对每个包围盒模型，确定所述包围盒模型在所述二维有序坐标空间中对应的二维包围盒模型；根据所述二维包围盒模型与所述第六点云坐标确定每个臂架与所述障碍物之间的距离。3.根据权利要求2所述的确定工程车辆臂架与障碍物距离的方法，其特征在于，所述根据所述二维包围盒模型与所述第六点云坐标确定每个臂架与所述障碍物之间的距离包括：在任意一个第六点云坐标对应的位置与所述二维包围盒模型之间的距离为零的情况下，确定所述包围盒模型对应的臂架与所述障碍物发生了碰撞；在确定所述臂架与所述障碍物未发生碰撞的情况下，控制所述二维包围盒模型多次移动，以确定所述二维包围盒模型在每个方向与所述第六点云坐标之间的距离，进而确定每个臂架与所述障碍物之间的距离。4.根据权利要求3所述的确定工程车辆臂架与障碍物距离的方法，其特征在于，所述在确定所述臂架与所述障碍物没有发生碰撞的情况下，控制所述二维包围盒模型多次移动，
以确定所述二维包围盒模型在每个方向与所述第六点云坐标之间的距离，进而确定每个臂架与所述障碍物之间的距离包括：控制所述二维包围盒模型向预设方向移动预设距离；在所述二维包围盒模型与任意一个第六点云坐标未重合的情况下，再次向所述预设方向移动所述预设距离；在所述二维包围盒模型与任意一个第六点云坐标重合的情况下，通过二分法控制所述二维包围盒模型多次移动，直至所述二维包围盒模型内部与任意一个第六点云坐标未重合，且所述二维包围盒模型的边界与所述任意一个或多个第六点云坐标重合；根据多次移动的位移量确定所述二维包围盒模型在预设方向与所述第六点云坐标之间的距离；确定所述二维包围盒模型在所有方向与所述第六点云坐标之间的距离，以确定每个臂架与所述障碍物之间的距离。5.根据权利要求1所述的确定工程车辆臂架与障碍物距离的方法，其特征在于，所述激光雷达装置安装在第一臂架上，所述根据每个臂架的转...

【专利技术属性】
技术研发人员：丰生日，李淇阳，徐翔，尹君，聂一彪，
申请(专利权)人：中联重科股份有限公司，
类型：发明
国别省市：