铲斗坐标标定方法和装置、更新方法和设备、挖掘机

技术领域

本公开涉及工程机械智能化领域，特别涉及一种铲斗坐标标定方法和装置、更新方法和设备、挖掘机。

背景技术

相关技术挖掘机散状物料挖掘作业工况而言，希望通过感知设备来检测料堆大小及目标挖掘点位置，并将目标位置告知挖掘机，实现挖掘机无人化的自动挖掘作业。

发明内容

发明人通过研究发现：在挖掘机散状物料挖掘作业现场，通过激光雷达来检测料堆大小及目标挖掘点位置，其中激光雷达安装在挖掘机外侧。通过角度传感器来检测铲斗的位置，其中角度传感器安装在挖掘机上。通过铲斗轨迹规划和控制实现向目标挖掘点的自动挖掘作业功能。在这种场景下，要想实现精准挖掘，目标挖掘点的测量必须准确，而精确测量的前提是坐标标定，即将铲斗、目标挖掘点在雷达坐标系下的坐标准确标定到挖掘机坐标系下，实现铲斗坐标与目标挖掘点的坐标在挖掘机坐标系下的统一。

相关技术坐标标定常用的方法有：直接测量法、人工取点法、场景特征法等。相关技术这些方法还存在以下不足：直接测量法，标定精度低，直接测量和人工取点往往存在各种误差，包括人为操作带来的误差。

鉴于以上技术问题中的至少一项，本公开提供了一种铲斗坐标标定方法和装置、更新方法和设备、挖掘机，可以利用激光雷达和挖掘机角度传感器，在不增加外部标定设备的情况下对铲斗坐标进行标定。

根据本公开的一个方面，提供一种铲斗坐标标定方法，包括：

获取铲斗的雷达点云数据和角度传感器数据；

根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标；

根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标；

根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵，其中，所述坐标标定矩阵为铲斗中间斗齿在雷达系下的坐标标定到挖掘机坐标系的坐标标定矩阵。

在本公开的一些实施例中，所述获取铲斗的雷达点云数据和角度传感器数据包括：在铲斗处于多个不同位置下，采集铲斗的雷达点云数据和角度传感器数据。

在本公开的一些实施例中，所述根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标包括：根据铲斗在每个空间位置采集的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，所述根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标包括：根据铲斗在每个空间位置采集的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，所述根据铲斗在每个空间位置采集的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标包括：根据铲斗在每个空间位置采集的雷达点云数据，基于隐式形状模型算法，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，所述根据铲斗在每个空间位置采集的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标包括：根据铲斗在每个空间位置采集的角度传感器数据，求得挖机装置运动学正解，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，所述根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵包括：

根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标构建雷达坐标系坐标和挖掘机坐标系坐标的数据对，并将多个数据对划分为训练集和测试集；

根据训练集数据，确定坐标标定矩阵；

使用测试集数验证坐标标定矩阵。

在本公开的一些实施例中，所述坐标标定矩阵为坐标旋转平移变换矩阵。

在本公开的一些实施例中，所述根据训练集数据，确定坐标标定矩阵包括：

初始化相关参数，其中，所述相关参数包括迭代次数；

随机选择预定数量的第一数据对；

判断第一数据对是否共线；

在第一数据对不共线的情况下，采用直接线性变换确定坐标标定矩阵。

在本公开的一些实施例中，所述根据训练集数据，确定坐标标定矩阵还包括：

采用坐标标定矩阵，将第二数据对中的雷达坐标系坐标变换得到挖掘机坐标系坐标，其中，第二数据对为训练集中除第一数据对外的其它数据对；

计算变换得到的挖掘机坐标系坐标与实际挖掘机坐标系坐标的距离偏差；

判断距离偏差是否小于预定距离阈值；

根据迭代次数、预定距离阈值判断，记录符合条件的内点，更新坐标标定矩阵；

计算内点概率并根据内点概率更新迭代次数。

根据本公开的另一方面，提供一种坐标标定更新方法，包括：

判断坐标标定矩阵的在线误差是否大于预定许用误差；

若坐标标定矩阵的在线误差大于预定许用误差，则判断采集的位置点数据对数量是否达到预定位置点数量；

在采集的位置点数据对数量等于预定位置点数量的情况下，采用如上述任一实施例所述的铲斗坐标标定方法确定新的坐标标定矩阵；

对坐标标定矩阵进行更新。

在本公开的一些实施例中，所述坐标标定更新方法还包括：

在采集的位置点数据对数量小于预定位置点数量的情况下，采集铲斗雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的1个坐标；

采集铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的1个坐标；

进行位置点数据对数量累计，之后再次执行判断采集的位置点数据对数量是否达到预定位置点数量的步骤。

在本公开的一些实施例中，所述确定铲斗中间斗齿在雷达坐标系下的1个坐标包括：基于隐式形状模型算法，得到铲斗中间斗齿在雷达坐标系下的1个坐标；判断模型相似度是否大于预定相似度；在模型相似度大于预定相似度的情况下，使用所述铲斗中间斗齿在雷达坐标系下的1个坐标。

在本公开的一些实施例中，所述确定铲斗中间斗齿在挖掘机坐标系下的1个坐标包括：基于角度传感器数据求得挖机装置运动学正解，确定铲斗中间斗齿在挖掘机坐标系下的1个坐标。

根据本公开的另一方面，提供一种铲斗坐标标定装置，包括：

数据获取模块，被配置为获取铲斗的雷达点云数据和角度传感器数据；

定位模块，被配置为根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标；根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标；

标定模块，被配置为根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵，其中，所述坐标标定矩阵为铲斗中间斗齿在雷达系下的坐标标定到挖掘机坐标系的坐标标定矩阵。

在本公开的一些实施例中，所述铲斗坐标标定装置用于执行实现如上述任一实施例所述的铲斗坐标标定方法的操作。

根据本公开的另一方面，提供一种坐标标定更新设备，包括：

判断装置，被配置为判断坐标标定矩阵的在线误差是否大于预定许用误差；在坐标标定矩阵的在线误差大于预定许用误差的情况下，判断采集的位置点数据对数量是否达到预定位置点数量；

铲斗坐标标定装置，被配置为在采集的位置点数据对数量等于预定位置点数量的情况下，采用铲斗坐标标定方法确定新的坐标标定矩阵；

更新装置，被配置为对坐标标定矩阵进行更新。

在本公开的一些实施例中，所述铲斗坐标标定装置为如上述任一实施例所述的铲斗坐标标定装置。

在本公开的一些实施例中，所述坐标标定更新设备用于执行实现如上述任一实施例所述的坐标标定更新方法的操作。

根据本公开的另一方面，提供一种计算机装置，包括：

存储器，用于存储指令；

处理器，用于执行所述指令，使得所述计算机装置执行实现如权上述任一实施例所述的铲斗坐标标定方法，和/或执行实现如上述任一实施例所述的坐标标定更新方法的操作。

根据本公开的另一方面，提供一种标定系统，包括激光雷达和角度传感器，还包括计算机装置、坐标标定更新设备和铲斗坐标标定装置中的至少一项，其中，所述计算机装置为如上述任一实施例所述的计算机装置，所述坐标标定更新设备为如上述任一实施例所述的坐标标定更新设备，所述铲斗坐标标定装置为如上述任一实施例所述的铲斗坐标标定装置。

根据本公开的另一方面，提供一种挖掘机，包括激光雷达，还包括计算机装置、坐标标定更新设备和铲斗坐标标定装置中的至少一项，其中，所述计算机装置为如上述任一实施例所述的计算机装置，所述坐标标定更新设备为如上述任一实施例所述的坐标标定更新设备，所述铲斗坐标标定装置为如上述任一实施例所述的铲斗坐标标定装置。

根据本公开的另一方面，提供一种计算机可读存储介质，其中，所述计算机可读存储介质存储有计算机指令，所述指令被处理器执行时实现如上述任一实施例所述的铲斗坐标标定方法，和/或实现如上述任一实施例所述的坐标标定更新方法的操作。

本公开可以利用激光雷达和挖掘机角度传感器，在不增加外部标定设备的情况下对铲斗坐标进行标定。

附图说明

为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本公开的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本公开一些实施例挖掘机散状物料的挖掘作业工况的示意图。

图2为本公开铲斗坐标标定方法一些实施例的示意图。

图3为本公开铲斗坐标标定方法另一些实施例的示意图。

图4为本公开坐标标定更新方法一些实施例的示意图。

图5为本公开坐标标定更新方法另一些实施例的示意图。

图6为本公开铲斗坐标标定装置一些实施例的示意图。

图7为本公开坐标标定更新设备一些实施例的示意图。

图8为本公开计算机装置一些实施例的结构示意图。

具体实施方式

下面将结合本公开实施例中的附图，对本公开实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本公开及其应用或使用的任何限制。基于本公开中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。

除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。

同时，应当明白，为了便于描述，附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为授权说明书的一部分。

在这里示出和讨论的所有示例中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它示例可以具有不同的值。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

发明人通过研究发现：相关技术直接测量法、人工取点法、场景特征法还存在以下不足：

1)人工取点法，费时耗力，直接测试和人工取点都需要人为干预，如果对大量的设备进行标定，则会带来大量的工作量，难以实现智能系统的量产化。

2)场景特征法，需要为传感器的标定设计特定的场景，无法实现对传感器的在线标定。

3)上述方法均无在作业过程中在线验证标定误差功能，尤其是当挖掘机与雷达的相对位置变化后，无法在线更新标定。

相关技术涉及一种传感器定位系统，能够基于表面数据计算至少一个或多个自动驾驶汽车传感器的位置，但其一，需要多次测量转换；其二，不具有误差验证，在线更新功能。

相关技还公开了一种用于校准安装在机器上的传感器设备的方法，能够实现安装在机器上的传感器装载的校准，但其一，需要采集具有多个特征的地形点云，进行两次配准才能实现对安装在机器上的传感器设备进行校准；其二，当传感器相对位置变化时，不能在线校准。

另一种相关技术能够实现离线标定，但其一，需要特定的包括棋盘标定板和“L”字形联合标定靶；其二，不能在线标定。

另一种相关技术能够实现离线标定，但其一，需要特定的标定版；其二，且标定板上的各特征点在作业机械的下车体坐标系中的第一坐标值的获取，需要通过测量工具如卷尺等进行手动测量，或控制作业机械进行自动测量。但手动测量麻烦而且很难测准，如何自动测量未做说明；其三，不能在线标定。

鉴于以上技术问题中的至少一项，本公开提供了一种铲斗坐标标定方法和装置、更新方法和设备、挖掘机，下面通过具体实施例对本公开进行说明。

图1为本公开一些实施例挖掘机散状物料的挖掘作业工况的示意图。如图1所示的作业场景包括挖掘机、待挖掘物料、激光雷达感知设备、运输车(作为卸料点，图中未给出)等组成。挖掘机停靠在待挖掘物料附近，其挖掘作业半径能够覆盖物料区域，在另一个场景中，当物料总量或位置随着挖掘作业施工变化时，挖掘机可以随着物料的位置变化而移动。

在本公开的一些实施例中，激光雷达可以为激光雷达感知设备。

在本公开的一些实施例中，如图1所示，激光雷达感知设备架设在待挖掘物料外侧，用于采集待挖掘物料的点云、以便得到适合的目标挖掘点；同时用于采集铲斗的点云，当铲斗中间斗齿露出在物料外面时，能够采集到含有铲斗中间斗齿的点云，通过隐式形状模型算法得到铲斗中间斗齿在雷达坐标系下的坐标PL。

在本公开的一些实施例中，如图1所示，挖掘机上安装有角度传感器，用于采集挖掘机的角度信息，通过运动学正解算法，计算得到铲斗中间斗齿在挖掘机坐标系下的坐标PW。

要想实现精准挖掘，必须进行坐标标定，本公开上述实施例将铲斗中间斗齿在雷达系下的坐标标定到挖掘机坐标系下，实现铲斗中间斗齿坐标与目标挖掘点的坐标在挖掘机坐标系下的统一。

图2为本公开铲斗坐标标定方法一些实施例的示意图。优选的，本实施例可由本公开铲斗坐标标定装置或本公开标定系统或本公开计算机装置或本公开坐标标定更新设备执行。图2实施例的方法可以包括步骤21至步骤24中的至少一个步骤，其中：

步骤21，获取铲斗的雷达点云数据和角度传感器数据。

在本公开的一些实施例中，步骤21可以包括：在铲斗处于多个不同位置下，采集铲斗的雷达点云数据和角度传感器数据。

步骤22，根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，步骤22可以包括：根据铲斗在每个空间位置采集的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，步骤22可以包括：根据铲斗在每个空间位置采集的雷达点云数据，基于隐式形状模型算法，确定铲斗中间斗齿在雷达坐标系下的坐标。

步骤23，根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，步骤23可以包括：根据铲斗在每个空间位置采集的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，步骤23可以包括：根据铲斗在每个空间位置采集的角度传感器数据，求得挖机装置运动学正解，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

步骤24，根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵，其中，所述坐标标定矩阵为铲斗中间斗齿在雷达系下的坐标标定到挖掘机坐标系的坐标标定矩阵。

在本公开的一些实施例中，所述坐标标定矩阵为坐标旋转平移变换矩阵。

在本公开的一些实施例中，步骤24可以包括步骤241-步骤243中的至少一个步骤，其中：

步骤241，根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标构建雷达坐标系坐标和挖掘机坐标系坐标的数据对，并将多个数据对划分为训练集和测试集。

步骤242，根据训练集数据，确定坐标标定矩阵。

步骤243，使用测试集数验证坐标标定矩阵。

图3为本公开铲斗坐标标定方法另一些实施例的示意图。优选的，本实施例可由本公开铲斗坐标标定装置或本公开标定系统或本公开计算机装置或本公开坐标标定更新设备执行。图3实施例的方法可以包括步骤31至步骤36中的至少一个步骤，其中：

步骤31:在N个不同位置下，采集铲斗的雷达点云数据和角度传感器数据。

在本公开的一些实施例中，N个不同位置，指的是铲斗相对于挖掘机坐标系，运动到N个不同的空间位置，N在程序中设定，其值至少大于4。铲斗雷达点云数据是基于雷达坐标系采集的数据。角度传感器数据是基于挖掘机坐标系采集的数据。

在本公开的一些实施例中，在采集数据时，可通过系统检测、控制使得N个不同的空间位置具有较大的差异性，比如按不同的回转间隔、不同的铲斗姿态、不同的臂架角度，来进行采集，避免采集位置过于集中，产生数据相关性现象，从而导致后续步骤中进行坐标标定矩阵计算时无法收敛的问题。

在本公开的一些实施例中，在采集数据时，可通过系统检测、控制使得在N个不同的空间位置时，铲斗在雷达的视角范围内，使得雷达能够采集到铲斗尽可能多的点云，保障后续采用隐式模式算法的配置精度，提高相似度。

在本公开的一些实施例中，在采集数据，进行一次校准的过程中，上车回转系统、挖掘机臂、铲斗可以自由运动，但应保持挖掘机下车与雷达的位置相对固定，否则因挖掘机和雷达相对位置变化，可能会导致标定不准确；当然，系统也可配置为，当检测相对位置变化时，重新标定。

步骤32，基于隐式形状模型算法，计算得到的N个铲斗中间斗齿在雷达坐标系下的坐标PL。

在本公开的一些实施例中，步骤32可以包括：针对铲斗在每个空间位置采集的铲斗雷达点云，基于隐式形状模型算法，计算得到铲斗中间斗齿在雷达坐标系下的坐标PL。

步骤33:基于角度传感器运动学正解，计算得到对应的N个铲斗中间斗齿在挖掘机坐标系下的坐标PW；

在本公开的一些实施例中，步骤33可以包括：针对铲斗在每个空间位置采集的角度传感器数据，基于运动学正解，得到铲斗中间斗齿在挖掘机坐标系下的坐标PW。

步骤34:构建(PL,PW)数据对，并随机划分为训练集和测试集。

在本公开的一些实施例中，步骤34可以包括：将上述步骤得到的铲斗中间斗齿坐标PL、PW按对应的位置一一对应，构建(PL,PW)数据对，并随机划分训练集和测试集，划分比例可根据累计采集的点的数量设定。

步骤35:对训练集数据使用RANSAC(RAndom SAmple Consensus，随机采样一致)估计算法得到坐标标定矩阵R|T。

在本公开的一些实施例中，步骤35可以包括：根据坐标标定模型PW＝R*PL+T，对训练集数据使用RANSAC估计算法，计算得到坐标标定矩阵R|T。本公开使用RANSAC估计算法可以避免噪声数据的干扰，估计得到R|T准确可靠。

在本公开的一些实施例中，坐标标定的目的，就是找到合适的R|T,将铲斗中间斗齿在雷达坐标系下的坐标变换到挖掘机坐标系下，并计算与对应点的误差，期望所有数据对的均方误差最小，即求公式(1)的最小值。

发明人通过研究发现：相关技术使用最小二乘法，DLT(Direct LinearTransformation，直接线性变换)对矩阵直接进行计算，但是该方法有时会因矩阵的数值计算、点对应不准确、尤其是存在离群点等原因，导致计算结果不稳定。

本公开方案的上述步骤中使用隐式形状模型算法根据铲斗点云来计算铲斗坐标，底层是利用实际铲斗点云与模型点云配准的方式，现场采集的铲斗点云与模型典型存在配准相似度，相似度越高，计算的铲斗坐标准确度越高，即铲斗坐标的准确度受模型相似度的影响，因此本方案使用RANSAC随机采样估计算法求解坐标标定矩阵R|T。

在本公开的一些实施例中，图2实施例的步骤242或图3实施例的步骤35可以包括步骤351至步骤359中的至少一个步骤，其中：

步骤351，初始化相关参数，其中，所述相关参数包括迭代次数、阈值、最大内点数、内点概率等参数。

步骤352至步骤359为迭代计算。

步骤352，随机选择预定数量的第一数据对，其中，所述数据对为点对。

在本公开的一些实施例中，预定数量可以为4。

在本公开的一些实施例中，步骤352可以包括：随机选择4个点对。

步骤353，判断第一数据对是否共线。如过共线，则返回步骤351。

步骤354，在第一数据对不共线的情况下，采用直接线性变换确定坐标标定矩阵，即，使用DLT计算R|Ti。

步骤355，采用坐标标定矩阵R|Ti，将第二数据对中的雷达坐标系坐标PLi变换得到挖掘机坐标系坐标PWi，其中，第二数据对为训练集中除第一数据对外的其它数据对。

步骤356，计算变换得到的挖掘机坐标系坐标与实际挖掘机坐标系坐标的距离偏差。

步骤357，判断距离偏差是否小于预定距离阈值。

步骤358，根据迭代次数、预定距离阈值判断，记录符合条件的内点，更新坐标标定矩阵R|T。

步骤359，计算内点概率并根据内点概率更新迭代次数。

经验证，本公开使用RANSAC估计算法，能够鲁棒的估计模型参数，能从包含大量局外点的数据集中估计出高精度的参数。

步骤36:使用测试集数验证坐标标定矩阵R|T。

本公开使用测试集数验证坐标标定矩阵R|T，如果误差满足使用要求，则完成标定。后续挖掘机作业过程，使用R|T将PW变换到雷达坐标系下，进行目标点挖掘和卸料点卸料。

下面通过具体实施例对本公开铲斗坐标标定方法进行说明。

具体实施例1：

经过现场试验，采集N＝8个不同位置的PL,PW；其中，

通过点云计算得到的铲斗中间斗齿在雷达坐标系下的坐标，数据如下：

PL＝{3.233 2.986 3.233 3.634 3.972 2.986 2.789 2.651

-0.415 -0.738 -0.888 -0.367 -0.662 -0.23 -0.52 -0.696

0.057 -0.204 -0.38 -0.058 -0.266 -0.322 -0.074 0.253}

通过角度传感器运动学正解，得到铲斗中间斗齿在挖机坐标系下的坐标：

PW＝{4.27 4.20 3.94 4.04 3.60 4.6 4.53 4.43//x

0.74 1.13 1.07 0.50 0.45 0.80 1.12 1.33//y

0.36 0.11 -0.07 0.24 0.01 0.01 0.24 0.58}//z

进行数据集划分，划分比例6:4,得到训练集,如下：

PL_train＝{3.233 2.986 3.233 3.634 3.972

-0.415 -0.738 -0.888 -0.367 -0.662

0.057 -0.204 -0.38 -0.058 -0.266}

PW_train＝{4.27 4.20 3.94 4.04 3.603

0.74 1.13 1.07 0.50 0.45 0.80

0.36 0.11 -0.07 0.24 0.01}

测试集如下：

PL_test＝{2.986 2.789 2.651

-0.23 -0.52 -0.696

-0.322 -0.074 0.253}

PW_test＝{4.6 4.53 4.43

0.80 1.12 1.33

0.01 0.24 0.58}

使用RANSAC估计算法坐标标定矩阵，得到R|T，其中：

R＝{-0.7098 0.7033-0.0408

-0.7028-0.7109-0.0273

-0.0482 0.0093 0.9988}

T＝{6.8549

2.7214

0.4704}

使用测试进行验证，res＝PW_test–(PL_test*R+T)，得到偏差矩阵，可以出每个点在x，y，z方向上的误差大小res，均方误差RE2，如下：

res＝{-0.0131 -0.0173 0.0435

-0.0048 0.01310.0162

-0.0073 0.01720.0088}

RE2＝0.0189

最大误差为4.35cm，均方误差1.89cm。满足散装物料挖掘作业需求。

图4为本公开坐标标定更新方法一些实施例的示意图。优选的，本实施例可由或本公开标定系统或本公开计算机装置或本公开坐标标定更新设备执行。图4实施例的方法可以包括步骤41至步骤44中的至少一个步骤，其中：

步骤41，判断坐标标定矩阵的在线误差是否大于预定许用误差。

步骤42，若坐标标定矩阵的在线误差大于预定许用误差，则判断采集的位置点数据对数量是否达到预定位置点数量。

步骤43，在采集的位置点数据对数量等于预定位置点数量的情况下，采用如上述任一实施例(例如图2或图3实施例)所述的铲斗坐标标定方法确定新的坐标标定矩阵。

步骤44，对坐标标定矩阵进行更新。

图5为本公开坐标标定更新方法另一些实施例的示意图。优选的，本实施例可由或本公开标定系统或本公开计算机装置或本公开坐标标定更新设备执行。图5实施例的方法可以包括步骤S1至步骤S9中的至少一个步骤，其中：

步骤S1:设置自动标定参数：位置点数N，预定相似度A，划分比例B，预定许用误差C。

在本公开的一些实施例中，位置点数N(位置点数量N)，指的是铲斗相对于挖掘机坐标系，运动到N个不同的空间位置，N在程序中设定，其值至少大于4。

在本公开的一些实施例中，预定相似度A，指的是隐式形状模型算法铲斗点云与模型铲斗点云的配准相似程度。

在本公开的一些实施例中，划分比例B，指的是(PL,PW)训练集与测试集的划分比例。

在本公开的一些实施例中，预定许用误差C，指的是满足使用要求的误差。所述自动标定参数可通过配置文件设定。

步骤S2:判断在线误差是否大于设定的预定许用误差C。

在本公开的一些实施例中，在线误差通过PW-R*PL+T公式计算。系统初次使用时、或者挖掘机与雷达的相对位置变化时，在线误差会大于设定的许用误差，这时会进入自动标定程序，进入步骤S3，标定成功后，会直接退出标定程序。

步骤S3:判断采集的点对数(位置点数据对数量)是否小于设定的位置点数量N。在采集的位置点数据对数量小于预定位置点数量的情况下，执行步骤S4；在采集的位置点数据对数量等于预定位置点数量的情况下，执行步骤S8。

步骤S4:采集铲斗雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的1个坐标。

在本公开的一些实施例中，步骤S4可以包括：采集铲斗雷达点云数据，基于隐式形状模型算法，得到1个铲斗中间斗齿坐标PL1。

在本公开的一些实施例中，采集数据时，根据铲斗相对于挖掘机的不同位置，铲斗在雷达的合适视觉角范围内进行采集，以便采集到合理的铲斗点云，增加模型配准成功率，提高模型相似度。

步骤S5:判断模型相似度是否大于预定设定值A。

在本公开的一些实施例中，若模型相似对大于预定设定值A，则接受基于隐式形状模型算法得到1个铲斗中间斗齿坐标PL1，并进入步骤S6，获取该位置铲斗中间斗齿在挖掘机坐标系下的坐标PW1。

步骤S6:采集铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的1个坐标。

在本公开的一些实施例中，步骤S6可以包括：采集铲斗的角度传感器数据，基于角度传感器运动学正解，计算1个铲斗中间斗齿坐标PW1。

步骤S7:位置点数累计。

在本公开的一些实施例中，步骤S7可以包括：统计配对成功的(PL1,PW1)点对数量。当数据等于N时进入步骤S8。

步骤S8:使用标定算法标定坐标标定矩阵R|T。

在本公开的一些实施例中，步骤S8可以包括：在采集的位置点数据对数量等于预定位置点数量的情况下，采用如上述任一实施例(例如图2或图3实施例)所述的铲斗坐标标定方法确定新的坐标标定矩阵。

步骤S9:自动更新坐标标定矩阵R|T。

本公开标定成功后，可自动更新R|T或提醒操作手来决定是否使用。

针对直接测量法、人工取点法、场景特征法等标定方法存在的标定精度低、费时耗力、需要特殊场景或专用标定设备、无在线验证，不能在线标定的问题，本公开提供一种基于激光雷达和角度传感器的坐标标定方法及自动更新方法：

其一，本公开不需要增加任何标定设备在现有系统上即可完成标定。

其二，本公开使用模型相识度过滤，采用RANSAC估计算法提高了标定精度。

其三，本公开对数据集随机划分，提供了在线验证功能。

其四，本公开能够在挖掘机与雷达的相对位置变化时，以及坐标标定矩阵产生漂移误差时，进行自动标定更新。

图6为本公开铲斗坐标标定装置一些实施例的示意图。如图6所示，本公开铲斗坐标标定装置可以包括数据获取模块61、定位模块62和标定模块63，其中：

数据获取模块61，被配置为获取铲斗的雷达点云数据和角度传感器数据。

在本公开的一些实施例中，数据获取模块61，被配置为在铲斗处于多个不同位置下，采集铲斗的雷达点云数据和角度传感器数据。

定位模块62，被配置为根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标；根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，定位模块62在根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标的情况下，被配置为根据铲斗在每个空间位置采集的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，在本公开的一些实施例中，定位模块62在根据铲斗的雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的坐标的情况下，根据铲斗在每个空间位置采集的雷达点云数据，基于隐式形状模型算法，确定铲斗中间斗齿在雷达坐标系下的坐标。

在本公开的一些实施例中，定位模块62在根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标的情况下，被配置为根据铲斗在每个空间位置采集的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

在本公开的一些实施例中，定位模块62在根据铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的坐标的情况下，被配置为根据铲斗在每个空间位置采集的角度传感器数据，求得挖机装置运动学正解，确定铲斗中间斗齿在挖掘机坐标系下的坐标。

标定模块63，被配置为根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵，其中，所述坐标标定矩阵为铲斗中间斗齿在雷达系下的坐标标定到挖掘机坐标系的坐标标定矩阵。

在本公开的一些实施例中，所述坐标标定矩阵为坐标旋转平移变换矩阵。

在本公开的一些实施例中，标定模块63，在根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标，确定坐标标定矩阵的情况下，被配置为根据铲斗中间斗齿在雷达坐标系下和在挖掘机坐标系下的坐标构建雷达坐标系坐标和挖掘机坐标系坐标的数据对，并将多个数据对划分为训练集和测试集；根据训练集数据，确定坐标标定矩阵；使用测试集数验证坐标标定矩阵。

在本公开的一些实施例中，标定模块63，在根据训练集数据，确定坐标标定矩阵的情况下，被配置为初始化相关参数，其中，所述相关参数包括迭代次数；随机选择预定数量的第一数据对；判断第一数据对是否共线；在第一数据对不共线的情况下，采用直接线性变换确定坐标标定矩阵；采用坐标标定矩阵，将第二数据对中的雷达坐标系坐标变换得到挖掘机坐标系坐标，其中，第二数据对为训练集中除第一数据对外的其它数据对；计算变换得到的挖掘机坐标系坐标与实际挖掘机坐标系坐标的距离偏差；判断距离偏差是否小于预定距离阈值；根据迭代次数、预定距离阈值判断，记录符合条件的内点，更新坐标标定矩阵；计算内点概率并根据内点概率更新迭代次数。

在本公开的一些实施例中，所述铲斗坐标标定装置用于执行实现如上述任一实施例(例如图2或图3实施例)所述的铲斗坐标标定方法的操作。

图7为本公开坐标标定更新设备一些实施例的示意图。如图7所示，本公开坐标标定更新设备可以包括判断装置71、铲斗坐标标定装置72和更新装置73，其中：

判断装置71，被配置为判断坐标标定矩阵的在线误差是否大于预定许用误差；在坐标标定矩阵的在线误差大于预定许用误差的情况下，判断采集的位置点数据对数量是否达到预定位置点数量。

铲斗坐标标定装置72，被配置为在采集的位置点数据对数量等于预定位置点数量的情况下，采用铲斗坐标标定方法确定新的坐标标定矩阵。

更新装置73，被配置为对坐标标定矩阵进行更新。

在本公开的一些实施例中，所述铲斗坐标标定装置为如上述任一实施例(例如图6实施例)所述的铲斗坐标标定装置。

在本公开的一些实施例中，判断装置71，还被配置为在采集的位置点数据对数量小于预定位置点数量的情况下，采集铲斗雷达点云数据，确定铲斗中间斗齿在雷达坐标系下的1个坐标；采集铲斗的角度传感器数据，确定铲斗中间斗齿在挖掘机坐标系下的1个坐标；进行位置点数据对数量累计，之后再次执行判断采集的位置点数据对数量是否达到预定位置点数量的操作。

在本公开的一些实施例中，判断装置71在确定铲斗中间斗齿在雷达坐标系下的1个坐标的情况下，被配置为基于隐式形状模型算法，得到铲斗中间斗齿在雷达坐标系下的1个坐标；判断模型相似度是否大于预定相似度；在模型相似度大于预定相似度的情况下，使用所述铲斗中间斗齿在雷达坐标系下的1个坐标。

在本公开的一些实施例中，判断装置71在确定铲斗中间斗齿在挖掘机坐标系下的1个坐标的情况下，被配置为基于角度传感器数据求得挖机装置运动学正解，确定铲斗中间斗齿在挖掘机坐标系下的1个坐标。

在本公开的一些实施例中，所述坐标标定更新设备用于执行实现如上述任一实施例(例如图4或图5实施例)所述的坐标标定更新方法的操作。

本公开上述实施例提供一种基于激光雷达和角度传感器的坐标标定装置及自动更新设备，利用激光雷达和挖掘机角度传感器，在不增加外部标定设备的情况下对铲斗坐标进行标定及自动更新，实现铲斗坐标与目标挖掘点的坐标在挖掘机坐标系下的统一。

图8为本公开计算机装置一些实施例的结构示意图。如图8所示，计算机装置包括存储器81和处理器82。

存储器81用于存储指令，处理器82耦合到存储器81，处理器82被配置为基于存储器存储的指令执行实现上述实施例(例如图2-图5任一实施例)涉及的方法。

如图8所示，该计算机装置还包括通信接口83，用于与其它设备进行信息交互。同时，该计算机装置还包括总线84，处理器82、通信接口83、以及存储器81通过总线84完成相互间的通信。

存储器81可以包含高速RAM存储器，也可还包括非易失性存储器(non-volatilememory)，例如至少一个磁盘存储器。存储器81也可以是存储器阵列。存储器81还可能被分块，并且块可按一定的规则组合成虚拟卷。

此外，处理器82可以是一个中央处理器CPU，或者可以是专用集成电路ASIC，或是被配置成实施本公开实施例的一个或多个集成电路。

根据本公开的另一方面，如图1所示，提供一种标定系统，包括激光雷达和角度传感器，还包括计算机装置、坐标标定更新设备和铲斗坐标标定装置中的至少一项，其中，所述计算机装置为如上述任一实施例(例如图8实施例)所述的计算机装置，所述坐标标定更新设备为如上述任一实施例(例如图7实施例)所述的坐标标定更新设备，所述铲斗坐标标定装置为如上述任一实施例(例如图6实施例)所述的铲斗坐标标定装置。

根据本公开的另一方面，如图1所示，提供一种挖掘机，包括激光雷达，还包括计算机装置、坐标标定更新设备和铲斗坐标标定装置中的至少一项，其中，所述计算机装置为如上述任一实施例(例如图8实施例)所述的计算机装置，所述坐标标定更新设备为如上述任一实施例(例如图7实施例)所述的坐标标定更新设备，所述铲斗坐标标定装置为如上述任一实施例(例如图6实施例)所述的铲斗坐标标定装置。

本公开提供一种基于激光雷达和角度传感器的坐标标定方法及自动更新系统，利用激光雷达和挖掘机角度传感器，在不增加外部标定设备的情况下对铲斗坐标进行标定及自动更新，可以实现铲斗坐标与目标挖掘点的坐标在挖掘机坐标系下的统一。

根据本公开的另一方面，提供一种计算机可读存储介质，其中，所述计算机可读存储介质存储有计算机指令，所述指令被处理器执行时实现如上述任一实施例(例如图2或图3实施例)所述的铲斗坐标标定方法，和/或实现如上述任一实施例(例如图4或图5实施例)所述的坐标标定更新方法的操作。

在本公开的一些实施例中，所述计算机可读存储介质可以为非瞬时性计算机可读存储介质。

本领域内的技术人员应明白，本公开的实施例可提供为方法、装置、或计算机程序产品。因此，本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本公开是参照根据本公开实施例的方法、设备(系统)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在上面所描述的计算机装置、铲斗坐标标定装置、数据获取模块、定位模块、标定模块、坐标标定更新设备、判断装置、铲斗坐标标定装置和更新装置可以实现为用于执行本申请所描述功能的通用处理器、可编程逻辑控制器(PLC)、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件或者其任意适当组合。

至此，已经详细描述了本公开。为了避免遮蔽本公开的构思，没有描述本领域所公知的一些细节。本领域技术人员根据上面的描述，完全可以明白如何实施这里公开的技术方案。

本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指示相关的硬件完成，所述的程序可以存储于一种非瞬时性计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。

本公开的描述是为了示例和描述起见而给出的，而并不是无遗漏的或者将本公开限于所公开的形式。很多修改和变化对于本领域的普通技术人员而言是显然的。选择和描述实施例是为了更好说明本公开的原理和实际应用，并且使本领域的普通技术人员能够理解本公开从而设计适于特定用途的带有各种修改的各种实施例。