¹盐城工学院机械工程学院优集学院，江苏 盐城

²徐州工程学院机电工程学院，江苏 徐州

收稿日期：2023年10月9日；录用日期：2023年11月20日；发布日期：2023年11月27日

摘要

为提升液压挖掘机在渠道维护中执行机构位置的控制性能，减少执行机构工作过程中产生的冲击和振动，设计了一种模糊算法来优化执行机构位置PID控制器的参数，使用五次多项式轨迹规划减少作业过程中冲击和振动。首先建立挖掘机运动学模型，确定位姿关系；其次对渠道维护作业中挖掘工况进行轨迹设计，得出液压缸位移与铲斗，动臂，斗杆关节角数学关系；然后根据挖掘机器人系统模型，以动臂液压缸为被控对象，对PID控制和模糊PID控制进行仿真。通过仿真分析基于模糊PID控制与PID控制相比，模糊PID控制的稳态误差和响应时间减小，其中响应时间下降了36%，验证了模糊PID控制算法的先进性。

关键词

挖掘机器人，轨迹控制，五次多项式，模糊算法

Research on Trajectory Planning and Control for Excavation Robot Channel Repairing

Wenjun Miao¹, Xiliang Ma^1,2*, Chengqiang Liu², Xuezhi Du¹, Bin Ma¹

¹School of Mechanical Engineering UGS College, Yancheng Institute of Technology, Yancheng Jiangsu

²School of Mechanical & Electrical Engineering, Xuzhou University of Technology, Xuzhou Jiangsu

Received: Oct. 9^th, 2023; accepted: Nov. 20^th, 2023; published: Nov. 27^th, 2023

ABSTRACT

In order to improve the control performance of the actuator position in the maintenance of the hydraulic excavator and reduce the impact and vibration of the actuator, a fuzzy algorithm was designed to optimize the parameters of the actuator position PID controller and reduce the impact and vibration during the operation by using quintic polynomial trajectory planning. Firstly, the kinematics model of excavator is established to determine the position and pose relationship. Secondly, the trajectory design of excavation conditions in channel maintenance work is carried out, and the mathematical relationship between hydraulic cylinder displacement and bucket, boom and bucket rod joint Angle is obtained. Then, according to the system model of the mining robot, with the hydraulic cylinder of the boom as the controlled object, the PID control and fuzzy adaptive control are simulated. Through simulation analysis, compared with PID control, the steady-state error and response time of fuzzy adaptive control are reduced, and the response time is reduced by 36%, which verifies the advanced nature of fuzzy PID control algorithm.

Keywords:Mining Robot, Trajectory Control, Quintic Polynomial, Fuzzy Algorithm

Copyright © 2023 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

随着现代工程建设的不断发展，挖掘机的作用日益重要。在进行渠道维护时，人工操作时间长，劳动强度大，施工周期长，由于技术水平和经验的限制，导致渠道的形状参数难以保证一致性和准确性。因此需要对挖掘机的运动轨迹进行规划和执行机构进行控制，实现高效作业，提高安全保障。要实现自主作业，轨迹生成需要各执行机构精确控制 [1] 。其次挖掘轨迹提前设计，并根据设计结果对其进行规划和控制是实现其自动化、智能化运行的关键 [2] 。

众多学者进行了挖掘机控制系统动态响应分析。翁文文在研究中将遗传算法中的选择和交叉算子融合到标准PSO算法中，从而提高了PSO算法的全局搜索能力，解决了PSO算法陷入局部最优问题，并显著提升了系统响应特性。经过对比IPSO算法、SPSO算法和PM算法发现，IPSO算法在系统响应速度和动态性能方面表现较好，并能够实现更高的控制精度 [3] 。刘沛采用了蚁群算法 [4] 优化PID位置伺服系统，利用蚁群算法的优势在复杂环境下搜索最优解，通过优化PID参数使得伺服系统更好地适应外部干扰，提高控制性能和稳定性，通过实验分析得到期望位移与实际位移近似相等 [4] 。对于挖掘机工装轨迹控制精度不高的问题，师平等通过优化RBF神经网络的结构和参数设置，提高系统的控制性能和准确度。研究结果表明，该方法相较于PID控制，在挖掘机工装轨迹控制方面具有更好的精度 [5] 。李海虹等将挖掘过程划分为多个小段，每个小段内使用可变阶多项式来表示轨迹。根据要求的加速度约束条件，计算出每个小段内所允许的最高加速度值。利用二次规划方法，根据这些约束条件反求出每个小段内的最高阶次数来降低机械振动和冲击力，提高操作的平稳性，延长设备寿命 [6] 。丛峰武等对巡检机器人步进电机设计了模糊PID控制器，将步进电机开环传递函数设为被控对象，通过simulink软件搭建仿真模型并与BP-PID [7] [8] 做了对比，验证了模糊PID响应速度更快 [9] 。

本文对挖掘机器人进行五次轨迹规划 ，避免因为突然加速或减速产生的振动和不平稳性，对挖掘机使用造成损坏。设计控制器对执行元件进行控制，使其能更好地完成规划任务。

2. 模型搭建与渠道挖掘点设计

2.1. 挖掘机模型搭建

如图1所示，挖掘机具有四个自由度，包括液压缸的伸缩(升降)自由度、液压缸的摆动(旋转)自由度、车身的旋转自由度以及铲斗的旋转自由度， ${\theta }_1,{\theta }_2,{\theta }_3,{\theta }_4$ 分别为回转，动臂，斗杆，铲斗关节角， $\phi$ 为铲斗齿尖末端角度值。挖掘机D-H坐标系下的运动学尺寸参数如表1所示。铲斗齿尖末端坐标点的表达式，如式(1)所示：

$\{\begin{array}{c}x=c_1\left(L_1+L_2{c}_2+L_3{c}_{23}+L_4{c}_{234}\right)\\ y=s_1\left(L_1+L_2{c}_2+L_3{s}_{23}+L_4{s}_{234}\right)\\ z=b_1+L_2{s}_2+L_3{s}_{23}+L_4{s}_{234}\\ \phi ={\theta }_2+{\theta }_3+{\theta }_4\end{array}$ (1)

式1中： $c_1=\cos {\theta }_1$ ; $c_2=\cos {\theta }_2$ ; $c_{23}=\cos \left({\theta }_2+{\theta }_3\right)$ ; $s_1=\sin {\theta }_1$ ; $c_{234}=\cos \left({\theta }_2+{\theta }_3+{\theta }_4\right)$ ; $s_2=\sin {\theta }_2$ ; $s_{23}=\sin \left({\theta }_2+{\theta }_3\right)$ ; $s_{234}=\sin \left({\theta }_2+{\theta }_3+{\theta }_4\right)$ 。

图1. 挖掘机模型搭建

表1. D-H坐标系下的运动学参数表

2.2. 修渠作业中挖掘工况轨迹设计

在不考虑回转的前提下，挖掘机器人通常在工作区域内进行任务规划，以提高工作效率与准确性。本文选取了一个类似半圆形的工作区域，以六个修理路径点作为轨迹控制点，进行研究。通过规划算法和时间优化方法，确定了挖掘机器人机械臂的运动轨迹，如图2所示，为本文所选的六个挖掘路径点，由所选挖掘点对挖掘机器人机械臂进行轨迹规划的研究。

根据图2可以确定挖掘机器人铲斗齿尖末端坐标点位置，结合表1中给出的关节角取值范围，利用逆运动学公式可求得在某一位姿下挖掘机器人各关节唯一角度值 [10] 。在工作过程中考虑到铲斗姿态角可以控制挖掘机器人的铲斗位置和角度，使其更容易达到满斗状态。调整铲斗姿态角，可以使铲斗在挖掘过程中的接触面积更大，提高挖掘效率和稳定性。在逆运动学计算中加入了铲斗姿态角 $\phi$ 的考虑，来提升挖掘机器人的工作效果，实现更高质量的挖掘操作。各关节给定坐标点分别对应角度，如表2所示。

图2. 笛卡尔空间挖掘路径点

表2. 笛卡尔空间到关节空间变换

3. 挖掘机运动轨迹规划与电液控制系统数学模型

3.1. 五次多项式轨迹规划与挖掘机液压系统建模

为减少挖掘机器人在渠道维护时产生剧烈抖动和振动，达到平滑、稳定控制的效果，应用起始和终止点的已知条件，以及中间点满足关节角度、角速度和角加速度连续的约束条件，结合动臂和斗杆，铲斗角度变化参数和时间，确定了一个五次多项式：

$\theta \left(t\right)=a_0+a_1\left(t-t_i\right)+a_2{\left(t-t_i\right)}^2+a_3{\left(t-t_i\right)}^3+a_4{\left(t-t_i\right)}^4+a_5{\left(t-t_i\right)}^5$ (2)

五次多项式轨迹规划是 $\theta \left(t\right)$ 相对于时间t的变化满足五次多项式变化，其表达式如式2所示，动臂，斗杆，铲斗关节空间轨迹规划曲线，如图3~5所示，挖掘机器人的加速度和曲率连续变化平缓，减少作业过程中冲击和振动，降低挖掘机器人故障率，提高挖掘机平稳性。由关节空间轨迹规划曲线根据式(1)可求得笛卡尔空间齿尖轨迹，如图6所示，其末端拟合曲线平滑。

图3. 关节角与时间变化曲线

图4. 角速度与时间变化曲线

图5. 角加速度与时间变化曲线

图6. 笛卡尔空间轨迹拟合

3.2. 关节角与液压缸长度的关系

挖掘机液压缸由液压系统驱动，为了完成给定的目标任务，在轨迹控制中需要各个液压缸共同作用 [11] 因此需要将关节角度轨迹规划转为液压缸驱动轨迹。式(3)为实验室搭建的模型通过几何法得到的关节角与液压缸长度的关系。

$\{\begin{array}{c}{\theta }_2={\cos }^{-1}\frac{C{O}^2+B{O}^2-B{D}^2}{2CO*BO}-\angle DOS-\angle BOH\\ {\theta }_3={180}^{\circ }-\angle DSO-\angle QSE-{\cos }^{-1}\frac{D{S}^2+E{S}^2-D{E}^2}{2DS*ES}\\ {\theta }_4={180}^{\circ }-\angle NQS-\angle VQK-\angle GQN-\angle GQK\end{array}$ (3)

3.3. 电液控制系统数学模型

液压挖掘机在进行挖掘工作时，通过调控其动臂、斗杆和铲斗三个部分的系统运动来完成各项操作。而每个部分的系统运动则是通过操纵液压缸的行程来实现的 [11] 。具体控制过程如下：首先，根据输入的期望液压缸行程电压信号，通过比例放大器将输入信号转换为电流，控制电液比例阀的开合程度，然后将其转换为控制液压缸行程的流量信号，流量信号可控制液压缸行程，使其准确到达预定位置，如图7所示。同时，为了确保液压挖掘机的行程与期望值一致，安装液压缸传感器，用于监测实际行程并反馈给控制系统。通过与期望行程进行比较，即可计算出行程误差。根据误差信号的大小，控制系统会进行相应的修正，以使液压缸的行程达到期望值，并保持稳定。

图7. 液压系统流程图

3.3.1. 电液比例阀传递函数

比例放大器可以将控制器输入的电压信号按照设定的比例转换为对应的电流信号输出。电流信号控制比例减压阀的开合程度，调整输出压力大小。多路阀中的阀芯会受到来自比例减压阀的压力作用，产生相应的阀芯位移 [3] ，式(4)为传递函数模型：

$G_1\left(S\right)=\frac{X_v\left(S\right)}{U\left(S\right)}=\frac{K{K}_P}{T_{v}s+1}$ (4)

式中：K——放大器比例系数； $K_P$ ——减压阀比例系数； $T_v$ ——减压阀时间常量；

3.3.2. 液压缸传递函数

通过对多路阀滑阀流量方程、液压缸流量连续性方程以及液压缸力平衡方程进行拉普拉斯变换计算，得到该系统的传递函数方程 [12] ，式(5)为传递函数表达式：

$G_2\left(S\right)=\frac{Y\left(S\right)}{X_V\left(S\right)}=\frac{K_q/A}{S\left(\frac{S^2}{{\omega }_h^2}+\frac{2{\zeta }_h}{{\omega }_h}+1\right)}$ (5)

式中： $K_q$ ——滑阀在稳态工作点附近操纵信号引起的流量变化比例(m²/s)； ${\omega }_h$ ——液压缸固有频率； ${\zeta }_h$ ——阻尼比；A——活塞平均面积。

4. 模糊PID控制器设计

模糊PID控制器是一种综合了PID控制器和模糊控制技术的方法。在模糊逻辑控制中，通过使用偏差E和偏差变化率EC作为输入信号，系统会根据这些输入信号的模糊规则，计算得出∆K_p、∆K_i和∆K_d这三个参数的调整量作为输出。在模糊PID控制中，输入和输出之间通过一定的模糊关系进行映射。由于偏差和偏差变化率会随着运行过程的改变而不断变化，模糊PID控制利用模糊关系实时调整这三个参数 [13] 。相较于PID控制器，模糊PID [14] [15] [16] 控制器能够自动实现对PID参数的最佳调整。通过模糊逻辑的方法，系统能够根据实际的控制需求和反馈信息，自动调节PID参数，以适应不同的工况和控制对象，从而满足挖掘机器人位姿控制的需求，模糊PID结构如图8所示。

图8. 模糊PID结构图

模糊规则控制表描述输入与输出的规则集合，模糊控制器则利用这些规则进行推理，生成对应的控制信号。模糊规则控制表是模糊控制器的基础，它决定了模糊控制器如何根据输入来产生输出。使得模糊控制系统能够应对非线性、模糊和不确定性的控制。通过以下步骤进行模糊控制：

(1) 确定控制器的输入变量为活塞位移偏差值E和偏差值变化率EC，输出变量为PID 3个参数调整量∆K_p、∆K_i和∆K_d

(2) 确定输入变量和输出变量的论域，活塞位移偏差值E的论域为[−0.03, 0.03]，偏差值变化率EC的论域为[−0.04, 0.04]，PID 3个参数调整量的论域均为[−3, 3]。

(3) 设计隶属函数，选择三角形隶属函数形状来描述输入变量和输出变量的隶属度

(4) 设计模糊控制规则表，确定不同输入变量隶属度之间的关系，使用IF-THEN规则，如“If A and B then C”，建立了参数∆K_p、∆K_i和∆K_d的模糊规则控制表如表3~5所示。

(5) 进行模糊推理。对输入变量的具体值进行模糊化，将其转化为对应隶属度。根据模糊控制规则表，由输入变量的隶属度进行模糊推理，得到模糊输出变量的隶属度。

(6) 解模糊化。使用重心法解模糊化方法，将模糊输出变量的隶属度转化为活塞位移控制量。

表3. ΔK_p的模糊规则控制表

表4. ΔK_i的模糊规则控制表

表5. ΔK_d的模糊规则控制表

5. 仿真分析

本文针对动臂液压缸进行研究，分析其在渠道维护时动态响应特性，将上述所搭建的控制器与被控对象模型连接，在simulink构建仿真模型并完成仿真测试，表6给出了仿真模型的各项参数。仿真搭建对比模型如图9所示，图10为2种算法在阶跃响应下的对比图。分别采用阶跃信号与动臂液压缸位移结果作为信号输入。由动臂各部分结构参数以及液压系统的参数，得到动臂系统开环传递函数，如式(6)所示：

$G\left(s\right)=\frac{0.\text{3}}{1.9*{10}^{-6}{s}^4+1.3*{10}^{-4}{s}^3+0.2s^2+s}$ (6)

通过图10可知模糊PID控制算法比PID算法有较大的优势，模糊PID控制器在系统响应速度方面优于PID控制器。根据仿真结果，模糊PID控制器的系统响应时间为2.07秒，而PID控制器的系统响应时间为3秒。使用模糊PID控制器能够更快地响应控制信号，并更快地将系统状态调整到期望的目标状态，使实际输出值更接近期望值。

表6. 仿真模型的各项参数

图9. 阶跃响应仿真模型搭建

图10. 阶跃响应曲线结果对比

为验证对动臂液压缸轨迹控制效果，将轨迹规划后动臂液压缸实际位移作为输入，搭建仿真模型如图11所示，轨迹控制仿真结果如图12所示。

PID控制算法的响应时间，稳态误差从图12可得，分别为3.04 s，0.002 m；模糊PID控制算法响应时间，稳态误差从图12可得，分别为1.92 s，0.00105 m，模糊PID响应速度较快，更适用于挖掘机器人。

图11. 液压缸位移输入仿真模型的搭建

图12. 轨迹控制仿真结果

6. 结论

本文针对挖掘机器人在渠道维护中提升执行机构控制精度问题，提出一种模糊算法，提升其动态响应特性，通过五次轨迹规划确保挖掘机器人在整个运动过程中的速度、加速度和姿态变化平滑过渡，减少运动过程中震荡和抖动。对执行机构仿真分析得出模糊PID算法适用于挖掘机器人渠道维护任务，并有以下的结论：

(1) 提出挖掘机器人渠道维护方法。根据渠道来设计挖掘控制点为轨迹规划提供数据，对挖掘机模型搭建，确定了铲斗、斗杆、动臂关节角变换范围和关节空间到笛卡尔空间的转换关系，确定挖掘动作时的铲斗齿尖运动轨迹，满足在渠道维护任务需求，为挖掘机器人进行自动化与智能化作业时，提供思路，为进行轨迹控制提供了技术支持。

(2) 利用电液控制系统的数学模型和参数，推导出其被控对象的传递函数，为仿真模型搭建提供了基础。

(3) 设计了模糊控制器，得到了模糊PID控制与PID控制在阶跃响应和液压缸为输入时的动态特性。通过在simulink中搭建了阶跃响应和动臂液压缸为输入轨迹控制模型，对两种控制器进行仿真，其中PID控制算法的响应时间，稳态误差，分别为3.04 s，0.002 m；模糊PID控制算法响应时间，稳态误差分别为1.92 s，0.00105 m，模糊PID响应速度较快，验证了模糊PID控制算法的先进性。

文章引用

苗文军,马西良,刘成强,杜学智,马 彬. 挖掘机器人渠道维护轨迹规划与控制研究
Research on Trajectory Planning and Control for Excavation Robot Channel Repairing[J]. 建模与仿真, 2023, 12(06): 5627-5638. https://doi.org/10.12677/MOS.2023.126511

参考文献