挖掘机铲斗作业轨迹确定方法、装置、设备及存储介质

技术领域

本发明涉及挖掘机控制的技术领域，尤其涉及挖掘机铲斗作业轨迹确定方法、装置、设备及存储介质。

背景技术

随着社会的发展，人们对挖掘机的作业能力和作业效率的要求日益提高，智能化和无人化技术在挖掘机上的运用也越来越广泛。将机器人技术应用到液压挖掘机上，可有效提高挖掘机的作业效率和可靠性，降低人为控制的比重，在作业中辅助人工以提高作业质量，通过智能控制和自动化控制技术将操作人员从人工繁重且重复性高的机械操作任务中解放出来。通过对挖掘机的运动学模型仿真分析，进行合理的轨迹规划，可以使挖掘机在保准一定精度的条件下快速、平稳地完成挖掘任务，提高挖掘机的作业质量和工作效率。

挖掘机铲斗作业轨迹计算是挖掘机轨迹规划的重要步骤之一，现有技术中，对挖掘机铲斗作业姿态和轨迹的研究均是通过倾角传感器直接测量挖掘机的动臂、斗杆和铲斗的姿态角，进而计算铲斗齿尖相对整机全局坐标的位置坐标，最后将多个位置坐标点拟合得到铲斗的作业轨迹。然而，倾角传感器存在不易安装且价格昂贵的问题，导致测量动臂、斗杆以及铲斗姿态角的成本较高，进而导致铲斗作业轨迹计算的成本较高。

发明内容

本发明提供挖掘机铲斗作业轨迹确定方法、装置、设备及存储介质，解决了现有技术对挖掘机铲斗作业轨迹计算的成本较高的问题。

为达到上述目的，本发明采用如下技术方案：

第一方面，本发明提供一种挖掘机铲斗作业轨迹确定方法，该方法包括：

分别以目标挖掘机的车体回转中心、动臂绕所述车体的第一旋转中心、斗杆绕所述动臂的第二旋转中心以及铲斗绕所述斗杆的第三旋转中心为原点对应建立车体坐标系、动臂坐标系、斗杆坐标系以及铲斗坐标系；

根据获取的所述车体坐标系的x轴与所述动臂坐标系的x轴的第一夹角、所述动臂坐标系的x轴与所述斗杆坐标系的x轴的第二夹角、所述斗杆坐标系的x轴与所述铲斗坐标系的x轴的第三夹角、所述第一旋转中心在所述车体坐标系的位置坐标、所述第一旋转中心到所述第二旋转中心的第一距离以及所述第二旋转中心到所述第三旋转中心的第二距离，确定所述铲斗坐标系到所述车体坐标系的总转换矩阵；

根据当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标，以及所述总转换矩阵确定所述当前时刻所述齿尖在所述车体坐标系的位置坐标；

分别根据设置在所述动臂上的第一加速度传感器、设置在所述斗杆上的第二加速度传感器以及设置在所述铲斗上的第三加速度传感器，获取所述当前时刻到下一时刻之间所述动臂、所述斗杆和所述铲斗的加速度数据，并根据所述加速度数据确定所述当前时刻到所述下一时刻所述动臂、所述斗杆和所述铲斗的转动距离；

根据所述动臂、所述斗杆和所述铲斗的转动距离，以及所述动臂加速度传感器到所述第一旋转中心的距离、所述斗杆加速度传感器到所述第二旋转中心的距离、所述铲斗加速度传感器到所述第三旋转中心的距离确定所述下一时刻对应的第一夹角、第二夹角和第三夹角的大小；

根据所述下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于所述总转换矩阵确定所述下一时刻所述齿尖在所述车体坐标系的位置坐标；

根据不同时刻所述齿尖在所述车体坐标系的位置坐标确定所述铲斗的作业轨迹。

在一种可能的实现方式中，所述根据获取的所述车体坐标系的x轴与所述动臂坐标系的x轴的第一夹角、所述动臂坐标系的x轴与所述斗杆坐标系的x轴的第二夹角、所述斗杆坐标系的x轴与所述铲斗坐标系的x轴的第三夹角、所述第一旋转中心在所述车体坐标系的位置坐标、所述第一旋转中心到所述第二旋转中心的第一距离以及所述第二旋转中心到所述第三旋转中心的第二距离，确定所述铲斗坐标系到所述车体坐标系的总转换矩阵，具体包括：

根据所述车体坐标系的x轴与所述动臂坐标系的x轴的第一夹角、以及所述第一旋转中心在所述车体坐标系的位置坐标，确定所述动臂坐标系到所述车体坐标系的第一转换矩阵；

根据所述动臂坐标系的x轴与所述斗杆坐标系的x轴的第二夹角、以及所述第一旋转中心到所述第二旋转中心的第一距离，确定所述斗杆坐标系到所述动臂坐标系的第二转换矩阵；

根据所述斗杆坐标系的x轴与所述铲斗坐标系的x轴的第三夹角、以及所述第二旋转中心到所述第三旋转中心的第二距离，确定所述铲斗坐标系到所述斗杆坐标系的第三转换矩阵；

根据所述第一转换矩阵、所述第二转换矩阵以及所述第三转换矩阵的乘积，确定所述铲斗坐标系到所述车体坐标系的总转换矩阵。

在一种可能的实现方式中，所述加速度数据包括所述当前时刻到所述下一时刻之间多个采样点的动臂加速度数据、斗杆加速度数据以及铲斗加速度数据；所述分别根据设置在所述动臂上的第一加速度传感器、设置在所述斗杆上的第二加速度传感器以及设置在所述铲斗上的第三加速度传感器，获取所述当前时刻到下一时刻之间所述动臂、所述斗杆和所述铲斗的加速度数据，并根据所述加速度数据确定所述当前时刻到所述下一时刻所述动臂、所述斗杆和所述铲斗的转动距离，具体包括：

根据设置在所述动臂上的第一加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的动臂加速度数据；根据设置在所述斗杆上的第二加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的斗杆加速度数据；根据设置在所述铲斗上的第三加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的铲斗加速度数据；

分别对多个所述采样点的所述动臂加速度数据、所述斗杆加速度数据、以及所述铲斗加速度数据进行拟合，得到对应的动臂加速度函数、斗杆加速度函数以及铲斗加速度函数；

分别对所述动臂加速度函数、所述斗杆加速度函数以及所述铲斗加速度函数两次积分，对应得到所述当前时刻到所述下一时刻所述动臂、所述斗杆和所述铲斗的转动距离。

在一种可能的实现方式中，根据所述动臂、所述斗杆和所述铲斗的转动距离，以及所述动臂加速度传感器到所述第一旋转中心的距离、所述斗杆加速度传感器到所述第二旋转中心的距离、所述铲斗加速度传感器到所述第三旋转中心的距离确定所述下一时刻对应的第一夹角、第二夹角和第三夹角的大小，具体包括：

根据所述动臂的转动距离和所述动臂加速度传感器到所述第一旋转中心的距离，确定所述当前时刻到所述下一时刻的第一角度变化量；根据所述斗杆的转动距离和所述斗杆加速度传感器到所述第二旋转中心的距离，确定所述当前时刻到所述下一时刻的第二角度变化量；根据所述铲斗的转动距离和所述铲斗加速度传感器到所述第三旋转中心的距离，确定所述当前时刻到所述下一时刻的第三角度变化量；

根据所述当前时刻的第一夹角、第二夹角和第三夹角的大小，以及所述第一角度变化量、所述第二角度变化量和所述第三角度变化量确定所述下一时刻的第一夹角、第二夹角和第三夹角的大小。

在一种可能的实现方式中，所述根据所述动臂的转动距离和所述动臂加速度传感器到所述第一旋转中心的距离，确定所述当前时刻到所述下一时刻的第一角度变化量，具体为：

根据第一公式确定所述当前时刻到所述下一时刻的第一角度变化量，所述第一公式具体为：

其中，Δ为第一角度变化量，s表示动臂的转动距离，l表示动臂加速度传感器到第一旋转中心的距离。

在一种可能的实现方式中，所述根据所述当前时刻的第一夹角、第二夹角和第三夹角的大小，以及所述第一角度变化量、所述第二角度变化量和所述第三角度变化量确定所述下一时刻的第一夹角、第二夹角和第三夹角的大小，具体包括：

根据所述当前时刻的第一夹角与所述第一角度变化量的差值，确定所述下一时刻的第一夹角的大小；根据所述当前时刻的第二夹角与所述第二角度变化量的差值，确定所述下一时刻的第二夹角的大小；根据所述当前时刻的第三夹角与所述第三角度变化量的差值，确定所述下一时刻的第三夹角的大小。

在一种可能的实现方式中，所述根据当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标，以及所述总转换矩阵确定所述当前时刻所述齿尖在所述车体坐标系的位置坐标，具体为：

根据所述总转换矩阵确定所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标与当前时刻所述齿尖在所述车体坐标系的位置坐标的第一转换关系；

所述第一转换关系具体为：

其中，

其中，θ表示所述当前时刻的第一夹角，α表示所述当前时刻的第二夹角，β表示所述当前时刻的第三夹角；(a

将所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标带入所述第一转换关系中，计算得到所述当前时刻所述齿尖在所述车体坐标系的位置坐标。

在一种可能的实现方式中，所述根据所述下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于所述总转换矩阵确定所述下一时刻所述齿尖在所述车体坐标系的位置坐标，具体为：

将所述第一转换关系中所述当前时刻的第一夹角、第二夹角和第三夹角替换为所述下一时刻的第一夹角、第二夹角和第三夹角，得到第二转换关系；

将所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标带入所述第二转换关系中，计算得到所述下一时刻所述齿尖在所述车体坐标系的位置坐标。

第二方面，本发明提供一种挖掘机铲斗作业轨迹确定装置，该装置包括：

模型处理模块，用于分别以目标挖掘机的车体回转中心、动臂绕所述车体的第一旋转中心、斗杆绕所述动臂的第二旋转中心以及铲斗绕所述斗杆的第三旋转中心为原点对应建立车体坐标系、动臂坐标系、斗杆坐标系以及铲斗坐标系；

总转换矩阵确定模块，用于根据获取的所述车体坐标系的x轴与所述动臂坐标系的x轴的第一夹角、所述动臂坐标系的x轴与所述斗杆坐标系的x轴的第二夹角、所述斗杆坐标系的x轴与所述铲斗坐标系的x轴的第三夹角、所述第一旋转中心在所述车体坐标系的位置坐标、所述第一旋转中心到所述第二旋转中心的第一距离以及所述第二旋转中心到所述第三旋转中心的第二距离，确定所述铲斗坐标系到所述车体坐标系的总转换矩阵；

第一位置确定模块，用于根据当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标，以及所述总转换矩阵确定所述当前时刻所述齿尖在所述车体坐标系的位置坐标；

转动距离确定模块，用于分别根据设置在所述动臂上的第一加速度传感器、设置在所述斗杆上的第二加速度传感器以及设置在所述铲斗上的第三加速度传感器，获取所述当前时刻到下一时刻之间所述动臂、所述斗杆和所述铲斗的加速度数据，并根据所述加速度数据确定所述当前时刻到所述下一时刻所述动臂、所述斗杆和所述铲斗的转动距离；

角度计算模块，用于根据所述动臂、所述斗杆和所述铲斗的转动距离，以及所述动臂加速度传感器到所述第一旋转中心的距离、所述斗杆加速度传感器到所述第二旋转中心的距离、所述铲斗加速度传感器到所述第三旋转中心的距离确定所述下一时刻对应的第一夹角、第二夹角和第三夹角的大小；

第二位置确定模块，用于根据所述下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于所述总转换矩阵确定所述下一时刻所述齿尖在所述车体坐标系的位置坐标；

作业轨迹确定模块，用于根据不同时刻所述齿尖在所述车体坐标系的位置坐标确定所述铲斗的作业轨迹。

在一种可能的实现方式中，所述总转换矩阵确定模块具体被配置为执行：

根据所述车体坐标系的x轴与所述动臂坐标系的x轴的第一夹角、以及所述第一旋转中心在所述车体坐标系的位置坐标，确定所述动臂坐标系到所述车体坐标系的第一转换矩阵；

根据所述动臂坐标系的x轴与所述斗杆坐标系的x轴的第二夹角、以及所述第一旋转中心到所述第二旋转中心的第一距离，确定所述斗杆坐标系到所述动臂坐标系的第二转换矩阵；

根据所述斗杆坐标系的x轴与所述铲斗坐标系的x轴的第三夹角、以及所述第二旋转中心到所述第三旋转中心的第二距离，确定所述铲斗坐标系到所述斗杆坐标系的第三转换矩阵；

根据所述第一转换矩阵、所述第二转换矩阵以及所述第三转换矩阵的乘积，确定所述铲斗坐标系到所述车体坐标系的总转换矩阵。

在一种可能的实现方式中，所述加速度数据包括所述当前时刻到所述下一时刻之间多个采样点的动臂加速度数据、斗杆加速度数据以及铲斗加速度数据；所述转动距离确定模块具体被配置为执行：

根据设置在所述动臂上的第一加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的动臂加速度数据；根据设置在所述斗杆上的第二加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的斗杆加速度数据；根据设置在所述铲斗上的第三加速度传感器获取所述当前时刻到所述下一时刻之间多个采样点的铲斗加速度数据；

分别对多个所述采样点的所述动臂加速度数据、所述斗杆加速度数据、以及所述铲斗加速度数据进行拟合，得到对应的动臂加速度函数、斗杆加速度函数以及铲斗加速度函数；

分别对所述动臂加速度函数、所述斗杆加速度函数以及所述铲斗加速度函数两次积分，对应得到所述当前时刻到所述下一时刻所述动臂、所述斗杆和所述铲斗的转动距离。

在一种可能的实现方式中，所述角度计算模块具体被配置为执行：

根据所述动臂的转动距离和所述动臂加速度传感器到所述第一旋转中心的距离，确定所述当前时刻到所述下一时刻的第一角度变化量；根据所述斗杆的转动距离和所述斗杆加速度传感器到所述第二旋转中心的距离，确定所述当前时刻到所述下一时刻的第二角度变化量；根据所述铲斗的转动距离和所述铲斗加速度传感器到所述第三旋转中心的距离，确定所述当前时刻到所述下一时刻的第三角度变化量；

根据所述当前时刻的第一夹角、第二夹角和第三夹角的大小，以及所述第一角度变化量、所述第二角度变化量和所述第三角度变化量确定所述下一时刻的第一夹角、第二夹角和第三夹角的大小。

在一种可能的实现方式中，在根据所述动臂的转动距离和所述动臂加速度传感器到所述第一旋转中心的距离，确定所述当前时刻到所述下一时刻的第一角度变化量时，所述角度计算模块具体被配置为执行：

根据第一公式确定所述当前时刻到所述下一时刻的第一角度变化量，所述第一公式具体为：

其中，Δ为第一角度变化量，s表示动臂的转动距离，l表示动臂加速度传感器到第一旋转中心的距离。

在一种可能的实现方式中，在根据所述当前时刻的第一夹角、第二夹角和第三夹角的大小，以及所述第一角度变化量、所述第二角度变化量和所述第三角度变化量确定所述下一时刻的第一夹角、第二夹角和第三夹角的大小时，所述角度计算模块具体被配置为执行：

根据所述当前时刻的第一夹角与所述第一角度变化量的差值，确定所述下一时刻的第一夹角的大小；根据所述当前时刻的第二夹角与所述第二角度变化量的差值，确定所述下一时刻的第二夹角的大小；根据所述当前时刻的第三夹角与所述第三角度变化量的差值，确定所述下一时刻的第三夹角的大小。

在一种可能的实现方式中，所述第一位置确定模块具体被配置为执行：

根据所述总转换矩阵确定所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标与当前时刻所述齿尖在所述车体坐标系的位置坐标的第一转换关系；

所述第一转换关系具体为：

其中，

其中，θ表示所述当前时刻的第一夹角，α表示所述当前时刻的第二夹角，β表示所述当前时刻的第三夹角；(a

将所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标带入所述第一转换关系中，计算得到所述当前时刻所述齿尖在所述车体坐标系的位置坐标。

在一种可能的实现方式中，所述第二位置确定模块具体被配置为执行：

将所述第一转换关系中所述当前时刻的第一夹角、第二夹角和第三夹角替换为所述下一时刻的第一夹角、第二夹角和第三夹角，得到第二转换关系；

将所述当前时刻所述铲斗的齿尖在所述铲斗坐标系的位置坐标带入所述第二转换关系中，计算得到所述下一时刻所述齿尖在所述车体坐标系的位置坐标。

第三方面，本发明提供了一种电子设备，所述电子设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述任一项所述的挖掘机铲斗作业轨迹确定方法。

第四方面，本发明提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述任一项所述的挖掘机铲斗作业轨迹确定方法。

本发明实施例提供的挖掘机铲斗作业轨迹确定方法在实际应用中，首先，以目标挖掘机的车体回转中心为原点建立车体坐标系，以动臂绕车体的第一旋转中心为原点建立动臂坐标系，以斗杆绕动臂的第二旋转中心为原点建立斗杆坐标系，以铲斗绕所述斗杆的第三旋转中心为原点建立铲斗坐标系；其次，确定铲斗坐标系与车体坐标系的总转换矩阵；再次，根据当前时刻铲斗的齿尖在铲斗坐标系的位置坐标以及总转换矩阵，确定当前时刻齿尖在车体坐标系的位置坐标；之后，根据分别根据设置在动臂上的第一加速度传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器，获取当前时刻到下一时刻之间动臂、斗杆和铲斗的加速度数据，并根据加速度数据确定当前时刻到下一时刻动臂、斗杆和铲斗的转动距离；再之后，根据动臂、斗杆和铲斗的转动距离，以及动臂加速度传感器到第一旋转中心的距离、斗杆加速度传感器到第二旋转中心的距离、铲斗加速度传感器到第三旋转中心的距离确定下一时刻对应的第一夹角、第二夹角和第三夹角的大小；再之后，根据下一时刻第一夹角、第二夹角和第三夹角的大小确定下一时刻齿尖在车体坐标系的位置坐标；最后，根据当前时刻和下一时刻齿尖在车体坐标系的位置坐标确定铲斗的作业轨迹；与现有的采用倾角传感器来测量动臂、斗杆和铲斗的姿态相比，本发明通过设置在动臂上的第一加速传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器间接测量动臂、斗杆和铲斗的姿态变化，从而计算出铲斗的作业轨迹，本发明不仅有效地降低了挖掘机铲斗作业轨迹计算的计算成本，而且计算方法简单，更有利于在挖掘机上推广使用。

附图说明

图1为本发明实施例提供的一种挖掘机铲斗作业轨迹确定方法的步骤流程图；

图2为本发明实施例提供的一种挖掘机铲斗作业轨迹确定方法中在H-D坐标系下挖掘机的结构简图；

图3为本发明实施例提供的一种挖掘机铲斗作业轨迹确定方法中在当前时刻和下一时刻的动臂回转示意图；

图4为图3的动臂回转角度变化简图；

图5为本发明实施例提供的一种挖掘机铲斗作业轨迹确定方法中从当前时刻到下一时刻动臂回转过程中的加速度数据变化示意图；

图6为本发明实施例提供的一种挖掘机铲斗作业轨迹确定方法中从当前时刻到下一时刻动臂回转过程中的速度数据变化示意图；

图7为本发明实施例提供的一种挖掘机铲斗作业轨迹确定装置的结构框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

以下，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本公开实施例的描述中，除非另有说明，“多个”的含义是两个或两个以上。另外，“基于”或“根据”的使用意味着开放和包容性，因为“基于”或“根据”一个或多个所述条件或值的过程、步骤、计算或其他动作在实践中可以基于额外条件或超出所述的值。

为了解决现有技术对挖掘机铲斗作业轨迹计算的成本较高的问题，本发明实施例提供了挖掘机铲斗作业轨迹确定方法、装置、设备及存储介质。

如图1所示，第一方面，本发明实施例提供了一种挖掘机铲斗作业轨迹确定方法，该方法包括：

步骤101、基于H-D模型，分别以目标挖掘机的车体回转中心、动臂绕车体的第一旋转中心、斗杆绕动臂的第二旋转中心以及铲斗绕斗杆的第三旋转中心为原点对应建立车体坐标系、动臂坐标系、斗杆坐标系以及铲斗坐标系。

其中，H-D(Denavit-Hartenberg)模型表示了对机器人连杆和关节进行建模的一种非常简单的方法。H-D建模的具体思路是：首先分别在机器人的每个关节处建立关节坐标系，确定关节刚性连杆参数，再依次将关节变化联系起来，计算出末端执行器相对于参考坐标系的变换关系得到坐标变换矩阵，从而建立机械臂运动学方程。

如图2所示，在本实施例中，D-H坐标系下目标挖掘机的结构简图包括车体11、动臂12、斗杆13以及铲斗14。

以目标挖掘机的车体11回转中心为原点建立车体坐标系O

以目标挖掘机的动臂12绕车体11的第一旋转中心为原点，建立动臂坐标系O

以目标挖掘机的斗杆13绕动臂12的第二旋转中心为原点建立斗杆坐标系O

以目标挖掘机的铲斗14绕斗杆13的第三旋转中心为原点对应建立铲斗坐标系O

在本发明实施例中，以20吨挖掘机为目标挖掘机，并在D-H坐标系下目标挖掘机的结构简图中分别以车体11回转中心、动臂12绕车体11的旋转中心、斗杆13绕动臂12的旋转中心以及铲斗14绕斗杆13的旋转中心为原点建立4个独立坐标系车体坐标系O

步骤102、根据获取的车体坐标系的x轴与动臂坐标系的x轴的第一夹角、动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角、斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角、第一旋转中心在车体坐标系的位置坐标、第一旋转中心到第二旋转中心的第一距离以及第二旋转中心到第三旋转中心的第二距离，确定铲斗坐标系到车体坐标系的总转换矩阵。

具体的，在挖掘机铲斗作业之前，测量出车体坐标系的x轴与动臂坐标系的x轴之间的第一夹角θ、动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角α、斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角β。

第一旋转中心O

当第一夹角θ、第二夹角α、第三夹角β、第一旋转中心O

步骤103、根据当前时刻铲斗的齿尖在铲斗坐标系的位置坐标，以及总转换矩阵确定当前时刻齿尖在车体坐标系的位置坐标。

具体的，在当前时刻铲斗的齿尖P在铲斗坐标系的初始位置坐标已知后，根据总转换矩阵可以计算得到齿尖P在车体坐标系中的初始位置坐标。

步骤104、分别根据设置在动臂12上的第一加速度传感器、设置在斗杆13上的第二加速度传感器以及设置在铲斗14上的第三加速度传感器，获取当前时刻到下一时刻之间动臂12、斗杆13和铲斗14的加速度数据，并根据加速度数据确定当前时刻到下一时刻动臂12、斗杆13和铲斗14的转动距离。

具体的，由于动臂12、斗杆13以及铲斗14的旋转运动是独立的，也就是说第一夹角θ、第二夹角α、第三夹角β的变化是独立的，因此我们通过分别在动臂12、斗杆13和铲斗14上安装第一加速度传感器、第二加速度传感器以及第三加速度传感器来间接计算出动臂12、斗杆13以及铲斗14旋转角度的变化量。

如图3所示，假设动臂12上的第一加速度传感器安装在铰接点B，方向垂直于第一旋转中心O

同理，斗杆13和铲斗14的转动距离以及转动角度和动臂12的计算方法相同，此处不再赘述。

步骤105、根据动臂12、斗杆13和铲斗14的转动距离，以及动臂加速度传感器到第一旋转中心的距离、斗杆加速度传感器到第二旋转中心的距离、铲斗加速度传感器到第三旋转中心的距离确定下一时刻对应的第一夹角、第二夹角和第三夹角的大小。

具体的，在上一步计算得到动臂12、斗杆13和铲斗14的转动距离之后，根据动臂12的转动距离以及动臂加速度传感器到第一旋转中心的距离可以计算得到动臂12的角度变化量，根据动臂转动前第一夹角的大小和动臂12的角度变化量的差值可以计算得到动臂转动后对应的第一夹角的大小。

第二夹角和第三夹角的计算方式与第一夹角的计算方式相同，此处不再赘述。

步骤106、根据下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于总转换矩阵确定下一时刻齿尖在车体坐标系的位置坐标。

具体的，在本实施例中，下一时刻转动后的第一夹角、第二夹角以及第三夹角分别以θ’、α’和β’表示，当转动后的θ’、α’和β’确定后，将θ’、α’和β’带入到总转换矩阵中，即可计算得到转动后的齿尖P在车体坐标系的位置坐标。

步骤107、根据不同时刻齿尖在车体坐标系的位置坐标确定铲斗的作业轨迹。

将铲斗齿尖的运动过程可以分为多个时间段，通过上述方式计算得到不同时间段目标挖掘机运动前和运动后齿尖P在车体坐标系的位置坐标，通过直线拟合或曲线拟合的方法对不同时刻得到的齿尖P在车体坐标系的位置坐标进行拟合，即可得到铲斗的作业轨迹。

本发明实施例提供的挖掘机铲斗作业轨迹确定方法在实际应用中，首先，基于H-D模型，以目标挖掘机的车体回转中心为原点建立车体坐标系，以动臂绕车体的第一旋转中心为原点建立动臂坐标系，以斗杆绕动臂的第二旋转中心为原点建立斗杆坐标系，以铲斗绕斗杆的第三旋转中心为原点建立铲斗坐标系；其次，确定铲斗坐标系与车体坐标系的总转换矩阵；再次，根据当前时刻铲斗的齿尖在铲斗坐标系的位置坐标以及总转换矩阵，确定当前时刻齿尖在车体坐标系的位置坐标；之后，根据分别根据设置在动臂上的第一加速度传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器，获取当前时刻到下一时刻之间动臂、斗杆和铲斗的加速度数据，并根据加速度数据确定当前时刻到下一时刻动臂、斗杆和铲斗的转动距离；再之后，根据动臂、斗杆和铲斗的转动距离，以及动臂加速度传感器到第一旋转中心的距离、斗杆加速度传感器到第二旋转中心的距离、铲斗加速度传感器到第三旋转中心的距离确定下一时刻对应的第一夹角、第二夹角和第三夹角的大小；再之后，根据下一时刻第一夹角、第二夹角和第三夹角的大小确定下一时刻齿尖在车体坐标系的位置坐标；最后，根据当前时刻和下一时刻齿尖在车体坐标系的位置坐标确定铲斗的作业轨迹。

与现有的采用倾角传感器来测量动臂、斗杆和铲斗的姿态相比，本发明通过设置在动臂上的第一加速传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器间接测量动臂、斗杆和铲斗的姿态变化，从而计算出铲斗的作业轨迹，本发明不仅有效地降低了挖掘机铲斗作业轨迹计算的计算成本，而且计算方法简单，更有利于在挖掘机上推广使用。

如图2所示，进一步的，根据获取的车体坐标系的x轴与动臂坐标系的x轴的第一夹角、动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角、斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角、第一旋转中心在车体坐标系的位置坐标、第一旋转中心到第二旋转中心的第一距离以及第二旋转中心到第三旋转中心的第二距离，确定铲斗坐标系到车体坐标系的总转换矩阵，具体包括：

根据车体坐标系的x轴与动臂坐标系的x轴的第一夹角、以及第一旋转中心在车体坐标系的位置坐标，确定动臂坐标系到车体坐标系的第一转换矩阵。

其中，根据D-H坐标系转换法，将动臂坐标系O

其中，

根据动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角、以及第一旋转中心到第二旋转中心的第一距离，确定斗杆坐标系到动臂坐标系的第二转换矩阵。

其中，根据D-H坐标系转换法，将斗杆坐标系O

其中，

根据斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角、以及第二旋转中心到第三旋转中心的第二距离，确定铲斗坐标系到斗杆坐标系的第三转换矩阵。

其中，根据D-H坐标系转换法，将铲斗坐标系O

其中，L

根据第一转换矩阵、第二转换矩阵以及第三转换矩阵的乘积，确定铲斗坐标系到车体坐标系的总转换矩阵。

具体的，根据公式(1)计算得到的第一转换矩阵、公式(2)计算得到的第二转换矩阵以及公式(3)得到的第三转换矩阵的乘积，得到铲斗坐标系O

如图3、图4所示，进一步的，加速度数据包括当前时刻到下一时刻之间多个采样点的动臂加速度数据、斗杆加速度数据以及铲斗加速度数据。

分别根据设置在动臂上的第一加速度传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器，获取当前时刻到下一时刻之间动臂、斗杆和铲斗的加速度数据，并根据加速度数据确定当前时刻到下一时刻动臂、斗杆和铲斗的转动距离，具体包括：

根据设置在动臂上的第一加速度传感器获取当前时刻到下一时刻之间多个采样点的动臂加速度数据；根据设置在斗杆上的第二加速度传感器获取当前时刻到下一时刻之间多个采样点的斗杆加速度数据；根据设置在铲斗上的第三加速度传感器获取当前时刻到下一时刻之间多个采样点的铲斗加速度数据。

分别对多个采样点的动臂加速度数据、斗杆加速度数据、以及铲斗加速度数据进行拟合，得到对应的动臂加速度函数、斗杆加速度函数以及铲斗加速度函数。

如图5所示，当前时刻到下一时刻之间的时间间隔为10秒，每隔0.05秒采样一次动臂加速度数据，通过直线拟合的方式对多个动臂加速度数据进行拟合，得到的动臂加速度函数为直线方程。

斗杆加速度函数与铲斗加速度函数的获取方式与动臂加速度函数的获取方式相同，此处不再赘述。

分别对动臂加速度函数、斗杆加速度函数以及铲斗加速度函数两次积分，对应得到当前时刻到下一时刻动臂、斗杆和铲斗的转动距离。

具体的，对加速度函数积分可以得到速度函数，对速度函数再次积分可以得到距离。也就是说，对获取的动臂加速度函数进行两次积分可以计算得到当前时刻到下一时刻动臂的转动距离。

如图6所示，为对动臂在0-10秒内动臂回转过程的速度变化数据，速度变化趋势符合加速度函数积分后的速度函数。

如图3、图4所示，进一步的，根据动臂、斗杆和铲斗的转动距离，以及动臂加速度传感器到第一旋转中心的距离、斗杆加速度传感器到第二旋转中心的距离、铲斗加速度传感器到第三旋转中心的距离确定下一时刻对应的第一夹角、第二夹角和第三夹角的大小，具体包括：

根据动臂的转动距离和动臂加速度传感器到第一旋转中心的距离，确定当前时刻到下一时刻的第一角度变化量。

根据斗杆的转动距离和斗杆加速度传感器到第二旋转中心的距离，确定当前时刻到下一时刻的第二角度变化量。

根据铲斗的转动距离和铲斗加速度传感器到第三旋转中心的距离，确定当前时刻到下一时刻的第三角度变化量。

其中，根据动臂的转动距离和动臂加速度传感器到第一旋转中心的距离，确定当前时刻到下一时刻的第一角度变化量，具体为：

根据第一公式确定当前时刻到下一时刻的第一角度变化量，第一公式具体为：

其中，Δ为第一角度变化量，s表示动臂的转动距离，l表示动臂加速度传感器到第一旋转中心的距离。

第二角度变化量和第三角度变化量的计算过程与第一角度变化量的计算过程相同，此处不再赘述。

根据当前时刻的第一夹角、第二夹角和第三夹角的大小，以及第一角度变化量、第二角度变化量和第三角度变化量确定下一时刻的第一夹角、第二夹角和第三夹角的大小。

其中，根据当前时刻的第一夹角、第二夹角和第三夹角的大小，以及第一角度变化量、第二角度变化量和第三角度变化量确定下一时刻的第一夹角、第二夹角和第三夹角的大小，具体包括：

根据当前时刻的第一夹角与第一角度变化量的差值，确定下一时刻的第一夹角的大小。

根据当前时刻的第二夹角与第二角度变化量的差值，确定下一时刻的第二夹角的大小。

根据当前时刻的第三夹角与第三角度变化量的差值，确定下一时刻的第三夹角的大小。

进一步的，根据当前时刻铲斗的齿尖在铲斗坐标系的位置坐标，以及总转换矩阵确定当前时刻齿尖在车体坐标系的位置坐标，具体包括：

根据总转换矩阵确定当前时刻铲斗的齿尖在铲斗坐标系的位置坐标与当前时刻齿尖在车体坐标系的位置坐标的第一转换关系。

在本实施例中，第一转换关系具体为：

其中，

其中，θ表示当前时刻的第一夹角，α表示当前时刻的第二夹角，β表示当前时刻的第三夹角；(a

将当前时刻铲斗的齿尖在铲斗坐标系的位置坐标带入第一转换关系中，计算得到当前时刻齿尖在车体坐标系的位置坐标。

进一步的，根据下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于总转换矩阵确定下一时刻齿尖在车体坐标系的位置坐标，具体包括：

将第一转换关系中当前时刻的第一夹角、第二夹角和第三夹角替换为下一时刻的第一夹角、第二夹角和第三夹角，得到第二转换关系。

将当前时刻铲斗的齿尖在铲斗坐标系的位置坐标带入第二转换关系中，计算得到下一时刻齿尖在车体坐标系的位置坐标。

在本发明实施例中，第一加速度传感器安装在铰点B，其安装方向垂直于O

此时，只需要求解出θ’，即可得到铲斗齿尖P在运动后(下一时刻)的位置坐标。

由于B点和O

根据公式(5)可以计算得到∠BO

之后，根据运动学原理，加速度的积分等于速度，速度的积分等于位移。也就是说劣弧BB’的长度可以通过对当前时刻到下一时刻之间的动臂加速度函数进行两次积分得到。

在本实施例中，对动臂加速度函数两次积分得的的劣弧BB’的长度为334.3785mm，而在目标挖掘机的三维模型中，动臂油杆收缩100mm，劣弧BB’的长度为323.4217mm，仿真结果与计算结果仅相差11mm，说明本计算方式具有可行性。

在本实施例中，角度变化量Δ＝∠BO

根据动臂转动前的第一夹角θ和第一角度变化量Δ的差值，可以计算得到动臂转动后的第一夹角θ’的大小。

之后，通过θ’替换总转换矩阵中的θ，根据公式(6)，可以计算得到动臂转动后铲斗的齿尖P在车体坐标系O

如图7所示，第二方面，本发明还提供了一种挖掘机铲斗作业轨迹确定装置，该装置包括：

模型处理模块201，用于分别以目标挖掘机的车体回转中心、动臂绕车体的第一旋转中心、斗杆绕动臂的第二旋转中心以及铲斗绕斗杆的第三旋转中心为原点对应建立车体坐标系、动臂坐标系、斗杆坐标系以及铲斗坐标系；

总转换矩阵确定模块202，用于根据获取的车体坐标系的x轴与动臂坐标系的x轴的第一夹角、动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角、斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角、第一旋转中心在车体坐标系的位置坐标、第一旋转中心到第二旋转中心的第一距离以及第二旋转中心到第三旋转中心的第二距离，确定铲斗坐标系到车体坐标系的总转换矩阵；

第一位置确定模块203，用于根据当前时刻铲斗的齿尖在铲斗坐标系的位置坐标，以及总转换矩阵确定当前时刻齿尖在车体坐标系的位置坐标；

转动距离确定模块204，用于分别根据设置在动臂上的第一加速度传感器、设置在斗杆上的第二加速度传感器以及设置在铲斗上的第三加速度传感器，获取当前时刻到下一时刻之间动臂、斗杆和铲斗的加速度数据，并根据加速度数据确定当前时刻到下一时刻动臂、斗杆和铲斗的转动距离；

角度计算模块205，用于根据动臂、斗杆和铲斗的转动距离，以及动臂加速度传感器到第一旋转中心的距离、斗杆加速度传感器到第二旋转中心的距离、铲斗加速度传感器到第三旋转中心的距离确定下一时刻对应的第一夹角、第二夹角和第三夹角的大小；

第二位置确定模块206，用于根据下一时刻的第一夹角、第二夹角以及第三夹角的大小，基于总转换矩阵确定下一时刻齿尖在车体坐标系的位置坐标；

作业轨迹确定模块207，用于根据不同时刻齿尖在车体坐标系的位置坐标确定铲斗的作业轨迹。

进一步的，总转换矩阵确定模块202具体被配置为执行：

根据车体坐标系的x轴与动臂坐标系的x轴的第一夹角、以及第一旋转中心在车体坐标系的位置坐标，确定动臂坐标系到车体坐标系的第一转换矩阵；

根据动臂坐标系的x轴与斗杆坐标系的x轴的第二夹角、以及第一旋转中心到第二旋转中心的第一距离，确定斗杆坐标系到动臂坐标系的第二转换矩阵；

根据斗杆坐标系的x轴与铲斗坐标系的x轴的第三夹角、以及第二旋转中心到第三旋转中心的第二距离，确定铲斗坐标系到斗杆坐标系的第三转换矩阵；

根据第一转换矩阵、第二转换矩阵以及第三转换矩阵的乘积，确定铲斗坐标系到车体坐标系的总转换矩阵。

进一步的，加速度数据包括当前时刻到下一时刻之间多个采样点的动臂加速度数据、斗杆加速度数据以及铲斗加速度数据；转动距离确定模块204具体被配置为执行：

根据设置在动臂上的第一加速度传感器获取当前时刻到下一时刻之间多个采样点的动臂加速度数据；根据设置在斗杆上的第二加速度传感器获取当前时刻到下一时刻之间多个采样点的斗杆加速度数据；根据设置在铲斗上的第三加速度传感器获取当前时刻到下一时刻之间多个采样点的铲斗加速度数据；

分别对多个采样点的动臂加速度数据、斗杆加速度数据、以及铲斗加速度数据进行拟合，得到对应的动臂加速度函数、斗杆加速度函数以及铲斗加速度函数；

分别对动臂加速度函数、斗杆加速度函数以及铲斗加速度函数两次积分，对应得到当前时刻到下一时刻动臂、斗杆和铲斗的转动距离。

进一步的，角度计算模块205具体被配置为执行：

根据动臂的转动距离和动臂加速度传感器到第一旋转中心的距离，确定当前时刻到下一时刻的第一角度变化量；根据斗杆的转动距离和斗杆加速度传感器到第二旋转中心的距离，确定当前时刻到下一时刻的第二角度变化量；根据铲斗的转动距离和铲斗加速度传感器到第三旋转中心的距离，确定当前时刻到下一时刻的第三角度变化量；

根据当前时刻的第一夹角、第二夹角和第三夹角的大小，以及第一角度变化量、第二角度变化量和第三角度变化量确定下一时刻的第一夹角、第二夹角和第三夹角的大小。

进一步的，在根据动臂的转动距离和动臂加速度传感器到第一旋转中心的距离，确定当前时刻到下一时刻的第一角度变化量时，角度计算模块205具体被配置为执行：

根据第一公式确定当前时刻到下一时刻的第一角度变化量，第一公式具体为：

其中，Δ为第一角度变化量，s表示动臂的转动距离，l表示动臂加速度传感器到第一旋转中心的距离。

进一步的，在根据当前时刻的第一夹角、第二夹角和第三夹角的大小，以及第一角度变化量、第二角度变化量和第三角度变化量确定下一时刻的第一夹角、第二夹角和第三夹角的大小时，角度计算模块205具体被配置为执行：

根据当前时刻的第一夹角与第一角度变化量的差值，确定下一时刻的第一夹角的大小；根据当前时刻的第二夹角与第二角度变化量的差值，确定下一时刻的第二夹角的大小；根据当前时刻的第三夹角与第三角度变化量的差值，确定下一时刻的第三夹角的大小。

进一步的，第一位置确定模块203具体被配置为执行：

根据总转换矩阵确定当前时刻铲斗的齿尖在铲斗坐标系的位置坐标与当前时刻齿尖在车体坐标系的位置坐标的第一转换关系；

第一转换关系具体为：

/>

其中，

其中，θ表示当前时刻的第一夹角，α表示当前时刻的第二夹角，β表示当前时刻的第三夹角；(a

将当前时刻铲斗的齿尖在铲斗坐标系的位置坐标带入第一转换关系中，计算得到当前时刻齿尖在车体坐标系的位置坐标。

进一步的，第二位置确定模块206具体被配置为执行：

将第一转换关系中当前时刻的第一夹角、第二夹角和第三夹角替换为下一时刻的第一夹角、第二夹角和第三夹角，得到第二转换关系；

将当前时刻铲斗的齿尖在铲斗坐标系的位置坐标带入第二转换关系中，计算得到下一时刻齿尖在车体坐标系的位置坐标。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

第三方面，本发明实施例还提供了一种电子设备，电子设备包括处理器和存储器，存储器中存储有至少一条指令、至少一段程序、代码集或指令集，至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现本发明实施例中的挖掘机铲斗作业轨迹确定方法。

本发明实施例还提供了一种计算机可读存储介质，存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，至少一条指令、至少一段程序、代码集或指令集由处理器加载并执行以实现本发明实施例中的挖掘机铲斗作业轨迹确定方法。

在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本发明实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘Solid State Disk(SSD))等。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何在本发明揭露的技术范围内的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。