CN109972688B - 挖掘控制方法、装置及挖掘机控制器 - Google Patents

Abstract

本申请实施例提供了一种挖掘控制方法、装置及挖掘机控制器，涉及机械技术领域，应用于挖掘机控制器，该方法通过获取到的挖掘机的初始状态数据和目标挖掘点相对挖掘机的相对位置计算出挖掘机的目标状态数据，然后再根据初始状态数据和目标状态数据对挖掘机的挖掘进行挖掘规划，得到挖掘轨迹数据，从而可以根据挖掘轨迹数据对挖掘机进行控制，实现自动挖掘，进而提高了挖掘效率，同时在挖掘作业的环境危险的情况下也可以保证操作人员的人身安全。

Description

挖掘控制方法、装置及挖掘机控制器

技术领域

本申请涉及机械技术领域，具体而言，涉及一种挖掘控制方法、装置及挖掘机控制器。

背景技术

随着工程建设的发展，挖掘机在矿山、农场、建筑等各类工程中都被广泛的应用，并担负了繁重的土石方作业任务，是施工中不可缺少的关键设备，但目前的挖掘机需要通过操作人员在挖掘机的驾驶室中进行人工操作，完成相关的任务，由于通过操作人员只能对一台挖掘机进行操作控制，从而会导致挖掘效率较低。

发明内容

有鉴于此，本申请实施例的目的在于提供一种挖掘控制方法、装置及挖掘机控制器，以改善现有技术中挖掘效率低的问题。

第一方面，本申请实施例提供了一种挖掘控制方法，应用于挖掘机控制器，所述方法包括：获取挖掘机处于初始状态时的初始状态数据，其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据；获取目标挖掘点相对所述挖掘机的相对位置；根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据，其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据；根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据；根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，通过获取到的挖掘机的初始状态数据和目标挖掘点相对挖掘机的相对位置计算出挖掘机的目标状态数据，然后再根据初始状态数据和目标状态数据对挖掘机的挖掘进行挖掘规划，得到挖掘轨迹数据，从而可以根据挖掘轨迹数据对挖掘机进行控制，实现自动挖掘，进而提高了挖掘效率，同时在挖掘作业的环境危险的情况下也可以保证操作人员的人身安全。

进一步地，所述根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据，包括：获取所述挖掘机随时间变化的角度数据，其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度；根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据。

在上述实现过程中，获取挖掘机随时间变化的角度数据，该角度数据可以表示出挖掘机各个关节之间角度的变化，该角度数据包括挖掘机的动臂与基座之间随时间变化的角度、挖掘机的动臂与小臂之间随时间变化的角度、以及挖掘机的小臂与挖斗之间随时间变化的角度，通过该角度数据可以对挖掘机的挖掘轨迹进行规划，从而可以对挖掘机进行自动挖掘控制，进而实现自动挖掘并提高挖掘效率。

进一步地，所述获取所述挖掘机随时间变化的角度数据，包括：根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据，其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。

在上述实现过程中，根据第一计算公式可以获取角度数据，该角度数据可以表示出挖掘机各个关节之间角度随时间的变化，不同时刻的角度不同，可以用于准确的对挖掘机的轨迹进行描述，从而对挖掘机的挖掘轨迹进行准确地规划，进而对挖掘机进行自动挖掘控制。

进一步地，所述根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据，包括：

根据第二计算公式

获取所述挖掘轨迹数据，其中，θ(t)表示所述挖掘轨迹数据中的角度数据中的其中一种数据，

表示所述挖掘轨迹数据中的挖掘期望角速度，

表示所述挖掘轨迹数据中的挖掘期望加速度，t_f为根据

计算得到的所述挖掘机的目标状态数据中表示所述挖掘机动臂与基座处于目标状态的时刻、所述挖掘机的动臂与小臂处于目标状态的时刻、所述挖掘机的小臂与挖斗处于目标状态的时刻的任意一个时刻，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，θ₀为所述初始状态数据，θ_f为所述目标状态数据。

在上述实现过程中，挖掘轨迹数据是根据角度数据以及初始状态数据、目标状态数据通过第二计算公式计算出的，从而通过该挖掘轨迹数据可以准确的对挖掘机的自动挖掘轨迹进行规划。

进一步地，所述根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘，包括：获取所述挖掘机的实时状态数据，其中，所述实时状态数据为从初始状态数据改变为目标状态数据的实时变化数据；根据

计算获得所述挖掘机的控制量，其中，k_p为比例系数，k_I为积分系数，t为时间，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，u(t)为所述挖掘机的控制量；根据所述挖掘机的控制量控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，对挖掘机从初始状态变为目标状态的过程中进行实时状态数据的获取，再对实时状态数据和挖掘轨迹数据进行计算得到误差数据，由于挖掘机在进行挖掘过程中的实时状态数据和挖掘轨迹数据之间不完全相等，从而可以根据误差数据进行计算获得挖掘机的控制量，进而实现对挖掘机进行挖掘控制，实现准确的自动挖掘。

进一步地，所述获取目标挖掘点相对所述挖掘机的相对位置，包括：通过视觉传感器获取包括所述目标挖掘点的图像；对所述图像中的所述目标挖掘点进行识别获得所述目标挖掘点的相对位置，其中，所述目标挖掘点的相对位置为以所述挖掘机的基座为原点的三维位置数据。

在上述实现过程中，先通过视觉传感器获取包括有目标挖掘点的图像，再对图像进行特征分析，可以识别出目标挖掘点，并通过图像处理进行计算获得目标挖掘点的相对位置，该相对位置为以挖掘机的基座为原点的三维位置数据，从而能够保证准确的对挖掘机的挖掘轨迹进行规划。

第二方面，本申请实施例提供了一种挖掘控制装置，应用于挖掘机控制器，所述装置包括：初始状态数据获取模块，用于获取挖掘机处于初始状态时的初始状态数据，其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据；目标挖掘点相对位置获取模块，用于获取目标挖掘点相对所述挖掘机的相对位置；目标状态数据获取模块，用于根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据，其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据；挖掘轨迹数据获取模块，用于根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据；挖掘控制模块，用于根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，挖掘控制装置通过初始状态数据获取模块和目标挖掘点相对位置获取模块，可以获取到挖掘机的初始状态数据和目标挖掘点相对所述挖掘机的相对位置，然后目标状态数据获取模块根据初始状态数据和目标挖掘点相对挖掘机的相对位置计算出挖掘机的目标状态数据，接着挖掘轨迹数据获取模块再根据初始状态数据和目标状态数据对挖掘机的挖掘进行挖掘规划，得到挖掘轨迹数据，从而可以根据挖掘轨迹数据对挖掘机进行控制，实现自动挖掘，进而提高了挖掘效率，同时在挖掘作业的环境危险的情况下也可以保证操作人员的人身安全。

进一步地，所述挖掘轨迹数据获取模块包括：角度数据获取单元，用于获取所述挖掘机随时间变化的角度数据，其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度；挖掘轨迹数据获取单元，用于根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据。

在上述实现过程中，角度数据获取单元可以获取挖掘机随时间变化的角度数据，该角度数据可以表示出挖掘机各个关节之间角度的变化，该角度数据包括挖掘机的动臂与基座之间随时间变化的角度、挖掘机的动臂与小臂之间随时间变化的角度、以及挖掘机的小臂与挖斗之间随时间变化的角度，通过该角度数据可以对挖掘机的挖掘轨迹进行规划，从而可以对挖掘机进行自动挖掘控制，进而实现自动挖掘并提高挖掘效率。

进一步地，所述角度数据获取单元包括：第一计算子单元，用于根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据，其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。

在上述实现过程中，第一计算子单元根据第一计算公式可以获取角度数据，该角度数据可以表示出挖掘机各个关节之间角度随时间的变化，不同时刻的角度不同，可以用于准确的对挖掘机的轨迹进行描述，从而对挖掘机的挖掘轨迹进行准确地规划，进而对挖掘机进行自动挖掘控制。

进一步地，所述挖掘轨迹数据获取单元，包括：第二计算子单元，用于根据第二计算公式

获取所述挖掘轨迹数据，其中，θ(t)表示所述挖掘轨迹数据中的角度数据中的其中一种数据，

表示所述挖掘轨迹数据中的挖掘期望角速度，

表示所述挖掘轨迹数据中的挖掘期望加速度，t_f为根据

计算得到的所述挖掘机的目标状态数据中表示所述挖掘机动臂与基座处于目标状态的时刻、所述挖掘机的动臂与小臂处于目标状态的时刻、所述挖掘机的小臂与挖斗处于目标状态的时刻的任意一个时刻，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，θ₀为所述初始状态数据，θ_f为所述目标状态数据。

在上述实现过程中，第二计算子单元通过第二计算公式计算挖掘轨迹数据，且根据角度数据以及初始状态数据、目标状态数据进行计算，可以保证挖掘轨迹数据的准确性，从而通过该挖掘轨迹数据可以准确的对挖掘机的自动挖掘轨迹进行规划。

进一步地，所述挖掘控制模块包括：实时状态数据获取单元，用于获取所述挖掘机的实时状态数据，其中，所述实时状态数据为从初始状态数据改变为目标状态数据的实时变化数据；挖掘机的控制量计算单元，用于根据

计算获得所述挖掘机的控制量，其中，k_p为比例系数，k_I为积分系数，t为时间，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，u(t)为所述挖掘机的控制量；控制单元，用于根据所述挖掘机的控制量控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，实时状态数据获取单元对挖掘机从初始状态变为目标状态的过程中进行实时状态数据的获取，挖掘机的控制量计算单元再对实时状态数据和挖掘轨迹数据进行计算得到误差数据，由于挖掘机在进行挖掘过程中的实时状态数据和挖掘轨迹数据之间不完全相等，从而挖掘机的控制量计算单元可以根据误差数据进行计算获得挖掘机的控制量，进而实现对挖掘机进行挖掘控制，实现准确的自动挖掘。

进一步地，所述目标挖掘点相对位置获取模块包括：图像获取单元，用于通过视觉传感器获取包括所述目标挖掘点的图像；相对位置获取单元，用于对所述图像中的所述目标挖掘点进行识别获得所述目标挖掘点的相对位置，其中，所述目标挖掘点的相对位置为以所述挖掘机的基座为原点的三维位置数据。

在上述实现过程中，图像获取单元先通过视觉传感器获取包括有目标挖掘点的图像，相对位置获取单元再对图像进行特征分析，可以识别出目标挖掘点，并通过图像处理进行计算获得目标挖掘点的相对位置，该相对位置为以挖掘机的基座为原点的三维位置数据，从而能够保证准确的对挖掘机的挖掘轨迹进行规划。

第三方面，本申请实施例提供一种挖掘机控制器，包括处理器以及存储器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如上述第一方面提供的所述方法中的步骤。

第四方面，本申请实施例提供一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如上述第一方面提供的所述方法中的步骤。

本申请的其他特征和优点将在随后的说明书阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请实施例了解。本申请的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请实施例提供的一种挖掘机控制器的结构框图；

图2为本申请实施例提供的一种挖掘控制方法的流程框图；

图3为本申请实施例提供的一种挖掘机的各个部件之间的角度示意图；

图4为本申请实施例提供的一种挖掘控制方法中步骤S150的流程框图；

图5为本申请实施例提供的一种挖掘控制装置的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，图1为本发明实施例提供的一种挖掘机控制器的结构示意图，所述挖掘机控制器可以包括：至少一个处理器110，例如CPU，至少一个通信接口120，至少一个存储器130和至少一个通信总线140。其中，通信总线140用于实现这些组件直接的连接通信。其中，本申请实施例中设备的通信接口120用于与其他节点设备进行信令或数据的通信。存储器130可以是高速RAM存储器，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。存储器130可选的还可以是至少一个位于远离前述处理器的存储装置。存储器130中存储有计算机可读取指令，当所述计算机可读取指令由所述处理器110执行时，挖掘机控制器执行下述图2所示方法过程。

请参考图2，图2为本申请实施例提供的一种挖掘控制方法的流程框图，该挖掘控制方法应用于挖掘机控制器，该控制器可以是设置在挖掘机上的，也可以是设置在远程控制端的，该方法包括如下步骤：

步骤S110：获取挖掘机处于初始状态时的初始状态数据。

挖掘机处于初始状态是指挖掘机还未开始进行挖掘前的初始状态，即初始状态时，挖掘机的各个部件之间处于初始位置，且相连接的两个部件之间处于初始角度。

可参照图3所示，图3为本申请实施例提供的一种挖掘机的各个部件之间的示意图，在图中线段L₁表示挖掘机的动臂，原点O表示挖掘机的基座与动臂L₁之间的连接处，θ₁表示挖掘机的动臂L₁所在直线与从原点O至x₁方向的直线之间的夹角，线段L₂表示挖掘机的小臂，θ₂表示挖掘机的小臂L₂与动臂L₁的反向延长线x₂方向的夹角，线段L₃表示挖掘机的挖斗挖口的口径，θ₃表示挖掘机虚的挖斗挖口的口径L₃与小臂L₂的反向延长线x₃方向的夹角。

所以，初始状态数据可以为所述挖掘机还未开始挖掘前的状态数据。特别的，初始状态数据包括表示挖掘机基座、动臂、小臂、挖斗两两之间的角度数据，初始状态数据还包括挖掘机基座与动臂连接处的角速度和加速度，挖掘机动臂与小臂连接处的角速度和加速度，挖掘机小臂与挖斗连接处的角速度和加速度。因此，若如图3所示的挖掘机处于初始状态，则可以获取初始状态数据(θ₁，θ₂，θ₃)用于表示挖掘机初始状态数据中的三个角度信息，可以理解的，挖掘机初始状态数据中的三个角速度数据和挖掘机初始状态数据中的三个加速度数据都可以用三维数据的形式进行表示。

其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据。特别的，初始状态数据包括表示挖掘机基座、动臂、小臂、挖斗两两部件之间的角度数据，初始状态数据还包括挖掘机基座与动臂连接处的角速度和加速度，挖掘机动臂与小臂连接处的角速度和加速度，挖掘机小臂与挖斗连接处的角速度和加速度。

步骤S120：获取目标挖掘点相对所述挖掘机的相对位置。

通过视觉传感器获取目标挖掘点的图像信息，视觉传感器可以为摄像头，其是安装在挖掘机上的，以方便对目标挖掘点进行拍摄，从而获得目标挖掘点的图像。

需要说明的是，挖掘机获取的图像中可能还包括有其他挖掘点，若需要确定目标挖掘点，则操作人员可在获取的图像中选择需要挖掘的挖掘点作为目标挖掘点，例如，挖掘机获取的图像中包括有挖掘点A、挖掘点B、挖掘点C，操作人员可以通过输入装置选择挖掘点B为目标挖掘点，使挖掘机可以完成对目标挖掘点的挖掘，挖掘机也可以通过其他方式，例如特征识别等实现对目标挖掘点的自动选取。

可选地，步骤S120中包括如下步骤：

先通过视觉传感器获取包括所述目标挖掘点的图像，然后对所述图像中的所述目标挖掘点进行识别获得所述目标挖掘点的相对位置。其中，所述目标挖掘点的相对位置为以所述挖掘机的基座为原点的三维位置数据。

作为一种实施方式，目标挖掘点的相对位置还可以通过激光雷达等方式进行获取。

特别的，该相对位置可以为三维数据，例如，该相对位置为(x，y，z)，该相对位置以挖掘机的基座为原点，如图3中的原点O。

可以根据挖掘机建立运动学模型，例如，请参照图3，可以根据挖掘机的初始状态建立运动学模型

其中，θ₁表示挖掘机的动臂与基座之间的角度，θ₂表示挖掘机的动臂与小臂之间的角度，θ₃表示挖掘机的小臂与挖斗之间的角度，θ表示所述挖掘机挖斗口面与x₁轴所在直线的夹角，c₁＝cosθ₁为根据θ₁计算得到的系数，c₁₂＝cos(θ₁+θ₂)为根据θ₁和θ₂计算得到的系数，c₁₂₃＝cos(θ₁+θ₂+θ₃)为根据θ₁、θ₂和θ₃计算得到的系数，s₁＝sinθ₁为根据θ₁计算得到的系数，s₁₂＝sin(θ₁+θ₂)为根据θ₁和θ₂计算得到的系数，s₁₂₃＝sin(θ₁+θ₂+θ₃)为根据θ₁、θ₂和θ₃计算得到的系数，(x,y,z)为根据挖掘机的初始状态计算得到的目标挖掘点的相对位置。

在上述实现过程中，先通过视觉传感器获取包括有目标挖掘点的图像，再对图像进行特征分析，可以识别出目标挖掘点，并通过图像处理进行计算获得目标挖掘点的相对位置，该相对位置为以挖掘机的基座为原点的三维位置数据，从而能够保证准确的对挖掘机的挖掘轨迹进行规划。

步骤S130：根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据。

其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据。例如，若挖掘机的初始状态数据为(θ₁，θ₂，θ₃)，相对位置(x，y，z)，且相对位置为以图3中的原点O为原点，根据运动学逆解算的方式可以推导出挖掘机臂在目标挖掘点时的目标状态数据(θ_1f，θ_2f，θ_3f)，可以理解的，挖掘机的动臂L₁的长度、小臂L₂的长度、挖斗挖口的口径L₃长度均可以直接从挖掘机预先存储在存储器中的参数中进行获取，也可以通过操作人员通过手动输入进行获取。

下面对目标状态数据的计算过程进行简单说明。

例如，获取到的相对位置为(x,y,z)，初始状态数据为，(θ₁，θ₂，θ₃)，根据运动学逆解算的方式有公式一

其中，x、y、z为相对位置中的已知量，θ表示挖斗端口面与被挖掘面的夹角，可以根据经验自主设置，c₁＝cosθ₁为根据θ₁计算得到的系数，c₁₂＝cos(θ₁+θ₂)为根据θ₁和θ₂计算得到的系数，c₁₂₃＝cos(θ₁+θ₂+θ₃)为根据θ₁、θ₂和θ₃计算得到的系数，s₁＝sinθ₁为根据θ₁计算得到的系数，s₁₂＝sin(θ₁+θ₂)为根据θ₁和θ₂计算得到的系数，s₁₂₃＝sin(θ₁+θ₂+θ₃)为根据θ₁、θ₂和θ₃计算得到的系数，则根据公式一可以变换得到公式二

则可以求出x′和y′，然后根据公式二进行组合变换得到公式三

由公式三可以计算得到

因此目标状态数据中的θ_2f＝arccosK，此外，根据公式二并结合

可以得到公式四

继而得到

因此目标状态数据中的

继而有目标状态数据中的θ_3f＝θ-θ_1f-θ_2f，最终根据逆运动学模型可以得到目标状态数据为

步骤S140：根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据。

例如，获得的初始状态数据为(θ₁，θ₂，θ₃)，计算出的目标状态数据为(θ_1f，θ_2f，θ_3f)，那么可以根据三次多项式的规划方法进行轨迹规划，计算从θ₁变化为θ_1f的挖掘轨迹数据，其中θ₁为t＝0时的初始状态数据，θ_1f为t＝10时的初始状态数据，例如，θ₁变化为θ_1f可能经历三个时刻，通过轨迹规划可以计算出当t＝1时，挖掘机可能的中间状态数据为θ_z1，当t＝3时，挖掘机可能的中间状态数据为θ_z3，当t＝5时，挖掘机可能的中间状态数据为θ_z5，则计算得到的挖掘轨迹数据包括θ_z1、θ_z3、θ_z5，可以理解的，在计算挖掘轨迹数据时的时间间隔可以根据挖掘作业的要求进行选取。

步骤S150：根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述获得挖掘轨迹数据后，挖掘机控制器可根据该挖掘轨迹数据控制挖掘机对目标挖掘点进行挖掘，从而实现对目标挖掘点的自动挖掘，进而提高了挖掘效率，由于该种挖掘方式不需要操作人员对挖掘机进行亲自操作，所以在挖掘作业的环境危险的情况下可以保证操作人员的人身安全。

可选地，获得挖掘轨迹数据的方式可以为：获取所述挖掘机随时间变化的角度数据；根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据。

其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度。

可选地，获得角度数据的方式可以包括如下步骤：

第一计算步骤：根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据。

其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。在上述实现过程中，根据第一计算公式可以获取角度数据，该角度数据可以表示出挖掘机各个关节之间角度随时间的变化，不同时刻的角度不同，可以用于准确的对挖掘机的轨迹进行描述，从而对挖掘机的挖掘轨迹进行准确地规划，进而对挖掘机进行自动挖掘控制。

明显的，根据第一计算公式计算出的θ(t)是包含有系数的根据时间t的变化而改变的函数，例如，可以获取表示挖掘机的基座和动臂之间角度的角度数据为θ(t)＝a₀+a₁t+a₂t²+a₃t³，如图3中所示的θ₁，也可以获取表示挖掘机的动臂和小臂之间角度的角度数据为θ(t)＝a₀+a₁t+a₂t²+a₃t³，如图3中所示的θ₂。

在上述实现过程中，获取挖掘机随时间变化的角度数据，该角度数据可以表示出挖掘机各个关节之间角度的变化，该角度数据包括挖掘机的动臂与基座之间随时间变化的角度、挖掘机的动臂与小臂之间随时间变化的角度、以及挖掘机的小臂与挖斗之间随时间变化的角度，通过该角度数据可以对挖掘机的挖掘轨迹进行规划，从而可以对挖掘机进行自动挖掘控制，进而实现自动挖掘并提高挖掘效率。

可选地，所述挖掘轨迹数据获取步骤包括如下步骤：

第二计算步骤：根据第二计算公式

获取所述挖掘轨迹数据。

其中，θ(t)表示所述挖掘轨迹数据中的角度数据中的其中一种数据，

表示所述挖掘轨迹数据中的挖掘期望角速度，

表示所述挖掘轨迹数据中的挖掘期望加速度，t_f为根据

计算得到的所述挖掘机的目标状态数据中表示所述挖掘机动臂与基座处于目标状态的时刻、所述挖掘机的动臂与小臂处于目标状态的时刻、所述挖掘机的小臂与挖斗处于目标状态的时刻的任意一个时刻，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，θ₀为所述初始状态数据，θ_f为所述目标状态数据。

特别地，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，是挖掘机的参数，一个挖掘机对应一个特定的v_max，可以通过操作人员通过输入获取的，也可以是预先设定好的。

显然，根据挖掘机的基座与动臂之间的初始状态数据、目标状态数据以及角度数据可以计算出挖掘轨迹数据中的关于挖掘机的基座与动臂的挖掘轨迹数据，根据挖掘机的动臂与小臂之间的初始状态数据、目标状态数据以及角度数据可以计算出挖掘轨迹数据中的关于挖掘机的动臂与小臂的挖掘轨迹数据，根据挖掘机的小臂与挖斗之间的初始状态数据、目标状态数据以及角度数据可以计算出挖掘轨迹数据中的关于挖掘机的小臂与挖斗的挖掘轨迹数据。在上述实现过程中，挖掘轨迹数据是根据角度数据以及初始状态数据、目标状态数据通过第二计算公式计算出的，从而通过该挖掘轨迹数据可以准确的对挖掘机的自动挖掘轨迹进行规划。

请参看图4，图4为本申请实施例提供的一种挖掘控制方法中步骤S150的流程框图，所述步骤S150包括如下步骤：

步骤S151：获取所述挖掘机的实时状态数据。

其中，所述实时状态数据为从初始状态数据改变为目标状态数据的实时变化数据。例如，当挖掘机开始进行挖掘时，在开始挖掘后的第一时刻，检测获取到该挖掘机此时的动臂与基座之间的角度为θ₁₁，动臂与小臂之间的角度为θ₁₂，小臂与挖斗之间的角度为θ₁₃，在开始挖掘后的第二时刻，检测获取到该挖掘机此时的动臂与基座之间的角度为θ₂₁，动臂与小臂之间的角度为θ₂₂，小臂与挖斗之间的角度为θ₂₃，在开始挖掘后的第i时刻，检测获取到该挖掘机此时的动臂与基座之间的角度为θ_i1，动臂与小臂之间的角度为θ_i2，小臂与挖斗之间的角度为θ_i3，所以，实时状态数据包括θ₁₁、θ₁₂、θ₁₃、θ₂₁、θ₂₂、θ₂₃、…、θ_i1、θ_i2、θ_i3，根据在挖掘机实际开始进行自动挖掘时在挖掘过程中的各个时刻的实时状态数据可以更准确的对挖掘机进行实际挖掘过程中的控制。

步骤S152：根据

计算获得所述挖掘机的控制量。

其中，k_p为比例系数，k_I为积分系数，t为时间，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，u(t)为所述挖掘机的控制量。例如，当t＝1时，可以测得实时状态数据为θ₁₁、θ₁₂、θ₁₃，可以表示为三维数据形式(θ₁₁，θ₁₂，θ₁₃)，挖掘轨迹数据为θ₁、θ₂、θ₃，可以表示为三维数据形式(θ₁，θ₂，θ₃)，则可以计算得到误差数据e(1)，表示当t＝1时误差数据值为(θ_e1，θ_e2，θ_e3)，其中，θ_e1＝θ₁₁-θ₁，θ_e2＝θ₁₂-θ₂，θ_e3＝θ₁₃-θ₃。

步骤S153：根据所述挖掘机的控制量控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，对挖掘机从初始状态变为目标状态的过程中进行实时状态数据的获取，再对实时状态数据和挖掘轨迹数据进行计算得到误差数据，由于挖掘机在进行挖掘过程中的实时状态数据和挖掘轨迹数据之间不完全相等，从而可以根据误差数据进行计算获得挖掘机的控制量，进而实现对挖掘机进行挖掘控制，实现准确的自动挖掘。

下面以一个具体的实施例对本申请中的挖掘控制方法进行说明。

例如，挖掘机准备开始对其挖掘范围内的目标挖掘点进行挖掘，此时刻记为t＝0，则需要获取t＝0时的初始状态数据，如，初始状态数据包括如图3所示的挖掘机基座与动臂之间的角度θ_a1、挖掘机动臂与小臂之间的角度θ_b1、挖掘机小臂与挖斗之间的角度θ_c1、挖掘机基座与动臂连接处的角速度

和加速度

挖掘机动臂与小臂连接处的角速度

和加速度

挖掘机小臂与挖斗连接处的角速度

和加速度

然后获取包括目标挖掘点的图像，该图像的获取可以通过视觉传感器，也可以通过其他方式进行获取，然后通过对图像进行识别得到目标挖掘点的相对位置(x，y，z)，其中，该相对位置以挖掘机的基座为原点，根据相对位置和初始状态数据可以计算出挖掘机的目标状态数据，如，计算出的目标状态数据包括挖掘机基座与动臂之间的角度θ_af、挖掘机动臂与小臂之间的角度θ_bf、挖掘机小臂与挖斗之间的角度θ_cf、挖掘机基座与动臂连接处的角速度

和加速度

挖掘机动臂与小臂连接处的角速度

和加速度

挖掘机小臂与挖斗连接处的角速度

和加速度

接着计算挖掘轨迹数据，同样的，挖掘轨迹数据中包括挖掘机基座与动臂之间的角度θ_a(t)、挖掘机动臂与小臂之间的角度θ_b(t)、挖掘机小臂与挖斗之间的角度θ_c(t)、挖掘机基座与动臂连接处的角速度

和加速度

挖掘机动臂与小臂连接处的角速度

和加速度

挖掘机小臂与挖斗连接处的角速度

和加速度

例如，若对挖掘机基座与动臂进行计算得到关于挖掘机基座与动臂的挖掘轨迹数据，则有θ_a(t)＝a₀+a₁t+a₂t²+a₃t³，初始状态数据有挖掘机基座与动臂之间的角度θ_a(0)＝θ₀和挖掘机基座与动臂连接处的角速度

目标状态数据有挖掘机基座与动臂之间的目标角度θ_a(t_f)＝θ_f和挖掘机基座与动臂连接处的目标角速度

由此可以整理出关于挖掘机基座与动臂的挖掘轨迹数据

然后需要通过比例-积分控制算法(Proportional Integral,PI)计算挖掘机的控制量，需要获取挖掘机的实时状态数据，如，挖掘机的实时状态数据包括挖掘机基座与动臂之间的角度θ_ai、挖掘机动臂与小臂之间的角度θ_bi、挖掘机小臂与挖斗之间的角度θ_ci、挖掘机基座与动臂连接处的角速度

和加速度

挖掘机动臂与小臂连接处的角速度

和加速度

挖掘机小臂与挖斗连接处的角速度

和加速度

接着计算出e(t)，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，因此，挖掘机基座与动臂的误差数据包括挖掘机基座与动臂之间的角度误差e(t)＝θ_ai(t)-θ_a1，挖掘机基座与动臂连接处角速度的误差

挖掘机基座与动臂连接处加速度的误差

由此，可以根据

对挖掘机的控制量u(t)进行计算，最终挖掘机可以根据挖掘机的控制量u(t)进行自动挖掘。

请参看图5，图5为本申请实施例提供的一种挖掘控制装置200的结构示意图，所述挖掘控制装置200包括：

初始状态数据获取模块210，用于获取挖掘机处于初始状态时的初始状态数据。

其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据。

目标挖掘点相对位置获取模块220，用于获取目标挖掘点相对所述挖掘机的相对位置。

目标状态数据获取模块230，用于根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据。

其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据。

挖掘轨迹数据获取模块240，用于根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据。

挖掘控制模块250，用于根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，挖掘控制装置200通过初始状态数据获取模块210和目标挖掘点相对位置获取模块，可以获取到挖掘机的初始状态数据和目标挖掘点相对所述挖掘机的相对位置，然后目标状态数据获取模块230根据初始状态数据和目标挖掘点相对挖掘机的相对位置计算出挖掘机的目标状态数据，接着挖掘轨迹数据获取模块240再根据初始状态数据和目标状态数据对挖掘机的挖掘进行挖掘规划，得到挖掘轨迹数据，从而可以根据挖掘轨迹数据对挖掘机进行控制，实现自动挖掘，进而提高了挖掘效率，同时在挖掘作业的环境危险的情况下也可以保证操作人员的人身安全。

可选地，所述挖掘轨迹数据获取模块240包括：

角度数据获取单元，用于获取所述挖掘机随时间变化的角度数据。

其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度。

挖掘轨迹数据获取单元，用于根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据。

在上述实现过程中，角度数据获取单元可以获取挖掘机随时间变化的角度数据，该角度数据可以表示出挖掘机各个关节之间角度的变化，该角度数据包括挖掘机的动臂与基座之间随时间变化的角度、挖掘机的动臂与小臂之间随时间变化的角度、以及挖掘机的小臂与挖斗之间随时间变化的角度，通过该角度数据可以对挖掘机的挖掘轨迹进行规划，从而可以对挖掘机进行自动挖掘控制，进而实现自动挖掘并提高挖掘效率。

可选地，所述角度数据获取单元包括：

第一计算子单元，用于根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据。

其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。

在上述实现过程中，第一计算子单元根据第一计算公式可以获取角度数据，该角度数据可以表示出挖掘机各个关节之间角度随时间的变化，不同时刻的角度不同，可以用于准确的对挖掘机的轨迹进行描述，从而对挖掘机的挖掘轨迹进行准确地规划，进而对挖掘机进行自动挖掘控制。

可选地，所述挖掘轨迹数据获取单元，包括：

第二计算子单元，用于根据第二计算公式

获取所述挖掘轨迹数据。

其中，θ(t)表示所述挖掘轨迹数据中的角度数据中的其中一种数据，

表示所述挖掘轨迹数据中的挖掘期望角速度，

表示所述挖掘轨迹数据中的挖掘期望加速度，t_f为根据

计算得到的所述挖掘机的目标状态数据中表示所述挖掘机动臂与基座处于目标状态的时刻、所述挖掘机的动臂与小臂处于目标状态的时刻、所述挖掘机的小臂与挖斗处于目标状态的时刻的任意一个时刻，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，θ₀为所述初始状态数据，θ_f为所述目标状态数据。

在上述实现过程中，第二计算子单元通过第二计算公式计算挖掘轨迹数据，且根据角度数据以及初始状态数据、目标状态数据进行计算，可以保证挖掘轨迹数据的准确性，从而通过该挖掘轨迹数据可以准确的对挖掘机的自动挖掘进行规划。

可选地，所述挖掘控制模块250包括：

实时状态数据获取单元，用于获取所述挖掘机的实时状态数据。

其中，所述实时状态数据为从初始状态数据改变为目标状态数据的实时变化数据。

挖掘机的控制量计算单元，用于根据

计算获得所述挖掘机的控制量。

其中，k_p为比例系数，k_I为积分系数，t为时间，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，u(t)为所述挖掘机的控制量。

控制单元，用于根据所述挖掘机的控制量控制所述挖掘机对所述目标挖掘点进行挖掘。

在上述实现过程中，实时状态数据获取单元对挖掘机从初始状态变为目标状态的过程中进行实时状态数据的获取，挖掘机的控制量计算单元再对实时状态数据和挖掘轨迹数据进行计算得到误差数据，由于挖掘机在进行挖掘过程中的实时状态数据和挖掘轨迹数据之间不完全相等，从而挖掘机的控制量计算单元可以根据误差数据进行计算获得挖掘机的控制量，进而实现对挖掘机进行挖掘控制，实现准确的自动挖掘。

可选地，所述目标挖掘点相对位置获取模块包括：

图像获取单元，用于通过视觉传感器获取包括所述目标挖掘点的图像。

相对位置获取单元，用于对所述图像中的所述目标挖掘点进行识别获得所述目标挖掘点的相对位置。

其中，所述目标挖掘点的相对位置为以所述挖掘机的基座为原点的三维相对位置数据。

在上述实现过程中，图像获取单元先通过视觉传感器获取包括有目标挖掘点的图像，相对位置获取单元再对图像进行特征分析，可以识别出目标挖掘点，并通过图像处理进行计算获得目标挖掘点的相对位置，该相对位置为以挖掘机的基座为原点的三维位置数据，从而能够保证准确的对挖掘机的挖掘进行规划。

本申请实施例提供一种可读取存储介质，所述计算机程序被处理器执行时，执行如图2所示方法实施例中挖掘机控制器所执行的方法过程。

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

综上所述，本申请实施例提供了一种挖掘控制方法、装置及挖掘机控制器，应用于挖掘机控制器，该方法通过获取到的挖掘机的初始状态数据和目标挖掘点相对挖掘机的相对位置计算出挖掘机的目标状态数据，然后再根据初始状态数据和目标状态数据对挖掘机的挖掘进行挖掘规划，得到挖掘轨迹数据，从而可以根据挖掘轨迹数据对挖掘机进行控制，实现自动挖掘，进而提高了挖掘效率，同时在挖掘作业的环境危险的情况下也可以保证操作人员的人身安全。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

Claims (6)

1.一种挖掘控制方法，应用于挖掘机控制器，其特征在于，所述方法包括：

获取挖掘机处于初始状态时的初始状态数据，其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据；

获取目标挖掘点相对所述挖掘机的相对位置；

根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据，其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据；

根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据；

根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘；

所述根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据，包括：

获取所述挖掘机随时间变化的角度数据，其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度；

根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据；

所述获取所述挖掘机随时间变化的角度数据，包括：

根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据，其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。

2.根据权利要求1所述的方法，其特征在于，所述根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据，包括：

根据第二计算公式

获取所述挖掘轨迹数据；

其中，θ(t)表示所述挖掘轨迹数据中的角度数据中的其中一种数据，

表示所述挖掘轨迹数据中的挖掘期望角速度，

表示所述挖掘轨迹数据中的挖掘期望加速度，t_f为根据

计算得到的所述挖掘机的目标状态数据中表示所述挖掘机的动臂与基座处于目标状态的时刻、所述挖掘机的动臂与小臂处于目标状态的时刻、所述挖掘机的小臂与挖斗处于目标状态的时刻的任意一个时刻，v_max为所述挖掘机的参数中的挖掘机臂液压缸最大的运动速度，θ₀为所述初始状态数据，θ_f为所述目标状态数据。

3.根据权利要求1-2任一所述的方法，其特征在于，所述根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘，包括：

获取所述挖掘机的实时状态数据，其中，所述实时状态数据为从初始状态数据改变为目标状态数据的实时变化数据；

根据

计算获得所述挖掘机的控制量，其中，k_p为比例系数，k_I为积分系数，t为时间，e(t)为所述挖掘轨迹数据与所述实时状态数据相减得到的误差数据，u(t)为所述挖掘机的控制量；

根据所述挖掘机的控制量控制所述挖掘机对所述目标挖掘点进行挖掘。

4.根据权利要求1所述的方法，其特征在于，所述获取目标挖掘点相对所述挖掘机的相对位置，包括：

通过视觉传感器获取包括所述目标挖掘点的图像；

对所述图像中的所述目标挖掘点进行识别获得所述目标挖掘点的相对位置，其中，所述目标挖掘点的相对位置为以所述挖掘机的基座为原点的三维位置数据。

5.一种挖掘控制装置，应用于挖掘机控制器，其特征在于，所述装置包括：

初始状态数据获取模块，用于获取挖掘机处于初始状态时的初始状态数据，其中，所述初始状态数据为所述挖掘机还未开始挖掘前的状态数据；

目标挖掘点相对位置获取模块，用于获取目标挖掘点相对所述挖掘机的相对位置；

目标状态数据获取模块，用于根据所述相对位置和所述初始状态数据计算所述挖掘机的目标状态数据，其中，所述目标状态数据为所述挖掘机对所述目标挖掘点完成挖掘动作时的状态数据；

挖掘轨迹数据获取模块，用于根据所述初始状态数据和所述目标状态数据生成得到所述挖掘机的挖掘轨迹数据；

挖掘控制模块，用于根据所述挖掘轨迹数据控制所述挖掘机对所述目标挖掘点进行挖掘，

所述挖掘轨迹数据获取模块包括：

角度数据获取单元，用于获取所述挖掘机随时间变化的角度数据，其中，所述角度数据包括所述挖掘机的动臂与基座之间随时间变化的角度、所述挖掘机的动臂与小臂之间随时间变化的角度、以及所述挖掘机的小臂与挖斗之间随时间变化的角度；

挖掘轨迹数据获取单元，用于根据所述角度数据以及所述初始状态数据、所述目标状态数据获得所述挖掘轨迹数据，

所述角度数据获取单元包括：

第一计算子单元，用于根据第一计算公式θ(t)＝a₀+a₁t+a₂t²+a₃t³获取所述角度数据，其中，a₀、a₁、a₂、a₃为系数，t为时间，θ(t)表示所述角度数据中的其中一种数据。

6.一种挖掘机控制器，其特征在于，包括存储器和处理器，所述存储器存储有计算机可读取指令，当所述计算机可读取指令由所述处理器执行时，运行如权利要求1-4任一所述方法中的步骤。

Images