[发明专利]一种挖掘机三维姿态显示及远程自动控制系统

摘要

本发明公开了一种挖掘机三维姿态显示及远程自动控制系统，通过拉线式位移传感器采集动臂液压缸、斗杆液压缸、铲斗液压缸的位移信号，由数据采集卡发送至计算机；通过电子罗盘采集车身回转角度信号，由RS232串口发送至计算机；计算机对数据进行存储和处理，实时显示挖掘机的三维动画仿真姿态；规划期望的铲斗末端的轨迹；根据工作装置实际的位移和车身回转信号，并经过运动学转换与期望的轨迹数据相比较，运行控制算法，矫正轨迹；采用CAN通讯方式，实现计算机和挖掘机专用DSP控制器数据的通讯；实时监测运行状态信息；实时显示X、Y、Z轴铲斗齿尖运行轨迹和铲斗齿尖的三维位移曲线；利用虚拟仪器的WEB服务实现远程自动控制。

主权项

1.一种挖掘机三维姿态显示及远程自动控制系统，其特征在于，包括挖掘机操作模块、数据采集模块、实时轨迹计算模块、轨迹控制模块、运行信息监测模块、数据存储模块和三维可视化模块；所述挖掘机操作模块包括操作手柄(201)、计算机(202)、DSP控制器(203)、电液比例阀(208)和多路阀(209)组成的油液控制回路，先导泵(207)、前泵(205)和后泵(206)组成的供油元件，动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)和回转马达(213)组成的执行机构，以及动臂液压缸拉线式位移传感器(101)、斗杆液压缸拉线式位移传感器(103)、铲斗液压缸拉线式位移传感器(105)、电子罗盘(215)、数据采集卡(216)和USB‑CAN卡(217)组成的数据采集机构；先导泵(207)依据操作手柄(201)和计算机(202)的控制信号来调节给电液比例阀(208)的供油大小和方向，从而产生相应的阀芯开度和动作方向；多路阀(209)接收来自电液比例阀(208)的信号并产生相应的阀芯开度，从而控制前泵(205)和后泵(206)的流量变化，使动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)和回转马达(213)产生相应的动作，DSP控制器(203)与计算机(202)通过USB‑CAN卡(217)总线接口实时通讯；在动臂液压缸(210)上安装有动臂拉线式位移传感器(101)，在斗杆液压缸(211)上安装有斗杆拉线式位移传感器(103)，在铲斗液压缸(212)上安装有铲斗拉线式位移传感器(105)，在驾驶室顶端上安装有电子罗盘(215)；所述数据采集模块采集动臂液压缸(210)、斗杆液压缸(211)和铲斗液压缸(212)的位移和车身的回转角度信息，并将动臂液压缸拉线式位移传感器(101)、斗杆液压缸拉线式位移传感器(103)、铲斗液压缸拉线式位移传感器(105)和电子罗盘(215)的信号输出至实时轨迹计算模块；所述实时轨迹计算模块根据接收的信号，计算出铲斗齿尖运动轨迹和三维位移曲线并输出至三维可视化模块；所述三维可视化模块根据接收的数据实时显示挖掘机的三维实时姿态；所述轨迹控制模块用于进行轨迹控制，包括实时轨迹控制模块和轨迹规划模块；所述实时轨迹控制模块直接调用USB‑CAN卡(217)中的通讯动态链接库文件实现动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)和回转马达(213)位移数据的发送，通过控制器局域网络CAN进行计算机(202)与DSP控制器(203)的数据交换，DSP控制器(203)接收到数据后实现对动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)和回转马达(213)运动的直接控制；所述轨迹规划模块用于规划出期望的铲斗末端的轨迹数据；所述运行信息监测模块用于实时显示挖掘机运行状态信息；所述数据存储模块用于存储挖掘机运行状态信息和铲斗齿尖运动轨迹；所述挖掘机运行状态信息包括：液压油温度，冷却水温度，机油压力，燃油油位，发动机转速，前泵主压力，后泵主压力，前泵比例阀电流，后泵比例阀电流，动臂手柄电压，斗杆手柄电压，铲斗手柄电压，回转手柄电压，左脚踏电压，右脚踏电压，动臂液压缸大腔压力，动臂液压缸小腔压力，斗杆液压缸大腔压力，斗杆液压缸小腔压力，铲斗液压缸大腔压力，铲斗液压缸小腔压力；系统执行如下步骤：步骤1、建立挖掘机的三维可视化模型：在SolidWorks中建立挖掘机的三维实体模型，完成动臂液压缸(210)、动臂(102)、斗杆液压缸(211)、斗杆(104)、铲斗液压缸(212)和铲斗(106)之间位置关系和运动关系的建立；将回转角度和由动臂(102)、斗杆(104)和铲斗(106)组成的工作装置上各液压缸的位移分别与对应模型坐标的旋转轴相连接，将动臂液压缸(210)、斗杆液压缸(211)和铲斗液压缸(212)的位移信息和车身回转角度转化为绕三维空间中各坐标轴的旋转量；步骤2、数据采集模块采集动臂拉线式位移传感器(101)，斗杆拉线式位移传感器(103)和铲斗拉线式位移传感器(105)的信号，并将采集到的电压信息转换成实际测量的位移值，通过电子罗盘(215)测得回转角度，将位移值和回转角度发送至实时轨迹计算模块；步骤3、实时轨迹计算模块计算出X、Y、Z轴铲斗齿尖运动轨迹和三维位移曲线；步骤4、运行信息监测模块通过USB‑CAN卡(217)与DSP控制器(203)的CAN口相连接，并将从DSP控制器(203)接收到的挖掘机运行信息报文ID字符串和设定的发送至CAN上的地址进行匹配，从而解析出对应的挖掘机运行状态信息并进行显示；步骤5、数据存储模块实时存储挖掘机运行状态信息和X、Y、Z轴铲斗齿尖运动轨迹；步骤3包括：步骤3‑1，建立D‑H坐标系下挖掘机的结构简图，在回转中心O点建立回转坐标系，θ₁为回转角度；动臂(102)和底座的铰接点C建立动臂坐标系，θ₂为动臂(102)关节角；斗杆(104)与动臂(102)铰接点F建立斗杆坐标系，θ₃为斗杆(104)关节角；斗杆(104)与铲斗(106)铰接点Q建立铲斗坐标系，θ₄为铲斗关节角；铲斗斗齿尖点V建立齿尖坐标系；A点为动臂液压缸与车身基座铰接点；B点为动臂液压缸与动臂铰接点；D点为斗杆液压缸与动臂铰接点；E点为斗杆液压缸与斗杆铰接点；F点为斗杆与动臂铰接点；Q点为斗杆与铲斗铰接点；N点为摇臂与斗杆铰接点；S点为铲斗液压缸与摇臂铰接点；K点为连杆与铲斗铰接点；步骤3‑2，根据以下公式正解计算出关节转角：将步骤2数据采集模块动臂拉线式位移传感器(101)、斗杆拉线式位移传感器(103)和铲斗拉线式位移传感器测量到的动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)的位移和电子罗盘(215)测得回转角度转换成关节角θ₂、θ₃、θ₄和θ₁，利用如下公式，根据关节角θ₂、θ₃、θ₄和θ₁计算出铲斗末端的坐标V(x，y，z)和铲斗姿态角ζ：其中，a₁为铰接点C与回转中心O点水平方向上的长度；d₁为铰接点C与回转中心O点在竖直方向上的长度；a₂为CF的长度；a₃为FQ的长度；a₄为QV的长度；所述轨迹控制模块进行轨迹控制的过程包括：步骤101，实时轨迹控制模块直接控制动臂液压缸(210)、斗杆液压缸(211)、铲斗液压缸(212)和回转马达(213)的动作，实时轨迹控制模块调用USB‑CAN卡的通讯动态链接库文件实现数据的发送，通过控制器局域网络CAN进行数据交换，设置好要发送的数据及发送数据的报文ID号后通过Transmit动态链接库文件直接发送至CAN总线，DSP控制器(203)接收到CAN总线上的数据后经过DSP控制器(203)内部程序的运算最终实现挖掘机运动的控制；步骤102，轨迹规划模块规划得到期望的铲斗齿尖运动轨迹；步骤103，通过期望的铲斗齿尖运动轨迹，得到铲斗末端的坐标V(x，y，z)、铲斗姿态角ζ、Q点坐标(x_q，y_q，z_q)和α、β和γ，α是CF与CQ的夹角，β是CQ与CV的夹角，γ是CV与水平面的夹角，根据下面的公式逆解计算出各关节的转角：其中，CQ和Q点坐标(x_q，y_q，z_q)计算公式如下：步骤104，通过各关节的转角控制工作装置沿着期望的铲斗末端轨迹运动，将关节转角转换成对应的液压缸长度；步骤105，采集实际动臂液压缸拉线式位移传感器(101)，斗杆液压缸拉线式位移传感器(103)，铲斗液压缸拉线式位移传感器(105)和电子罗盘(215)的信号与期望的轨迹数据相比较，通过比例积分微分PID控制算法形成闭环反馈控制，判断误差，产生控制量u(t)；步骤106，DSP控制器(203)通过CAN总线通讯方式接收控制量u(t)和对应的控制电压信号，将控制电压信号转换成电流信号，然后输入到电液比例阀(208)，多路阀(209)接收来自电液比例阀(208)的信号产生相应的阀芯开度，从而控制前泵(205)，后泵(206)的流量变化，使动臂液压缸(210)，斗杆液压缸(211)，铲斗液压缸(212)和回转马达(213)产生相应的动作；步骤102包括：通过如下的五次多项式对运动轨迹进行插值：s(t)＝b₀+b₁t+...+b_n‑2t^n‑2+b_n‑1t^n‑1(n＝6)其中，s(t)为期望的运动轨迹，b₀～b_n‑1为系数，t为运动时间，设轨迹从起点s₀(x₀，y₀，z₀)到终点s₁(x₁，y₁，z₁)的总时间为t_b，约束条件为：最终规划得到期望的铲斗齿尖运动轨迹为：步骤105包括：步骤105‑1，控制量u(t)计算公式如下：其中，t为时间，e(t)为输入r(t)与输出y(t)的偏差，e(t)＝y(t)‑r(t)，K_P为比例增益，K_I为积分增益，K_D为微分增益；步骤105‑2，采用经典的Ziegler‑Nichols(ZN)法来初步确定控制参数K_P、K_I和K_D的范围；步骤105‑3，计算粒子适应度，每个粒子代表一组K_P、K_I和K_D参数，使用适应度来评价粒子所获最优位置的好坏，并作为后续粒子速度和位置更新的依据，利用下面的定义的目标函数公式计算各粒子的适应度J_ITAE：其中，T_i为积分时间，e(t)为输入与输出的偏差，输入r(t)为期望的动臂液压缸(210)，斗杆液压缸(211)，铲斗液压缸(212)和回转马达(213)的轨迹数据，输出y(t)为实际动臂液压缸拉线式位移传感器(101)、斗杆液压缸拉线式位移传感器(103)、铲斗液压缸拉线式位移传感器(105)和电子罗盘(215)的信号；步骤105‑4，更新粒子最优解和整个粒子群最优解，针对每个粒子，如果当前位置的适应度好于该粒子目前所找到的最优解，则更新粒子的个体最优解，如果当前位置的适应度好于整个粒子群目前所找到的最优解，则更新整个粒子群目前所找到的最优解，否则保持不变；步骤105‑5，执行遗传操作：根据计算所得每个粒子的适应度，对粒子群执行选择和交叉操作；步骤105‑6，更新粒子状态，通过如下的公式来更新第i个粒子在第t+1次迭代时自己的速度和位置其中，为第i个粒子在第t次迭代时自己的速度，为第i个粒子在第t次迭代时自己的位置，为个体历史最佳位置，为全局粒子群最佳位置，w为惯性权重，c₁,c₂为学习因子，均分布在范围[0,4]内；r₁,r₂为分布在[0,1]内的随机数；第t次迭代时的惯性权重w^t的调整方式由下式表示：其中，w_max与w_min分别为惯性权重上限值和下限值，t_max为最大迭代次数，k为非线性控制因子；步骤105‑7，检验迭代是否结束：如果当前迭代次数达到了预先设定最大迭代次数，则停止迭代，优化结束，否则，转到步骤105‑3；还包括远程网络模块，远程网络模块创建一个网页地址并嵌入计算机(202)中，在互联网内的任意一台计算机上通过WEB网页实时监控程序的运行情况，实现对挖掘机的远程操控。