[发明专利]一种基于卡尔曼滤波算法的装载机铲斗受力测试方法

权利要求书

1.一种基于卡尔曼滤波算法的装载机铲斗受力测试方法，其特征在于：

所述方法具体过程如下：

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

在动臂缸上安装的第一压力传感器测得动臂缸的管路压力为P₁，在摇臂缸上安装的第二压力传感器测得摇臂缸的管路压力为P₂，动臂缸出力F₁与摇臂缸出力F₂由以下公式获得：

其中，动臂缸液压杆的直径为d₁，摇臂缸液压杆的直径为d₂，动臂缸和摇臂缸的机械效率均为η_m，d₁、d₂、η_m均为设计值；

步骤二：测量动臂缸与摇臂缸的位移，并计算出铲斗工作位置姿态；令动臂与前车架安装铰点的回转中心为O点，令动臂缸与前车架铰接点回转中心为A点，令动臂缸与动臂铰接点回转中心为B点，令动臂质心为Q点，令动臂与铲斗铰接点回转中心为M点，令摇臂缸与摇臂铰接点回转中心为C点，令摇臂与动臂铰接点回转中心为D点，令摇臂与连杆铰接点回转中心为E点，令连杆与铲斗铰接点回转中心为N点，令铲斗质心为S点，铲斗去除斗齿后的斗尖位置为受力位置，令其为U点；其中，点B、C、D、E、M、N、Q、S和U均为动点；

点A、B、C、D、E、N、M、Q、S和U坐标分别为(i_x，i_y)，其中i＝A、B、C、D、E、N、M、Q、S、U；

以O点为坐标原点，以重力方向为x方向，以水平向前方向为y方向，令动臂缸行程最短、摇臂缸行程最长位置为初始位置，令各个动点初始坐标分别为B_{x_0}、B_{y_0}、C_{x_0}、C_{y_0}、D_{x_0}、D_{y_0}、E_{x_0}、E_{y_0}M_{x_0}、M_{y_0}、N_{x_0}、N_{y_0}、Q_{x_0}、Q_{y_0}、S_{x_0}、S_{y_0}、U_{x_0}、U_{y_0}，各个动点初始坐标均为设计值；

通过点A、B、C、D、E、N、M、Q、S和U的坐标表示铲斗的工作位置姿态，各点坐标的计算过程如下：

∠AOB的变化值在三角形ΔAOB中得：

Δ∠AOB＝∠AOB-∠AOB_(l_AB＝min(l_AB))

Δ∠AOB为∠AOB在任意位置的变化值；

Q点坐标如下：

Q_y＝l_QO·sin(∠QOA)

Q_x＝l_QO·cos(∠QOA)

M点的坐标如下：

M_y＝l_MO·sin(∠MOA)

M_x＝l_MO·cos(∠MOA)

D点坐标如下：

D_y＝l_DO·sin(∠DOA)

D_x＝l_DO·cos(∠DOA)

C点坐标如下：

C_y＝l_DO·sin(∠COA)

C_x＝l_DO·cos(∠COA)

E点坐标如下：

在初始位置时于三角形ΔMDE中求解∠MDE的初值∠MDE_0为：

在初始位置中：

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

∠MDE的变化值Δ∠MDE由∠CDO的变化值Δ∠CDO得到：

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

其中：

A＝2·l_DE·l_NM·sin(∠MDE)

B＝2·l_NM·(l_DE·cos(∠MDE)-l_DM)

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

那么，∠MND的变化值Δ∠MND表征为：

Δ∠MND＝∠MND-∠MND_0

同时，Δ∠MND亦表征∠SMD的变化值Δ∠SMD和∠UMD的变化值Δ∠UMD，即：

Δ∠SMD＝Δ∠MND

Δ∠UMD＝Δ∠MND

由此，S点的坐标为：

U点坐标为：

N点坐标为：

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

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

在ΔOCD和ΔNED中通过几何原理求解点D到直线OC的距离，设为l₂，点D到直线NE的距离设为l₃，因此：

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

公式中是力F₁对O点的力臂，是力F₃对M点的力臂，以通过上述的姿态计算获得，G_Q是动臂的重力，G_S是铲斗的重力；

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

卡尔曼滤波算法公式为：

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分别通过实际测量获取，并在卡尔曼滤波计算过程中被过滤；

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

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

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

P_k|k-1＝AP_k-1|k-1A′+Q₁··············(2)

P_k-1|k-1是X_k-1|k-1的协方差矩阵，P_0|0＝P₀＝E；

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

代入后：

在实际应用中，根据不同的车型重量，Q₁的取值适当调整；

K_k＝P_k|k-1H′/(HP_k|k-1H′)+Q₂·················(3)

其中：

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

在实际应用中，根据不同的车型重量，Q₂的取值适当调整；

K_k是卡尔曼滤波增益；

X_k|k＝X_k-1|k-1+K_kZ_k-HX_k|k-1·················(4)

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

Z_k是k时刻测量得到的Z矩阵

P_k|k＝(E-K_kH)P_k|k-1······················(5)

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