CN114137837B - 一种面向无人挖掘的自动装载控制方法 - Google Patents
一种面向无人挖掘的自动装载控制方法 Download PDFInfo
- Publication number
- CN114137837B CN114137837B CN202111393634.4A CN202111393634A CN114137837B CN 114137837 B CN114137837 B CN 114137837B CN 202111393634 A CN202111393634 A CN 202111393634A CN 114137837 B CN114137837 B CN 114137837B
- Authority
- CN
- China
- Prior art keywords
- control
- bucket
- data
- state
- loading
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000009412 basement excavation Methods 0.000 title claims abstract description 21
- 239000000463 material Substances 0.000 claims abstract description 26
- 230000008569 process Effects 0.000 claims abstract description 14
- 238000012545 processing Methods 0.000 claims abstract description 9
- 238000007599 discharging Methods 0.000 claims abstract description 8
- 238000005065 mining Methods 0.000 claims abstract description 6
- 230000006870 function Effects 0.000 claims description 49
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000006243 chemical reaction Methods 0.000 claims description 18
- 230000007246 mechanism Effects 0.000 claims description 15
- 238000004891 communication Methods 0.000 claims description 14
- 230000005540 biological transmission Effects 0.000 claims description 11
- 125000004122 cyclic group Chemical group 0.000 claims description 8
- 238000013500 data storage Methods 0.000 claims description 7
- 238000012546 transfer Methods 0.000 claims description 7
- 230000033228 biological regulation Effects 0.000 claims description 6
- 238000013461 design Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 239000003638 chemical reducing agent Substances 0.000 claims description 3
- 238000012790 confirmation Methods 0.000 claims description 3
- 230000003993 interaction Effects 0.000 claims description 3
- 238000004590 computer program Methods 0.000 claims description 2
- 238000002474 experimental method Methods 0.000 description 7
- 238000012360 testing method Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000012795 verification Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000008859 change Effects 0.000 description 2
- 240000007263 Pinus koraiensis Species 0.000 description 1
- 235000011615 Pinus koraiensis Nutrition 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000052 comparative effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000001629 suppression Effects 0.000 description 1
- 238000013024 troubleshooting Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
一种面向无人挖掘的自动装载控制方法，本发明属于工程机械无人化控制技术领域，本发明为了实现挖掘机械无人化作业中的自动装载功能，特别是解决大惯性、强干扰下的回转控制问题，以避免回转过程发生机械冲击、制动不平稳等现象，适应铲斗物料装载量、卸料位置、承机面坡度、物料力学特性等工况变化。本发明包括：数据接收与处理；铲斗对准装载目标：装载目标与挖掘机械的相对位姿关系，基于预先设定的装载策略计算装载位置，进而基于挖掘机械工作装置的几何模型求解期望关节角度，作为关节角度控制的参考输入；铲斗卸料；铲斗复位与状态更新。本发明能够实现铲斗对准、卸料、复位等作业流程的自动执行，从而完成无人化挖掘机械的自动装载任务，提升露天采矿等非理想工况下挖掘机械装载作业的安全性与作业效率。
Description
技术领域
本发明属于工程机械无人化控制技术领域，涉及一种面向挖掘机械自动装载任务的抗干扰控制技术。
背景技术
挖掘机械是指用铲斗挖掘高于或低于承机面的物料并装入运输车辆或卸至堆料场的土方机械，是矿山开采、剥离作业等工程领域中必不可少的重要装备。传统挖掘机械依赖操作员手动操控，存在作业效率低、安全风险高、设备损耗大等问题。近年来，卡特彼勒、小松、太原重工等国内外挖掘机械设计与制造厂商先后投入无人化技术研究，希望通过无人挖掘节约用人成本、提高采掘效率、降低设备损耗与挖掘作业的安全风险。
在无人化挖掘作业中，自动装载控制是一项关键且困难的问题。自动装载任务要求挖掘机械在无人操控的条件下控制铲斗自动对准运输车辆或其他卸料目标，进而将挖掘的物料精准地投放在目标位置。在实际作业工况下，铲斗物料质量、承机面坡度、外界回转阻力作用等诸多干扰与不确定性给挖掘机械的自动装载控制带来了巨大挑战。此外，现有挖掘机械依靠回转动作实现铲斗水平移动、从而进行装载作业，其回转控制***需带动包括铲斗、铲臂、驾驶室等部分在内的整个上车车身转动，具有显著的大惯性、大时滞特点，这进一步增加了自动装载控制的难度。
现有挖掘机械大多依赖于西门子等自动化设备厂商研制开发的电机驱动器、调速控制***实现自动或半自动控制，其采用的控制方法主要包括比例-积分-微分(PID)控制或模糊自适应控制两种。然而，PID控制对扰动的抑制能力较弱，在装载过程中由于铲斗位置、物料质量、承机面坡度等影响，回转控制***的参数会发生显著变化，使PID的控制效果大打折扣。针对***参数变化问题，模糊自适应控制通过根据模糊规则在线调整控制律实现扰动抑制。然而在实际工程应用中，其隶属度函数与模糊表的设计难度较大、参数整定难度较高，在调试过程中会严重磨损挖掘机械的机械结构、降低使用寿命。此外，该方法要求对控制***准确建模，难以应用于不同的挖掘机和挖掘场景；且长时间使用后，挖掘机械参数随损耗变化，造成该方法控制性能下降。因此，现有方法仅能实现有人操作下的半自动控制以及理想工况下的全自动控制，适用于建筑作业、道路平整等简单工况，难以应对露天采矿等环境恶劣、挖掘负载大、安全性要求高的应用场景。
发明内容
本发明的主要目的是实现挖掘机械无人化作业中的自动装载功能，特别是解决大惯性、强干扰下的回转控制问题，以避免回转过程发生机械冲击、制动不平稳等现象，适应铲斗物料装载量、卸料位置、承机面坡度、物料力学特性等工况变化。
为解决上述问题，本发明提出一种面向无人挖掘的自动装载控制方法，即针对自动装载任务设计了一套完整的自动化作业流程与抗干扰控制方法。利用本发明方法开发的自动装载控制***基于可编程逻辑控制器(PLC)来实现。
本发明所述的面向无人挖掘的自动装载控制方法实现自动装载作业流程的包括如下步骤：
步骤一：数据接收与处理
PLC控制器接收由北斗定位***提供的运输车辆的位姿信息与挖掘机械位姿信息，解算运输车辆与挖掘机械的相对位姿关系；接收挖掘机械各关节编码器角度、速度以及变频调速的电流、电压等数据，作为实时控制的反馈信息；接收挖掘机械各电机工作状态与操作人员的应急控制指令，并进行逻辑判断，保证无人挖掘的安全工作；
步骤二：铲斗对准装载目标
由步骤一解算得到的运输车辆与挖掘机械的相对位姿关系，基于预先设定的装载策略计算装载位置，进而基于挖掘机械工作装置的几何模型求解期望关节角度，作为关节角度控制的参考输入；基于步骤一接收的反馈信息，采用设计的扩张状态观测器估计回转控制***总扰动，并通过设计的自抗扰控制器补偿干扰，使工作装置各关节的实际角度趋近并稳定在期望角度，从而达到将铲斗对准装载目标的控制目标；
步骤三：铲斗卸料
当铲斗对准装载目标后，将铲斗中装载的物料卸载到运载车辆中；对于有开斗机构的正铲式挖掘机械，通过开斗机构打开铲斗卸料；同时，控制工作装置关节角度使铲斗姿态变化为预设的倾倒物料位姿并维持数秒；完成倾倒后，恢复铲斗位姿、关闭铲斗；
步骤四：铲斗复位与状态更新
卸料完成后，挖掘机械即完成一个采挖-装载的作业循环，此时需恢复挖掘机械的工作状态、使其处于等待下一个作业循环的指令；采取预先设定的复位状态、采用与步骤二相同的自抗扰控制器驱动挖掘机械各关节达到预设的初始角度，实现铲斗复位；复位后上报***运行状态、重置上一作业循环中的相关数据，进入等待状态；
通过时间中断、实时反馈状态判断自动判断每一步骤的结束，完成作业步骤的自动跳转，从而实现装载作业过程的自动运行。
进一步地，在步骤一中，采取循环通信方式读取电机信息，设计自由报文并完成数据互联；循环通信的PLC程序实现包括数据存储，数据转换，数据传输三部分，并将其整合在一个函数块中供主函数调用。
进一步地，在步骤一中，将循环通信的数据存储分为发送数据区、接收数据区、控制数据区和状态数据区，分类存储自动装载控制的各项数据；自动装载控制程序运行时，不同功能的程序块(除循环通信以外的)依照流程向指定区域读写数据，实现数据交换；数据转换部分包括控制或反馈数据的转换以及控制指令的转换；在下发控制数据或接收反馈数据时，需进行数据类型转换或单位转换解算步骤，解算电机输出轴转动角度时，将编码器值右移11位获得脉冲计数，再将脉冲数除以1024获得转动角度；对于控制指令数据中输入输出的启动、故障确认命令，向发送数据区控制字中写入输出控制字，从接收数据区状态字解读输入指令并存储运行状态与故障状态等信息，以供流程控制程序判断作业状态与保证作业安全；数据传输部分采用PLC提供的一致性数据读写指令实现，将发送数据区的控制数据传送到变频器等执行机构驱动控制单元中，同时将驱动控制单元中的反馈数据读取到接收数据区中。
进一步地，在步骤二中，挖掘机械通过电机经减速器驱动回转平台旋转，以电机输出力矩作为控制量，以回转角度作为输出量，将阻力等外部作用力作为***外部扰动，构建回转***模型(对应回转电机，推压电机、提升电机的对应模型与回转***模型类似)：
其中，u为控制量电机输出力矩，y为输出量回转角度，b表示相对增益，w表示外部扰动，g表示内部扰动；g与w均未知，b部分已知，已知部分记作b0；t表示时间；
定义f为包含了内扰与外扰的总扰动，则公式(1)可以改写为：
选取状态变量：则/>为包含了扰动的扩张状态，将公式(2)转换为连续的状态空间描述：
其中，C＝[1 0 0]；
对应的连续线性扩张状态观测器(LESO)为
z表示自动装载控制***的状态量；
重写观测器方程为：
公式(5)中的表示f的观测值，估值；
定义ω0为观测器的带宽，将反馈矩阵的极点设置在-ω0，即：
λ(s)＝|sI-(A-LC)|＝(s+ω0)3 (6)
s表示反馈矩阵极点对应的值；
得观测器的增益矩阵为：
控制器采用PD控制方法：
u0＝kp(r-z1)-kdz2 (7)
r表示电机目标角度；z1表示电机当前角度，z2表示相邻时刻电机转角偏差之差；
定义ωc为控制器的带宽，kp＝ωc 2,kd＝2ωc；
控制信号为：
闭环传递函数为一个二阶***：
忽略使用零阶保持法(ZOH)对公式(3)进行离散化；
公式(10)中k表示时刻；
其中：H＝C，h为采样周期，Φ、Γ中k表示泰勒展开中项的系数；
计算得：
构造离散扩张状态观测器方程：
公式(11)中k表示时刻；
其中ΦE＝[Φ-ΦLcH],ΓE＝[ΓΦLc],HE＝[I-LcH],JE＝[0Lc]；
配置离散特征方程极点在同一位置β处，计算扩张状态观测器增益矩阵Lc：
λ(z)＝|zI-(Φ-ΦLH)|＝(z-β)3 (12)
令β＝e-ω0h，计算得增益矩阵为：
将增益矩阵代入可得：
由此得到离散化的线性自抗扰控制器。
所述离散化的线性自抗扰控制器由扩张状态观测器和线性反馈控制率两部分组成，使用梯形图编程语言对两部分设计函数块进行实现；电机采用力矩控制模式，PLC向变频器发送力矩指令，变频器返回电机的编码器数值及实际转速；设计离散线性自抗扰控制器扩张状态观测器模块函数，扩张状态观测器的输入为控制输入u以及实际回转角度y，输出为角度观测值y1、速度观测值y2和总扰动观测值y3，A、B、C、D为离散状态观测器方程的对应矩阵；
离散情况下，计算下一周期状态z(k+1)是通过当前周期z(k)与当前周期输入计算所得，状态变量z(k)在数据块中存储，输入u为上一周期线性反馈控制函数的输出，y为当前时刻的位置；在程序中先对状态变量进行更新，然后计算输出，并将状态变量存储；
设计线性反馈控制率函数块，函数输入为目标位置r、位置观测值y、速度观测值v、总扰动观测值y3、最大输出力矩f，函数输出为控制力矩u；
线性控制率采取PD控制方式，并将传递函数的极点配置在-ωc处，即kp＝ωc 2,kd＝2ωc,u0＝kp(r-y)-kdv,u＝(u0-f)/b0，并对输出值进行限幅。
离散化的线性自抗扰控制器还具循环中断的功能，中断时间设为h，每经过h时间，中断中的程序运行一次，对***状态进行采样并生成控制信号；在中断程序中获取目标位置信号，获取电机的实际角度值，调用扩张状态观测器函数块生成状态观测值，调用控制率函数块生成控制信号，并将控制信号写入循环通讯数据块的对应位置，实现对回转角度的自抗扰控制。
进一步地，在步骤三中，铲斗卸料电机采用力矩控制模式、铲斗位姿微调采用角度增量控制，因此铲斗卸料部分可以看作一个开关量，在不同工作状态下采取不同的力矩主令进行控制。
进一步地，在步骤四中，通过基于PLC开发的人机交互界面监视***装载作业流程、***工作状态以及控制性能。
一种面向无人挖掘的自动装载控制***，所述***包括：
数据接收与处理模块：用于接收由北斗定位***提供的运输车辆的位姿信息与挖掘机械位姿信息，解算运输车辆与挖掘机械的相对位姿关系；接收挖掘机械各关节编码器角度、速度以及变频调速的电流、电压等数据，作为实时控制的反馈信息；接收挖掘机械各电机工作状态与操作人员的应急控制指令，并进行逻辑判断，保证无人挖掘的安全工作；
铲斗对准装载目标模块：读取由数据接收与处理模块解算得到的运输车辆与挖掘机械的相对位姿关系，基于预先设定的装载策略计算装载位置，进而基于挖掘机械工作装置的几何模型求解期望关节角度，作为关节角度控制的参考输入；基于接收的反馈信息，采用设计的扩张状态观测器估计回转控制***总扰动，并通过设计的自抗扰控制器补偿干扰，使工作装置各关节的实际角度趋近并稳定在期望角度，从而达到将铲斗对准装载目标的控制目标；
铲斗卸料模块：当铲斗对准装载目标后，将铲斗中装载的物料卸载到运载车辆中；对于有开斗机构的正铲式挖掘机械，通过开斗机构打开铲斗卸料；同时，控制工作装置关节角度使铲斗姿态变化为预设的倾倒物料位姿并维持数秒；完成倾倒后，恢复铲斗位姿、关闭铲斗；
铲斗复位与状态更新模块：卸料完成后，挖掘机械即完成一个采挖-装载的作业循环，此时需恢复挖掘机械的工作状态、使其处于等待下一个作业循环的指令；采取预先设定的复位状态、采用与铲斗对准装载目标模块相同的自抗扰控制器驱动挖掘机械各关节达到预设的初始角度，实现铲斗复位；复位后上报***运行状态、重置上一作业循环中的相关数据，进入等待状态；通过时间中断、实时反馈状态判断自动判断每一步骤的结束，完成作业步骤的自动跳转，从而实现装载作业过程的自动运行。
一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述一种面向无人挖掘的自动装载控制方法的步骤。
本发明至少具有以下有益技术效果：
本发明能够实现铲斗对准、卸料、复位等作业流程的自动执行，从而完成无人化挖掘机械的自动装载任务，提升露天采矿等非理想工况下挖掘机械装载作业的安全性与作业效率。
相比现有技术方案，本发明具有以下优势：(1)抗干扰能力强，将***中未建模部分、模型不确定性及外部扰动输入的综合作用看作***总扰动，通过引入扩张状态观测器对总扰动进行观测和补偿，相比模糊自适应控制方法具有更强的抗干扰能力；(2)参数整定难度小，基于自抗扰控制原理进行了控制器结构简化，将参数数量减少到3个，且各个参数物理意义明确，降低了参数的整定难度；(3)可有效抑制扰动、减少参数整定所需调试时间，提高挖掘机械的作业效率及使用寿命，经济优势明显。
附图说明
图1是自动装载控制流程图；
图2是自动装载控制***通信架构；
图3是数据存储结构；
图4是自抗扰回转控制***框图；
图5是ESO函数接口；
图6是卸料控制程序；
图7是自动装载流程控制程序；
图8是自动装载程序人机交互界面；
图9是抗扰性能对比验证结果；
图10是自抗扰控制验证实验台；
图11是自动抗扰控制实验曲线；
图12是无人挖掘机械自动装载实验；
图13是自动装载实验的实际状态曲线。
具体实施方式
本发明提出的自动装载控制方案按图1所示作业流程执行，主要包括数据接收与处理、铲斗对准装载目标、铲斗卸料、铲斗复位与状态更新四个步骤。执行上述步骤的自动装载控制程序基于PLC实现，通过时间中断、反馈状态判断等方法自动判断流程中每一步骤的结束、完成作业步骤的自动跳转，从而实现装载作业过程的自动运行。自动装载控制的通信架构如图2所示，运行于PLC上的自动装载控制程序由工业总线读取所需数据，其中的抗干扰控制程序段基于状态反馈估计并补偿***总干扰、计算得到抗干扰的控制量，进而借由工业总线下发至电机驱动器、变频器等下层执行器，从而实现抗干扰的自动装载控制。方案中各步骤的具体实施方式为：
步骤一：数据接收与处理
执行自动装载任务时，首先需接收由卫星定位或视觉识别定位等定位***提供的运输车辆或其他装载目标的位姿信息与挖掘机械位姿信息，进而解算运输车辆与挖掘机械的相对位姿关系、解算挖掘装置各关节的期望状态。同时，还需接收挖掘机械各关节编码器角度、速度以及变频调速的电流、电压等数据，作为闭环控制的反馈信息。此外，还需接收挖掘机械各电机等设备状态与操作人员的应急控制指令并进行逻辑判断，以保证挖掘机械安全、正常工作。
本方案采取循环通信方式读取电机信息，设计自由报文并完成数据互联。循环通信的PLC程序实现包括数据存储，数据转换，数据传输三部分，并将其整合在一个函数块中供主函数调用。
为方便数据发送与接收，将数据块分为发送数据区、接收数据区、控制数据区和状态数据区，分类存储自装装载控制***的各项数据。程序运行时，不同功能的程序块依照流程向指定区域读写数据，实现数据交换。数据存储结构如图3所示。
数据转换部分包括控制或反馈数据的转换以及控制指令的转换。在下发控制数据或接收反馈数据时，需进行数据类型转换或单位转换等解算步骤。例如，1024位增量式编码器的反馈数据为32位整型数据，其中后11位为细分辨率数据。解算电机输出轴转动角度时，将编码器值右移11位获得脉冲计数，再将脉冲数除以1024获得转动角度。除了上述数值型的状态，还需处理布尔型的指令数据。对于控制指令数据中输入输出的启动、故障确认等命令，向发送数据区控制字中写入输出控制字，从接收数据区状态字解读输入指令并存储运行状态与故障状态等信息，以供流程控制程序判断作业状态与保证作业安全。
数据传输部分采用PLC提供的一致性数据读写指令实现，将发送数据区的控制数据传送到变频器等执行机构驱动控制单元中，同时将驱动控制单元中的反馈数据读取到接收数据区中。传统的I/O读写命令一次性只能写入最多4个连续字节数据，采用一致性数据读写的方式进行数据传输、避免了数据读写的不同步，保证获取数据的时间一致性。
步骤二：铲斗对准装载目标
基于步骤一解算得到装载目标与挖掘机械的相对位姿关系，由预先设定的装载策略函数计算装载位置，进而根据逆运动学模型函数求解装载位置对应的期望关节角度与期望执行机构状态值，如电机输出轴角度、液压杆伸长量等。期望执行机构状态值即为执行机构的控制目标，作为闭环***的参考输入。基于实时反馈的状态信息，通过抗干扰控制程序补偿干扰并控制执行机构状态收敛并稳定在期望状态，从而达到将铲斗对准装载目标的控制目标。针对回转控制中的大惯性、强干扰问题，采用离散线性自抗扰回转控制方法。
挖掘机械一般通过电机经减速器驱动回转平台旋转。以电机输出力矩作为控制量，以回转角度作为输出量，将阻力等外部作用力作为***外部扰动，构建回转***模型：
其中，u为控制量电机输出力矩，y为输出量回转角度，b表示相对增益，w表示外部扰动，g表示内部扰动。g与w均未知，b部分已知，已知部分记作b0。定义f为包含了内扰与外扰的总扰动，则公式(1)可以改写为：
选取状态变量：则/>为包含了扰动的扩张状态，将公式(2)转换为连续的状态空间描述：
其中，C＝[1 0 0]。
对应的连续线性扩张状态观测器(LESO)为
重写观测器方程为：
定义ω0为观测器的带宽，将反馈矩阵的极点设置在-ω0，即：
λ(s)＝|sI-(A-LC)|＝(s+ω0)3 (6)
得观测器的增益矩阵为：
控制器采用PD控制方法：
u0＝kp(r-z1)-kdz2 (7)
定义ωc为控制器的带宽，kp＝ωc 2,kd＝2ωc。
控制信号为：
闭环传递函数为一个二阶***：
忽略使用零阶保持法(ZOH)对公式(3)进行离散化。
其中：H＝C，h为采样周期。
计算得：
构造离散扩张状态观测器方程：
其中ΦE＝[Φ-ΦLcH],ΓE＝[ΓΦLc],HE＝[I-LcH],JE＝[0Lc]。
配置离散特征方程极点在同一位置β处，计算扩张状态观测器增益矩阵Lc：
λ(z)＝|zI-(Φ-ΦLH)|＝(z-β)3 (12)
令β＝e-ω0h，计算得增益矩阵为：
将增益矩阵代入可得：
由此得到离散化的线性自抗扰控制器，基于该控制器的回转控制***结构框图如图5所示。该控制器由扩张状态观测器和线性反馈控制率两部分组成，在博图软件中使用梯形图编程语言对两部分设计函数块进行实现。电机采用力矩控制模式，PLC向变频器发送力矩指令，变频器返回电机的编码器数值及实际转速。设计离散线性自抗扰控制器扩张状态观测器模块函数，扩张状态观测器的输入为控制输入u以及实际回转角度y，输出为角度观测值y1、速度观测值y2和总扰动观测值y3，A、B、C、D为离散状态观测器方程的对应矩阵。
离散情况下，计算下一周期状态z(k+1)是通过当前周期z(k)与当前周期输入计算所得，状态变量z(k)在数据块中存储，输入u为上一周期线性反馈控制函数的输出，y为当前时刻的位置。在程序中先对状态变量进行更新，然后计算输出，并将状态变量存储起来，如图5所示。
设计线性反馈控制率函数块，函数输入为目标位置r、位置观测值y、速度观测值v、总扰动观测值y3、最大输出力矩f，函数输出为控制力矩u。
线性控制率采取PD控制方式，并将传递函数的极点配置在-ωc处，即kp＝ωc 2,kd＝2ωc,u0＝kp(r-y)-kdv,u＝(u0-f)/b0，并对输出值进行限幅。
在程序中添加一个循环中断程序块，中断时间设为h，每经过h时间，中断中的程序运行一次，对***状态进行采样并生成控制信号。在中断程序中获取目标位置信号，获取电机的实际角度值，调用扩张状态观测器函数块生成状态观测值，调用控制率函数块生成控制信号，并将控制信号写入循环通讯数据块的对应位置，实现对回转角度的自抗扰控制。
步骤三：铲斗卸料
当铲斗对准装载目标后，将铲斗中装载的物料卸载到运载车辆或其他装载目标中。对于有开斗机构的正铲式挖掘机械，通过开斗机构打开铲斗卸料。同时，控制工作装置关节角度使铲斗姿态微调一定角度并维持数秒。完成倾倒后，恢复铲斗姿态、关闭铲斗。该流程由卸料控制程序模块执行。铲斗卸料电机采用力矩控制模式、铲斗位姿微调采用角度增量控制，因此铲斗卸料部分可以看作一个开关量，在不同工作状态下采取不同的力矩主令进行控制即可，如图6所示。
步骤四：铲斗复位与状态更新
卸料完成后，挖掘机械即完成一个采挖-装载的作业循环，此时需恢复挖掘机械的工作状态、使其处于等待下一个作业循环的指令。本方案采取预先设定的复位状态，步骤四采用与步骤二相同的离散线性自抗扰控制器驱动挖掘机械各关节达到预设的初始角度，实现铲斗复位。复位后上报***运行状态、重置上一作业循环中的相关数据，进入等待状态。
上述步骤分别编写为程序块，进而通过主函数的作业状态判断依次执行，实现完整的自动装载功能，如图7所示。此外，为便于开发测试、***维护与故障检修，本方案还基于PLC开发了人机交互界面，可监视***装载作业流程、***工作状态以及控制性能，如图8所示。
为验证所提出抗干扰控制方法的有效性、测试自控装载***软硬件功能、整定***参数，本方案采用下述方法进行测试验证：
1.控制***数值仿真：对抗干扰控制器与被控对象数学模型组成的闭环***进行数值仿真，增加***扰动输入并观测***响应的波动情况，从而初步整定控制器参数、验证控制效果。对抗干扰控制与PID控制方法的仿真对比结果如图9所示，所提出的抗干扰控制方法相比PID方法更好地控制了***的扰动、大大降低了回转角度偏差。
2.控制***半实物仿真：搭建如图10所示的实验台，一台电机用于模拟回转电机，另一条电机以力矩输出方式模拟***的干扰力矩。将开发的自动装载程序运行于PLC上，并以实验台的模拟回转电机作为被控对象测试自动装载过程。测试结果如图11所示，本发明提出的自动装载控制方案有效抑制了力矩扰动、使得模拟回转电机平稳快速运行。
3.控制***实物验证：采用如图12所示的无人挖掘机械作为被控对象验证所开发的自动装载***控制效果，令无人挖掘机械挖起物料后运行自动装载***将物料自动卸载于目标卸料位置。实验中无人挖掘机械可以自动将物料卸载于目标区域，卸料过程平稳、快速，且可适应承机面坡度不同、摩擦阻力等外界干扰，证明了本发明的有效性。实际控制曲线如图13所示。
Claims (9)
1.一种面向无人挖掘的自动装载控制方法，其特征在于，所述方法的实现过程为：
步骤一：数据接收与处理
接收由北斗定位***提供的运输车辆的位姿信息与挖掘机械位姿信息，解算运输车辆与挖掘机械的相对位姿关系；接收挖掘机械各关节编码器角度、速度以及变频调速的电流、电压等数据，作为实时控制的反馈信息；接收挖掘机械各电机工作状态与操作人员的应急控制指令，并进行逻辑判断，保证无人挖掘的安全工作；
步骤二：铲斗对准装载目标
由步骤一解算得到的运输车辆与挖掘机械的相对位姿关系，基于预先设定的装载策略计算装载位置，进而基于挖掘机械工作装置的几何模型求解期望关节角度，作为关节角度控制的参考输入；基于步骤一接收的反馈信息，采用设计的扩张状态观测器估计回转控制***总扰动，并通过设计的自抗扰控制器补偿干扰，使工作装置各关节的实际角度趋近并稳定在期望角度，从而达到将铲斗对准装载目标的控制目标；
步骤三：铲斗卸料
当铲斗对准装载目标后，将铲斗中装载的物料卸载到运载车辆中；对于有开斗机构的正铲式挖掘机械，通过开斗机构打开铲斗卸料；同时，控制工作装置关节角度使铲斗姿态变化为预设的倾倒物料位姿并维持数秒；完成倾倒后，恢复铲斗位姿、关闭铲斗；
步骤四：铲斗复位与状态更新
卸料完成后，挖掘机械即完成一个采挖-装载的作业循环，此时需恢复挖掘机械的工作状态、使其处于等待下一个作业循环的指令；采取预先设定的复位状态、采用与步骤二相同的自抗扰控制器驱动挖掘机械各关节达到预设的初始角度，实现铲斗复位；复位后上报***运行状态、重置上一作业循环中的相关数据，进入等待状态；
通过时间中断、实时反馈状态判断自动判断每一步骤的结束，完成作业步骤的自动跳转，从而实现装载作业过程的自动运行；
在步骤二中，挖掘机械通过电机经减速器驱动回转平台旋转，以电机输出力矩作为控制量，以回转角度作为输出量，将阻力等外部作用力作为***外部扰动，构建回转***模型：
其中，u为控制量电机输出力矩，y为输出量回转角度，b表示相对增益，w表示外部扰动，g表示内部扰动；g与w均未知，b部分已知，已知部分记作b0；t表示时间；
定义f为包含了内扰与外扰的总扰动，则公式(1)可以改写为：
选取状态变量：则/>为包含了扰动的扩张状态，将公式(2)转换为连续的状态空间描述：
其中，C＝[1 0 0]；
对应的连续线性扩张状态观测器(LESO)为
z表示自动装载控制***的状态量；
重写观测器方程为：
公式(5)中的表示f的观测值，估值；
定义ω0为观测器的带宽，将反馈矩阵的极点设置在-ω0，即：
λ(s)＝|sI-(A-LC)|＝(s+ω0)3 (6)
s表示反馈矩阵极点对应的值；
得观测器的增益矩阵为：
控制器采用PD控制方法：
u0＝kp(r-z1)-kdz2 (7)
r表示电机目标角度；z1表示电机当前角度，z2表示相邻时刻电机转角偏差之差；
定义ωc为控制器的带宽，kp＝ωc 2,kd＝2ωc；
控制信号为：
闭环传递函数为一个二阶***：
忽略使用零阶保持法(ZOH)对公式(3)进行离散化；
公式(10)中k表示时刻；
其中：h为采样周期，Φ、Γ中k表示泰勒展开中项的系数；
计算得：
构造离散扩张状态观测器方程：
公式(11)中k表示时刻；
其中ΦE＝[Φ-ΦLcH],ΓE＝[ΓΦLc],HE＝[I-LcH],JE＝[0Lc]；
配置离散特征方程极点在同一位置β处，计算扩张状态观测器增益矩阵Lc；
λ(z)＝|zI-(Φ-ΦLH)|＝(z-β)3 (12)
令计算得增益矩阵为：
将增益矩阵代入可得：
由此得到离散化的线性自抗扰控制器。
2.根据权利要求1所述的一种面向无人挖掘的自动装载控制方法，其特征在于，在步骤一中，采取循环通信方式读取电机信息，设计自由报文并完成数据互联；循环通信的PLC程序实现包括数据存储，数据转换，数据传输三部分，并将其整合在一个函数块中供主函数调用。
3.根据权利要求1或2所述的一种面向无人挖掘的自动装载控制方法，其特征在于，在步骤一中，将循环通信的数据存储分为发送数据区、接收数据区、控制数据区和状态数据区，分类存储自动装载控制的各项数据；自动装载控制程序运行时，不同功能的程序块依照流程向指定区域读写数据，实现数据交换；
数据转换部分包括控制或反馈数据的转换以及控制指令的转换；在下发控制数据或接收反馈数据时，需进行数据类型转换或单位转换解算步骤，解算电机输出轴转动角度时，将编码器值右移11位获得脉冲计数，再将脉冲数除以1024获得转动角度；对于控制指令数据中输入输出的启动、故障确认命令，向发送数据区控制字中写入输出控制字，从接收数据区状态字解读输入指令并存储运行状态与故障状态等信息，以供流程控制程序判断作业状态与保证作业安全；
数据传输部分采用PLC提供的一致性数据读写指令实现，将发送数据区的控制数据传送到变频器等执行机构驱动控制单元中，同时将驱动控制单元中的反馈数据读取到接收数据区中。
4.根据权利要求3所述的一种面向无人挖掘的自动装载控制方法，其特征在于，所述离散化的线性自抗扰控制器由扩张状态观测器和线性反馈控制率两部分组成，使用梯形图编程语言对两部分设计函数块进行实现；电机采用力矩控制模式，PLC向变频器发送力矩指令，变频器返回电机的编码器数值及实际转速；设计离散线性自抗扰控制器扩张状态观测器模块函数，扩张状态观测器的输入为控制输入u以及实际回转角度y，输出为角度观测值y1、速度观测值y2和总扰动观测值y3，A、B、C、D为离散状态观测器方程的对应矩阵；
离散情况下，计算下一周期状态z(k+1)是通过当前周期z(k)与当前周期输入计算所得，状态变量z(k)在数据块中存储，输入u为上一周期线性反馈控制函数的输出，y为当前时刻的位置；在程序中先对状态变量进行更新，然后计算输出，并将状态变量存储；
设计线性反馈控制率函数块，函数输入为目标位置r、位置观测值y、速度观测值v、总扰动观测值y3、最大输出力矩f，函数输出为控制力矩u；
线性控制率采取PD控制方式，并将传递函数的极点配置在-ωc处，即kp＝ωc 2,kd＝2ωc,u0＝kp(r-y)-kdv,u＝(u0-f)/b0，并对输出值进行限幅。
5.根据权利要求4所述的一种面向无人挖掘的自动装载控制方法，其特征在于，离散化的线性自抗扰控制器还具循环中断的功能，中断时间设为h，每经过h时间，中断中的程序运行一次，对***状态进行采样并生成控制信号；在中断程序中获取目标位置信号，获取电机的实际角度值，调用扩张状态观测器函数块生成状态观测值，调用控制率函数块生成控制信号，并将控制信号写入循环通讯数据块的对应位置，实现对回转角度的自抗扰控制。
6.根据权利要求5所述的一种面向无人挖掘的自动装载控制方法，其特征在于，在步骤三中，铲斗卸料电机采用力矩控制模式、铲斗位姿微调采用角度增量控制，因此铲斗卸料部分可以看作一个开关量，在不同工作状态下采取不同的力矩主令进行控制。
7.根据权利要求1、2、4、5、6任一项所述的一种面向无人挖掘的自动装载控制方法，其特征在于，在步骤四中，通过基于PLC开发的人机交互界面监视***装载作业流程、***工作状态以及控制性能。
8.一种面向无人挖掘的自动装载控制***，其特征在于，所述***包括：
数据接收与处理模块：用于接收由北斗定位***提供的运输车辆的位姿信息与挖掘机械位姿信息，解算运输车辆与挖掘机械的相对位姿关系；接收挖掘机械各关节编码器角度、速度以及变频调速的电流、电压等数据，作为实时控制的反馈信息；接收挖掘机械各电机工作状态与操作人员的应急控制指令，并进行逻辑判断，保证无人挖掘的安全工作；
铲斗对准装载目标模块：读取由数据接收与处理模块解算得到的运输车辆与挖掘机械的相对位姿关系，基于预先设定的装载策略计算装载位置，进而基于挖掘机械工作装置的几何模型求解期望关节角度，作为关节角度控制的参考输入；基于接收的反馈信息，采用设计的扩张状态观测器估计回转控制***总扰动，并通过设计的自抗扰控制器补偿干扰，使工作装置各关节的实际角度趋近并稳定在期望角度，从而达到将铲斗对准装载目标的控制目标；
铲斗卸料模块：当铲斗对准装载目标后，将铲斗中装载的物料卸载到运载车辆中；对于有开斗机构的正铲式挖掘机械，通过开斗机构打开铲斗卸料；同时，控制工作装置关节角度使铲斗姿态变化为预设的倾倒物料位姿并维持数秒；完成倾倒后，恢复铲斗位姿、关闭铲斗；
铲斗复位与状态更新模块：卸料完成后，挖掘机械即完成一个采挖-装载的作业循环，此时需恢复挖掘机械的工作状态、使其处于等待下一个作业循环的指令；采取预先设定的复位状态、采用与铲斗对准装载目标模块相同的自抗扰控制器驱动挖掘机械各关节达到预设的初始角度，实现铲斗复位；复位后上报***运行状态、重置上一作业循环中的相关数据，进入等待状态；通过时间中断、实时反馈状态判断自动判断每一步骤的结束，完成作业步骤的自动跳转，从而实现装载作业过程的自动运行；
所述铲斗对准装载目标模块还包括：
挖掘机械通过电机经减速器驱动回转平台旋转，以电机输出力矩作为控制量，以回转角度作为输出量，将阻力等外部作用力作为***外部扰动，构建回转***模型：
其中，u为控制量电机输出力矩，y为输出量回转角度，b表示相对增益，w表示外部扰动，g表示内部扰动；g与w均未知，b部分已知，已知部分记作b0；t表示时间；
定义f为包含了内扰与外扰的总扰动，则公式(1)可以改写为：
选取状态变量：则/>为包含了扰动的扩张状态，将公式(2)转换为连续的状态空间描述：
其中，
对应的连续线性扩张状态观测器(LESO)为
z表示自动装载控制***的状态量；
重写观测器方程为：
公式(5)中的表示f的观测值，估值；
定义ω0为观测器的带宽，将反馈矩阵的极点设置在-ω0，即：
λ(s)＝|sI-(A-LC)|＝(s+ω0)3 (6)
s表示反馈矩阵极点对应的值；
得观测器的增益矩阵为：
控制器采用PD控制方法：
u0＝kp(r-z1)-kdz2 (7)
r表示电机目标角度；z1表示电机当前角度，z2表示相邻时刻电机转角偏差之差；
定义ωc为控制器的带宽，kp＝ωc 2,kd＝2ωc；
控制信号为：
闭环传递函数为一个二阶***：
忽略使用零阶保持法(ZOH)对公式(3)进行离散化；
公式(10)中k表示时刻；
其中：h为采样周期，Φ、Γ中k表示泰勒展开中项的系数；
计算得：
构造离散扩张状态观测器方程：
公式(11)中k表示时刻；
其中ΦE＝[Φ-ΦLcH],ΓE＝[Γ ΦLc],HE＝[I-LcH],JE＝[0 Lc]；
配置离散特征方程极点在同一位置β处，计算扩张状态观测器增益矩阵Lc；
λ(z)＝|zI-(Φ-ΦLH)|＝(z-β)3 (12)
令计算得增益矩阵为：
将增益矩阵代入可得：
由此得到离散化的线性自抗扰控制器。
9.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-7中任一项所述的一种面向无人挖掘的自动装载控制方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111393634.4A CN114137837B (zh) | 2021-11-23 | 2021-11-23 | 一种面向无人挖掘的自动装载控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111393634.4A CN114137837B (zh) | 2021-11-23 | 2021-11-23 | 一种面向无人挖掘的自动装载控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114137837A CN114137837A (zh) | 2022-03-04 |
| CN114137837B true CN114137837B (zh) | 2024-06-04 |
Family
ID=80390882
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111393634.4A Active CN114137837B (zh) | 2021-11-23 | 2021-11-23 | 一种面向无人挖掘的自动装载控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114137837B (zh) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117806260B (zh) * | 2024-02-29 | 2024-05-14 | 哈尔滨恒达交通设备技术开发有限公司 | 一种基于大数据的智能物料控制监控方法和*** |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102193501A (zh) * | 2011-03-05 | 2011-09-21 | 中国船舶重工集团公司第七一二研究所 | 一种大型矿用挖掘机优化回转特性的变结构数字控制器 |
| CN102354104A (zh) * | 2005-09-19 | 2012-02-15 | 克利夫兰州立大学 | 控制器、观测器及其应用 |
| CN104270041A (zh) * | 2014-09-26 | 2015-01-07 | 广州航海学院 | 一种基于自抗扰控制技术的绞刀电机同步调速控制*** |
| CN113383130A (zh) * | 2019-02-01 | 2021-09-10 | 株式会社小松制作所 | 工程机械的控制***、工程机械及工程机械的控制方法 |
| CN113489403A (zh) * | 2021-07-06 | 2021-10-08 | 中铁工程装备集团有限公司 | 一种盾构机驱动***的多电机均载协同控制装置及方法 |
| CN113605483A (zh) * | 2021-07-19 | 2021-11-05 | 江苏徐工工程机械研究院有限公司 | 一种挖掘机自动作业控制方法和装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8180464B2 (en) * | 2002-04-18 | 2012-05-15 | Cleveland State University | Extended active disturbance rejection controller |
-
2021
- 2021-11-23 CN CN202111393634.4A patent/CN114137837B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102354104A (zh) * | 2005-09-19 | 2012-02-15 | 克利夫兰州立大学 | 控制器、观测器及其应用 |
| CN102193501A (zh) * | 2011-03-05 | 2011-09-21 | 中国船舶重工集团公司第七一二研究所 | 一种大型矿用挖掘机优化回转特性的变结构数字控制器 |
| CN104270041A (zh) * | 2014-09-26 | 2015-01-07 | 广州航海学院 | 一种基于自抗扰控制技术的绞刀电机同步调速控制*** |
| CN113383130A (zh) * | 2019-02-01 | 2021-09-10 | 株式会社小松制作所 | 工程机械的控制***、工程机械及工程机械的控制方法 |
| CN113489403A (zh) * | 2021-07-06 | 2021-10-08 | 中铁工程装备集团有限公司 | 一种盾构机驱动***的多电机均载协同控制装置及方法 |
| CN113605483A (zh) * | 2021-07-19 | 2021-11-05 | 江苏徐工工程机械研究院有限公司 | 一种挖掘机自动作业控制方法和装置 |
Non-Patent Citations (2)
| Title |
|---|
| PLC在自动装载机控制***中的应用;刘燕平;矿山机械;20080610;第36卷(第11期);39-41 * |
| 基于PLC的自动装载控制***研究;刘学申;万方学位论文数据库;20211202;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114137837A (zh) | 2022-03-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Ha et al. | Robotic excavation in construction automation | |
| US5953977A (en) | Simulation modeling of non-linear hydraulic actuator response | |
| CN114137837B (zh) | 一种面向无人挖掘的自动装载控制方法 | |
| Lee et al. | Precision motion control of robotized industrial hydraulic excavators via data-driven model inversion | |
| Dao et al. | High accuracy contouring control of an excavator for surface flattening tasks based on extended state observer and task coordinate frame approach | |
| WO2022069467A1 (de) | Verfahren zum bestimmen des gewichts einer beladung einer mobilen arbeitsmaschine, lernverfahren für ein datenbasiertes modell und mobile arbeitsmaschine | |
| Gu et al. | Proportional‐Integral‐Plus control of an intelligent excavator | |
| Maeda | Learning and reacting with inaccurate prediction: applications to autonomous excavation | |
| Kang et al. | Path tracking for a hydraulic excavator utilizing proportional-derivative and linear quadratic control | |
| Gu et al. | Linear and nonlinear control of a robotic excavator | |
| Gu et al. | Improved control of intelligent excavator using proportional-integral-plus gain scheduling | |
| Kadu et al. | Design, analysis and motion control of hydraulic excavator using discrete sliding mode control with inertial delay | |
| DE102019205297A1 (de) | Hydrostatisches Arbeitsgerät und Verfahren zu dessen Steuerung | |
| Danko | Loading excavator analysis for trajectory control improvement | |
| Ishihara et al. | Automated Loading Operation for Mass-Production Hydraulic Excavators by Nonlinear Model Predictive Control | |
| EP4130394A1 (de) | Verfahren zur überwachung und/oder durchführung einer bewegung eines arbeitsgeräts sowie arbeitsgerät und computerprogrammprodukt | |
| Jun et al. | Workspace analysis and motion control strategy of robotic mine anchor drilling truck manipulator based on the WOA-FOPID algorithm | |
| Roozbahani et al. | Coordinate-based control for a materials handling equipment utilizing real-time simulation | |
| Maeda et al. | Improving operational space control of heavy manipulators via open-loop compensation | |
| Yung et al. | Automation of front end loaders: Self leveling task | |
| Zweiri et al. | Model-based automation for heavy duty mobile excavator | |
| Reginald et al. | Tracking control of force, position, and contour for an excavator with co-simulation | |
| Ha et al. | A control architecture for robotic excavation in construction | |
| Liu et al. | Adaptive regulation of rigid-link electrically driven robots with uncertain kinematics | |
| CN115387426B (zh) | 作业机械的控制方法、装置、设备及作业机械 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |