挖掘机控制方法、装置、挖掘机和存储介质

技术领域

本公开涉及运动规划领域，尤其涉及一种挖掘机控制方法、装置、挖掘机和存储介质。

背景技术

挖掘机作为一种挖掘并移动物料的机械，在采矿和建筑领域有广泛的应用。由于其应用的广泛性，挖掘机操作员必须掌握多种多样的操作技术以适应不同的工况和需求。这使得培养合格的操作员成为耗时耗力的工作。此外，由于操作的复杂性和恶劣的工作环境，操作员难以长时间保持高效的工作状态，这使得挖掘机自动化的需求愈加高涨。

现有的挖掘机系统主要由人来操作，完成诸如直线沟壑挖掘、土方移动运输、平整操作面等高层任务，效率较低。

发明内容

本公开要解决的一个技术问题是，提供一种挖掘机控制方法、装置、挖掘机和存储介质，能够提高挖掘机的工作效率。

根据本公开一方面，提出一种挖掘机控制方法，包括：根据运输车位置和高度信息、以及挖掘机的位置信息，确定挖掘机从挖料到卸料完成过程中的初始位姿、第一过渡点位姿、第二过渡点位姿和末端位姿；利用直线运动规划算法，计算挖掘机从初始位姿到第一过渡点位姿，每个关节在各时刻的第一角度信息；利用曲线运动规划算法，计算挖掘机从第一过渡点位姿到第二过渡点位姿，每个关节在各时刻的第二角度信息；利用直线运动规划算法，计算挖掘机从第二过渡点位姿到末端位姿，每个关节在各时刻的第三角度信息；以及基于第一角度信息、第二角度信息和第三角度信息，控制挖掘机的各个关节运动，以执行挖掘作业。

在一些实施例中，初始位姿为挖掘机的铲斗触地，未开始挖掘时的挖掘机的位姿；第一过渡点位姿为铲斗挖掘完毕时的挖掘机的位姿；第二过渡点位姿为铲斗移动到运输车上方，下降预定高度，处于静态并且未卸料时的挖掘机的位姿；以及末端位姿为铲斗卸料完毕时的挖掘机的位姿。

在一些实施例中，利用直线运动规划算法，计算挖掘机从初始位姿到第一过渡点位姿，每个关节在各时刻的第一角度信息包括：对初始位姿对应的位姿旋转矩阵和第一过渡点位姿对应的初始位姿变换矩阵进行插值处理，得到第一位姿旋转矩阵插值序列；以及根据第一位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在一些实施例中，对初始位姿对应的位姿旋转矩阵和第一过渡点位姿对应的初始位姿变换矩阵进行插值处理，得到第一位姿旋转矩阵插值序列包括：根据铲斗在挖料过程中的速度信息，对速度进行插值并进行归一化，得到第一速度序列；依次按照第一速度序列，对挖掘机从初始位姿到第一过渡点位姿的铲斗的位移进行插值，得到第一位移序列，对铲斗的姿态进行插值，得到第一姿态序列；以及基于第一位移序列和第一姿态序列的组合，得到第一位姿旋转矩阵插值序列。

在一些实施例中，根据曲线运动规划算法，第一过渡点位姿到第二过渡点位姿之间被划分为第一子位姿、第二子位姿、第三子位姿和第四子位姿，其中，第一子位姿为铲斗从挖掘完毕规划运动到能够保持物料不漏状态时的位姿；第二子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第一目标位置时的位姿；第三子位姿为挖掘机的旋转体与车体之间的关节运动到目标关节角度时的铲斗的位姿；以及第四子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第二目标位置，且铲斗处于运动状态时的位姿。

在一些实施例中，挖掘机包括第一关节、第二关节、第三关节和第四关节，其中，第一关节为挖掘机的旋转体与车体之间的关节，第二关节为旋转体与挖掘机的大臂之间的关节，第三关节为大臂与挖掘机的小臂之间的关节，第四关节为挖掘机的小臂与铲斗之间的关节。

在一些实施例中，利用曲线运动规划算法，计算从第一过渡点位姿到第二过渡点位姿，挖掘机每个关节在各时刻的第二角度信息包括：确定铲斗能够保持物料不漏时，第四关节的角度取值范围；根据第一过渡点位姿对应的初始位姿变换矩阵以及第四关节的角度取值范围，得到第一子位姿对应的第一位姿变换矩阵；根据第一位姿变换矩阵、第二过渡点位姿对应的第五位姿变换矩阵、以及第四关节的角度取值范围，得到第二子位姿对应的第二位姿变换矩阵；根据第二位姿变换矩阵和第五位姿变换矩阵，得到第三子位姿对应的第三位姿变换矩阵；根据第三位姿变换矩阵、第五位姿变换矩阵以及第四关节的角度取值范围，得到第四子位姿对应的第四位姿变换矩阵；利用直线运动规划算法，分别计算初始位姿变换矩阵到第一位姿变换矩阵之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵到第二位姿变换矩阵之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵到第三位姿变换矩阵之间各个关节带有时间戳的角度序列、第三位姿变换矩阵到第四位姿变换矩阵之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵到第五位姿变换矩阵之间各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在一些实施例中，根据第一过渡点位姿对应的初始位姿变换矩阵以及第四关节的角度取值范围，得到第一子位姿对应的第一位姿变换矩阵包括：根据第一过渡点位姿对应的初始位姿变换矩阵，反解计算出挖掘机每个关节的初始角度；若预定时刻第四关节的角度位于角度取值范围，则保持第一关节的角度，若预定时刻第四关节的角度未位于角度取值范围，则调整第四关节的角度至角度取值范围，并求解第一关节、第二关节和第三关节的角度；以及根据第一关节、第二关节、第三关节和第四关节的角度，确定第一位姿变换矩阵。

在一些实施例中，根据第一位姿变换矩阵、第五位姿变换矩阵、以及第四关节的角度取值范围，得到第二位姿变换矩阵包括：根据第一位姿变换矩阵和第五位姿变换矩阵，得到第一中间位姿；根据第一中间位姿，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第二位姿变换矩阵。

在一些实施例中，根据第二位姿变换矩阵和第五位姿变换矩阵，得到第三位姿变换矩阵包括：根据第二位姿变换矩阵，反解计算出第二关节、第三关节和第四关节的角度；根据第五位姿变换矩阵，反解出第一关节的角度；根据第一关节、第二关节、第三关节和第四关节的角度，确定第三位姿变换矩阵。

在一些实施例中，根据第三位姿变换矩阵、第五位姿变换矩阵以及第四关节的角度取值范围，得到第四位姿变换矩阵包括：根据第三位姿变换矩阵和第五位姿变换矩阵，得到第二中间位姿；根据第二中间位姿，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第四位姿变换矩阵。

在一些实施例中，对初始位姿变换矩阵到第一位姿变换矩阵之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵到第二位姿变换矩阵之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵到第三位姿变换矩阵之间各个关节带有时间戳的角度序列、第三位姿变换矩阵到第四位姿变换矩阵之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵到第五位姿变换矩阵之间各个关节的带有时间戳的角度序列，进行互补处理。

在一些实施例中，利用直线运动规划算法，计算挖掘机从第二过渡点位姿到末端位姿，每个关节在各时刻的第三角度信息包括：对第二过渡点位姿对应的第五位姿变换矩阵和末端位姿对应的末端位姿变换矩阵进行插值处理，得到第二位姿旋转矩阵插值序列；以及根据第二位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在一些实施例中，对第二过渡点位姿对应的第五位姿变换矩阵和末端位姿对应的末端位姿变换矩阵进行插值处理，得到第二位姿旋转矩阵插值序列包括：根据铲斗在卸料过程中的速度信息，对速度进行插值并进行归一化，得到第二速度序列；依次按照第二速度序列，对挖掘机从第二过渡点位姿到末端位姿的铲斗的位移进行插值，得到第二位移序列，对铲斗的姿态进行插值，得到第二姿态序列；以及基于第二位移序列和第二姿态序列的组合，得到第二位姿旋转矩阵插值序列。

根据本公开的另一方面，还提出一种挖掘机控制装置，包括：位姿确定模块，被配置为根据运输车位置和高度信息、以及挖掘机的位置信息，确定挖掘机从挖料到卸料完成过程中的初始位姿、第一过渡点位姿、第二过渡点位姿和末端位姿；角度计算模块，被配置为利用直线运动规划算法，计算挖掘机从初始位姿到第一过渡点位姿，每个关节在各时刻的第一角度信息；利用曲线运动规划算法，计算挖掘机从第一过渡点位姿到第二过渡点位姿，每个关节在各时刻的第二角度信息；利用直线运动规划算法，计算挖掘机从第二过渡点位姿到末端位姿，每个关节在各时刻的第三角度信息；以及控制模块，被配置为基于第一角度信息、第二角度信息和第三角度信息，控制挖掘机的各个关节运动，以执行挖掘作业。

根据本公开的另一方面，还提出一种挖掘机控制装置，包括：存储器；以及耦接至存储器的处理器，处理器被配置为基于存储在存储器的指令执行如上述的挖掘机控制方法。

根据本公开的另一方面，还提出一种挖掘机，包括：上述的挖掘机控制装置。

根据本公开的另一方面，还提出一种非瞬时性计算机可读存储介质，其上存储有计算机程序指令，该指令被处理器执行时实现上述的挖掘机控制方法。

本公开实施例中，通过对铲斗初始位姿到铲斗末端位姿的运动特点，采用分段运动规划机制，并融合了直线和曲线运动规划算法，在不同的分段运动规划中采用相应的运动规划算法，使得铲齿快速地从初始姿态收敛到末端姿态，在运动过程中使得铲斗的开口面朝上并与地面保持平行状态，从而使得铲斗物料一直保持不漏，提高了挖掘机的工作效率，降低了操作者的作业强度。

通过以下参照附图对本公开的示例性实施例的详细描述，本公开的其它特征及其优点将会变得清楚。

附图说明

构成说明书的一部分的附图描述了本公开的实施例，并且连同说明书一起用于解释本公开的原理。

参照附图，根据下面的详细描述，可以更加清楚地理解本公开，其中：

图1为本公开的挖掘机控制方法的一些实施例的流程示意图；

图2为本公开的不同状态下的挖掘机示意图；

图3为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图4为本公开的插值算法的一些实施例的示意图；

图5为本公开的插值算法的另一些实施例的示意图；

图6为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图7为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图8为本公开的起点重合的向量示意图；

图9为本公开的稳定区域示意图；

图10为本公开的铲斗旋转中心工作截面示意图；

图11为本公开的能够保持物料不漏的位置区域示意图；

图12为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图13为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图14为本公开的挖掘机控制方法的另一些实施例的流程示意图；

图15为本公开的挖掘机控制装置的一些实施例的结构示意图；

图16为本公开的挖掘机控制装置的另一些实施例的结构示意图；

图17为本公开的挖掘机控制装置的另一些实施例的结构示意图；以及

图18为本公开的挖掘机运物料与卸料示意图。

具体实施方式

现在将参照附图来详细描述本公开的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。

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

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本公开及其应用或使用的任何限制。

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

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

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

为使本公开的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本公开进一步详细说明。

图1为本公开的挖掘机控制方法的一些实施例的流程示意图。

在步骤110，根据运输车位置和高度信息、以及挖掘机的位置信息，确定挖掘机从挖料到卸料完成过程中的初始位姿、第一过渡点位姿、第二过渡点位姿和末端位姿。

在一些实施例中，初始位姿为挖掘机的铲斗触地，未开始挖掘时的挖掘机的位姿。例如，挖掘机已经行驶至待挖掘的物料堆和待装载的运输车之间的指定位置，斗齿触地，开口向下，未开时挖掘的状态。运输车例如为无人驾驶矿用卡车，简称矿卡。

第一过渡点位姿为铲斗挖掘完毕时的挖掘机的位姿。例如，铲斗挖掘完毕，铲斗开口向内时的姿态。

第二过渡点位姿为铲斗移动到运输车上方，下降预定高度，处于静态并且未卸料时的挖掘机的位姿。例如，挖掘机完成物料的挖掘后，保持物料不漏，将物料运输至运输车或其他地点，此时，铲斗在运输车上方，开口向上，内容物不漏，即将卸料时的状态。

末端位姿为铲斗卸料完毕时的挖掘机的位姿。例如，铲斗卸料完毕，开口向下，并且位于运输车上方。

在步骤120，利用直线运动规划算法，计算挖掘机从初始位姿到第一过渡点位姿，每个关节在各时刻的第一角度信息。

在一些实施例中，对初始位姿对应的位姿旋转矩阵和第一过渡点位姿对应的初始位姿变换矩阵进行插值处理，得到第一位姿旋转矩阵插值序列；根据第一位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在步骤130，利用曲线运动规划算法，计算挖掘机从第一过渡点位姿到第二过渡点位姿，每个关节在各时刻的第二角度信息。

在一些实施例中，得到满足各关节物理约束的内容物搬运与释放任务的曲线运动规划算法过程中，继续采用分段规划的思想。

根据曲线运动规划算法，第一过渡点位姿到第二过渡点位姿之间被划分为第一子位姿、第二子位姿、第三子位姿和第四子位姿，其中，第一子位姿为铲斗从挖掘完毕规划运动到能够保持物料不漏状态时的位姿；第二子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第一目标位置时的位姿；第三子位姿为挖掘机的旋转体与车体之间的关节运动到目标关节角度时的铲斗的位姿；以及第四子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第二目标位置，且铲斗处于运动状态时的位姿。

在步骤140，利用直线运动规划算法，计算挖掘机从第二过渡点位姿到末端位姿，每个关节在各时刻的第三角度信息。

在一些实施例中，对第二过渡点位姿对应的第五位姿变换矩阵和末端位姿对应的末端位姿变换矩阵进行插值处理，得到第二位姿旋转矩阵插值序列；以及根据第二位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在步骤150，基于第一角度信息、第二角度信息和第三角度信息，控制挖掘机的各个关节运动，以执行挖掘作业。

在上述实施例中，挖掘机运输物料的连续动作主要包括抬起挖掘机操作臂、旋转挖掘机操作臂和下降挖掘机操作臂，由于这一系列的连续动作中既包括直线运动也包括曲线运动，该实施例采用分段规划，融合了直线和曲线两种运动规划算法，能够提升挖掘机操作臂的运动效率，并且降低操作者的作业强度。

在本公开的一些实施例中，如图2所示，挖掘机包括第一关节、第二关节、第三关节和第四关节，其中，第一关节为挖掘机的旋转体与车体之间的关节，第二关节为旋转体与挖掘机的大臂之间的关节，第三关节为大臂与挖掘机的小臂之间的关节，第四关节为挖掘机的小臂与铲斗之间的关节。第一关节的角度为θ1，第二关节的角度为θ2，第三关节的角度为θ3，第四关节的角度为θ4。挖掘机从初始位姿到第一过渡点位姿，第一关节的角度θ1不变，即挖掘机没有旋转。

在执行本公开的挖掘机控制方法之前，应该建立挖掘机操作臂运动学模型，以及设置挖掘机的物理约束条件。

在一些实施例中，基于MDH(Modified_D-H，改进的D-H)方法建立挖掘机操作臂运动学模型，运动学参数列表如表1所示：

表1挖掘机Modified_D-H参数表

其中，j为挖掘机操作臂坐标系序号，α为当前连杆长度，a为连杆扭转角，d为连杆偏移，d为关节角度。关节5表示固结于铲齿的坐标系，该坐标系随铲齿而动。a

表2典型参数

在一些实施例中，挖掘机的物理约束条件指各关节在运动过程中应该满足角度约束、速度约束、加速度约束。物理约束参数如表3所示：

表3物理约束参数

下面将对挖掘过程中，调用直线运动规划算法，根据铲斗初始位姿和第一过渡点位姿对抬起挖掘机操作臂的直线运动进行规划的方法进行介绍。

图3为本公开的挖掘机控制方法的另一些实施例的流程示意图。

在步骤310，根据铲斗在挖料过程中的速度信息，对速度进行插值并进行归一化，得到第一速度序列。

如图4所示，为速度-时间曲线示意图，曲线的面积为1，从0插值到1，速度先匀加速、后匀速、最后匀减速。插值算法最终得到分布在[0,1]区间的符合图4所示速度曲线特征的插值序列，即相当于在连续的曲线中进行等间隔离散采样，间隔的值取决于采样数目Num，间隔值

在步骤320，依次按照第一速度序列，对挖掘机从初始位姿到第一过渡点位姿的铲斗的位移进行插值，得到第一位移序列，对铲斗的姿态进行插值，得到第一姿态序列。

如图5所示，顺序按照插值序列对位移进行直线插值。对于任意末端在向量

顺序按照插值序列对姿态进行插值。需要将初末姿态转化为单位四元数，然后按照得到的插值序列进行插值，该过程可以利用已经成熟的方法，此处不再进一步阐述。

在姿态的插值过程中，由于角度的旋转周期性，存在着两种插值方向，考虑到实际应用中，需要快速高效的由初始姿态收敛到第一过渡点姿态，因此，选取能够最短由初始姿态收敛到第一过渡点姿态的插值方向。

在步骤330，基于第一位移序列和第一姿态序列的组合，得到第一位姿旋转矩阵插值序列。

将位移和姿态组合在一起，构成齐次转换矩阵序列，中间部分由插值算法插值得到。整个插值算法的效果是：从位移的角度来说，铲齿先加速后匀速最后减速；从姿态的角度来说，铲斗相对于地面先加速旋转后匀速最后减速直到收敛到第一过渡点位姿。

在步骤340，根据第一位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

在一些实施例中，如图6所示，该步骤340包括步骤610-6120。

在步骤610，初始化角度序列。

在步骤620，遍历第一位姿旋转矩阵插值序列。

在步骤630，通过对铲斗位姿求反解获取其他关节的角度。

在步骤640，判断新加入的四个角度中任一个角度是否符合对应的角度限制，若是，则执行步骤650，否则，执行步骤651。

在步骤650，判断是否遍历结束，若是，则执行步骤660，否则，继续执行步骤620。

在步骤651，确认规划失败，调整初始位姿和第一过渡点位姿。

在步骤660，获取各关节角度序列。

在步骤670，根据控制器的时间间隔，计算各个关节序列中个点的角速度和角加速度。

在步骤680，判断任一关节序列点的角速度和角加速度是否超出范围，若是，则执行步骤690，否则，执行步骤6100。

在步骤690，使用二分法增加该序列点的间隔时间，后续继续执行步骤670。

在步骤6100，判断是否遍历结束，若结束，则执行步骤6110，否则，继续执行步骤670。

在步骤6110，在序列的相邻两个角度间线性插入n个角度值，其中，n＝点间的间隔时间/控制器的时间间隔。

在步骤6120，获取每个时间点对应的角度值序列。该角度序列中相邻角度的到达时间间隔为t

具体算法实现如下所示：

在上述实施例中，在基础插值算法中，得到了数量为Num的从初始位姿到第一过渡点位姿的齐次旋转矩阵插值序列T

下面将对在旋转挖掘机操作臂的过程中，调用曲线运动规划算法，根据第一过渡点位姿和第二过渡点位姿对旋转挖掘机操作臂的曲线运动进行规划的方法的介绍。

图7为本公开的挖掘机控制方法的另一些实施例的流程示意图。

在步骤710，确定铲斗能够保持物料不漏时，第四关节的角度取值范围。

首先，需要定义什么是物料不漏。在一些实施例中，如图2所示，定义向量

如图8所示，

据此角度体系、现场地面平整度以及规划的挖掘机运动轨迹，有物料不漏的稳定角度α

在定义好S以后，能根据实际情况，去限制铲斗的运动范围，保证整个挖掘机操作臂在操作时，始终保持铲斗与水平面平行，无论动臂、斗杆如何运动，均可使铲斗与水平面平行，从而保证铲斗中物料不漏。

在实际应用中，根据现场工况定义S的范围，如果S定义为[150,-150]，θ

可以看到，在当前的参数设置情况下，不是任意的铲斗旋转中心位置都能找到一个符合角度物理约束的θ4取值，使得能够保持物料不漏。这就说明要想在超出稳定范围之外进行内容物的搬运，需要结合挖掘机底盘在世界坐标系的运动规划，使得最终需要搬运到的目标位置在稳定区域内，以此来完成需求任务。

上述实施例中，以起点为关节2终点为关节3的向量，在水平面的投影单位向量和以起点为关节4终点为铲齿的单位向量之间的夹角，构建铲斗与水平地面之间的夹角的角度体系，根据该角度体系、现场地面平整度以及规划的挖掘机运动轨迹确定铲斗稳定区间。根据该稳定区间挖掘机操作臂的运动进行规划，使得铲斗在整个物料运输流程中铲斗的开口面始终与地面平行，从而保证铲斗内的物料保持不漏。

在步骤720，根据第一过渡点位姿对应的初始位姿变换矩阵T0以及第四关节的角度取值范围，得到第一子位姿对应的第一位姿变换矩阵T1。

在一些实施例中，根据第一过渡点位姿对应的初始位姿变换矩阵，反解计算出挖掘机每个关节的初始角度；若预定时刻第四关节的角度位于角度取值范围，则保持第一关节的角度，若预定时刻第四关节的角度未位于角度取值范围，则调整第四关节的角度至角度取值范围，并求解第一关节、第二关节和第三关节的角度；以及根据第一关节、第二关节、第三关节和第四关节的角度，确定第一位姿变换矩阵T1。

已知挖掘机的前三个角度(θ1，θ2，θ3)和以

已知挖掘机的四个角度(θ1，θ2，θ3，θ4)，能够得到以

基于上面规划算法，在给定初始位姿变换矩阵T0后，反解出初始的挖掘机的四个角度；接着在给定铲斗物料稳定角度区间的基础上，得到能够使得内容物保持不漏的θ4的可取范围。有时可能会由于稳定区间过于严苛如设置为[180,180]，以及θ4的角度物理可行范围过小，导致能够保持内容物不漏的θ4的可取范围为空，此时抛出一错误终止算法，认为此种情况不是算法要处理的。判断目前的θ4的值是否在该范围内，如果是，则θ4的值保持不变，说明此时不需要调整即可保证物料不漏，否则使规划θ4的值到达该范围内的某一值，这个值可以是该范围的中间值。得到调整后的θ1，θ2，θ3，θ4的值，得到T

在步骤730，根据第一位姿变换矩阵T1、第二过渡点位姿对应的第五位姿变换矩阵Tf、以及第四关节的角度取值范围，得到第二子位姿对应的第二位姿变换矩阵T2。

在一些实施例中，根据第一位姿变换矩阵T1和第五位姿变换矩阵Tf，得到第一中间位姿T_mid1；根据第一中间位姿T_mid1，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第二位姿变换矩阵T2。

如图12所示，该过程包括步骤1210-1280。

在步骤1210，输入挖斗的第一位姿变换矩阵T1、第五位姿变换矩阵Tf和物料不漏的挖斗角度区间S。

在步骤1220，通过二分法求取第一中间位姿T_mid1。

在步骤1230，根据第一中间位姿T_mid1，反解其他三个关节的角度θ1，θ2，θ3。

在步骤1240，根据θ1，θ2，θ3求正解，计算铲斗角度θ4。

在步骤1250，判断铲斗角度θ4是否符合区间S，若是，则执行步骤1260，否则，执行步骤1270。

在步骤1260，根据已得到的θ1，θ2，θ3，θ4的值，求出第二位姿变换矩阵T2。

在步骤1270，判断是否超出错误阈值，若是，则执行步骤1280，否则，继续执行步骤1220。

在步骤1280，规划失败。

具体算法实现过程如下所示：

在步骤740，根据第二位姿变换矩阵T2和第五位姿变换矩阵Tf，得到第三子位姿对应的第三位姿变换矩阵T3。

在一些实施例中，根据第二位姿变换矩阵T2，反解计算出第二关节、第三关节和第四关节的角度；根据第五位姿变换矩阵Tf，反解出第一关节的角度；根据第一关节、第二关节、第三关节和第四关节的角度，确定第三位姿变换矩阵T3。

在该步骤之前的规划算法，均未对θ1的值做出改动。根据第二位姿变换矩阵T2反解得到的θ2，θ3，θ4的值不变，根据第五位姿变换矩阵Tf，反解出θ1的值，其中，θ1的初值为为T2反解得到的θ1的值，末值为T

在步骤750，根据第三位姿变换矩阵T3、第五位姿变换矩阵Tf以及第四关节的角度取值范围，得到第四子位姿对应的第四位姿变换矩阵T4。

在一些实施例中，根据第三位姿变换矩阵T3和第五位姿变换矩阵Tf，得到第二中间位姿T_mid2；根据第二中间位姿T_mid2，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第四位姿变换矩阵T4。

如图13所示，该过程包括步骤1310-1380。

在步骤1310，输入挖斗的第三位姿变换矩阵T3、第五位姿变换矩阵Tf和物料不漏的挖斗角度区间S。

在步骤1320，通过二分法求取第二中间位姿T_mid2。

在步骤1330，根据第二中间位姿T_mid2，反解其他三个关节的角度θ1，θ2，θ3。

在步骤1340，根据θ1，θ2，θ3求正解，计算铲斗角度θ4。

在步骤1350，判断铲斗角度θ4是否负荷区间S，若是，则执行步骤1360，否则，执行步骤1370。

在步骤1360，根据已得到θ1，θ2，θ3，θ4的值，求出第四位姿变换矩阵T4。

在步骤1370，判断是否超出错误阈值，若是，则执行步骤1380，否则，继续执行步骤1320。

在步骤1380，规划失败。

具体算法实现过程如下所示：

在步骤760，利用直线运动规划算法，分别计算初始位姿变换矩阵T0到第一位姿变换矩阵T1之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵T1到第二位姿变换矩阵T2之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵T2到第三位姿变换矩阵T3之间各个关节带有时间戳的角度序列、第三位姿变换矩阵T3到第四位姿变换矩阵T4之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵T4到第五位姿变换矩阵Tf之间各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

给定的各个中间的齐次转换矩阵，并不是一步直达，为了满足各关节的速度和加速度约束，中间需要有很多中间矩阵，进而得到角度序列。该角度序列的初末值运用正运动学可得到T

在上述实施例中，在曲线运动规划中，进一步采用分段规划的思想，在保持铲斗物料不漏的前提下，首先从第一过渡点位姿处在垂直于地面的方向上最大限度的向目标位置移动，然后控制挖掘机回转关节旋转至目标角度，再继续在平行于地面的方向上最大限度的向目标位置移动，最后收敛到第二过渡点位姿。该规划方法能节省工作时间，提高工作效率。

在一些实施例中，对初始位姿变换矩阵T0到第一位姿变换矩阵T1之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵T1到第二位姿变换矩阵T2之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵T2到第三位姿变换矩阵T3之间各个关节带有时间戳的角度序列、第三位姿变换矩阵T3到第四位姿变换矩阵T4之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵T4到第五位姿变换矩阵Tf之间各个关节的带有时间戳的角度序列，进行互补处理。

例如，最终的角度序列为上述各角度序列的顺序拼接，但有时为了提高执行效率，缩短执行时间，有些序列是有交叉的，但肯定会经过T

下面将对在释放物料过程中，继续调用直线运动规划算法，根据第二过渡点位姿和铲斗末端位姿对挖掘机操作臂的直线运动进行规划进行介绍。

图14为本公开的挖掘机控制方法的另一些实施例的流程示意图。

在步骤1410，根据铲斗在卸料过程中的速度信息，对速度进行插值并进行归一化，得到第二速度序列。

其中，对速度进行插值并进行归一化的过程与步骤310类似，此处不再进一步阐述。

在步骤1420，依次按照第二速度序列，对挖掘机从第二过渡点位姿到末端位姿的铲斗的位移进行插值，得到第二位移序列，对铲斗的姿态进行插值，得到第二姿态序列。

对铲斗位移和姿态进行插值的过程与步骤320类似，此处不再进一步阐述。

在步骤1430，基于第二位移序列和第二姿态序列的组合，得到第二位姿旋转矩阵插值序列。

在步骤1440，根据第二位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

该步骤1440与步骤340类似，此处不再进一步阐述。

本公开的上述实施例中，采用满足各关节物理约束的内容物运输运动规划算法，并采用分段规划的思想，将直线规划和空间曲线规划进行组合规划，在挖掘机和矿卡的配合工作空间中选择一系列过渡点，使用上述两种规划方式来组合规划，能够以最快的速度完成整个装卸货流程，提高工作效率。

图15为本公开的挖掘机控制装置的一些实施例的结构示意图，该挖掘机控制装置包括位姿确定模块1510、角度计算模块1520和控制模块1530。

位姿确定模块1510被配置为根据运输车位置和高度信息、以及挖掘机的位置信息，确定挖掘机从挖料到卸料完成过程中的初始位姿、第一过渡点位姿、第二过渡点位姿和末端位姿。

在一些实施例中，初始位姿为挖掘机的铲斗触地，未开始挖掘时的挖掘机的位姿；第一过渡点位姿为铲斗挖掘完毕时的挖掘机的位姿；第二过渡点位姿为铲斗移动到运输车上方，下降预定高度，处于静态并且未卸料时的挖掘机的位姿；以及末端位姿为铲斗卸料完毕时的挖掘机的位姿。

角度计算模块1520被配置为利用直线运动规划算法，计算挖掘机从初始位姿到第一过渡点位姿，每个关节在各时刻的第一角度信息；利用曲线运动规划算法，计算挖掘机从第一过渡点位姿到第二过渡点位姿，每个关节在各时刻的第二角度信息；利用直线运动规划算法，计算挖掘机从第二过渡点位姿到末端位姿，每个关节在各时刻的第三角度信息。

在一些实施例中，角度计算模块1520对初始位姿对应的位姿旋转矩阵和第一过渡点位姿对应的初始位姿变换矩阵进行插值处理，得到第一位姿旋转矩阵插值序列；以及根据第一位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

根据铲斗在挖料过程中的速度信息，对速度进行插值并进行归一化，得到第一速度序列；依次按照第一速度序列，对挖掘机从初始位姿到第一过渡点位姿的铲斗的位移进行插值，得到第一位移序列，对铲斗的姿态进行插值，得到第一姿态序列；以及基于第一位移序列和第一姿态序列的组合，得到第一位姿旋转矩阵插值序列。

在一些实施例中，根据曲线运动规划算法，第一过渡点位姿到第二过渡点位姿之间被划分为第一子位姿、第二子位姿、第三子位姿和第四子位姿，其中，第一子位姿为铲斗从挖掘完毕规划运动到能够保持物料不漏状态时的位姿；第二子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第一目标位置时的位姿；第三子位姿为挖掘机的旋转体与车体之间的关节运动到目标关节角度时的铲斗的位姿；以及第四子位姿为铲斗在保持物料不漏的前提下，在垂直于地面的方向上最大限度的移动到第二目标位置，且铲斗处于运动状态时的位姿。

挖掘机包括第一关节、第二关节、第三关节和第四关节，其中，第一关节为挖掘机的旋转体与车体之间的关节，第二关节为旋转体与挖掘机的大臂之间的关节，第三关节为大臂与挖掘机的小臂之间的关节，第四关节为挖掘机的小臂与铲斗之间的关节。

角度计算模块1520确定铲斗能够保持物料不漏时，第四关节的角度取值范围；根据第一过渡点位姿对应的初始位姿变换矩阵以及第四关节的角度取值范围，得到第一子位姿对应的第一位姿变换矩阵；根据第一位姿变换矩阵、第二过渡点位姿对应的第五位姿变换矩阵、以及第四关节的角度取值范围，得到第二子位姿对应的第二位姿变换矩阵；根据第二位姿变换矩阵和第五位姿变换矩阵，得到第三子位姿对应的第三位姿变换矩阵；根据第三位姿变换矩阵、第五位姿变换矩阵以及第四关节的角度取值范围，得到第四子位姿对应的第四位姿变换矩阵；利用直线运动规划算法，分别计算初始位姿变换矩阵到第一位姿变换矩阵之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵到第二位姿变换矩阵之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵到第三位姿变换矩阵之间各个关节带有时间戳的角度序列、第三位姿变换矩阵到第四位姿变换矩阵之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵到第五位姿变换矩阵之间各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

例如，根据第一过渡点位姿对应的初始位姿变换矩阵，反解计算出挖掘机每个关节的初始角度；若预定时刻第四关节的角度位于角度取值范围，则保持第一关节的角度，若预定时刻第四关节的角度未位于角度取值范围，则调整第四关节的角度至角度取值范围，并求解第一关节、第二关节和第三关节的角度；以及根据第一关节、第二关节、第三关节和第四关节的角度，确定第一位姿变换矩阵。

再例如，根据第一位姿变换矩阵和第五位姿变换矩阵，得到第一中间位姿；根据第一中间位姿，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第二位姿变换矩阵。

再例如，根据第二位姿变换矩阵，反解计算出第二关节、第三关节和第四关节的角度；根据第五位姿变换矩阵，反解出第一关节的角度；根据第一关节、第二关节、第三关节和第四关节的角度，确定第三位姿变换矩阵。

再例如，根据第三位姿变换矩阵和第五位姿变换矩阵，得到第二中间位姿；根据第二中间位姿，反解计算出第一关节、第二关节和第三关节的角度；根据第一关节、第二关节和第三关节的角度，正解出第四关节的角度；若正解出的第四关节的角度，满足角度取值范围，则根据第一关节、第二关节、第三关节和第四关节的角度，确定第四位姿变换矩阵。

在一些实施例中，角度计算模块1520对初始位姿变换矩阵到第一位姿变换矩阵之间各个关节的带有时间戳的角度序列、第一位姿变换矩阵到第二位姿变换矩阵之间各个关节的带有时间戳的角度序列、第二位姿变换矩阵到第三位姿变换矩阵之间各个关节带有时间戳的角度序列、第三位姿变换矩阵到第四位姿变换矩阵之间各个关节的带有时间戳的角度序列、以及第四位姿变换矩阵到第五位姿变换矩阵之间各个关节的带有时间戳的角度序列，进行互补处理。

在一些实施例中，角度计算模块1520对第二过渡点位姿对应的第五位姿变换矩阵和末端位姿对应的末端位姿变换矩阵进行插值处理，得到第二位姿旋转矩阵插值序列；以及根据第二位姿旋转矩阵插值序列以及控制关节角度时的时间间隔，得到各个关节的带有时间戳的角度序列，其中，每个关节在运动过程中满足对应关节的角度约束条件、速度约束条件和加速度约束条件。

根据铲斗在卸料过程中的速度信息，对速度进行插值并进行归一化，得到第二速度序列；依次按照第二速度序列，对挖掘机从第二过渡点位姿到末端位姿的铲斗的位移进行插值，得到第二位移序列，对铲斗的姿态进行插值，得到第二姿态序列；以及基于第二位移序列和第二姿态序列的组合，得到第二位姿旋转矩阵插值序列。

控制模块1530被配置为基于第一角度信息、第二角度信息和第三角度信息，控制挖掘机的各个关节运动，以执行挖掘作业。

在上述实施例中，提出满足各关节物理约束的物料运输运动规划算法，同时采用分段规划的思想，直线规划和空间曲线规划进行组合规划，在挖掘机和运输车的配合工作空间中选择一系列过渡点，使用上述两种规划方式来组合规划，能够以最快的速度完成整个装卸货流程，实现复杂的自动化任务，提高了工作效率。

图16为本公开的挖掘机控制装置的另一些实施例的结构示意图，该挖掘机控制装置包括存储器1610和处理器1620。其中：存储器1610可以是磁盘、闪存或其它任何非易失性存储介质。存储器1610用于存储上述实施例中的指令。处理器1620耦接至存储器1610，可以作为一个或多个集成电路来实施，例如微处理器或微控制器。该处理器1620用于执行存储器中存储的指令。

在该实施例中，通过存储器存储数据指令，再通过处理器处理上述指令，能够针对复杂和恶劣的工作环境，降低操作者的作业强度，提高挖掘机的作业效率。

图17为本公开的挖掘机控制装置的另一些实施例的结构示意图，该挖掘机控制装置包括挖掘机协同作业管理模块1710、以太网通信模块1720、CAN总线通信模块1730、机械臂位姿定位模块1740、中央处理器1750、动力单元模块1760、机械臂电控制单元模块1770、轨迹规划模块1780。其中，挖掘机协同作业管理模块1710与轨迹规划模块1780运行在中央处理器1750上。挖掘机协同作业管理模块1710与轨迹规划模块1780之间可以数据共享。挖掘机协同作业管理模块1710通过以太网通信模块1720建立与矿卡的通信连接，获取矿卡的位姿数据、车体规格数据、矿卡运行状态等数据。轨迹规划模块1780可以接受挖掘机与矿卡的位姿数据、挖掘机规格数据、机械臂各关节及连杆角度数据。挖掘机的位姿数据、挖掘机规格数据、机械臂各关节及连杆角度数据均是从机械臂位姿定位模块1740获取。中央处理器1750具有CAN总线及以太网通信接口。中央处理器1750根据轨迹规划模块1780输出的轨迹规划结果通过CAN总线通信模块1730传输给机械臂电控单元模块1770，机械臂电控单元模块1770将控制信息传递给动力系统，驱动机械臂按照规划的动作运行。

在本公开的一些实施例中，保护一种挖掘机，该挖掘机包括上述实施例中的挖掘机控制装置，将挖掘机控制装置部署到挖掘机上后，该挖掘机可以实现复杂的自动化任务，具有较好的工程实用化价值，能够极大解放人力，提高生产效率。

在一些具体实施例中，如图18所示，挖掘机停在适合给矿卡装载的位置，司机通过操作机器完成挖掘动作。司机按下自动搬运按钮。轨迹规划单元根据挖掘机与矿卡的位姿数据、挖掘机规格数据、机械臂各关节及连杆角度数据、矿卡货箱高度，初始位姿，按照本申请的运动规划方法，确定第一过渡点、第二过渡点、末尾位姿点。并进行分段规划，输出带有时间戳的角度序列。角度序列数据通过CAN总线通信单元传输给机械臂电控单元模块，机械臂电控单元模块将控制信息传递给动力系统，驱动机械臂按照规划的动作运行。至此完成搬运、卸料动作。

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

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

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

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

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

虽然已经通过示例对本公开的一些特定实施例进行了详细说明，但是本领域的技术人员应该理解，以上示例仅是为了进行说明，而不是为了限制本公开的范围。本领域的技术人员应该理解，可在不脱离本公开的范围和精神的情况下，对以上实施例进行修改。本公开的范围由所附权利要求来限定。