CN100464036C - 用于液压挖掘机工作装置的轨迹控制系统及方法 - Google Patents

Abstract

一种用于液压挖掘机工作装置的轨迹控制系统及方法，是通过DSP接收动臂角度检测机构、斗杆角度检测机构、铲斗角度检测机构测量信号及相应液压缸压力信号、发动机转速信号，获得相应的电压值，对数据进行存储和处理；存储设定的工作装置的目标位置和姿态；根据工作装置的实际测量角度信号，与通过逆运算获得的数据相比较，产生校正信号；驱动工作装置中驱动油缸动作；采用CAN总线通讯方式，实现数据传输；由前馈控制与非线性PID控制算法相结合的方法进行控制。优点是能够实现液压挖掘机工作装置的自动操纵，精确而稳定地控制液压挖掘机工作装置的位置和姿态，使工作装置按照预先设定的轨迹运动，完成作业任务，缩短作业周期，提高作业效率和作业质量。

Description

用于液压挖掘机工作装置的轨迹控制系统及方法

技术领域

本发明是涉及一种用于液压挖掘机工作装置的轨迹控制系统及方法。

背景技术

1、液压挖掘机主要用于土方工程施工等场合，驾驶员在操纵液压挖掘机工作装置时需要具有一定的熟练性，在直线平整和斜坡挖掘过程中，往往要靠驾驶员的操作经验来完成复杂的动作，增加了驾驶员的劳动强度，降低了劳动生产率。而采用自动操纵可大幅度提高作业效率和作业质量，更好的完成作业任务。

2、目前已有的一些控制装置中未能考虑到液压挖掘机转速、负载以及工作装置姿态的变化对控制性能的影响，造成稳定性和控制精度变差，影响挖掘机工作装置作业的精度。

3、常用的普通位置伺服控制方法，大多数是采用比较典型的PID算法，但针对液压挖掘机工作装置这样特殊的对象，PID参数不易整定，往往不能获得良好的控制效果，并且由于外部载荷变化大，参数不易整定，工况复杂，工作装置各构件之间的耦合关系，造成PID调节器的控制性能变差。

发明内容

本发明的目的就是提供一种用于液压挖掘机工作装置轨迹控制系统及方法，通过该控制系统能够实现液压挖掘机工作装置自动操纵，准确而稳定地控制工作装置的位置和姿态，使工作装置按照预先设定的轨迹运动，完成作业任务。DSP控制器201根据角度传感器获得的动臂斗杆和铲斗的相对转动角度信息及压力、发动机转速信息，由DSP控制器201经过数据处理部分计算后，发出控制量，分别驱动动臂液压缸斗杆液压缸和铲斗液压缸动作，从而准确地控制铲斗的位置和姿态，实现液压挖掘机工作装置的自动操纵。

本发明的解决方案是这样的：

构成本发明的系统包括有动臂液压缸208、斗杆液压缸209、铲斗液压缸210，主控阀207，电磁比例先导阀205，齿轮泵211，溢流阀206，滤油器212，动臂104，斗杆106，铲斗108，其特征在于：

1、在动臂104与工作装置102的铰接处、动臂104与斗杆106的铰接处、斗杆106与铲斗108铰接处分别设置有动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107；

2、具有DSP控制器201，用于接收通过A/D数据转换送入的动臂角度检测机构103、斗杆

角度检测机构105、铲斗角度检测机构107测量的角度信号，以及动臂液压缸208压力、斗杆液压缸209压力、铲斗液压缸210压力信号、发动机转速信号，得到角度的电压值，以及压力和转速电压值，对接收的数据进行数据存储和处理；以及存储预先设定的工作装置的目标位置和姿态；通过对挖掘机工作装置实际的位置、姿态与预先设定的位置、姿态进行比较，产生校正信号；

3、DSP控制器201具有控制输出部分，用于发出的控制指令，产生占空比不同的PWM信号；由驱动放大部分将信号放大，产生相应的驱动电流，驱动先导电磁比例阀205动作；

4、DSP控制器201具有数据显示部分：用于显示液压挖掘机当前的工作状态、液压挖掘机工作装置102中铲斗108的空间位置和姿态信息；

5、具有操纵手柄(1、2)，将操纵手柄移动信号通过数字量传输，以控制器局部网总线的数据传输方式，发送给DSP控制器201。

在本发明中采用的控制方法为：

1、由动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107检测动臂(104)、斗杆(106)和铲斗(108)的相对转动角度，将检测数据送入DSP控制器201；

2、由数据转换部分根据角度传感器采集的角位移信息，通过计算换算成为计算机能够识别的数字量；

3、由DSP控制器201进行数据处理：测量得到的液压挖掘机工作装置102中动臂104、斗杆106和铲斗108相对转动角度信号，通过算法运算后获得工作装置中铲斗108的空间坐标和姿态；反之，如果预先设定铲斗在空间的位置和姿态，则可以通过逆运算获得工作装置中动臂104、斗杆106和铲斗108的相对转动角度；

4、由DSP控制器201进行校正器算法运算部分，根据工作装置中动臂、斗杆和铲斗的实际测量角度信号，与通过逆运算获得的相对转动角度相比较，产生校正信号；

5、由DSP控制器201输出控制信号：将校正器产生的校正信号，转换为相应的PWM信号，通过放信号大，驱动电磁比例先导阀205动作，使主控阀207动作，从而推动液压缸动作；

6、由DSP控制器201进行数据存储部分，来存储测量信号，并在示教挖掘时，记录挖掘过程中动臂104、斗杆106和铲斗108的运行轨迹，控制系统根据记录的数据，形成自动挖掘时的挖掘轨迹序列，当控制器发出挖掘命令时，控制系统执行相应的控制算法，实现示教挖掘；数据存储部分还存储测量得到的数据，包括液压挖掘机工作装置102各机构的相对转动角度、发动机转速和液压缸压力等信息；存储预先设定的工作装置在典型作业时铲斗齿尖的运动轨迹信息。

7、由DSP控制器201将液压挖掘机当前的工作状态、液压挖掘机工作装置102中铲斗108的空间位置和姿态信息送到显示部分进行显示；

8、通过操纵手柄(1、2)与DSP控制器201之间通过CAN总线方式进行数据传输。

本发明的优点是能够实现液压挖掘机工作装置的自动操纵，精确而稳定地控制液压挖掘机工作装置的位置和姿态，使工作装置按照预先设定的轨迹运动，完成作业任务，缩短作业周期，提高作业效率和作业质量。

附图说明

附图是本发明的实施例。

附图1是本发明安装在液压挖掘机的示意图。

附图2液压挖掘机工作装置电液控制系统示意图。

附图3根据本发明实施例的DSP控制器的结构原理图。

附图4CAN总线联接示意图。

附图5操纵手柄示意图。

附图6动臂斗杆或铲斗角度检测机构示意图。

附图7液压挖掘机工作装置空间位置、姿态与角度变换关系示意图。

附图8校正控制器结构图。

附图9发动机转速变化增益补偿曲线示意图。

附图10负载变化增益补偿曲线示意图。

附图11是非线性PID控制参数KP修正示意图。

附图12是非线性PID控制参数KD修正示意图。

附图13是非线性PID控制参数KI修正示意图。

附图14液压挖掘机平整场地作业示意图。

附图15液压挖掘机直线挖掘作业示意图。

图附16操纵手柄运动方向示意图。

具体实施方式

本发明的控制系统包括有动臂液压缸208、斗杆液压缸209、铲斗液压缸210，主控阀207，电磁比例先导阀205，齿轮泵211，溢流阀206，滤油器212，动臂104，斗杆106，铲斗108，其中：

1、在动臂104与工作装置102的铰接处、动臂104与斗杆106的铰接处、斗杆106与铲斗108铰接处分别设置有动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107；

2、具有DSP控制器201，用于接收通过A/D数据转换送入的动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107测量的角度信号，以及动臂液压缸208压力、斗杆液压缸209压力、铲斗液压缸210压力信号以及发动机转速转速信号，得到角度的电压值，以及压力和转速电压值，对接收的数据进行数据存储和处理；以及存储预先设定的工作装置的目标位置和姿态；通过对挖掘机工作装置实际的位置、姿态与预先设定的位置、姿态进行比较，产生校正信号；

3、DSP控制器201具有控制输出部分，用于发出的控制指令，产生占空比不同的PWM信号；由驱动放大部分将信号放大，产生相应的驱动电流，驱动先导电磁比例阀205动作；

4、DSP控制器201具有数据显示部分：用于显示液压挖掘机当前的工作状态、液压挖掘机工作装置102中铲斗108的空间位置和姿态信息；

5、具有操纵手柄(1、2)，将操纵手柄移动信号通过数字量传输，以CAN总线的数据传输方式发送给DSP控制器201。

压力传感器218、219、220分别测量动臂液压缸208、斗杆液压缸209和铲斗液压缸210的压力，根据来自压力传感器的电压值获得各液压缸的负载信息。

DSP控制器201通过CAN总线与其他控制器之间进行数据交换。

动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107是采用转动式角度传感器，检测动臂104、斗杆106和铲斗108之间相对转动的角度。

本实施例提供了一个控制系统101包括DSP控制器201，如图3所示，用来实现挖掘机工作装置轨迹控制，其中DSP控制器201作为核心控制单元，其型号为TMS320LF2407，具有25ns的指令周期，40MIPS的处理能力，片内有32K×16的FLASH EEPROM，作为程序存储器，2.5K×16的RAM。2组×8路16位PWM输出，并带有可编程死区保护和PDP保护。2组×8路10位A/D转换器，具有多种同步触发的模式，内置看门狗电路。片内有锁相环电路，可外接较低频的振荡器。具有CAN总线通讯接口。

来自各传感器的信号输入到安装在液压挖掘机上的DSP控制器201，DSP控制器201根据测量信号，以及操纵手柄信号，经过数据处理及算法运算后输出PWM信号，经过放大电路放大，输出控制电流，驱动电磁比例阀205动作，主控阀207根据来在电磁比例阀205的先导压力，控制动臂液压缸208、斗杆液压缸209和铲斗液压缸210动作，使铲斗按预先设计的轨迹动作，实现挖掘机工作装置的自动操纵，完成相应的作业任务。

本发明中角度检测机构，如图6所示，包括：一个角度传感器303，通过外罩304固定在动臂306下绞点的销轴305处，由销轴301与固定在动臂上的销301，由拨杆302带动与之联接的角度传感器303轴转动，相对转动的角度以电压形式给出，用来检测动臂104相对与回转平台平面相对转动的角度；具有与图6所示相同的角度检测机构，安装在斗杆与动臂联接销轴处的角度传感器，用来检测斗杆106与动臂104之间的相对转动角度；具有与图6所示相同的角度检测机构，安装在斗杆与铲斗联接销轴处的角度检测机构，用来检测铲斗108与斗杆106之间的相对转动的角度。

数据存储部分由DSP中32K×16的FLASH EEPROM实现，用来存储测量信号，并在示教挖掘过程中，记录挖掘过程中，动臂104、斗杆106和铲斗108的相对转动角度，DSP控制器201根据记录的数据，形成自动挖掘时的挖掘轨迹序列，当DSP控制器201发出挖掘命令时，控制系统执行相应的控制算法，实现示教挖掘。

数据转换部分由A/D实现。连接到控制器上的各种传感器包括：角度传感器、发动机转速传感器、压力传感器，DSP控制器201将采集的测量信号，经过A/D转换后，实现模拟信号向数字信号转换，将测量信号的电压值等转换为相应的数字量，通过接口送入DSP控制器201中。

控制输出部分由DSP控制器201的PWM信号输出部分、输入/输出电路202，信号调理电路203，和放大驱动电路204，驱动电磁比例先导阀205动作，使主控阀207动作，从而推动液压缸动作；

数据显示部分，采用128×64液晶显示器，显示当前工作状态以及铲斗在空间的位置和姿态等信息。

数据传输采用CAN总线通讯方式，通过CAN总线将两个传统的控制手柄(1、2)及其他控制设备连接起来构成局域网，如图4所示。其连接方式采用两根双绞线，终端带有120欧姆的补偿电阻。CAN总线信号部分，实现现场设备与控制设备的信号连接，省去了大量导线，提高了控制信号的可靠性，抗干扰能力强。CAN总线适合于液压挖掘机工作环境恶劣，保证控制的实时性和灵活性，可扩展性，与其它外围设备构成多控制器网络结构；采用发送报文的编码方法和非破坏性优先级总线总裁技术，抗干扰能力强、可靠性高、具有较高的实时性。CAN总线通讯协议符合SAE J1939的标准，CAN物理层符合SAE J1939/15标准，通信速率为250Kbit/s，终端电阻为120欧姆。

一个控制手柄，将操纵手柄(1、2)移动信号通过数字量传输，发送给控制装置(CAN总线的数据传输方式)。

DSP控制器201还可以通过CAN总线与其他控制器之间进行数据交换；来自操纵手柄(1、2)的操纵信号及各个传感器的信号包括由CAN总线传输的信号输入到DSP控制器201，控制器201根据检测到的信号，执行相应的控制算法，控制电磁比例先导阀205动作，从而控制主控阀207，推动液压缸伸出或者缩进，实现对挖掘机工作装置的电控操纵。

作为上述的控制模式，操作控制手柄(1、2)上的按钮A或者B，可实现不同自动作业模式选择。操纵手柄(1、2)作为一个具有CAN总线接口的设备，连接到DSP控制器201的CAN总线接口上。操纵手柄(1、2)上包括控制按钮，如图5、图16所示。操纵手柄具有CAN总线设备接口，其通讯符合SAEJ1939协议。当按下模式选择按钮A或B时，由控制软件根据所选择的功能，执行程序，实现相应的控制功能。根据不同的按钮操作对应的挖掘方式包括：平整场地作业方式，直线挖掘作业方式以及示教挖掘方式。

控制装置的信号转换部分包括：一个输入接口电路(203、204)，用来从角度传感器215、216、217接收检测信号，测量动臂104、斗杆106和铲斗108的相对转角，压力传感器218、219、220信号及转速传感器信号(未示出)，经过信号调理电路202及输入/输出接口电路203，送入DSP控制器201的A/D接口端。

本发明的控制方法为：

1、由动臂角度检测机构103、斗杆角度检测机构105、铲斗角度检测机构107检测动臂机构相对转动角度，将检测数据送入DSP控制器201；

2、由数据转换部分根据角度传感器采集的角位移信息，通过计算换算成为计算机能够识别的数字量；

3、由DSP控制器201进行数据处理：测量得到的液压挖掘机工作装置102中动臂104、斗杆106和铲斗108相对转动角度信号，通过算法运算后获得工作装置中铲斗的空间坐标；反之，如果预先设定铲斗在空间坐标系中的位置和姿态，则可以通过逆运算获得工作装置中动臂104、斗杆106和铲斗108的相对转动角度；

4、由DSP控制器201进行校正器算法运算部分，根据实际测量的角度位置信号与预先给定的角度位置相比较，产生校正信号；

5、DSP控制器201输出控制信号，产生占空比不同的PWM信号；由驱动放大部分将信号放大，产生相应的驱动电流，驱动先导电磁比例阀205动作，使主控阀207动作，从而推动液压缸动作；

6、由DSP控制器201进行数据存储部分，存储测量信号，并在示教挖掘过程，记录挖掘过程中动臂104、斗杆106和铲斗108的运行轨迹，控制系统根据记录的数据，形成自动挖掘时的挖掘轨迹序列，当DSP控制器201发出挖掘命令时，控制系统执行相应的控制算法，实现示教挖掘；数据存储部分还存储测量得到的数据，包括液压挖掘机工作装置102各机构的相对转动角度、发动机转速和液压缸压力等信息；存储预先设定的工作装置在典型作业时铲斗齿尖的运行轨迹信息。

7、DSP控制器201的数据显示部分用于显示液压挖掘机当前的工作状态、液压挖掘机工作装置102中铲斗108的空间位置和姿态信息；

8、通过操纵手柄(1、2)与DSP控制器201之间通过CAN总线方式进行数据传输；DSP控制器201还可以通过CAN总线与其他控制器之间进行数据交换；

所述的控制算法是采用非线性PID算法，当DSP控制器201发出挖掘命令时，经非线性PID校正器进行计算，作为输入控制量，控制挖掘机工作装置运动。

在挖掘机工作装置(102)位置输出端取出信号，进行前馈校正，前馈校正产生的控制量与非线性PID校正器产生控制量一起，作为输入控制量。

根据检测的角度，采用动力学方程，通过计算获得重力力矩，计算出的重力补偿参数与本发明采用的非线性PID校正器产生控制量一起，作为挖掘机工作装置的输入控制量。

当同时采用前馈校正和重力参数补偿一起与非线性PID校正器产生控制量一起，作为挖掘机工作装置的输入控制量时，可以得到最佳的控制效果。

本实施例就是采用前馈校正和重力参数补偿一起与非线性PID校正器产生控制量一起，作为挖掘机工作装置的输入控制量。

本发明数据处理部分：

附图7中，铲斗齿尖V的空间直角坐标系是以液压挖掘机回转中心做为坐标原点，Z轴与液压挖掘机回转中心线重合，垂直于地面方向，X轴与水平面重合，并指向挖掘机前进的方向，Y轴沿着水平面与X轴垂直。l₀表示C点到Y轴的距离；d₁表示C点到X轴的距离；l₁表示动臂C点到F点的距离；l₂表示动臂F点到Q点的距离；l₃表示动臂Q点到V点的距离。θ₁表示线段CF与水平面的夹角；θ₂表示线段CF与FQ的夹角；θ₃表示线段FQ与QV的夹角；V_xV_yV_z表示铲斗在空间坐标系中的坐标，β表示铲斗齿尖V与铲斗绞接点Q的连线与水平面的夹角；

根据图7所示，DSP控制器201的数据处理部分计算铲斗108在空间直角坐标系中的位置和姿态V_xV_yV_zβ，通过逆运算获得工作装置中动臂104、斗杆106和铲斗108之间的相对转角θ₁θ₂θ₃；将测量得到的工作装置中动臂104、斗杆106和铲斗108相对转动角度θ₁θ₂θ₃，通过算法运算后获得工作装置中铲斗108的空间坐标和姿态。

液压挖掘机电液伺服系统存在滞后、非线性的特性，为了提高液压挖掘机电液伺服系统的控制特性，提高控制系统的稳定性，使其调整迅速、超调小，在控制回路增加PID校正器。针对液压挖掘机这样特殊的对象，普通PID算法往往不能获得良好的控制效果。如图8所示，根据本发明提供的校正控制器，包括反馈补偿特性的非线性PID控制、前馈补偿控制，以及重力补偿控制。PID校正器属于反馈控制，前馈补偿控制包括负载以及转速补偿，根据检测的角度由动力学方程，获得重力力矩，通过计算获得重力补偿参数K，DSP控制器201经过非线性PID校正器及前馈补偿运算后，产生控制信号，转换为相应的PWM信号，通过放大驱动电路放大后，为比例电磁阀提供控制电流，驱动电磁比例先导阀205动作，最终通过主控阀207推动液压缸动作。

前馈补偿控制包括负载增益补偿系数Kf以及发动机转速增益补偿系数Ke，以及动力学补偿参数，其中动力学补偿参数由动力学方程根据检测的角度，通过计算获得重力项力矩补偿参数K，并作为前馈补偿量的一部分，与校正器输出的控制量一起，作为电磁比例先导阀205的输入控制量，驱动电磁比例先导阀205的动作。根据发动机转速补偿曲线如图9所示，DSP控制器201输出的每个电磁比例先导阀的电流值，与发动机转速变化增益补偿曲线中得到的校正系数相乘，将乘积作为修正过的电磁比例阀控制指令数值，输出到被控制的电磁比例先导阀205，实现根据发动机转速变化而进行前馈补偿。

发动机转速是由驾驶员设置油门的位置决定的，不同的油门位置对应不同的转速信号，即使发动机油门位置固定，发动机的转速也会随着负载的变化而变化，由于齿轮泵211是直接连接到发动机上的，所以如果发动机转速改变，那么齿轮泵211的排量也发生变化。另外，如果发动机的转速因为外加负载等变化，则齿轮泵211的转速会因为发动机的转速的变化而变化并且泵的输出量也发生变化，因此即使输出给电磁比例先导阀205的电流值是一样的，液压缸的伸出速度也要发生变化。为了补偿由于发动机转速变化造成泵流量变化，或者负载变化造成发动机转速变化对泵流量的影响，提高控制系统的性能，在发动机上安装转速传感器，对发动机速度变化进行补偿。DSP控制器201测得的发动机转速值，根据如图9所示的发动机转速变化增益补偿曲线，得到发动机转速增益补偿系数Ke，DSP控制器201输出的每个电磁比例先导阀的电流值与发动机转速增益补偿系数Ke相乘，将乘积作为修正过的电磁比例阀控制指令数值，输出到被控制的电磁比例先导阀205，实现根据发动机转速变化而进行前馈补偿。

DSP控制器201根据动臂液压缸208斗杆液压缸209及铲斗液压缸210中的压力信号，对由于负载变化造成的流量分配问题，进行前馈补偿控制。动臂、斗杆和铲斗液压缸的负载检测由压力传感器(218、219、220)构成等，并根据来自压力传感器的信息检测液压缸的负载信息，如图10所示的负载增益校正曲线，得到负载增益补偿系数Kf，控制部分输出的比例阀的PWM控制信号与其相乘，经过放大驱动后，作为电磁比例阀205的输入控制信号，驱动比例阀动作，最终推动液压缸运动，实现对工作装置的位置和姿态的精确控制，并具有一定的稳定性。

根据对液压挖掘机进行动力学分析，根据工作装置重力项力矩和工作装置动臂、斗杆和铲斗的质量进行计算，得到重力补偿参数K，作为前馈补偿量的一部分，与非线性PID校正器给出的控制量相加，作为电磁比例先导阀205的控制电流值，驱动电磁比例先导阀205的动作。

校正控制器中反馈控制采用非线性PID算法，其中KP随误差e变化的修正曲线形状如图11所示，当控制开始系统具有较快的响应速度，比例增益KP较大，当误差e逐渐减小时，比例增益随着减小；KD随误差e变化的修正曲线形状如图12所示，KI随误差e变化的修正曲线形状如图13所示，当误差e信号较大时，积分增益KI相对较小，防止产生震荡，减小超调量，当误差e较小时，积分增益KI增大，消除系统的稳态误差。

如下描述DSP控制器201按照预先设定的轨迹进行挖掘作业，如：平整场地、直线挖掘和示教挖掘作业。如图14所示，当用上述方式的挖掘机轨迹控制系统101进行平整场地作业时，铲斗108对地的倾角保持不变，铲斗齿尖处与地面平行运动。

在进行实时控制时，根据来自DSP控制器201的数据存储器中设定的铲斗齿尖在空间的目标位置和姿态，由DSP控制器201中的数据处理部分进行逆运算，获得动臂104、斗杆106与铲斗108的目标转动角度，DSP控制器201根据目标转动角度与实际角度的偏差大小，通过前馈补偿和反馈控制，产生相应的控制信号，控制先导比例电磁阀205动作，从而驱动相应的液压缸动作，使铲斗齿尖沿着预先设定的轨迹运动，实现各种挖掘作业。DSP控制器201通过存储在程序存储器中预先设定铲斗齿尖的直线运动空间坐标，由逆运动学计算获得动臂、斗杆和铲斗的目标相对转动角度，与经过传感器测量的动臂104、斗杆106和铲斗108的实际相对转动角度进行比较，根据偏差大小，由DSP控制器201输出控制部分产生PWM控制信号，经过信号放大电路203，驱动电磁比例先导阀205，使主控阀207动作，从而推动液压缸动作，使液压挖掘机工作装置102中的铲斗齿尖沿着平行于地面方向运动，实现平整场地作业。

如附图15所示，与平整场地作业过程相同，通过前面所述的运动学正运算后得出铲斗108在空间的位置坐标，以及目标位置坐标，然后由逆运算获得动臂104、斗杆106和铲斗108的目标相对转动角度，DSP控制器201通过计算，发出控制量，驱动相应的液压缸动作，按照预先设定的直线挖掘作业控制程序，实现直线挖掘作业。

示教挖掘作业就是首先由人工操纵挖掘机工作装置102，DSP控制器201分别按时间序列记录铲斗108、斗杆106和动臂104的相对转角，并将采集的信号存储在DSP控制器201的数据存储部分，示教操作完成后，由DSP控制器201根据存储的运行轨迹，重复上述操作，实现示教挖掘作业。

如上所述根据用于本发明的一种液压挖掘机工作装置轨迹控制系统101，提供一种用于液压挖掘机工作装置102轨迹控制系统，通过该控制系统101能够实现液压挖掘机工作装置102的自动操纵，精确而稳定地控制液压挖掘机的工作装置的位置和姿态，使工作装置按照预先设定的轨迹运动，完成作业任务，缩短作业周期，提高作业效率和作业质量，所以在液压挖掘机中采用该控制系统101，具有很高的实用性和应用价值。

Claims (10)

1.【权利要求1】一种用于液压挖掘机工作装置的轨迹控制系统，包括有动臂液压缸(208)、斗杆液压缸(209)、铲斗液压缸(210)，主控阀(207)，电磁比例先导阀(205)，齿轮泵(211)，溢流阀(206)，滤油器(212)，动臂(104)，斗杆(106)，铲斗(108)，其特征在于：

(1)、在动臂(104)与工作装置(102)的铰接处、动臂(104)与斗杆(106)的铰接处、斗杆(106)与铲斗(108)铰接处分别设置有动臂角度检测机构(103)、斗杆角度检测机构(105)、铲斗角度检测机构(107)；

(2)、具有数字信号处理器，用于接收通过A/D数据转换送入的动臂角度检测机构(103)、斗杆角度检测机构(105)、铲斗角度检测机构(107)测量的角度信号，以及动臂液压缸(208)压力、斗杆液压缸(209)压力、铲斗液压缸(210)压力信号、发动机转速信号，得到角度的电压值，以及压力和转速电压值，对接收的数据，进行数据存储和处理；以及存储预先设定的工作装置的目标位置和姿态；通过对挖掘机工作装置中铲斗(108)的实际位置、姿态与预先设定的位置、姿态进行比较，产生校正信号；

(3)、数字信号处理器(201)具有控制输出部分，用于发出控制指令，产生占空比不同的脉宽调制信号；由驱动放大部分将信号放大，产生相应的驱动电流，驱动先导电磁比例阀(205)动作；

(4)、数字信号处理器(201)具有数据显示部分：用于显示液压挖掘机当前的工作状态、液压挖掘机工作装置(102)中铲斗(108)的空间位置和姿态信息；

(5)、具有操纵手柄(1、2)，将操纵手柄移动信号通过数字量传输，以控制器局部网总线的数据传输方式发送给数字信号处理器(201)。

2.【权利要求2】根据权利要求1所述的用于液压挖掘机工作装置的轨迹控制系统，其特征在于压力传感器(218、219、220)分别测量动臂液压缸(208)、斗杆液压缸(209)和铲斗液压缸(210)的压力，根据来自压力传感器的电压值获得各液压缸的负载信息。

3.【权利要求3】根据权利要求1所述的用于液压挖掘机工作装置的轨迹控制系统，

其特征在于数字信号处理器(201)通过控制器局部网总线，与其他控制器之间进行数据交换。

4.【权利要求4】根据权利要求1所述的用于液压挖掘机工作装置的轨迹控制系统，其特征在于动臂角度检测机构(103)、斗杆角度检测机构(105)、铲斗角度检测机构(107)是采用转动式角度传感器，检测相对转动的角度。

5.【权利要求5】一种用于液压挖掘机工作装置的轨迹控制方法，其特征在于：

(1)、由动臂角度检测机构(103)、斗杆角度检测机构(105)、铲斗角度检测机构(107)测量动臂(104)、斗杆(106)和铲斗(108)相对转动角度，将测量数据送入数字信号处理器(201)；

(2)、由数据转换部分根据角度传感器采集的角位移信息，通过计算换算成为计算机能够识别的数字量；

(3)、由数字信号处理器(201)进行数据处理：测量得到的液压挖掘机工作装置(102)中动臂(104)、斗杆(106)和铲斗(108)相对转动角度信号，通过算法运算后获得工作装置中铲斗(108)的空间位置和姿态；反之，如果预先设定铲斗(108)在空间的位置和姿态，则通过逆运算获得工作装置中动臂(104)、斗杆(106)和铲斗(108)的相对转动角度；

(4)、由数字信号处理器(201)进行校正器算法运算部分，根据工作装置中动臂、斗杆和铲斗的实际测量角度信号，与通过逆运算获得的相对转动角度相比较，产生校正信号；

(5)、由数字信号处理器(201)输出控制信号：将校正器产生的校正信号，转换为相应的脉宽调制信号，通过放信号大，驱动电磁比例先导阀(205)动作，使主控阀(207)动作，从而推动液压缸动作；

(6)、由数字信号处理器(201)进行数据存储，存储工作装置的位置和姿态的测量数据，在示教挖掘时，记录挖掘过程中动臂(104)、斗杆(106)和铲斗(108)的运行轨迹，控制系统根据记录的数据，形成自动挖掘时的挖掘轨迹序列，当控制器发出挖掘命令时，控制系统执行相应的控制算法，实现示教挖掘；数据存储部分还存储测量得到的数据，包括液压挖掘机工作装置(102)各机构的相对转动角度、发动机转速和液压缸压力等信息；存储预先设定的工作装置在典型作业时铲斗齿尖的运行轨迹信息；

(7)、由数字信号处理器(201)将液压挖掘机当前的工作状态、液压挖掘机工作装置(102)中铲斗(108)的空间位置和姿态信息送到显示部分进行显示；

(8)、通过操纵手柄(1、2)与控制器(201)之间通过CAN总线方式进行数据传输。

6.【权利要求6】根据权利要求5所述的用于液压挖掘机工作装置的轨迹控制方法，其特征在于：铲斗齿尖的空间坐标系是以液压挖掘机回转中心作为坐标原点，空间坐标系包括与液压挖掘机回转中心线重合且垂直于地面的坐标轴、与水平面重合且指向液压挖掘机前进方向的坐标轴、沿着水平面且与挖掘机前进方向垂直的坐标轴。由数字信号处理器(201)的数据处理部分计算铲斗齿尖在空间坐标系中的位置和姿态，通过逆运算获得工作装置中动臂(104)、斗杆(106)和铲斗(108)之间的相对转角；反之，测量得到的工作装置中动臂(104)、斗杆(106)和铲斗(108)相对转动角度，通过算法运算后获得工作装置中铲斗齿尖的空间坐标。

7.【权利要求7】根据权利要求5所述的用于液压挖掘机工作装置的轨迹控制方法，其特征在于：所述的控制算法是采用非线性PID算法，当数字信号处理器(201)发出挖掘命令时，根据工作装置设定值与实际值的偏差，经非线性PID校正器进行计算，作为输入控制量，控制挖掘机工作装置(102)运动。

8.【权利要求8】根据权利要求7所述的用于液压挖掘机工作装置的轨迹控制方法，其特征在于：在挖掘机工作装置(102)位置输出端取出信号，进行前馈校正，前馈校正产生的控制量与非线性PID校正器产生控制量一起，作为输入控制量。

9.【权利要求9】根据权利要求7所述的用于液压挖掘机工作装置的轨迹控制方法，其特征在于：根据检测的角度，由动力学方程计算挖掘机工作装置(102)的重力力矩，获得重力补偿参数K，与非线性PID校正器产生控制量一起，作为输入控制量。

10.【权利要求10】根据权利要求8所述的用于液压挖掘机工作装置的轨迹控制方法，其特征在于：前馈校正采用负载增益补偿系数Kf以及发动机转速增益补偿系数Ke，在发动机上安装转速传感器，由数字信号处理器(201)测得的发动机转速值，根据发动机转速变化增益补偿曲线，得到发动机转速增益补偿系数Ke，数字信号处理器(201)输出的每个电磁比例先导阀的电流值与发动机转速增益补偿系数Ke相乘，将乘积作为修改过的电磁比例阀控制指令数值，输出到被控制的电磁比例先导阀(205)，实现根据发动机转速变化而进行前馈补偿；数字信号处理器(201)根据动臂液压缸(208)斗杆液压缸(209)及铲斗液压缸(210)中的压力传感器(218、219、220)信息检测液压缸的压力值，通过负载变化增益补偿曲线，得到负载增益补偿系数Kf，控制部分输出的比例阀的脉宽调制信号与其相乘，经过放大驱动后，作为电磁比例阀的输入控制信号，驱动电磁比例先导阀(205)动作，最终推动液压缸运动，实现对工作装置的位置和姿态的精确控制。

Images