挖掘机及其铲斗定位方法、电子设备、存储介质与流程

本发明涉及挖掘机技术领域，尤其涉及挖掘机及其铲斗定位方法、电子设备、存储介质。

背景技术：

目前，常用的挖掘机铲斗坐标计算方法主要有以下三种：

1、激光引导：通过架设激光发射器以及铲斗激光接收器，激光发射器扫出激光基准平面，铲斗激光接收器接收激光信号，由车载控制器根据激光基准平面引导挖掘机做平面的挖掘工作。

2、全站仪引导：类似于激光引导，但全站仪可测量铲斗三维坐标，可进行三维引导。

3、全球定位系统引导：同样可进行三维引导，通过在挖掘机车身安装卫星接收机，拉线位移传感器，利用车身固有机械结构模型可精确推算铲斗坐标，并且可适应于铲斗水下作业。

其中，激光引导为一维引导，不能做横向和纵向的引导；全站仪引导虽然可进行三维引导，但是当铲斗在水下作业时，该方案便难以精确测量铲斗坐标；激光引导、全站仪引导受环境影响较大，例如在进行水下工作时，由于光线在水中的折射很难判断铲斗位置。全球定位系统引导同样为三维引导，可适应于铲斗水下作业，但在实际施工过程中受工作环境影响较大，且机械臂姿态多变，对铲斗坐标计算的精度难以达到施工精度要求。

基于此，提供一种能够适应于各种施工环境并进行精确计算定位的挖掘机及铲斗定位方法，成了目前亟待解决的技术问题。

技术实现要素：

为了克服现有技术的不足，本发明的目的之一在于提供挖掘机，其能够解决挖掘机难以精确计算定位铲斗位置的问题。

本发明的目的之一采用如下技术方案实现：

挖掘机，挖掘机设有四个动态传感器，包括车身倾角传感器、动臂倾角传感器、斗杆倾角传感器、铲斗倾角传感器，车身倾角传感器设于挖掘机车身以测量车身倾角、动臂倾角传感器设于挖掘机动臂以测量动臂倾角、斗杆倾角传感器设于挖掘机斗杆以测量斗杆倾角、铲斗倾角传感器设于挖掘机铲斗以测量铲斗倾角。

为了克服现有技术的不足，本发明的目的之二在于提供铲斗定位方法，其能够解决挖掘机难以精确计算定位铲斗位置的问题。

本发明的目的之二采用如下技术方案实现：

铲斗定位方法，应用于如本发明目的之一的挖掘机，包括以下步骤：

步骤s1、建立挖掘机运动学模型：采用右手定则规定正方向，建立的坐标系包括地理坐标系、导航坐标系、载体坐标系，动臂坐标系、斗杆坐标系、铲斗坐标系、铲尖坐标系和根据gnss建立的卫星坐标系；

其中，载体坐标系的原点设于挖掘机动臂支点，导航坐标系的原点设于挖掘机底盘旋转中心位置；

俯仰角为导航坐标系绕地理坐标系x轴旋转的欧拉角，横滚角为导航坐标系绕地理坐标系y轴旋转的欧拉角，航向角为导航坐标系绕地理坐标系z轴旋转的欧拉角；

步骤s2、计算动臂支点坐标：根据天线相位中心在地理坐标系中的坐标，及由rtk测量得到的天线相位中心在地理坐标系中的位置矢量，计算动臂支点坐标；

步骤s3、计算挖掘机动臂旋转角度：通过挖掘机机械参数、动臂倾角计算动臂旋转角度；

步骤s4、计算挖掘机斗杆旋转角度：根据斗杆倾角、动臂旋转角度计算斗杆旋转角度；

步骤s5、计算挖掘机铲斗旋转角度：根据铲斗倾角、动臂旋转角度、斗杆旋转角度计算铲斗旋转角度；

步骤s6、计算铲尖位置：根据动臂支点坐标，通过建立dh矩阵模型计算铲尖位置。

以卫星定位为基础，结合动态传感器建立挖掘机运动学模型，采用动臂支点作为载体坐标系的原点，通过测量天线到动臂支点处的相对位置得到动臂支点坐标，进一步计算动臂旋转角度、斗杆旋转角度，再结合dh矩阵模型精确计算铲尖位置，通过卫星定位及动态传感器测量精度、灵敏度高、对施工环境要求较低，且利用dh矩阵对于多自由角度的挖掘机机械臂具有很好的适应性，在机械臂较多的情况下依旧能够保证计算的精准度。

优选的，步骤s6还包括以下步骤：

步骤s61、于动臂支点建立dh坐标系，根据挖掘机运动学模型得到dh参数；

步骤s62、根据dh参数及动臂支点坐标计算铲尖坐标，获取铲尖位置。

优选的，载体坐标系的原点设于挖掘机动臂支点，用右手定则规定正方向，载体坐标系x轴正向指向挖掘机驾驶室右侧，载体坐标系y轴正向指向挖掘机车头方向，载体坐标系z轴正向指向挖掘机顶部。

优选的，定义动臂于挖掘机机身的动臂支点为第一机械点、挖掘机动臂油缸与动臂连接处为第二机械点、动臂与挖掘机斗杆连接处为第三机械点，斗杆与挖掘机斗杆油缸间设有第一连杆，斗杆油缸与挖掘机铲斗间设有第二连杆，定义第一连杆与斗杆连接处为第四机械点、斗杆与铲斗连接处为第五机械点、第二连杆与斗杆油缸连接处为第六机械点、第二连杆与铲斗连接处为第七机械点、铲尖为第八机械点；挖掘机机械参数包括第一机械点与第二机械点连接形成的第一参考线及其长度、第一机械点与第三机械点连接形成的第二参考线及其长度、第二机械点与第三机械点连接形成的第三参考线及其长度、第三机械点与第四机械点连接形成的第四参考线及其长度、第三机械点与第五机械点连接形成的第五参考线及其长度、第四机械点与第五机械点连接形成的第六参考线及其长度、第四机械点与第六机械点连接形成的第七参考线及其长度、第五机械点与第七机械点连接形成的第八参考线及其长度、第六机械点与第七机械点连接形成的第九参考线及其长度、第六机械点与第八机械点连接形成的第十参考线及其长度。

优选的，动臂倾角传感器设于第二参考线，斗杆倾角传感器设于第五参考线，铲斗倾角传感器设于第十参考线。

为了克服现有技术的不足，本发明的目的之三在于提供一种电子设备，其能够解决挖掘机难以精确计算定位铲斗位置的问题。

本发明的目的之三采用如下技术方案实现：

一种电子设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，其特征在于：所述处理器执行所述程序时实现如本发明目的之二的铲斗定位方法。

为了克服现有技术的不足，本发明的目的之四在于提供一种存储介质，其能够解决挖掘机难以精确计算定位铲斗位置的问题。

本发明的目的之四采用如下技术方案实现：

一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如本发明目的之二的铲斗定位方法。

相比现有技术，本发明的有益效果在于：

以卫星定位为基础，结合动态传感器建立挖掘机运动学模型，采用动臂支点作为载体坐标系的原点，通过测量天线到动臂支点处的相对位置得到动臂支点坐标，根据计算得到的动臂旋转角度、斗杆旋转角度，结合dh矩阵模型精确计算定位铲尖，通过卫星定位及动态传感器测量精度、灵敏度高、对施工环境要求较低，且利用dh矩阵对于多自由角度的挖掘机机械臂具有很好的适应性，在机械臂较多的情况下依旧能够保证计算的精准度，该挖掘机及其铲斗定位方法能够对挖掘机进行精确计算定位。

附图说明

图1为本发明较佳实施例的铲斗定位方法的流程图；

图2为本发明较佳实施例的铲斗定位方法的挖掘机运动学模型示意图；

图3为本发明较佳实施例的铲斗定位方法的动臂、斗杆、铲斗旋转角度示意图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明做进一步描述：

如图1-3所示，涉及挖掘机及其铲斗定位方法。

本发明较佳示例的挖掘机，设有四个动态传感器，包括车身倾角传感器、动臂倾角传感器、斗杆倾角传感器、铲斗倾角传感器，车身倾角传感器设于挖掘机车身以测量车身倾角、动臂倾角传感器设于挖掘机动臂以测量动臂倾角、斗杆倾角传感器设于挖掘机斗杆以测量斗杆倾角、铲斗倾角传感器设于挖掘机铲斗以测量铲斗倾角。

本发明所提“gnss”指本发明基于卫星导航定位技术采用的全球导航卫星系统。“挖掘机运动学模型”的建立，以gnss为基础，在实际操作中，仅需要确定用动臂支点作为原点，利用已知卫星天线到车体动臂支点处的三维坐标以及俯仰角、横滚角、航向角，测量天线到动臂支点处的相对位置即可进一步计算动臂支点的高斯坐标，避免了现有技术中在建立挖掘机运动学模型时惯用的测量方法(如以车身旋转中心为载体坐标系原点)以至于实际应用的测量场景受限，且测量难度较大。

本发明所示“动态传感器”为重力导向的动态mems倾角传感器，其测量精度、灵敏度高，通过对动态mems倾角传感器的应用能够精确知道倾角大小，且提高了机械臂在动态情况下的倾角测量精度，同时，本发明采用的mems传感器有四个，各传感器对应于挖掘机上的位置测量挖掘机机身及各机械臂相应的倾角，操作方便。

本发明所示rtk，即real-timekinematic，指载波相位差分技术，本发明较佳示例通过rtk测量得出天线相位中心在地理坐标系中的位置矢量。

配合本发明实施例所示的挖掘机及其铲斗定位方法，还包括一种电子设备，该电子设备包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，处理器执行程序时实现如本发明实施例所示的铲斗定位方法。

同样地，配合本发明实施例所示的挖掘机及其铲斗定位方法，还包括一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现如本发明实施例所示的铲斗定位方法。

如图1所示，一种铲斗定位方法，用于本发明较佳实施例示例的挖掘机，包括以下步骤：

步骤s1、建立挖掘机运动学模型；

步骤s2、计算动臂支点坐标；

步骤s3、计算挖掘机动臂旋转角度；

步骤s4、计算挖掘机斗杆旋转角度；

步骤s5、计算挖掘机铲斗旋转角度；

步骤s6、计算铲尖位置。

其中，步骤s1、建立挖掘机运动学模型

采用右手定则规定正方向，建立的坐标系包括地理坐标系g系、导航坐标系n系、载体坐标系b系，动臂坐标系o1、斗杆坐标系o2、铲斗坐标系o3、铲尖坐标系o4和根据gnss建立的卫星坐标系ognss；其中，载体坐标系的原点设于挖掘机动臂支点，导航坐标系的原点设于挖掘机底盘旋转中心位置；俯仰角为导航坐标系绕地理坐标系x轴旋转的欧拉角，横滚角为导航坐标系绕地理坐标系y轴旋转的欧拉角，航向角为导航坐标系绕地理坐标系z轴旋转的欧拉角，以本发明的车身倾角传感器为例，俯仰角、横滚角、航向角逆时针旋转时为正。

如图2所示，本发明较佳实施例的挖掘机运动学模型，其中，θ、φ、ψ分别为导航坐标系绕地理坐标系x轴、y轴、z轴旋转的欧拉角(即分别为俯仰角、横滚角、航向角)；θ1、θ2、θ3分别为动臂坐标系o1、斗杆坐标系o2、铲斗坐标系o3的连杆转动角度；d1、d2、d3分别为动臂坐标系o1、斗杆坐标系o2、铲斗坐标系o3的连杆长度。

其中，本发明较佳示例的挖掘机的动态传感器，动臂倾角传感器对应设于d1处、斗杆倾角传感器对应设于d2处、铲斗倾角传感器对应设于d3处。

较佳地，采用右手定则规定正方向，逆时针方向旋转为正，本发明较佳实施例示例载体坐标系的原点设于挖掘机动臂支点，载体坐标系x轴正向指向挖掘机驾驶室右侧，y轴正向指向挖掘机车头方向，z轴正向指向挖掘机顶部。

步骤s2、计算动臂支点坐标

由rtk测量得到天线相位中心在地理坐标系中的位置矢量，并对该适量进行欧拉变换，再结合天线相位中心在地理坐标系中的坐标，进一步计算动臂支点坐标，得到动臂支点坐标：

其中，为动臂支点的地理坐标、为天线相位中心在地理坐标系中的坐标、为天线相位中心在地理坐标系中的位置矢量，该动臂支点的地理坐标为动臂支点在高斯坐标系中的坐标。

步骤s3、计算挖掘机动臂旋转角度

较佳地，定义动臂于机身的动臂支点为第一机械点(即点a或o1)、挖掘机动臂油缸与动臂连接处为第二机械点(即点b)、动臂与挖掘机斗杆连接处为第三机械点(即点l)，斗杆与挖掘机斗杆油缸间设有第一连杆，斗杆油缸与挖掘机铲斗间设有第二连杆，定义第一连杆与斗杆连接处为第四机械点(即点c)、斗杆与铲斗连接处为第五机械点(即点d)、第二连杆与斗杆油缸连接处为第六机械点(即点e)、第二连杆与铲斗连接处为第七机械点(即点f)、铲尖为第八机械点(即点g)。

本实施例涉及的挖掘机机械参数包括第一机械点与第二机械点连接形成的第一参考线(lab)及其长度、第一机械点与第三机械点连接形成的第二参考线(lal或d1)及其长度、第二机械点与第三机械点连接形成的第三参考线(llb)及其长度、第三机械点与第四机械点连接形成的第四参考线(llc)及其长度、第三机械点与第五机械点连接形成的第五参考线(lld或d2)及其长度、第四机械点与第五机械点连接形成的第六参考线(lcd)及其长度、第四机械点与第六机械点连接形成的第七参考线(lce)及其长度、第五机械点与第七机械点连接形成的第八参考线(ldf)及其长度、第六机械点与第七机械点连接形成的第九参考线(lef)及其长度、第六机械点与第八机械点连接形成的第十参考线(ldg或d3)及其长度。其中，各参考线长度可在实际施工过程中结合本实施例较佳示例的挖掘机通过刻度尺等方式进行尺寸测量。

动臂倾角传感器设于第二参考线d1，斗杆倾角传感器设于第五参考线d2，铲斗倾角传感器设于第十参考线d3。

通过挖掘机机械参数，结合实际挖掘机各参考线之间的连接关系得出各个参考线之间的角度，进一步计算得到对应的参考角度，结合实际挖掘机操作，通过挖掘机机械参数、参考线、参考角度，及由动臂倾角传感器得到的动臂倾角计算动臂旋转角度。

同时，本发明较佳示例的挖掘机的动臂支点、底盘中心、及各机械臂等之间存在连接关系的结构，为刚性连接关系，各个位置及角度相互间存在对应关系，由建立的挖掘机运动学模型可计算出各自对应的关系，当中涉及的计算方法为常规计算手段。

步骤s4、计算挖掘机斗杆旋转角度

根据斗杆倾角传感器测量得到的斗杆倾角、s3计算得到的动臂旋转角度，通过本发明较佳示例建立的挖掘机运动学模型计算斗杆旋转角度。

步骤s5、计算挖掘机铲斗旋转角度

根据铲斗倾角传感器测量得到的铲斗倾角、s3得到的动臂旋转角度、s4得到的斗杆旋转角度计算斗杆旋转角度；

步骤s6、计算铲尖位置

根据动臂支点坐标，通过建立dh矩阵模型计算铲尖位置。

该计算铲尖位置的步骤可拆分为以下步骤：

步骤s61、在以挖掘机动臂支点为载体坐标系原点的基础上，于动臂支点建立dh坐标系，结合步骤s3、s4、s5的计算针对dh坐标系进行推算，同时，根据建立的挖掘机运动学模型结合上述步骤的计算可得到dh参数。利用dh矩阵，对于多自由度的挖掘机机械臂具有很好的适应性，即使机械臂较多的情况依旧能够确定计算定位。

步骤s61、将上一步骤得到的dh参数代入dh矩阵，同时，结合的动臂支点坐标对铲尖坐标进行推算，但由于地理坐标系和导航坐标系物理定义不一致，根据二者的物理定义结合数学进行形式转换，得出铲尖于地理坐标系的坐标，从而定位铲尖，由挖掘机控制室根据定位结果进行作业。

铲尖于地理坐标系的坐标为：

其中，为动臂支点的地理坐标，为铲尖中间位置坐标，为铲尖中间位置在地理坐标系中的相对位置矢量。

具体实施示例如下：

于动臂支点处建立dh坐标系，其中，xi与zi-1垂直，xi与zi-1相交，根据挖掘机运动学模型可得dh参数，如表所示：

表中：αi为从zi到zi+1绕xi逆时针旋转角度；di为从zi到zi+1沿xi+1平移的距离；li为从xi到xi+1沿zi平移的距离；θi为从xi到xi+1绕zi逆时针旋转角度。

i相对于i+1的变化矩阵为：

代入参数，得到

由于地理坐标系和导航坐标系定义不一致，需要进行旋转。为先绕地理坐标系z轴转-90度，然后再绕地理坐标系x轴转-90度，得出：

代入上式：

进一步地，

代入动臂支点的地理坐标，即可得出铲尖的地理坐标：

本发明较佳示例的公式，结果均定义为地理坐标系中的坐标情况，公式的推算过程为常规计算手段，在此不赘述。

因采用的计算手段略有不同，本发明所示例的计算方法、计算结果根据实际操作及测量结果可以存在一定的差异，而这些差异属于本发明保护范围内。

本发明涉及的挖掘机及其铲斗定位方法，以卫星定位为基础，结合动态传感器建立挖掘机运动学模型，采用动臂支点作为载体坐标系的原点，结合dh矩阵模型精确计算铲尖位置，通过卫星定位及动态传感器测量精度、灵敏度高，对施工环境要求较低、环境适应性强，且利用dh矩阵对于多自由角度的挖掘机机械臂具有很好的适应性，在机械臂较多的情况下依旧能够保证计算的精准度，该挖掘机及其铲斗定位方法能够对挖掘机进行精确计算定位。

对本领域的技术人员来说，可根据以上描述的技术方案以及构思，做出其它各种相应的改变以及形变，而所有的这些改变以及形变都应该属于本发明权利要求的保护范围之内。