一种挖掘机挖机臂动力学控制方法与流程

1.本发明涉及一种挖掘机挖机臂动力学控制方法，属于挖掘机控制技术领域。


背景技术：

2.挖掘机，又称挖掘机械，是用铲斗挖掘高于或低于承机面的物料，并装入运输车辆或卸至堆料场的一种土方机械。其作为一种移动物料的机械，在采矿和建筑领域有广泛的应用。由于其应用的广泛性，挖掘机操作员必须掌握多种多样的操作技术以适应不同的工况和需求。这使得培养合格的操作员成为耗时耗力的工作。此外，由于操作的复杂性和恶劣的工作环境，操作员难以长时间保持高效的工作状态。这使得挖掘机自动化控制的需求愈加高涨。
3.目前的几种挖掘机自动控制方案多数使用简单的位置反馈控制方案，某些方案辅之以速度前馈。但这些自动控制方案对反馈信号和目标信号均只有简单处理，并且其前馈模型也较为粗略，难以处理挖掘机的非线性问题，导致这些自动控制方案只能跟踪某几种预设轨迹，且轨迹速度受到极大限制，没能充分利用挖掘机的性能。
4.现有挖掘机的自动控制大多是针对一种特定的工况或轨迹进行设计的，想要切换至其它轨迹必须对控制器重新进行设计或训练。此外，由于控制器结构的限制，当前的自动控制系统往往不能充分利用挖掘机的全部功率，只能跟踪一些缓慢运动的轨迹。
5.第一种现有技术中一种改进的电机三环控制方法，只涉及到一种改进的电机三环控制方法，并不涉及到动力学前馈，而且在电流环pid输出时也没有加调节因子。
6.第二种现有技术中一种机器人控制方法、装置、电子设备及可读存储介质，用的是速度前馈，没有采用力矩前馈，不能做到精确的力控，而且该方案的摩擦力建模，没有考虑库仑摩擦力、粘滞摩擦力和stribeck摩擦力。
7.第三种现有技术中一种机器人控制系统，只是概述了一个机器人控制系统，并不涉及到动力学前馈控制和三环pid控制。
8.基于以上现有技术，本领域技术人员急需要解决如下技术问题：
9.(1)解决电磁阀的“死区”的问题。
10.(2)解决对挖机臂精确控制的问题。
11.(3)解决低速时“爬行”、高速时精度差的问题。


技术实现要素：

12.目的：为了克服现有技术中存在的不足，本发明提供一种挖掘机挖机臂动力学控制方法，采用动力学前馈+三环pid反馈的控制模式，实时计算挖机臂运动时产生的惯性力、科式力、向心力、重力和摩擦力，设计了挖机臂控制系统，其能提高挖机臂运动时的动态精度，提高挖掘机运动时可靠性和稳定性。
13.技术方案：为解决上述技术问题，本发明采用的技术方案为：
14.一种挖掘机挖机臂动力学控制方法，包括如下步骤：
15.步骤s1：对预先构建的挖机臂动力学模型进行参数线性化处理，得到挖机臂动力学模型的线性方程。
16.步骤s2：对挖机臂动力学模型的线性方程进行重组，得到重组后的线性方程。
17.步骤s3：在挖机臂的动臂、斗杆和铲斗上设置传感器，并获取激励轨迹下挖机臂的动臂、斗杆和铲斗的各个关节的力矩、角度、角速度和角加速度数据。
18.步骤s4：对各个关节的力矩、角度、角速度和角加速度数据进行滤波处理，并利用滤波后的数据求解重组后的线性方程中的满秩回归矩阵，利用满秩回归矩阵的条件数最小的准则对预先设计的激励轨迹计算公式进行优化，获取激励轨迹计算公式中的参数。
19.步骤s5:基于滤波后的数据应用最小二乘法辨识重组后的线性方程中的最小惯性参数矩阵的各参数，将最小惯性参数矩阵作为挖机臂动力学参数矩阵。
20.步骤s6：将挖机臂动力学参数矩阵带入重组后的线性方程中，获得辨识后的挖机臂动力学前馈模型。
21.步骤s7：将挖机臂动力学前馈模型输出的前馈信号输入预先构建的三环pid控制体系，三环pid控制体系输出挖机臂各关节的控制结果。
22.作为优选方案，还包括步骤s8,步骤s8:将三环pid控制体系输出挖机臂各关节的控制结果与预设的挖机臂各关节的控制结果进行匹配，当匹配度小于匹配阈值时，返回步骤s3，重复步骤s3-s6获得更理想的挖机臂动力学前馈模型。
23.作为优选方案，所述预先构建的挖机臂动力学模型的计算公式如下：
[0024][0025]
其中，为控制力矩，q、分别为挖机臂各个关节的角度、角速度和角加速度，为惯性力矩，为科氏力矩和向心力矩的耦合力矩，为重力矩，为摩擦力矩。
[0026]
作为优选方案，
[0027]
其中：为对取符号函数，pc为库仑摩擦力系数，pv为粘滞摩擦力系数，ps为stribeck摩擦力系数。
[0028]
作为优选方案，所述挖机臂动力学模型的线性方程的计算公式如下：
[0029][0030]
其中：为控制力矩，为回归矩阵，q、分别为挖机臂各个关节的角度、角速度和角加速度，为惯性参数矩阵。
[0031]
作为优选方案，所述重组后的线性方程的计算公式如下：
[0032][0033]
其中，为控制力矩，为满秩回归矩阵，为最小惯性参数矩阵。
[0034]
作为优选方案，表示n个关节的动力学参数，pn表示第n个关节
的动力学参数。
[0035]
关节i的最小惯性矩阵元素可以表示为：
[0036]
pr＝[i
xx
,i
xy
,i
xz
,i
yy
,i
yz
,i
zz
,mc
x
,mcy,mcz,m,pc,pv,ps]
t
[0037]
其中，i
xx
,i
xy
,i
xz
,i
yy
,i
yz
,i
zz
为关节i在x、y、z各方向的惯性张量矩阵的6个参数，m为关节i质量，c
x
，cy，cz分别为关节i质心位置在关节坐标系x、y、z方向的分量，pc为库仑摩擦力系数，pv为粘滞摩擦力系数，ps为stribeck摩擦力系数，ρ为关节i单元体密度。
[0038]
作为优选方案，所述预先设计的激励轨迹计算公式如下：
[0039][0040]
其中，wf为基频，各关节应选取相同基频保证整个运动的周期性，n表示谐波个数，l表示谐波，t表示时刻，为系数，q
i0
为常数项，i代表关节。
[0041]
作为优选方案，获取的激励轨迹计算公式中的参数为q
i0
。
[0042]
作为优选方案，挖机臂动力学模型重组后的线性方程中辨识的参数为最小惯性参数矩阵的各参数。
[0043]
作为优选方案，预先构建三环pid控制体系包括：位置环、速度环和电流环。
[0044]
位置环：输入的位置控制指令p_ref与位置反馈指令p_fb进行融合后输入位置控制模块，位置控制模块输出目标速度指令v_ref，控制模块采用比例调节。
[0045]
速度环：输入的目标速度指令v_ref与速度反馈指令v_fb进行融合后输入速度控制模块，速度控制模块输出目标加速度指令a_ref，速度控制模块采用比例和积分调节。
[0046]
电流环：输入的目标加速度指令a_ref，加速度反馈指令a_fb和挖机臂动力学前馈模型输出的前馈信号进入融合后输入电流控制模块，电流控制模块的输出乘以一个电流系数k后作为关节电磁阀控制信号，电流控制模块有比例、积分和微分调节。
[0047]
有益效果：本发明提供的一种挖掘机挖机臂动力学控制方法，相比现有技术中往往只针对于某一特定的工况或轨迹的控制方案，能够精确计算每个运动周期中挖机臂各个关节所需的输出力矩，提升了挖机臂的动态控制精度，解决了以往控制中挖机臂在运动时存在低速时“爬行”、高速时精度差的问题。
[0048]
相对于现有技术而言，本发明具有的有益技术效果：
[0049]
(1)在构建挖机臂动力学模型的过程中，针对摩擦力模型部分进行了优化，综合引入了库仑摩擦力、粘滞摩擦力和斯特里贝克摩擦力系数，使得构建出的动力学模型能够更好地匹配挖掘机的液压系统，适应液压缸的非线性特性，解决了电磁阀的“死区”的问题。
[0050]
(2)设计最优激励轨迹,并根据传感器组获取的参数以及控制结果匹配度两种优化机制对挖机臂的最优激励轨迹进行优化，进一步通过参数辨识，使得挖机臂的控制更加精确。
[0051]
(3)将三环pid控制体系与动力学前馈模型进行融合，利用动力学前馈模型输出的前馈信号对三环pid控制体系进行优化，提升挖机臂的动态控制精度；将电流环pid的输出乘以一个电流系数k，并基于激励轨迹构建电流系数k的动态调整机制，提高了挖掘机运动时可靠性和稳定性，解决现有技术中挖掘机控制中存在的低速时“爬行”、高速时精度差的
问题。
附图说明
[0052]
图1为本发明控制方法的流程示意图。
[0053]
图2为本发明控制系统的结构示意图。
具体实施方式
[0054]
下面结合具体实施例对本发明作更进一步的说明。
[0055]
如图1所示，一种挖掘机挖机臂动力学控制方法，包括如下步骤：
[0056]
步骤s1：构建挖机臂动力学模型。
[0057]
步骤s2：对挖机臂动力学模型进行参数线性化处理，得到挖机臂动力学模型的线性方程。
[0058]
步骤s3：根据挖机臂动力学模型的线性方程进行重组，得到重组后的线性方程，并从重组后的线性方程中提取最小惯性参数矩阵作为动力学参数矩阵。
[0059]
步骤s4：设计激励轨迹。
[0060]
步骤s5：在挖机臂的动臂、斗杆和铲斗设置传感器，并获取激励轨迹下挖机臂的动臂、斗杆和铲斗的各个关节力矩、角度、角速度和角加速度数据。
[0061]
步骤s6：对关节力矩、角度、角速度和角加速度数据进行滤波处理，并利用滤波后的数据求解挖机臂动力学回归矩阵，利用求解出的挖机臂动力学回归矩阵对步骤s4中的激励轨迹进行优化，获取激励轨迹中的待优化参数。
[0062]
步骤s7:应用最小二乘法基于步骤s6中滤波处理后的数据辨识步骤s3中的最小惯性参数矩阵的各参数，得到辨识后的挖机臂动力学参数矩阵。
[0063]
步骤s8：将辨识后的挖机臂动力学参数矩阵带入参数线性化后的挖机臂动力学模型重组后的线性方程中，获得辨识后的挖机臂动力学前馈模型。
[0064]
步骤s9：构建三环pid控制体系，并融合步骤s8中挖机臂动力学前馈模型输出的前馈信号对整个三环pid控制体系进行优化，获得优化后的三环pid控制体系，用于改善挖机臂运行时的动态特性，减少pid控制器调节的偏差，提升对挖机臂的动态控制精度。
[0065]
步骤s10：将步骤s9中优化后的三环pid控制体系输出的控制结果与预设的控制结果进行匹配，当匹配度小于匹配阈值时，对步骤s4中的激励轨迹进行进一步的优化，筛选更理想的激励轨迹进行参数辨识，获取准确的挖机臂惯性参数和动力学模型。
[0066]
进一步的，所述挖机臂动力学模型采用牛顿-欧拉方程进行推导，计算公式如下：
[0067][0068]
其中：为控制力矩，q、分别为挖机臂各个关节的角度、角速度和角加速度，为惯性力矩，为科氏力矩和向心力矩的耦合力矩，为重力矩，为摩擦力矩。
[0069]
其中，摩擦力矩采用非线性摩擦模型，由库仑摩擦力、粘滞摩擦力和stribeck斯特
里贝克摩擦力组成，计算公式如下：
[0070][0071]
其中：为对取符号函数，pc为库仑摩擦力系数，pv为粘滞摩擦力系数，ps为stribeck摩擦力系数。
[0072]
常规的动力学方程主要包括四个部分：惯性力矩、科式力和向心力矩、重力矩和摩擦力。惯性力矩与各个关节的加速度相关，科式力和向心力矩与各个关节的速度相关，重力矩与各个关节的当前角度相关，摩擦力与各个关节的当前速度相关。
[0073]
本发明考虑到挖机臂各个关节质量较大，而且为了提高工作效率，多数情况下各个关节运行速度也较大，因此在构建动力学模型时保留了惯性力矩、科式力和向心力矩、重力矩，而且在建立摩擦力模型时，常规的仅是考虑库仑摩擦力和粘滞摩擦力，但是考虑到液压缸的非线性特性，以及电磁阀的“死区”问题，仅仅考虑库仑摩擦力和粘滞摩擦力是不够的，而且液压缸低速时会有“爬行”现象，单纯的库仑摩擦力+粘滞摩擦力模型不能匹配挖掘机的液压系统，基于此，本方案不仅考虑了库仑摩擦力和粘滞摩擦力，还考虑了stribeck摩擦力的影响。
[0074]
挖机臂动力学模型中包含三类参数：运动学参数、惯性参数和摩擦参数。
[0075]
其中：运动学参数为关节长度、扭角等已知量。
[0076]
惯性参数为耦合力矩，重力矩。
[0077]
摩擦参数为库仑摩擦力、粘滞摩擦力和stribeck斯特里贝克摩擦力。
[0078]
惯性参数和摩擦参数难以直接测量。
[0079]
进一步的，对挖机臂动力学模型进行参数线性化处理，具体步骤如下：
[0080]
由于构建的挖机臂动力学模型不是线性的，在进行参数辨识时，会带来很多麻烦，因此需要对推导的挖机臂动力学模型的方程进行参数线性化处理。对动力学模型进行参数线性化处理属于现有技术，在此不再赘述。
[0081]
将挖机臂动力学模型进行参数线性化处理，得到等效的线性方程，线性方程计算公式如下：
[0082][0083]
其中：为回归矩阵，由关节的角度、角速度、角加速度计算得到，与动力学参数无关；为待辨识的惯性参数矩阵。此时的回归矩阵并非满秩，无法直接通过回归矩阵进行参数辨识。
[0084]
进一步的，将线性方程进行重组，得到重组后的线性方程，并提取重组后的线性方程中最小惯性参数矩阵具体步骤如下：
[0085]
线性方程中参数之间并不是相互独立的，参数相互独立是取得较高辨识精度的前提，因此需要对待辨识参数的独立性进行分析，目的是找到一组相互独立的参数集合，即最小参数集。最小参数集是参数的组合值，目前动力学参数辨识大多只能辨识出这一组合值，但组合值并不影响动力学参数的计算，完全可以取代实际参数，而且还可以简化动力学模型。
[0086]
利用封闭形式规则的线性关系对惯性参数矩阵中的元素进行消去和重组，记重组后可辨识的最小惯性参数矩阵为对应的满秩回归矩阵记为对应的满秩回归矩阵记为同样是由角度、角速度与角加速度构成的矩阵，重组后的线性方程，计算公式如下：
[0087][0088]
其中，表示n个关节的动力学参数，pn表示第n个关节的动力学参数。
[0089]
针对其中关节i的最小惯性矩阵元素可以表示为：
[0090]
pr＝[i
xx
,i
xy
,i
xz
,i
yy
,i
yz
,i
zz
,mc
x
,mcy,mcz,m,pc,pv,ps]
t
[0091]
其中：
[0092]ixx
,i
xy
,i
xz
,i
yy
,i
yz
,i
zz
为关节i在x、y、z各方向的惯性张量矩阵的6个参数。
[0093]ixx
＝∫∫∫v(y2+zz)ρdv
[0094]ixy
＝∫∫∫vxyρdv
[0095]ixz
＝∫∫∫vxzρdv
[0096]iyy
＝∫∫∫v(x2+z2)ρdv
[0097]iyz
＝∫∫∫vyzρdv
[0098]izz
＝∫∫∫v(x2+y2)ρdv
[0099]
m为关节i质量，c
x
，cy，cz分别为关节i质心位置在关节坐标系x、y、z方向的分量，pc为库仑摩擦力系数，pv为粘滞摩擦力系数，ps为stribeck摩擦力系数，ρ为关节i单元体密度。
[0100]
进一步的，所述设计激励轨迹，并求取激励轨迹中参数，具体步骤如下：
[0101]
本发明将挖机臂各个关节的目标位置用五阶傅里叶级数表示，之所以选择用五阶傅里叶级数来表示目标位置，是因为由目标位置算出来的目标速度、目标加速度是连续的，这样在进行参数辨识时不会引入其他误差。采用关节i傅里叶级数作为激励轨迹，分别控制挖机臂各个关节运动到目标位置。由于傅里叶级数具有周期性，可利用多次采样求平均值提高信号的信噪比，得到比较理想的数据。
[0102]
所述关节i的傅里叶级数作为激励轨迹的计算公式如下：
[0103][0104]
其中，wf为基频，各关节应选取相同基频保证整个运动的周期性，n表示谐波个数，l表示谐波，t表示时刻，为系数，用于构成激励轨迹优化问题的自由度，q
i0
为常数项。
[0105]
为提高辨识精度，需选取合适的指标函数激励轨迹。满秩回归矩阵的条件数反映辨识方法的抗噪声能力和参数估计收敛率，条件数越大，表明矩阵病态程度越大，因此利用公式5满足各关节角度、角速度、角加速度取值范围的边界条件下，将的条件数最小作为激励轨迹优化准则，利用优化算法求取激励轨迹中的和q
i0
值。
[0106]
进一步的，对挖机臂的动臂、斗杆和铲斗设置传感器，并获取激励轨迹下挖机臂各
个关节力矩、角度、角速度和角加速度数据，具体步骤如下：
[0107]
挖机臂的动臂、斗杆和铲斗关节分别安装有倾角传感器、角速度传感器，各个关节油缸进油口和出油口分别安装有压强传感器，采集各个传感器的数据，根据油缸截面积和压强传感器数据可计算出各个关节的力矩。由于不能测得各个关节运动时的角加速度，因此需对各个关节运动角速度进行差分，来获得各个关节的运动角加速度。
[0108]
进一步的，对获取的力矩、角度、角速度和角加速度数据进行滤波处理，并利用滤波后的数据对激励轨迹进行优化，获得优化后的激励轨迹，具体步骤如下：
[0109]
由于差分过程会带来噪声，因此需要设计在线滤波器，同时对力矩、角度、角速度和角加速度进行滤波处理。常规滤波器包括在线滤波器和离线滤波器，由于在线滤波器相比于离线滤波器没有滞后性，因此本方案选择在线滤波器对采集到的数据进行滤波处理，在线滤波器的设计属于常规设计，在此不再赘述。
[0110]
需要说明的是，将滤波处理后的数据反馈至激励轨迹中，根据滤波数据计算出对激励轨迹进行优化。
[0111]
进一步的，应用最小二乘法基于滤波处理后的数据辨识提取的最小惯性参数矩阵得到挖机臂的动力学参数矩阵，具体步骤如下：
[0112]
基于滤波处理后的数据采用最小二乘法来辨识各个关节最小惯性矩阵元素pr，得到挖机臂的动力学参数矩阵需要说明的是，最小二乘法是现有技术，在此不再赘述。
[0113]
进一步的，将挖机臂的动力学参数矩阵带入构建的挖机臂动力学模型中，完成挖机臂动力学前馈模型的构建。
[0114]
前馈模型的核心是通过建立动态的惯量模型来补偿系统内部控制所提供的控制量，从而减小电流环中相邻控制周期内的偏差，改善挖机臂运行时的动态特性，减少位置误差，提高运动精度。通过辨识参数后的挖机臂动力学模型可以计算出当前状态下的理论力矩值并叠加到电流环中，减少pid控制器调节的偏差，加快误差的收敛速度，进而改善各关节动态响应特性。
[0115]
进一步的，构建三环pid控制体系，并融合挖机臂动力学前馈模型输出的前馈信号对整个三环pid控制体系进行优化，得到三环pid控制体系输出控制结果，提升挖机臂的动态控制精度，具体步骤如下：
[0116]
对于挖机臂这种质量大、刚度大的系统，想要在其运动时达到高速高精的控制效果，是有一点难度的。当前挖机臂控制一般采用的都是位置控制或者速度控制，再加上简单的速度前馈，这种控制方式忽略了外界的扰动对挖机臂控制的干扰，外界的扰动主要分为两个方面：
[0117]
1、外界环境的干扰。
[0118]
2、建模不精确或者系统参数辨识不精确带来挖机臂自身系统的扰动。
[0119]
基于此，本发明提出了三环pid反馈控制，三环pid反馈控制能压制外界环境的扰动，但是，单纯的三环pid反馈控制是不够的，因为最终设定的pid参数不能完美的适合系统，这相当于无法解决挖机臂控制的系统扰动。基于此，本发明设计了基于挖机臂系统的动力学建模，构建了三环pid反馈+动力学前馈控制器。
[0120]
构建三环pid反馈+动力学前馈控制器的主要目的是为了实现挖机臂的转矩控制，
挖机臂在运动过程中，由于挖机臂本身质量很大，单纯的位置控制或者速度控制达不到较好的控制效果，特别是当挖机臂运动速度较快时，惯性力、向心力、科式力以及重力综合影响挖机臂的控制，加上挖机臂类似于一个悬臂梁结构，如果不能对挖机臂系统进行精准建模，不仅达不到高速高精的控制效果，而且在低速时控制精度也达不到期望值。再者，挖机臂刚度很大，非常适合用刚体动力学对其建模。因此，本发明构建了挖机臂的三环pid反馈+动力学前馈控制器，如图2所示。
[0121]
三环pid控制系统，包括：位置环、速度环和电流环。
[0122]
位置环：输入位置控制指令p_ref，即当前控制挖机臂达到需要到达的位置，通过挖机臂中倾角传感器采集当前各个关节的位置作为位置反馈指令p_fb，常规的位置环控制一般采用比例调节或者比例积分调节，本发明中设计的位置控制模式下系统进行了所有3个环的运算，此时的系统运算量最大，动态响应速度也最慢，为了尽可能提高动态响应速度，位置环中位置控制模块只采用比例调节，输出目标速度指令v_ref。
[0123]
速度环：位置环pid的输出即为目标速度指令v_ref，通过挖机臂中速度传感器采集的当前各个关节的速度作为速度反馈指令v_fb，速度环中速度控制模块采用比例和积分调节，输出目标加速度指令a_ref，综合比例和积分的优点，用比例调节快速抵消干扰的影响，同时利用积分环节来消除残差。
[0124]
电流环：速度环pid的输出即为目标加速度指令a_ref，通过挖机臂中速度差分得到的当前各个关节的加速度作为加速度反馈指令a_fb，加入挖机臂动力学前馈模型输出的前馈信号，电流环中电流控制模块有比例、积分和微分调节。将电流环pid的输出乘以一个电流系数k，其中，电流系数k可以通过动态调节的形式来选取。即，给定一条运动轨迹，将轨迹离散化，通过分析整个运动周期中理论轨迹和实际轨迹的偏差，如果各个离散时刻偏差的平方和过大，则动态调整参数k。最终，根据系统控制需要的精度，找到偏差平方和较小时对应的参数k。
[0125]
电流控制模块的输出信号给电磁阀，即构建了三环pid反馈+动力学前馈控制器。
[0126]
三环pid控制又称位置环、速度环、电流环串级控制，最外环的位置期望做差经过位置环pid控制后的值作为速度环的期望，中间环的速度期望做差经过速度环pid控制后的值作为电流环的期望，这样做可以使得系统更快的达到期望的位置和速度，并且减少在期望的位置和速度处震荡。
[0127]
s10:将三环pid控制系统输出的控制结果与预设的控制结果进行匹配，当匹配度小于匹配阈值时，对激励轨迹进行进一步的优化。
[0128]
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。