CN113605483B - 一种挖掘机自动作业控制方法和装置 - Google Patents

Abstract

本发明公开一种挖掘机自动作业控制方法和装置，方法包括：根据导航定位信号计算挖掘机铲斗的实时作业位置；根据挖掘机所处作业区域的环境信息以及实时作业位置，建立挖掘机作业环境电子地图；根据挖掘机动作执行机构的运行状态信息进行挖掘机的姿态计算；根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。本发明能够实现挖掘机的全过程自动作业，提升挖掘机的作业效率和作业质量。

Description

一种挖掘机自动作业控制方法和装置

技术领域

本发明涉及挖掘机控制技术领域，特别是一种挖掘机自动作业控制方法和装置。

背景技术

挖掘机被广泛应用于在矿山开采、建筑破拆、路桥建设等各种施工领域。现有挖掘机的操作主要还是人工操作，施工作业效率和质量会因为操作人员的水平而存在差异，难以达到统一要求，从而影响挖掘机的施工效率和施工精度。此外，在某些特殊行业的作业环境异常恶劣，比如：毒气废气场合、垃圾清理、抢险救灾、隧道开挖、防爆作业、放射性场合等，这些恶劣的作业环境给作业人员带来极大的困难和不便，会对人体造成伤害，不适合人现场作业。因此，研制一种能够实现远程遥控甚至是无人操控自动作业的挖掘机成为不少用户的需求。

现有挖掘机自动或半自动作业的实现方式有以下几种：

1.每次自主移动作业前通过视觉传感器在作业地面标识出挖掘机可识别的行进路径标记，而且在每完成预定次数的工作循环后，再通过视觉传感器识别出行进路径并重新确定下部机构与行进路径的相对方向。这种方式一方面比较费时，另一方面在多粉尘的作业环境中，视觉传感器的识别效果会受影响，进而会影响施工作业的质量；

2.由人通过操作录制按钮进行挖掘动作的轨迹录制操作，实现一种半自动作业模式。这种作业方式并没有完全把操作人员解放出来；

3.通过视觉传感器获取包括有目标挖掘点的图像，再对图像进行特征分析，识别出目标挖掘点，并通过图像处理进行计算获得目标挖掘点的相对位置。在多粉尘的作业环境中，这种实现方式中视觉传感器的识别效果有待考证。

发明内容

本发明的目的是提供一种挖掘机自动作业控制方法和装置，能够实现挖掘机的全过程自动作业，提升挖掘机的作业效率和作业质量。本发明采用的技术方案如下。

一方面，本发明提供一种挖掘机自动作业控制方法，包括：

获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。

可选的，所述导航定位信号包括从安装于挖掘机铲斗上的差分GPS高精度定位系统、惯性导航单元中的一个或多个获取的定位信号，

方法还包括：根据获取的导航定位信号计算挖掘机铲斗的运动速度。运动速度的计算可用于反馈控制的实现，提升控制精度。挖掘机铲斗的实时作业位置主要通过差分GPS高精度定位系统定位获取。

可选的，所述环境信息包括从安装于挖掘机铲斗上的视觉传感器、激光雷达、毫米波雷达中的一个或多个获取的环境信息；

视觉传感器安装于挖掘机驾驶室顶的前部和后部，以及车身左、右两侧，用于采集车身周围环境信息；激光雷达安装于挖掘机驾驶室顶部，用于对作业环境进行360°的环境信息采集；毫米波雷达安装于下车体前、后、左、右部，用来采集车身周围障碍物信息。

可选的，所述运行状态信息包括从安装于挖掘机上的角度传感器、油缸位移传感器、陀螺仪获取的检测信号；

所述进行挖掘机的姿态计算包括：根据角度传感器、油缸位移传感器的检测信号实时计算传感器安装位置处机构的姿态，并折算到铲斗斗齿，得到铲斗斗齿的位姿信息；以及利用陀螺仪的检测信号确定回转中心实时姿态。即可得到包括动臂、斗杆、铲斗和回转中心实时姿态数据的姿态计算结果。

可选的，所述进行挖掘作业任务规划包括：

响应于挖掘机卸载动作执行完毕或待执行新的挖掘作业，确定待挖掘的挖掘点；

规划铲斗从实时所处位置到挖掘点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至挖掘点位置时，能够避开障碍物的路径，根据所规划的路径，得到挖掘过程中对应各执行机构的控制参数序列；

响应于挖掘机挖掘动作执行完毕，确定卸载点；

规划铲斗从实时所处位置到卸载点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至卸载点位置时，能够避开障碍物的路径，根据所规划的路径，得到卸载过程中对应各执行机构的控制参数序列。

可选的，所述挖掘点的确定方法包括：

将待挖掘的作业面离散为均匀的栅格；

将最后挖掘完毕的作业面位置作为当前挖掘点，寻找其所对应的栅格点周围8个方向上的邻接栅格点，针对各邻接栅格点，分别按照以下代价函数计算挖掘点选择代价f1：

f1＝g+d+e

式中，g表示邻接栅格点到起始挖掘点对应的栅格点的欧氏距离；d表示邻接栅格点已记录的挖掘深度，e表示邻接栅格点到当前挖掘点的欧式距离；

选择代价f1最小的邻接栅格点对应的作业面位置，作为下一挖掘点。

初始挖掘点可根据挖掘机铲斗的作业位置预设，然后按照前述的挖掘点确定方法确定后续的挖掘点。

可选的，对于每一个栅格，其挖掘深度d初始化为0.0，挖掘过程中，每次挖掘按照深度变化梯度Δd＝1.0更新铲掘深度为d_t+1＝d_t+Δd，其中d_t表示第t次挖掘后的深度，d_t+1表示第t+1次挖掘后的深度；

选择挖掘点时，仅对实时d值小于预设最大深度d_max的栅格点计算代价函数。d_max值的设定可根据作业需要来定。

以上方案能够实现，一旦一个栅格点被拓展到，其d值即根据实际挖掘深度而更新记录，如果d值到达了最大铲掘深度d_max，那么此时这个节点将不再被选择为新的挖掘点。代价项d的设置使得每次挖掘点选择能够优先选择未挖掘的点，进而可以保证每次挖掘的满斗率相对较大。代价项g、d、e三者的配合也使得挖掘点的选择过程不会漏过每一处作业点，并保证最终的挖掘都能够达到预设定的挖掘深度。对于作业面的栅格分辨率的设定，根据经验，可设定为大小为1m×1m的栅格。

可选的，路径规划时，规划回转中心回转时动臂和斗杆从实时所处位置提升至卸载点位置时能够避开障碍物的路径，包括：

获取回转中心的回转角、动臂相对于车体水平面的转角、斗杆相对于动臂的转角以及铲斗相对于斗杆的转角、回转中心到动臂销轴的距离、动臂销轴到斗杆销轴的距离、斗杆销轴到铲斗销轴的距离以及铲斗销轴到铲斗斗齿齿尖的距离；

获取障碍物位置信息；

基于获取到的信息，利用预设的第二代价函数在预构建的臂架斗杆构型空间中选择使得代价值最小的最优构型，根据所述最优构型确定臂架和斗杆在回转中心回转过程中的转角。

可选的，挖掘机的构型空间的采样数学模型包括以挖掘机车体回转中心O₁、动臂绕车体的转轴中心O₂、斗杆绕动臂的转轴中心O₃、铲斗绕斗杆的轴中心O₄、铲斗斗齿O₅为原点建立四个独立坐标系(X₁、Y₁、Z₁)、(X₂、Y₂、Z₂)、(X₃、Y₃、Z₃)、(X₄、Y₄、Z₄)、(X₅、Y₅、Z₅)；

用X_i＝[θ_2ei，θ_3ei]^T表示臂架斗杆构型空间内任一采样点的构型，其中θ_2ei、θ_3ei分别表示该构型中动臂和斗杆的转角；

所述第二代价函数表示为：

f2＝c_risk+c_ref

上式中，f2为代价值，c_risk表示与障碍物的碰撞风险，c_ref表示采样点构型相对于期望构型X_e＝[θ_2e，θ_3e]^T的偏离程度，所述期望构型对应挖机回转至目标位置时臂架和斗杆的位姿；

所述利用预设的第二代价函数在预构建的臂架斗杆构型空间中选择使得代价值最小的最优构型包括：

模拟回转一设定角度，根据构型空间内的采样点构型及障碍物位置信息计算c_risk，根据采样点构型和期望构型计算c_ref，得到构型空间内使得f₂最小且不会碰撞的采样点构型，将其作为最优构型。

可选的，所述根据构型空间内的采样点构型及障碍物位置信息计算c_risk包括，利用预设的碰撞检查算法计算模拟回转过程中采样点构型对应的碰撞检查结果，根据碰撞检查结果利用下式确定c_risk的值：

上式中，w_risk表示碰撞风险在第二代价中的计算权重，该值越大，表示碰撞风险代价项在总的代价里占的比重就大，该值越小，表示碰撞风险代价项c_risk的比重就越小。碰撞检查算法的原理即假设回转了设定角度，臂架斗杆按照采样构型变化至相应的位姿，计算这一过程中工作装置与障碍物的碰撞风险，具体可参考现有技术。

可选的，根据采样点构型和期望构型计算c_ref利用下式计算：

c_ref＝w_ref||X_i-X_e||

式中，w_ref表示采样构型与期望构型之间的偏离在代价计算中的权重，该值越大，说明偏离代价项c_ref在总代价里占的比重就越高。

以上方案，本发明可通过调整w_ref和w_risk两个系数，来达到对碰撞风险代价项和期望构型偏离项的一种平衡。

可选的，所述采样点构型在构型空间内从期望构型向外围方向以设定的采样步长最多迭代选取两次，若第一次迭代得到的采样点构型中存在最优构型则输出该最优构型，若不存在则进行第二次迭代选取，若第二次迭代得到的采样点构型中存在最优构型则输出该最优构型，若不存在，则改变模拟回转角度后重新进行采样点构型迭代选取和第二代价函数计算，直至获得最优构型，或者直至模拟回转角度小于预设的最小转角，则确定不存在所述最优构型。不存在最优构型也就表示无论如何回转都将无法避免碰撞，此时可输出警告信息提醒现场人员对障碍物进行处理后再行施工。

可选的，路径规划时，回转中心的模拟回转角度的初始值为α₀＝θ_1e-θ_1start，其中θ_1e是回转中心期望构型的回转角度，θ_1start是回转中心在进行一个工作循环进行挖掘时的回转角度，即从初始位置或上一个循环结束位置到新的循环挖掘点位置的回转角度；

当第二代价函数计算无法得到最优的臂架斗杆构型时，所述改变模拟回转角度根据下式改变：

式中，α_i、α_i+1分别代表改变前后的模拟回转角；

所述预设的最小转角为

第二方面，本发明提供一种挖掘机自动作业控制装置，包括：

实时作业位置确定模块，被配置用于获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

作业环境确定模块，被配置用于获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

姿态计算模块，被配置用于实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

作业任务规划模块，被配置用于根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

以及，控制输出模块，被配置用于基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。

有益效果

本发明的挖掘机自动作业控制方法能够实现全自动挖掘作业，使操作人员从危险工作环境中解放出来，有效保障操作人员安全。

通过回转过程动态避障算法设计，能够有效避免与障碍物之间的碰撞，保障了作业设备的施工安全。

本发明还可利用神经网络技术，吸取熟练操作者的经验数据，实现自动的控制参数计算，能够更好的提高作业效率。

附图说明

图1所示为挖掘机自动作业控制系统的一种实施例框架示意图；

图2所示为作业面栅格划分示意图；

图3所示为作业面各栅格代价值记录和子节点拓展方式示意图；

图4所示为挖掘机自动作业控制方法的一种实施例流程图；

图5所示为挖掘机动作执行机构运动学示意图；

图6所示为构型空间采样原理示意图；

图7所示为离散数值优化的寻找可行解的算法流程示意图。

具体实施方式

以下结合附图和具体实施例进一步描述。

实施例1

本实施例介绍一种挖掘机自动作业控制方法，可由挖掘机的控制中心执行，方法具体包括：

获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。

实施例1-1

实施例1的方法可具体实现在本实施例的一种挖掘机自动作业系统中，结合图1，该系统包括导航定位机构、位姿检测机构、环境检测机构、挖掘机动作执行机构，以及挖掘机自动作业控制装置，挖掘机自动作业控制装置执行实施例1的方法，具体实施内容以下具体说明。

导航定位机构包括差分GPS高精度定位系统、惯性导航单元等，其能够获取高精度的定位信号，使得挖掘机自动作业控制装置能够实时计算得到更准确的挖掘机运动速度和作业位置；

位姿检测机构包括安装在工作装置(动臂、斗杆、铲斗)和回转中心的角度传感器、油缸位移传感器、陀螺仪等，用于为挖掘机自动作业控制装置实时计算挖掘机自身姿态信息提供基础数据；

环境检测机构包括视觉传感器、激光雷达、毫米波雷达等，视觉传感器可安装于挖掘机驾驶室顶的前部和后部，以及车身左、右两侧，用于采集车身周围环境信息；激光雷达安装于挖掘机驾驶室顶部，用于对作业环境进行360°的环境信息采集；毫米波雷达安装于下车体前、后、左、右部，用来采集车身周围障碍物信息。挖掘机自动作业控制装置利用环境检测机构采集的环境信息识别周围作业环境，建立作业环境电子地图；

挖掘机动作执行机构即由挖掘机自动作业控制装置控制用于驱动挖掘机回转中心、臂架、斗杆等动作的机构；

基于上述各机构组成，本实施例中挖掘机自动作业控制装置可通过挖掘机控制中心实现以下功能模块：

实时作业位置确定模块，用于获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

作业环境确定模块，用于获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

姿态计算模块，用于实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

作业任务规划模块，用于根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

以及，控制输出模块，用于基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。

参考图4所示，挖掘机的自动作业控制过程所涉及的内容如下：

(1)计算当前作业位置：利用导航定位模块中的差分GPS高精度定位系统，计算出挖掘机当前作业位置；

(2)提取作业环境信息，建立环境地图：利用安装在挖掘机上的视觉传感器、激光雷达、毫米波雷达等传感器，识别周围作业区域环境，建立作业环境地图；

(3)计算工作装置姿态：利用安装在工作装置(动臂、斗杆、铲斗)上的角度传感器、油缸位移传感器等提供的信息实时计算姿态，并折算到铲斗斗齿，得到位姿信息，利用陀螺仪等检测回转实时姿态；

(4)初始挖掘点选取：将作业区域划分为离散化、等面积的均匀栅格，确定初始挖掘点；

(5)路径规划(当前位置—初始挖掘点)：读取当前铲斗斗齿所处位置和选取的初始挖掘点位置，结合运动学方程，规划两个位置之间的运动路径，生成铲斗斗齿运动轨迹；

(6)回转避障(当前位置——初始挖掘点)：根据环境感知信息，识别空间障碍物，规划出能够避开障碍物的路径。在构型空间中离散地采样，计算障碍物碰撞风险，通过预先设计的代价函数，选择最优控制量，实现动臂提升、回转过程中的避障；

(7)机构运动(当前位置——初始挖掘点)：根据规划好的当前位置到挖掘点的路径，通过执行机构驱动工作装置到初始挖掘点；

(8)自动挖掘：按照规划的初始挖掘点，生成控制序列，控制工作装置，自动完成铲斗的挖掘过程；

(9)初始卸载点选取：根据环境地图，确定初始卸载点；

(10)路径规划(初始挖掘点—初始卸载点)：读取选取的初始挖掘点和选取的初始卸载点位置，结合运动学方程，规划两个位置之间的运动路径，生成铲斗斗齿运动轨迹；

(11)回转避障(初始挖掘点——初始卸载点)：根据环境感知信息，识别空间障碍物，规划出能够避开障碍物的路径。在构型空间中离散地采样，设计代价函数，计算障碍物碰撞风险，选择最优控制量，实现动臂提升、回转过程中的避障；

(12)机构运动(初始挖掘点—初始卸载点)：根据规划好的初始挖掘点到初始卸载点的路径和回转避障生成的路径，通过执行机构驱动工作装置到初始卸载点；

(13)自动卸载：按照选取的初始卸载点，生成控制序列，控制工作装置，自动完成卸载过程；

(14)后续挖掘点选取：根据预先设计的代价函数，按照当前挖掘点的节点向相邻节点拓展，求取相邻区域各子节点的代价值，选择代价值最低的子节点作为下一个挖掘点；

(15)路径规划(初始卸载点—后续挖掘点)：读取当前铲斗斗齿在初始卸载点的位置和后续挖掘点位置，结合运动学方程，规划两个位置之间的运动路径，生成铲斗斗齿运动轨迹；

(16)回转避障(初始卸载点—后续挖掘点)：根据环境感知信息，识别空间障碍物，规划出能够避开障碍物的路径。在构型空间中离散地采样，通过预设的代价函数，计算障碍物碰撞风险，选择最优控制量，实现动臂提升、回转过程中的避障；

(17)机构运动(初始卸载点—后续挖掘点)：根据路径规划结果生成初始卸载点到后续挖掘点的路径和回转避障生成的路径，控制执行机构到后续挖掘点；

(18)自动挖掘：按照规划的后续挖掘点，生成控制序列，控制工作装置，自动完成铲斗的挖掘过程；

(19)后续卸载点选取：根据构型空间和欧式空间的映射关系，使用遗传算法求解，规划出期望的后续卸载点。

(20)路径规划(后续挖掘点—后续卸载点)：读取选取的后续挖掘点的位置和后续卸载点位置，结合运动学方程，规划两个位置之间的运动路径，生成铲斗斗齿运动轨迹；

(21)回转避障(后续挖掘点——后续卸载点)：根据环境感知信息，识别空间障碍物，规划出能够避开障碍物的路径。在构型空间中离散地采样，根据代价函数，计算障碍物碰撞风险，选择最优控制量，实现动臂提升、回转过程中的避障；

(22)机构运动(后续挖掘点—后续卸载点)：结合路径规划子模块下发生成的后续挖掘点到后续卸载点的路径和回转避障生成的路径，由运动控制模块控制执行机构到后续卸载点；

(23)自动卸载：按照选取的后续卸载点，生成控制序列，控制工作装置，自动完成卸载过程。

(24)后续挖掘点选取：根据代价函数，按照当前挖掘点的节点向相邻节点拓展，求取相邻区域各子节点的代价值，选择代价值最低的子节点作为下一个挖掘点；

(25)路径规划(后续卸载点—后续挖掘点)：读取选取的后续卸载点和后续的后续挖掘点位置，结合运动学方程，规划两个位置之间的运动路径，生成铲斗斗齿运动轨迹；

(26)回转避障(后续卸载点—后续挖掘点)：根据环境感知信息，识别空间障碍物，规划出能够避开障碍物的路径。在构型空间中离散地采样，设计代价函数，计算障碍物碰撞风险，选择最优控制量，实现动臂提升、回转过程中的避障；

(27)机构运动(后续卸载点—后续挖掘点)：结合路径规划子模块下发生成的后续卸载点到后续挖掘点的路径和回转避障生成的路径，由运动控制模块控制执行机构到后续挖掘点。后续重复执行(18)-(27)实现挖掘机在每个动作循环中的挖掘和卸载动作。

为实现上述控制流程，本实施例中，作业任务规划模块所需实现的功能包括挖掘点选取、路径规划、自动挖掘、回转避障、卸载点选取、自动卸载等六个子功能模块，各子功能模块分别采用相应算法，实现整个自动作业任务规划，最终通过控制挖掘机动作执行机构实现挖掘机的自动作业。所进行的挖掘作业任务规划包括：

响应于挖掘机卸载动作执行完毕或待执行新的挖掘作业，确定待挖掘的挖掘点；

规划铲斗从实时所处位置到挖掘点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至挖掘点位置时，能够避开障碍物的路径，根据所规划的路径，得到挖掘过程中对应各执行机构的控制参数序列；

响应于挖掘机挖掘动作执行完毕，确定卸载点；

规划铲斗从实时所处位置到卸载点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至卸载点位置时，能够避开障碍物的路径，根据所规划的路径，得到卸载过程中对应各执行机构的控制参数序列。以下具体说明。

一、关于挖掘点选取

作用是自动规划挖掘点，初始挖掘点可根据挖掘机铲斗的作业位置预设，然后按照前述的挖掘点确定方法确定后续的挖掘点。

本实施例在进行后续挖掘点选取时，将作业区域划分为离散化、等面积的均匀栅格，参考图2，选取新的挖掘点时，根据预先设计的代价函数，按照从当前节点向相邻节点拓展的思想，求取相邻区域各子节点的代价值，选择代价值最低的子节点作为下一个挖掘点。

对于传统操作人员来说，挖掘点的选取是一种模糊的策略，往往会选择上一个挖掘点的近邻点，受到这种启发，对于自动作业过程中的挖掘点的选取，本发明将这种模糊策略建模成一个数学模型，提出了一种作业面栅格化策略。

如图2所示，针对作业面(以土堆为例)，将整个作业面划分为离散化、等面积的均匀栅格，图中，深色栅格为作业面的边界。

图3中，在作业面的均匀栅格中，每一个栅格的铲掘深度d初始化为0.0，每次挖掘之后的深度增加Δd＝1.0，挖掘过程中，每次挖掘后均更新各节点的挖掘深度数据，对于下一个的挖掘点的选择，本实施例涉及八方拓展子节点的方式，并设计挖掘点选择代价函数为：

f1＝g++d+e

式中，g表示邻接栅格点到起始挖掘点对应的栅格点的欧氏距离；d表示邻接栅格点已记录的挖掘深度，e表示邻接栅格点到当前挖掘点的欧式距离；选择代价f1最小的邻接栅格点对应的作业面位置，作为下一挖掘点。且选择挖掘点时，仅对实时d值小于预设最大深度d_max的栅格点计算代价函数。d_max值的设定可根据作业需要来定。

有了代价项，可以使得挖掘点的扩散是沿着目标点逐渐向外进行的，能够保证挖掘范围的连续性；一旦一个栅格点被拓展到，其d值即根据实际挖掘深度而更新记录，如果d值到达了最大铲掘深度d_max，那么此时这个节点将不再被选择为新的挖掘点。代价项d的设置使得每次挖掘点选择能够优先选择未挖掘的点，进而可以保证每次挖掘的满斗率相对较大。代价项g、d、e三者的配合也使得挖掘点的选择过程不会漏过每一处作业点，并保证最终的挖掘都能够达到预设定的挖掘深度。对于作业面的栅格分辨率的设定，根据经验，可设定为大小为1m×1m的栅格。

二、关于路径规划和回转避障

这部分内容包括对挖掘机动臂铲斗从挖掘点到卸载点的路径规划，以及从卸载点到下一挖掘点的路径规划，所规划得到的路径须能实现回转过程中的避障，最终得到能够实现回转避障路径的挖掘机控制参数。

对于动臂提升回转过程的避障，本实施例通过在构型空间中离散地采样，设计代价函数，计算障碍物碰撞风险，选择最优控制量来实现。

在自动挖掘和自动卸载时，则可根据预设的构型空间和欧式空间的映射关系，使用遗传算法求解，得到期望的挖掘点或卸载点位姿。

具体的，路径规划时，规划回转中心回转时动臂和斗杆从实时所处位置提升至卸载点位置时能够避开障碍物的路径，包括：

获取回转中心的回转角、动臂相对于车体水平面的转角、斗杆相对于动臂的转角以及铲斗相对于斗杆的转角、回转中心到动臂销轴的距离、动臂销轴到斗杆销轴的距离、斗杆销轴到铲斗销轴的距离以及铲斗销轴到铲斗斗齿齿尖的距离；

获取障碍物位置信息；

基于获取到的信息，利用预设的第二代价函数在预构建的臂架斗杆构型空间中选择使得代价值最小的最优构型，根据所述最优构型确定臂架和斗杆在回转中心回转过程中的转角。

结合图5和图6介绍回转避障算法。图5是挖掘机构型空间采样的数学模型，包括分别以挖掘机车体回转中心、动臂绕车体的转轴中心、斗杆绕动臂的转轴中心、铲斗绕斗杆的轴中心、铲斗斗齿为原点建立四个独立坐标系(X₁、Y₁,Z₁),(X₂、Y₂、Z₂)、(X₃、Y₃、Z₃)、(X₄、Y₄、Z₄)、(X₅、Y₅、Z₅)，其中θ₁为回转中心的回转角，θ₂为动臂相对于车体水平面的转角，θ₃为斗杆相对于动臂的转角，θ₄为铲斗相对于斗杆的转角，l₁为回转中心到动臂销轴的距离，l₂为动臂销轴到斗杆销轴的距离，l₃为斗杆销轴到铲斗销轴的距离，l₄为铲斗销轴到铲斗斗齿齿尖的距离。

图6中，θ_2max，θ_2min，θ_3max,θ_3min,分别表示动臂和斗杆的最大最小极限转角，中心圆点表示期望的构型X_e＝[θ_2e，θ_3e]^T，阴影部分矩形框表示动臂和斗杆组成的构型空间，Δθ表示采样距离，图6的采样空间，共有25个状态构成。

为了确定合适的转角，本实施例设计了回转代价函数，

f2＝c_risk+c_ref

式中，f2为代价值，c_risk表示与障碍物的碰撞风险，c_ref表示采样点构型相对于期望构型X_e＝[θ_2e，θ_3e]^T的偏离程度。

模拟回转一设定角度，根据构型空间内的采样点构型及障碍物位置信息计算c_risk，根据采样点构型和期望构型计算c_ref，得到构型空间内使得f2最小且不会碰撞的采样点构型，即可将其作为最优构型。

为了简化碰撞风险的计算，将碰撞检查的结果碰撞设为1，未碰撞设为0，有如下表示：

上式中，w_risk表示碰撞风险在第二代价中的计算权重，该值越大，表示碰撞风险代价项在总的代价里占的比重就大，该值越小，表示碰撞风险代价项c_risk的比重就越小。

在碰撞检查过程中，通过激光雷达、毫米波雷达、视觉传感器等获得障碍物坐标信息，通过姿态计算模块获得挖机臂位姿信息，模拟挖掘机回转一定的角度α，调用预设的碰撞检查算法计算碰撞检查结果。

对于c_ref的计算，使用构型的差分计算，

c_ref＝w_ref||X_i-X_e||

式中：X_i是某一个采样点的构型，w_ref表示采样构型与期望构型之间的偏离在代价计算中的权重，该值越大，说明偏离代价项c_ref在总代价里占的比重就越高。

通过调整w_risk和w_ref两个系数，可达到对碰撞风险代价项和期望构型偏离项的平衡。

基于构型空间采样的算法时间复杂度为O(f(N))，其中f(N)表示计算碰撞风险代价值和期望构型代价值的时间复杂度，N表示采样构型的个数。对于上图期望构型的左下角的一个采样构型X_i＝[θ_2ei，θ_3ei]^T的值的计算，给出计算公式：

为了降低时间复杂度，本实施例只根据期望构型向外围采样2层，如图6所示。又由于该算法是分辨率完备的，所以当第一次采样不能找到有效解时，将采样步长增加，继续进行第二轮采样，如此往复，直到找到可行解或给出没有可行解的结论。算法流程图如图7所示，包括：

(1)期望构型和采样步长的初始化；

(2)子状态采样，计算子状态构型；

(3)进行碰撞检查和偏离程度计算；

(4)增判断代价值最小的构型是否是可行解，如果代价值最小，输出可行解；如果代价值不是最小，判断公式Δθ是否大于θ_2max与θ_2min的差值，如果是，无可行解；如果不是，增加采样步长，返回第(2)步。

在这个自动作业的算法框架中，存在两层循环，一层是步长的迭代过程，另一层是向前模拟的回转角度迭代过程，对于模拟的回转角度α的设计，采用二分法。即：

其中，α₀＝θ_1e-θ_1start，θ_1e是挖机回转的期望构型的回转角度，θ_1start是挖机每一个工作循环的挖掘时的回转角度。在向前模拟回转一定角度进行碰撞检查时，第一次的模拟回转角度就是α₀，当所有采样的构型的碰撞检查结果为0时，说明采样点都没有发生碰撞，此时根据回转代价函数选择代价最小的采样构型，结束采样过程，挖机按角度α₀完成回转。当所有采样构型的碰撞检查结果为1时，回转角度

进行第二轮的模拟碰撞检查；当采样的构型的碰撞检查既有0又有1时，说明在此轮采样中存在可行解，输出代价最小的可行解即可。

为了使算法收敛，本发明设置了一个最小的模拟转角

当迭代过程中模拟转角小于此值，则说明此次采样的构型无论怎么回转也不能完成避障操作，此时将模拟的回转角α_i复位到α₀，将采样步长Δθ增加，进入下一轮采样过程。

为了保证收敛，且结合工程实际经验，对迭代的量分别设置一个阈值，当达到阈值停止迭代，且算法依然没有不收敛的趋势，输出一个无法生成可行解的结论，也就意味着此时的笛卡尔空间由于障碍物约束的存在不能完成自动作业循环。

三、关于卸载点的选取

就是确定在从挖掘点到卸载点的回转避障算法中的期望构型X_e＝[θ_2e，θ_3e]^T。在欧式空间中，期望目标点的x，y的坐标值很好确定，因为卸载点通常是运料车的车斗，根据激光雷达或者视觉传感器等信息即可确定车斗的中心，那么也就确定了目标点的两个坐标值。对于第三个坐标值高度信息，通常根据作业经验，设定一个距离车斗高度为h的值，此时目标点p_goal＝[x_g，y_g，h_g]。我们根据构型空间和欧式空间的映射关系，使用遗传算法求出逆解，即c_e＝[θ_2e，θ_3e，θ_4e]^T，去掉铲斗旋转角度θ_4e，即可得期望构型X_e＝[θ_2e，θ_3e]^T。

四、关于自动挖掘和自动卸载，可以通过人工示教实现，首先由操作人员进行挖掘和卸载等作业，采集记录作业全过程中油缸控制序列；然后利用神经网络，学习有经验的操作人员的控制指令，通过复现完成自动作业。

利用本发明可实现挖掘机的全自动挖掘作业，使操作人员从危险工作环境中解放出来，有效保障了操作人员安全；回转过程动态避障，有效保障了作业设备的施工安全；自动挖掘、自动卸载过程能够吸取熟练操作者的经验数据，以更好的提高作业效率。

实施例2

本实施例介绍一种挖掘机自动作业控制装置，包括：

实时作业位置确定模块，被配置用于获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

作业环境确定模块，被配置用于获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

姿态计算模块，被配置用于实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

作业任务规划模块，被配置用于根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

以及，控制输出模块，被配置用于基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令。

以上各功能模块的具体功能实现参考实施例1的相关内容。

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

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

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

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

以上结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

Claims (12)

1.一种挖掘机自动作业控制方法，其特征是，包括：

获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令；

其中，所述进行挖掘作业任务规划包括：

响应于挖掘机卸载动作执行完毕或待执行新的挖掘作业，确定待挖掘的挖掘点；

规划铲斗从实时所处位置到挖掘点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至挖掘点位置时，能够避开障碍物的路径，根据所规划的路径，得到挖掘过程中对应各执行机构的控制参数序列；

响应于挖掘机挖掘动作执行完毕，确定卸载点；

规划铲斗从实时所处位置到卸载点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至卸载点位置时，能够避开障碍物的路径，根据所规划的路径，得到卸载过程中对应各执行机构的控制参数序列；

进行挖掘作业任务规划时，所述挖掘点的确定方法包括：

将待挖掘的作业面离散为均匀的栅格；

将最后挖掘完毕的作业面位置作为当前挖掘点，寻找其所对应的栅格点周围8个方向上的邻接栅格点，针对各邻接栅格点，分别按照以下代价函数计算挖掘点选择代价f1：

f1＝g+d+e

式中，g表示邻接栅格点到起始挖掘点对应的栅格点的欧氏距离；d表示邻接栅格点已记录的挖掘深度，e表示邻接栅格点到当前挖掘点的欧式距离；

选择代价f1最小的邻接栅格点对应的作业面位置，作为下一挖掘点。

2.根据权利要求1所述的方法，其特征是，所述导航定位信号包括从安装于挖掘机铲斗上的差分GPS高精度定位系统、惯性导航单元中的一个或多个获取的定位信号；

方法还包括：根据获取的导航定位信号计算挖掘机铲斗的运动速度。

3.根据权利要求1所述的方法，其特征是，所述环境信息包括从安装于挖掘机铲斗上的视觉传感器、激光雷达、毫米波雷达中的一个或多个获取的环境信息；

视觉传感器安装于挖掘机驾驶室顶的前部和后部，以及车身左、右两侧，用于采集车身周围环境信息；激光雷达安装于挖掘机驾驶室顶部，用于对作业环境进行360°的环境信息采集；毫米波雷达安装于下车体前、后、左、右部，用来采集车身周围障碍物信息。

4.根据权利要求1所述的方法，其特征是，所述运行状态信息包括从安装于挖掘机上的角度传感器、油缸位移传感器、陀螺仪获取的检测信号；

所述进行挖掘机的姿态计算包括：根据角度传感器、油缸位移传感器的检测信号实时计算传感器安装位置处机构的姿态，并折算到铲斗斗齿，得到铲斗斗齿的位姿信息；以及利用陀螺仪的检测信号确定回转中心实时姿态。

5.根据权利要求1所述的方法，其特征是，对于每一个栅格，其挖掘深度d初始化为0.0，挖掘过程中，每次挖掘按照深度变化梯度Δd＝1.0更新铲掘深度为d_t+1＝d_t+Δd，其中d_t表示第t次挖掘后的深度，d_t+1表示第t+1次挖掘后的深度；

选择挖掘点时，仅对实时d值小于预设最大深度d_max的栅格点计算代价函数。

6.根据权利要求1所述的方法，其特征是，路径规划时，规划回转中心回转时动臂和斗杆从实时所处位置提升至卸载点位置时能够避开障碍物的路径，包括：

获取回转中心的回转角、动臂相对于车体水平面的转角、斗杆相对于动臂的转角以及铲斗相对于斗杆的转角、回转中心到动臂销轴的距离、动臂销轴到斗杆销轴的距离、斗杆销轴到铲斗销轴的距离以及铲斗销轴到铲斗斗齿齿尖的距离；

获取障碍物位置信息；

基于获取到的信息，利用预设的第二代价函数在预构建的臂架斗杆构型空间中选择使得代价值最小且不会碰撞的最优构型，根据所述最优构型确定臂架和斗杆在回转中心回转过程中的转角。

7.根据权利要求6所述的方法，其特征是，挖掘机的构型空间的采样数学模型包括以挖掘机车体回转中心O₁、动臂绕车体的转轴中心O₂、斗杆绕动臂的转轴中心O₃、铲斗绕斗杆的轴中心O₄、铲斗斗齿O₅为原点建立四个独立坐标系(X₁、Y₁、Z₁)、(X₂、Y₂、Z₂)、(X₃、Y₃、Z₃)、(X₄、Y₄、Z₄)、(X₅、Y₅、Z₅)；

用X_i＝[θ_2ei，θ_3ei]^T表示臂架斗杆构型空间内任一采样点的构型，其中θ_2ei、θ_3ei分别表示该构型中动臂和斗杆的转角；

所述第二代价函数表示为：

f2＝c_risk+c_ref

上式中，f2为代价值，c_risk表示与障碍物的碰撞风险，c_ref表示采样点构型相对于期望构型X_e＝[θ_2e，θ_3e]^T的偏离程度，所述期望构型对应挖机回转至目标位置时臂架和斗杆的位姿；

所述利用预设的第二代价函数在预构建的臂架斗杆构型空间中选择使得代价值最小且不会碰撞的最优构型包括：

模拟回转一设定角度，根据构型空间内的采样点构型及障碍物位置信息计算c_risk，根据采样点构型和期望构型计算c_ref，得到构型空间内使得f2最小且不会碰撞的采样点构型，将其作为最优构型。

8.根据权利要求7所述的方法，其特征是，所述根据构型空间内的采样点构型及障碍物位置信息计算c_risk包括，利用预设的碰撞检查算法计算模拟回转过程中采样点构型对应的碰撞检查结果，根据碰撞检查结果利用下式确定c_risk的值：

上式中，w_risk表示碰撞风险在第二代价中的计算权重。

9.根据权利要求7所述的方法，其特征是，根据采样点构型和期望构型计算c_ref利用下式计算：

c_ref＝w_ref||X_i-X_e||

式中，w_ref表示采样构型与期望构型之间的偏离在代价计算中的权重。

10.根据权利要求7所述的方法，其特征是，所述采样点构型在构型空间内从期望构型向外围方向以设定的采样步长最多迭代选取两次，若第一次迭代得到的采样点构型中存在最优构型则输出该最优构型，若不存在则进行第二次迭代选取，若第二次迭代得到的采样点构型中存在最优构型则输出该最优构型，若不存在，则改变模拟回转角度后重新进行采样点构型迭代选取和第二代价函数计算，直至获得最优构型，或者直至模拟回转角度小于预设的最小转角，则确定不存在所述最优构型。

11.根据权利要求10所述的方法，其特征是，路径规划时，回转中心的模拟回转角度的初始值为α₀＝θ_1e-θ_1start，其中θ_1e是回转中心期望构型的回转角度，θ_1start是回转中心在开始进行一个新的工作循环进行挖掘时的回转角度；

当第二代价函数计算无法得到最优的臂架斗杆构型时，所述改变模拟回转角度根据下式改变：

式中，α_i、α_i+1分别代表改变前后的模拟回转角；

所述预设的最小转角为

12.一种挖掘机自动作业控制装置，其特征是，包括：

实时作业位置确定模块，被配置用于获取导航定位信号，根据导航定位信号计算挖掘机铲斗的实时作业位置；

作业环境确定模块，被配置用于获取挖掘机所处作业区域的环境信息，根据所述环境信息以及所述实时作业位置，建立挖掘机作业环境电子地图；

姿态计算模块，被配置用于实时获取挖掘机动作执行机构的运行状态信息，根据所述运行状态信息进行挖掘机的姿态计算；

作业任务规划模块，被配置用于根据所述挖掘机作业环境电子地图以及姿态计算结果，进行挖掘作业任务规划，得到作业任务过程控制参数；

以及，控制输出模块，被配置用于基于所述作业任务过程控制参数，输出用于控制挖掘机动作执行机构的控制指令；

其中，所述作业任务规划模块进行挖掘作业任务规划包括：

响应于挖掘机卸载动作执行完毕或待执行新的挖掘作业，确定待挖掘的挖掘点；

规划铲斗从实时所处位置到挖掘点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至挖掘点位置时，能够避开障碍物的路径，根据所规划的路径，得到挖掘过程中对应各执行机构的控制参数序列；

响应于挖掘机挖掘动作执行完毕，确定卸载点；

规划铲斗从实时所处位置到卸载点位置之间运动的路径，以及回转中心回转过程中，动臂和斗杆从实时所处位置提升至卸载点位置时，能够避开障碍物的路径，根据所规划的路径，得到卸载过程中对应各执行机构的控制参数序列；

进行挖掘作业任务规划时，所述挖掘点的确定方法包括：

将待挖掘的作业面离散为均匀的栅格；

将最后挖掘完毕的作业面位置作为当前挖掘点，寻找其所对应的栅格点周围8个方向上的邻接栅格点，针对各邻接栅格点，分别按照以下代价函数计算挖掘点选择代价f1：

f1＝g+d+e

式中，g表示邻接栅格点到起始挖掘点对应的栅格点的欧氏距离；d表示邻接栅格点已记录的挖掘深度，e表示邻接栅格点到当前挖掘点的欧式距离；

选择代价f1最小的邻接栅格点对应的作业面位置，作为下一挖掘点。

Images