CN114991236B - 一种挖掘机智能控制系统及工作装置轨迹规划控制方法 - Google Patents

Abstract

本发明公开一种挖掘机智能控制系统及工作装置轨迹规划控制方法，包括：倾角传感器，所述倾角传感器用于实时监测挖掘机臂梁倾角；惯导传感器，所述惯导传感器用于实时监测车身回转角度；上位机控制器，所述上位机控制器与所述倾角传感器、惯导传感器连接，上位机控制器接收传感器的检测信号，计算挖掘机实时位姿与各执行器位置，上位机控制器对输入挖掘参数进行轨迹规划，并将其转换为挖掘机对应执行器各自目标运动轨迹；挖掘机内部控制器，所述挖掘机内部控制器与所述上位机控制器连接，上位机控制器根据各执行器规划目标运动轨迹与实际位置，计算控制参数并输出给挖掘机内部控制器。本发明对原有挖掘机控制系统改动小，有利于批量化改装、生产。

Description

一种挖掘机智能控制系统及工作装置轨迹规划控制方法

技术领域

本发明涉及一种挖掘机智能控制系统及工作装置轨迹规划控制方法，属于液压挖掘机智能控制领域。

背景技术

传统液压挖掘机存在自动化程度低、操作复杂、劳动强度高等问题。为提高作业精度和效率，适应危险恶劣的作业环境，减少人力成本，智能化是液压挖掘机发展的主要趋势。

目前，国内外智能挖掘机存在改造成本高、周期长、技术复杂等缺点，同时难以与实际工况相匹配。另外，目前智能挖掘机采用磁致伸缩传感器、拉线传感器测量油缸的位移，存在安装尺寸大，易受碰撞、可靠性差等问题。

发明内容

针对上述现有技术存在的问题，本发明提供一种挖掘机智能控制系统及工作装置轨迹规划控制方法。

为了实现上述目的，本发明采用的一种挖掘机智能控制系统，包括：

倾角传感器，所述倾角传感器用于实时监测挖掘机臂梁倾角；

惯导传感器，所述惯导传感器用于实时监测车身回转角度；

上位机控制器，所述上位机控制器与所述倾角传感器、惯导传感器连接，上位机控制器接收传感器的检测信号，计算挖掘机实时位姿与各执行器位置，上位机控制器对输入挖掘参数进行轨迹规划，并将其转换为挖掘机对应执行器各自目标运动轨迹；

挖掘机内部控制器，所述挖掘机内部控制器与所述上位机控制器连接，上位机控制器根据各执行器规划目标运动轨迹与实际位置，计算控制参数并输出给挖掘机内部控制器。

作为改进，所述倾角传感器有三个，分别安装在挖掘机的动臂、斗杆臂、斗杆臂连杆上；所述惯导传感器安装在挖掘机的回转车身上方。

作为改进，所述控制参数包括控制多路阀信号与泵流量匹配信号。

作为改进，所述上位机控制器、倾角传感器、惯导传感器、挖掘机内部控制器之间信号传输均通过CAN通信。

另外，本发明还提供了一种采用所述挖掘机智能控制系统的工作装置轨迹规划控制方法，包括轨迹规划进程C1和轨迹控制进程C2；

所述轨迹规划进程C1在接收到输入的挖掘参数后开始运行，根据挖掘参数规划各执行器的目标轨迹；规划的各执行器的目标轨迹是随时间变化的连续值，输出时按照时间间隔t₀依次输出对应的数值给轨迹控制进程C2；输出完毕后，最后输出值不再变化，停止运行；

所述轨迹控制进程C2一直运行，接收轨迹规划进程C1的规划目标数值和倾角传感器、惯导传感器的检测反馈数值，经控制器计算后输出控制信号给挖掘机内部控制器。

作为改进，所述轨迹规划进程C1在上位机控制器中运行，具体包括以下步骤：

(a)当前状态感知：接收倾角传感器、惯导传感器的检测反馈值，经计算可得出关节角值，经运动学正解可得出当前位姿；

(b)初始化输出：将当前感知位姿转换成执行器目标值输出，由于输出目标值和执行器实际值相同，执行器静止不动，完成初始化；

(c)笛卡尔空间关键点规划：根据输入的挖掘参数，包括挖掘点(x1，y1，z1)、挖掘长度L、挖掘深度H、卸料点(x2，y2，z2)、卸料角度α、卸料长度b，确定笛卡尔关键位姿点D_i:[x_i，y_i，z_i，ξ_i]和该点速度

(d)关节角空间轨迹规划：将规划的笛卡尔空间关键点通过适应性逆解N程序转换为关节空间点速度通过雅可比逆矩阵J^-1转换为关节空间速度采用过中间点的三次多项式轨迹规划，规划出各执行器的目标轨迹L_j，j＝0～3；

(e)规划目标值输出：规划出各执行器的目标轨迹L_j是时间t的函数，按照时间间隔t₀输出目标值r_j(t)＝L_j(t)，t＝t+t₀。

作为改进，所述轨迹控制进程C2在上位机控制器中运行，计算并输出给挖掘机内部控制器的控制信号u_j和流量匹配信号Q，步骤如下：

(a)控制信号u：输入目标信号r(t)，倾角传感器、惯导传感器信号经计算后得到执行器实时位置信号L′(t)，采用PV控制器进行控制，得到控制信号u并输出给挖掘机内部控制器；

(b)流量匹配信号Q：对执行器的目标轨迹进行微分得到执行器目标轨迹速度根据执行器参数可以计算各执行器所需流量Q_j，则总的流量匹配信号Q＝∑Q_j。

作为改进，所述步骤(a)中PV控制器：

所述步骤(b)中，回转马达执行器流量：式中，n为马达转速，V为马达排量；液压缸执行器流量：/>式中，A_j为液压缸运动时有效作用面积。

最后，本发明还提供了一种挖掘机，所述挖掘机上安装有所述挖掘机智能控制系统。

与现有技术相比，本发明的有益效果是：

1)该智能控制系统、轨迹规划与控制方法适用于所有挖掘机机型，仅需在现有挖掘机上加装上位机和传感器，对原本挖掘机控制系统改动小，有利于实现批量化改装、生产。

2)将工作装置轨迹规划与轨迹控制分为两个进程，明确了轨迹规划与轨迹控制程序执行关系。

3)通过感应当前位置，从当前位置点进行轨迹规划，更符合挖掘机动作习惯，使挖掘机动作更为流畅。

4)采用倾角传感器，将倾角反馈转换为执行器液压缸长度直接控制，控制目标更为明确。与伸出杆的位移传感器相比，安装体积较小，减小了碰撞和土石掉落砸中，抗振动性强；与激光位移检测传感器相比，不受粉尘等影响，抗干扰性好。

附图说明

图1为本发明的结构示意图；

图2为本发明智能挖掘机D-H坐标系建模和倾角传感器安装位置示意图；

图3为本发明工作装置轨迹规划与控制流程图；

图4为本发明的适应性逆解流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面对本发明进行进一步详细说明。但是应该理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限制本发明的范围。

除非另有定义，本文所使用的所有的技术术语和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同，本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。

结合图1所示，一种挖掘机智能控制系统，包括上位机控制器、倾角传感器、惯导传感器、挖掘机内部控制器；

其中，所述倾角传感器有三个，分别安装在挖掘机的动臂、斗杆臂、斗杆臂连杆上，倾角传感器实时监测各个臂梁倾角；

所述惯导传感器安装在挖掘机的回转车身上方，惯导传感器实时监测车身回转角；

上位机控制器、倾角传感器、惯导传感器、挖掘机内部控制器之间信号传输均通过CAN通信。

作为实施例的改进，所述上位机控制器与所述倾角传感器、惯导传感器连接，接收传感器的检测信号，计算挖掘机实时位姿与各执行器位置；所述上位机控制器对输入挖掘参数进行轨迹规划，并将其转换为挖掘机对应执行器各自目标运动轨迹；

所述上位机控制器与所述挖掘机内部控制器连接，根据各执行器规划目标运动轨迹与实际位置，计算控制参数并输出给挖掘机内部控制器，控制参数包括控制多路阀信号与泵流量匹配信号。

作为实施例的改进，所述挖掘机内部控制器为任一型号的反铲挖掘机内部控制器，所做改动为其原有的挖掘机手柄控制信号输入被上位机控制器的输出的执行器控制参数取代；原有的挖掘机泵流量匹配参数被上位机控制器的输出的流量匹配参数取代。

如图2所示，根据实施例的智能挖掘机D-H坐标系建模和倾角传感器安装位置，得到笛卡尔空间-关节空间-执行器空间-传感检测空间转换关系。

①运动学正解

对挖掘机执行装置进行D-H建模，得到运动学正解，此过程为本领域常规知识，不再过多赘述，挖掘机位姿为：[x，y，z，ξ]；

式中：为D-H建模关节角，ξ为铲斗倾角，/> 其余参数为D-H模型参数；

②运动学逆解

根据铲斗位姿[x，y，z，ξ]求解关节角为运动学逆解，采用几何法可求得智能挖掘机运动学唯一逆解：

式中，o₃点在坐标系1中坐标为：

o₃＝(a₁c₁+a₃c₁₂₃+a₂c₁₂-a₃cos(π+ξ)，a₁s₁+a₃s₁₂₃+a₂s₁₂-a₃sin(π+ξ))，

③适应性逆解

如图4所示，由于挖掘机工作的特殊性，其末端位姿并不需要极高的控制精度，因此，当轨迹规划到某些无法到达的特殊点时，需要进行适应性改变。通过对整个挖掘轨迹的研究，挖掘完成后至卸料过程中铲斗倾角须保持极限位置时ξ_max、ξ_min，引起无解或超出极限，因此，将铲斗关节角角取得极限值/>即根据/>求解其余关节角：

④雅可比矩阵

机器人雅可比揭示了操作空间与关节空间的映射关系，表示了操作空间与关节空间的速度映射关系：

式中：v为笛卡尔空间中的广义速度，为关节空间中的关节速度；J(q)为雅可比矩阵。

⑤关节角-执行器长度

如图2所示，关节角对应回转马达转动角度，/>对应液压缸长度：

式中，∠FGI＝∠FGO₃-∠IGO₃，∠IGO₃＝∠IGJ+∠O₃GJ，在ΔIGJ中/>在ΔO₃GJ中/>在ΔO₃GJ中

⑥倾角传感器-关节角

如图1、图2倾角传感器安装位置，可得关节角度：

式中， ∠O₃GI＝∠O₃GK+θ′₃＝θ′₂+∠O₂O₃G+θ′₃θ′₁、θ′₂、θ′₃分别为倾角传感器实际修正值，修正了安装偏差。

⑦倾角传感器-液压缸长度

通过倾角传感器检测，可得液压缸实时反馈长度：

式中，∠AO₁B＝∠AO₁x₁+∠BO₁O₂+θ′₁，∠CO₂D＝π+(-θ′₁+θ′₂)-∠CO₂O₁-∠DO₂O₃，∠FGI＝∠FGO₃-∠IGO₃，∠IGO₃＝∠O₃GK+θ′₃＝θ′₂+∠O₂O₃G+θ′₃。

如图3所示，一种工作装置轨迹规划与控制方法，包含两个进程：轨迹规划进程C1和轨迹控制进程C2，两个进程分别运行；

所述轨迹规划进程C1在接收到输入的挖掘参数后开始运行，根据挖掘参数规划各执行器的目标轨迹；规划的各执行器的目标轨迹是随时间变化的连续值，输出时按照时间间隔t₀依次输出对应的数值给轨迹控制进程C2；输出完毕后，最后输出值不再变化，停止运行；

所述轨迹控制进程C2时刻运行，接收轨迹规划进程C1的规划目标数值和传感器的检测反馈数值，经控制器计算后输出控制信号给挖掘机内部控制器。

其中，所述轨迹规划进程C1在上位机控制器中运行，具体包括以下步骤：

(a)当前状态感知：接收传感器的检测反馈值，经⑥计算可得出关节角值，经①运动学正解可得出当前位姿；

(b)初始化输出：将当前感知位姿由⑤转换成执行器目标值输出，由于输出目标值和执行器实际值相同，执行器静止不动，完成初始化；

(c)笛卡尔空间关键点规划：根据输入的挖掘参数，包括挖掘点(x1，y1，z1)、挖掘长度L、挖掘深度H、卸料点(x2，y2，z2)、卸料角度α、卸料长度b，确定笛卡尔关键位姿点D_i：[x_i，y_i，z_i，ξ_i]和该点速度

(d)关节角空间轨迹规划：将规划的笛卡尔空间关键点通过③适应性逆解N程序转换为关节空间点速度通过④雅可比逆矩阵转换为关节空间速度采用过中间点的三次多项式轨迹规划，规划出各执行器的目标轨迹L_j，j＝0～3；

(e)规划目标值输出：规划出各执行器的目标轨迹L_j是时间t的函数，按照时间间隔t₀输出目标值r_j(t)＝L_j(t)，t＝t+t₀。

其中的，上述轨迹控制进程C2在上位机控制器中运行，计算并输出给挖掘机内部控制器的控制信号u_j和流量匹配信号Q，步骤如下：

(a)控制信号u：输入目标信号r(t)，传感器信号经⑦计算后得到执行器实时位置信号L’(t)，采用PV控制器进行控制，得到控制信号u并输出给挖掘机内部控制器；

PV控制器：

(b)流量匹配信号Q：对执行器的目标轨迹进行微分得到执行器目标轨迹速度根据执行器参数可以计算各执行器所需流量Q_j，则总的流量匹配信号Q＝∑Q_j；

回转马达执行器流量：式中，n为马达转速，V为马达排量；液压缸执行器流量：/>式中，A_j为液压缸运动时有效作用面积。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换或改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种挖掘机智能控制系统，其特征在于，包括：

倾角传感器，所述倾角传感器用于实时监测挖掘机臂梁倾角；

惯导传感器，所述惯导传感器用于实时监测车身回转角度；

上位机控制器，所述上位机控制器与所述倾角传感器、惯导传感器连接，上位机控制器接收传感器的检测信号，计算挖掘机实时位姿与各执行器位置，上位机控制器对输入挖掘参数进行轨迹规划，并将其转换为挖掘机对应执行器各自目标运动轨迹；

挖掘机内部控制器，所述挖掘机内部控制器与所述上位机控制器连接，上位机控制器根据各执行器规划目标运动轨迹与实际位置，计算控制参数并输出给挖掘机内部控制器；

所述挖掘机智能控制系统的工作装置轨迹规划控制方法，包括轨迹规划进程C1和轨迹控制进程C2；

所述轨迹规划进程C1在接收到输入的挖掘参数后开始运行，根据挖掘参数规划各执行器的目标轨迹；规划的各执行器的目标轨迹是随时间变化的连续值，输出时按照时间间隔t₀依次输出对应的数值给轨迹控制进程C2；输出完毕后，最后输出值不再变化，停止运行；

所述轨迹控制进程C2一直运行，接收轨迹规划进程C1的规划目标数值和倾角传感器、惯导传感器的检测反馈数值，经控制器计算后输出控制信号给挖掘机内部控制器；

所述轨迹规划进程C1在上位机控制器中运行，具体包括以下步骤：(a)当前状态感知：接收倾角传感器、惯导传感器的检测反馈值，经计算可得出关节角值，经运动学正解可得出当前位姿；(b)初始化输出：将当前感知位姿转换成执行器目标值输出，由于输出目标值和执行器实际值相同，执行器静止不动，完成初始化；(c)笛卡尔空间关键点规划：根据输入的挖掘参数，包括挖掘点(x1，y1，z1)、挖掘长度:、挖掘深度H、卸料点(x2，y2，z2)、卸料角度α、卸料长度b，确定笛卡尔关键位姿点D_i：[x_i，y_i，z_i，ξ_i]和该点速度(d)关节角空间轨迹规划：将规划的笛卡尔空间关键点通过适应性逆解N程序转换为关节空间点速度通过雅可比逆矩阵J^-1转换为关节空间速度/>采用过中间点的三次多项式轨迹规划，规划出各执行器的目标轨迹L_j，j＝0～3；(e)规划目标值输出：规划出各执行器的目标轨迹L_j是时间t的函数，按照时间间隔t₀输出目标值r_j(t)＝L_j(t)，t＝t+t₀；

所述轨迹控制进程C2在上位机控制器中运行，计算并输出给挖掘机内部控制器的控制信号u_j和流量匹配信号Q，步骤如下：(a)控制信号u：输入目标信号r(t)，倾角传感器、惯导传感器信号经计算后得到执行器实时位置信号L′(t)，采用PV控制器进行控制，得到控制信号u并输出给挖掘机内部控制器；(b)流量匹配信号Q：对执行器的目标轨迹进行微分得到执行器目标轨迹速度根据执行器参数可以计算各执行器所需流量Q_j，则总的流量匹配信号Q＝∑Q_j。

2.根据权利要求1所述的一种挖掘机智能控制系统，其特征在于，所述倾角传感器有三个，分别安装在挖掘机的动臂、斗杆臂、斗杆臂连杆上；所述惯导传感器安装在挖掘机的回转车身上方。

3.根据权利要求1所述的一种挖掘机智能控制系统，其特征在于，所述控制参数包括控制多路阀信号与泵流量匹配信号。

4.根据权利要求1所述的一种挖掘机智能控制系统，其特征在于，所述上位机控制器、倾角传感器、惯导传感器、挖掘机内部控制器之间信号传输均通过CAN通信。

5.根据权利要求1所述的一种工作装置轨迹规划控制方法，其特征在于，所述步骤(a)中PV控制器：

所述步骤(b)中，回转马达执行器流量：式中，n为马达转速，V为马达排量；液压缸执行器流量：/>j＝1～3，式中，A_j为液压缸运动时有效作用面积。

6.一种挖掘机，其特征在于，所述挖掘机上安装有权利要求1-5任一项所述挖掘机智能控制系统。