一种基于卡尔曼滤波算法的装载机铲斗受力测试方法与流程

本发明属于工程机械测试技术领域，具体涉及一种基于卡尔曼滤波算法的装载机铲斗受力测试方法。

背景技术：

装载机广泛应用于筑路、建筑、矿山等领域。装载机铲斗受力的测试，一方面可以为分布式驱动装载机的转矩分配提供分配依据，另一方面可以为装载机载荷谱测试提供数据依据，因此高效、可靠、方便地测量装载机铲斗受力是装载机智能化设计和可靠性设计的必要条件。

现有的装载机铲斗受力测试技术中，如专利“转载该机铲斗受力测试装置”，“专利号：zl201520539630.6”，需要专用的销轴力传感器，改装复杂成本高。又如专利“装载机铲斗受力测试方法”，“专利号：cn105973510a”需要在工作装置上贴装应变片，难以满足装载机在复杂、恶劣环境作业时的测试使用。

技术实现要素：

针对上述现有技术中的不足，本发明提供了一种基于卡尔曼滤波算法的装载机铲斗受力测试方法，本发明以简单可行的方法解决装载机在复杂、恶劣环境中作业时铲斗受力的测试问题。结合说明书附图，本发明的技术方案如下：

一种基于卡尔曼滤波算法的装载机铲斗受力测试方法，所述方法具体过程如下：

步骤一：测量并计算动臂缸出力与摇臂缸出力；

步骤二：测量动臂缸与摇臂缸的位移，并计算出铲斗工作位置姿态：

步骤三：建立力矩平衡组，并通过卡尔曼滤波算法推导铲斗在重力方向的受力和在水平前进方向的受力。

所述步骤一的具体过程如下：

在动臂缸上安装的第一压力传感器测得动臂缸的管路压力为p1，在摇臂缸上安装的第二压力传感器测得摇臂缸的管路压力为p2，动臂缸出力f1与摇臂缸出力f2可由以下公式获得：

其中，动臂缸液压杆的直径为d1，摇臂缸液压杆的直径为d2，动臂缸和摇臂缸的机械效率均为ηm，d1、d2、ηm均为设计值。

所述步骤二的具体过程如下：

令动臂与前车架安装铰点的回转中心为o点，令动臂缸与前车架铰接点回转中心为a点，令动臂缸与动臂铰接点回转中心为b点，令动臂质心为q点，令动臂与铲斗铰接点回转中心为m点，令摇臂缸与摇臂铰接点回转中心为c点，令摇臂与动臂铰接点回转中心为d点，令摇臂与连杆铰接点回转中心为e点，令连杆与铲斗铰接点回转中心为n点，令铲斗质心为s点，铲斗去除斗齿后的斗尖位置为受力位置，令其为u点；其中，点b、c、d、e、m、n、q、s和u均为动点；

上述各点坐标分别为(ix，iy)，其中i＝a、b、c、d、e、n、m、q、s、u；

以o点为坐标原点，以重力方向为x方向，以水平向前方向为y方向，令动臂缸行程最短、摇臂缸行程最长位置为初始位置，令各个动点初始坐标分别为bx_0、by_0、cx_0、cy_0、dx_0、dy_0、ex_0、ey_0mx_0、my_0、nx_0、ny_0、qx_0、qy_0、sx_0、sy_0、ux_0、uy_0，上述各坐标均为设计值；

通过上述各点坐标表示铲斗的工作位置姿态，各点坐标的计算过程如下：

∠aob的变化值在三角形δaob中可得：

δ∠aob＝∠aob-∠aob_(lab＝min(lab))

δ∠aob为∠aob在任意位置的变化值；

q点坐标如下：

qy＝lqo·sin(∠qoa)

qx＝lqo·cos(∠qoa)

m点的坐标如下：

my＝lmo·sin(∠moa)

mx＝lmo·cos(∠moa)

d点坐标如下：

dy＝ldo·sin(∠doa)

dx＝ldo·cos(∠doa)

c点坐标如下：

cy＝ldo·sin(∠coa)

cx＝ldo·cos(∠coa)

e点坐标如下：

在初始位置时于三角形δmde中可以求解∠mde的初值∠mde_0为：

在初始位置中：

同理，求出在初始位置状态下的lde、lem；

∠mde的变化值δ∠mde可由∠cdo的变化值δ∠cdo得到：

根据平面四连杆的输入输出特性可得：

其中：

a＝2·lde·lnm·sin(∠mde)

b＝2·lnm·(lde·cos(∠mde)-ldm)

∠mnd在初始位置的初始值∠mnd_0通过以下公式求得：

那么，∠mnd的变化值δ∠mnd表征为：

∠mnd＝∠mnd-∠mnd_0

同时，δ∠mnd亦表征∠smd的变化值δ∠smd和∠umd的变化值δ∠umd，即：

δ∠smd＝δ∠mnd

δ∠umd＝δ∠mnd

由此，s点的坐标为：

u点坐标为：

n点坐标坐标为：

所述步骤三的具体过程如下：

对d点取力矩平衡，将摇臂缸出力f2折算为连杆的受力f3，具体计算过程如下所示：

在δocd和δned中可通过几何原理求解点d到直线oc的距离，设为l2，点d到直线ne的距离设为l3，因此：

分别对o点和m点取力矩平衡，得到公式：

公式中是力f1对o点的力臂，是力f3对m点的力臂，可以通过上述的姿态计算获得，gq是动臂的重力，gs是铲斗的重力；

采用卡尔曼滤波算法对铲斗受力进行计算具体过程如下；

卡尔曼滤波算法公式为：

x(k)＝a·x(k-1)+w(k)

z(k)＝h·x(k)+v(k)

公式中x为状态量，a为状态矩阵，w为状态噪声，z为观测量，h为观测矩阵，v为观测噪声；

其中：

a＝e，其中，e为单位矩阵；

状态噪声w和观测噪声v分别通过实际测量获取，并卡尔曼滤波计算过程中被过滤；

下面进行卡尔曼滤波算法公式推导：

xk|k-1＝axk-1|k-1·····················(1)

＝xk-1|k-1

式中，xk|k-1是上一时刻的状态对现在时刻状态的预测，xk-1|k-1是上一时刻状态的最优结果，其中

代入铲斗在重力方向的受力fy和在水平前进方向的受力fx后，

pk|k-1＝apk-1|k-1a′+q1······················(2)

pk-1|k-1是xk-1|k-1的协方差矩阵，p0|0＝p0＝e。

q1是对测量方程的置信系数矩阵，该值越大，表示对测量方程的依赖度越强，再次模型中，取值如下。

代入后：

在实际应用中，根据不同的车型重量，q1的取值可以适当调整。

kk＝pk|k-1h′/(hpk|k-1h′)+q2··········(3)

其中：

q2是对测量方程的置信系数矩阵，该值越大，表示对测量方程的依赖度越强，再次模型中，取值如下：

在实际应用中，根据不同的车型重量，q2的取值可以适当调整。

kk是卡尔曼滤波增益；

xk|k＝xk-1|k-1+kkzk-hxk|k-1····················(4)

式中xk|k是最优卡尔曼滤波值

zk是k时刻测量得到的z矩阵

pk|k＝(e-kkh)pk|k-1·························(5)

通过上述公式(1)、(2)、(3)、(4)和(5)，进行迭代计算，即推导出铲斗在重力方向的受力fy和在水平前进方向的受力fx。

与现有技术相比，本发明的有益效果在于：

本发明所述的装载机铲斗受力测试方法是基于液压系统压力传感器，结合液压缸行程的无线激光传感器测量，并利用卡尔曼滤波算法实时计算装载机铲斗受力情况，既能应对复杂恶劣的作业环境，同时可以降低系统噪声和环境噪声所产生的影响，此外，本发明所述测试方法无需改装，简单易行。

附图说明

图1为采用本发明所述测试方法的装载机铲斗连接结构及受力分析示意图。

图中：

1-前车架，2.1-第一激光位移传感器，2.2-第二激光位移传感器，

3-动臂缸，4.1-第一压力传感器，4.2-第二压力传感器，

5-摇臂缸，6-动臂，7-摇臂，

8-连杆，9-铲斗。

具体实施方式

为进一步阐述本发明的技术方案，结合说明书附图，本发明的具体实施方式如下：

如图1所示，采用本发明所述测试方法的装载机铲斗工作装置由前车架1、动臂6、动臂缸3、摇臂7、摇臂缸5、连杆8以及铲斗9组成，其中，动臂缸3的缸体端与前车架1的下方铰接，动臂缸3的推杆端与动臂6的中上部铰接，动臂6的后端与前车架1的上方铰接，动臂6的中部与摇臂7的中部铰接，动臂6的前端与铲斗9的后侧下方铰接，摇臂缸5的缸体端与前车架1的上方铰接，且摇臂缸5与前车架1铰接点的回转轴线和动臂6与前车架1交接点的回转轴线共线，摇臂缸5的的推杆端与摇臂7的一端铰接，摇臂7的另一端与连杆8的一端铰接，连杆8的另一端与铲斗9的后侧上方铰接，在动臂缸3缸体端的液压管路上安装有第一压力传感器4.1，第一压力传感器4.1用于测量动臂缸3的液压管路压力，在摇臂缸5缸体端的液压管路上安装有第二压力传感器4.2，第二压力传感器4.2用于测量摇臂缸5的液压管路压力，在动臂缸3的推杆一侧安装有第一激光位移传感器2.1，第一激光位移传感器2.1用于测量动臂缸3的推杆行程，在摇臂缸5的推杆一侧安装有第二激光位移传感器2.2，第二激光位移传感器2.2用于测量摇臂缸5的推杆行程。

为了方便计算，并考虑工程实际情况，合理简化模型，本发明技术方案做一下设定：

1、由于装载机作业过程中作业速度和作业加速度较低，本发明所述测试方法中，忽略铲斗运动加速度的影响；

2、由于连杆8和摇臂7的重量远小于动臂6和铲斗9的重量，为了简化计算，忽略连杆8和摇臂7的重量；

3、设铲斗9的受力集中于去除铲齿后的铲尖位置，即u点。

本发明所述基于卡尔曼滤波算法的装载机铲斗受力测试方法具体如下：

步骤一：测量并计算动臂缸出力f1与摇臂缸出力f2：

在动臂缸3上安装的第一压力传感器4.1测得动臂缸6的管路压力为p1，在摇臂缸5上安装的第二压力传感器4.2测得摇臂缸5的管路压力为p2，设定动臂缸3和摇臂缸5均采用差动连接的方式，动臂缸液压杆的直径为d1，摇臂缸液压杆的直径为d2，动臂缸3和摇臂缸5的机械效率均为ηm；

动臂缸出力f1与摇臂缸出力f2可由以下公式获得：

其中d1、d2、ηm，为设计值，可直接获取；

步骤二：测量动臂缸与摇臂缸的位移，并计算出铲斗姿态：

如图1所示，令动臂6与前车架1安装铰点的回转中心(同时也是摇臂缸5与前车架1安装铰点回转中心)为o点，令动臂缸3与前车架1铰接点回转中心为a点，令动臂缸3与动臂6铰接点回转中心为b点，令动臂质心为q点，令动臂6与铲斗9铰接点回转中心为m点，令摇臂缸5与摇臂7铰接点回转中心为c点，令摇臂7与动臂6铰接点回转中心为d点，令摇臂7与连杆8铰接点回转中心为e点，令连杆8与铲斗9铰接点回转中心为n点，令铲斗质心为s点，铲斗去除斗齿后的斗尖位置为受力位置，令其为u点；其中，点b、c、d、e、m、n、q、s和u均为动点；

上述各点的坐标分别为(ix，iy)其中(i＝a、b、c、d、e、n、m、q、s、u)

以o点为坐标原点，以重力方向为x方向，以水平向前方向为y方向，令动臂缸3行程最短、摇臂缸5行程最长位置为初始位置，令各个动点初始坐标分别为bx_0、by_0、cx_0、cy_0、dx_0、dy_0、ex_0、ey_0mx_0、my_0、nx_0、ny_0、qx_0、qy_0、sx_0、sy_0、ux_0、uy_0，上述各坐标均为设计值，可直接获取；

下面详细解释通过几何方法求解各点坐标：

∠aob的变化值在三角形δaob中可得：

δ∠aob＝∠aob-∠aob_(lab＝min(lab))

利用上述公式可以求得∠aob在任意位置的变化值；

q点坐标如下：

qy＝lqo·sin(∠qoa)

qx＝lqo·cos(∠qoa)

m点的坐标如下：

my＝lmo·sin(∠moa)

mx＝lmo·cos(∠moa)

d点坐标如下：

dy＝ldo·sin(∠doa)

dx＝ldo·cos(∠doa)

c点坐标如下：

cy＝ldo·sin(∠coa)

cx＝ldo·cos(∠coa)

e点坐标如下：

下面求解s、u点坐标：

在初始位置时于三角形δmde中可以求解∠mde的初值∠mde_0为：

在初始位置中：

同理可以求出在初始位置状态下的lde、lem；

∠mde的变化值δ∠mde可由∠cdo的变化值δ∠cdo得到：

根据平面四连杆的输入输出特性可得：

其中：

a＝2·lde·lnm·sin(∠mde)

b＝2·lnm·(lde·cos(∠mde)-ldm)

∠mnd在初始位置的初始值∠mnd_0可以在初始位置时，通过以下公式求得：

那么，∠mnd的变化值δ∠mnd可以表征为：

δ∠mnd＝∠mnd-∠mnd_0

同时，δ∠mnd也可以表征∠smd的变化值δ∠smd和∠umd的变化值δ∠umd。

即：

δ∠smd＝δ∠mnd

δ∠umd＝δ∠mnd

由此，s点的坐标为：

u点坐标为：

n点坐标坐标为：

步骤三：建立力矩平衡组，并通过卡尔曼滤波算法推导铲斗在重力方向的受力fx和在水平前进方向的受力fy：

对d点取力矩平衡，将摇臂缸出力f2折算为连杆的受力f3，具体计算过程如下所示：

在δocd和δned中可通过几何原理求解点d到直线oc的距离，设为l2，点d到直线ne的距离设为l3因此：

分别对o点和m点取力矩平衡，得到公式：

公式中是力f1对o点的力臂，是力f3对m点的力臂，可以通过上述的姿态计算获得，gq是动臂的重力，gs是铲斗的重力；

装载机作业过程中噪声大，并且存在摩擦、简化计算等引起的误差需要进行补偿计算，同时为了保证计算的实时性和一步预测性，这里采用卡尔曼滤波算法对铲斗受力进行计算；

卡尔曼滤波公式为：

x(k)＝a·x(k-1)+w(k)

z(k)＝h·x(k)+v(k)

公式中x为状态量，a为状态矩阵，w为状态噪声，z为观测量，h为观测矩阵，v为观测噪声；

其中：

a＝e，其中，e为单位矩阵；

状态噪声w和观测噪声v分别由车辆模型和传感器决定，需要进行实际测量获取；在卡尔曼滤波中，状态噪声w和观测噪声v这两个变量是要通过滤波算法被过滤掉的量；

下面进行卡尔曼滤波算法公式推导：

xk|k-1＝axk-1|k-1············(1)

＝xk-1|k-1

式中，xk|k-1是上一时刻的状态对现在时刻状态的预测，xk-1|k-1是上一时刻状态的最优结果，其中

代入本技术方案索要推到的铲斗在重力方向的受力fy和在水平前进方向的受力fx后，

pk|k-1＝apk-1|k-1a′+q1········(2)

pk-1|k-1是xk-1|k-1的协方差矩阵，p0|0＝p0＝e。

q1是对测量方程的置信系数矩阵，该值越大，表示对测量方程的依赖度越强，再次模型中，取值如下。

代入后：

在实际应用中，根据不同的车型重量，q1的取值可以适当调整。

kk＝pk|k-1h′/(hpk|k-1h′)+q2··············(3)

其中：

q2是对测量方程的置信系数矩阵，该值越大，表示对测量方程的依赖度越强，再次模型中，取值如下：

在实际应用中，根据不同的车型重量，q2的取值可以适当调整。

kk是卡尔曼滤波增益；

xk|k＝xk-1|k-1+kkzk-hxk|k-1·········(4)

式中xk|k是最优卡尔曼滤波值

zk是k时刻测量得到的z矩阵

pk|k＝(e-kkh)pk|k-1···········(5)

通过上述公式(1)、(2)、(3)、(4)和(5)，进行迭代计算，即可推导出铲斗在重力方向的受力fy和在水平前进方向的受力fx。