CN102620890B - 铰接式装载机动态重心位置检测系统及方法 - Google Patents

Abstract

本发明公开了一种装载机动态重心位置的检测系统及方法，所述系统主要由工作载荷检测模块I、工作装置姿态检测模块II、前后车体相对位置检测模块III、后桥位置检测模块IV、燃油余量检测模块V、司机重量检测模块VI、数据处理模块VII和铰接式装载机VIII组成，所述的工作载荷检测模块I、工作装置姿态检测模块II、前后车体相对位置检测模块III、后桥位置检测模块IV、燃油余量检测模块V、司机重量检测模块VI通过电缆与数据处理模块VII相连，数据处理模块VII固定在铰接式装载机VIII的驾驶室d内。本发明克服了传统技术只能检测静态重心位置的缺陷，并能适应各种机型，具有较好的通用性。

Description

铰接式装载机动态重心位置检测系统及方法

技术领域

本专利属于测试技术领域，具体涉及一种铰接式装载机动态重心位置检测系统及方法。

背景技术

铰接式装载机的重心位置的确定在主动安全技术的发展中具有举足轻重的作用。而主动安全技术要求在任意时刻、任意状态下都能得到装载机重心的准确位置，因此对铰接式装载机重心的实时动态检测是十分必要的。

现有技术中对装载机重心位置的测量是将装载机静置于试验台上，通过转动试验台测定装载机的重心位置。在测试的过程中，装载机不能产生运动，即检测出的是装载机的静态重心位置。同时这种检测方法需要有较大的实验平台，造价较高且检测地点有限制。而由于铰接式装载机在转向时前、后车体之间发生了相对转动；同时，装载机工作载荷会发生较大变化、驾驶室内司乘人员变化以及燃油系统的变化使得装载机的重心位置并不是一成不变的。而在铰接式车辆主动防倾翻技术研究中，必须知道装载机重心的实时位置。因此，就不能直接用静态的重心位置来代替动态重心位置。

发明内容

本发明的目的是提供一种装载机动态重心位置的检测系统及方法，克服传统技术只能检测静态重心位置的缺陷，并能适应各种机型，具有较好的通用性。

本发明为解决上述技术问题，通过以下技术方案实现：

一种铰接式装载机动态重心位置检测系统，主要由工作载荷检测模块Ⅰ、工作装置姿态检测模块Ⅱ、前后车体相对位置检测模块Ⅲ、后桥位置检测模块Ⅳ、燃油余量检测模块Ⅴ、司机重量检测模块Ⅵ、数据处理模块Ⅶ和铰接式装载机Ⅷ组成，所述的工作载荷检测模块Ⅰ、工作装置姿态检测模块Ⅱ、前后车体相对位置检测模块Ⅲ、后桥位置检测模块Ⅳ、燃油余量检测模块Ⅴ、司机重量检测模块Ⅵ通过电缆与数据处理模块Ⅶ相连，数据处理模块Ⅶ固定在铰接式装载机Ⅷ的驾驶室d内。

所述的工作载荷检测模块Ⅰ由铲斗下铰点力传感器1、摇臂缸液压传感器2和A/D转换器甲3组成，所述的铲斗下铰点力传感器1能测量力的大小及方向，并安装在动臂与铲斗相连的下铰点q处，摇臂缸液压传感器2选用输出信号为电压的三线制液压传感器，并固定在摇臂缸f的供油回路上。所述的A/D转换器甲3具有高转换速率特性。

因为各机型的液压系统存在差异，各个液压缸之间的压力有可能有差别，本专利选择多个液压传感器，可以加大其适用范围，增强了通用性。

所述的工作装置姿态检测模块Ⅱ由动臂缸直线位移传感器4、摇臂缸直线位移传感器5和A/D转换器乙6组成，所述的动臂缸直线位移传感器4固定在动臂缸j的缸筒与缸杆之间，摇臂缸直线位移传感器5固定在摇臂缸f的缸筒与缸杆之间。动臂缸直线位移传感器4的轴线与动臂缸j的缸筒轴线平行，摇臂缸直线位移传感器5的轴线与摇臂缸f的缸筒轴线平行。

所述的前后车体相对位置检测模块Ⅲ由转向缸直线位移传感器7和A/D转换器丙8组成，所述的转向缸直线位移传感器7固定在转向缸L的缸筒与缸杆之间。转向缸直线位移传感器7的轴线与转向缸L的缸筒轴线平行。该模块也可以由角位移传感器和A/D转换器组成，所述的角位移传感器安装在前车与后车的角耳上。角位移传感器轴线与转向销轴轴线平行。

所述的后桥位置检测模块Ⅳ由摆动销角位移传感器9和A/D转换器丁10组成，所述的摆动销角位移传感器9安装在摆动销a处。摆动销角位移传感器9的轴线与摆动销a轴线平行。

所述的燃油余量检测模块Ⅴ由直接采用车辆自带的燃油余量检测系统11和信号调理电路12组成，所述的信号调理电路12可以将自带的燃油余量检测系统11的输出信号转化为数据采集模块Ⅶ兼容的数据信号。

所述的司机重量检测模块Ⅵ由司机重力传感器13和A/D转换器戊14组成，所述的司机重力传感器13安装在座椅c上。

所述的数据处理模块Ⅶ由数据采集卡15、计算机16、显示仪17和外壳18组成，所述的数据采集卡15选用多路高频数据采集卡。所述的计算机16配备有高速计算核心，可以及时处理采集到的数据。所述的外壳18将数据采集卡15、计算机16和显示仪17整合在一起，并与铰接式装载机Ⅷ固连。

所述的铰接式装载机Ⅷ由前车e、后车b、后桥m和工作装置g组成，前车e和后车b之间采用旋转铰接，并通过转向缸L的伸缩改变其相对位置，后车b和后桥m之间采用旋转铰接，工作装置g通过动臂缸j和摇臂缸f与前车e相连，控制动臂缸j和摇臂缸f的伸缩即可改变工作装置g的姿态。

一种铰接式装载机动态重心位置检测方法，包括以下步骤：

（一）将所述的铰接式装载机动态重心位置检测系统中的各模块正确安装在待测的铰接式装载机Ⅷ上的相应位置，并进行连接和标定；

（二）启动铰接式装载机动态重心位置检测系统，并检查各模块工作是否正常；

（三）根据待测的铰接式装载机Ⅷ的型号以及设计参数，可得其以下各部件的重量：前车重量M₄，后车重量M₅，后桥重量M₆，铲斗重量M₇和动臂重量M₈；以及以下各部件的静态重心位置：前车静态重心

后车静态重心

后桥静态重心

铲斗静态重心

和动臂静态重心

将上述各部件的重量和静态重心位置输入到计算机16中；

（四）启动待测的铰接式装载机Ⅷ；

（五）利用各模块对相应部件进行动态重心的实时计算：工作载荷检测模块Ⅰ联合工作装置姿态检测模块Ⅱ计算动臂动态重心

铲斗动态重心

工作载荷的重量M₁以及工作载荷动态重心

前后车体相对位置检测模块Ⅲ计算前车动态重心

后桥位置检测模块Ⅳ计算后桥动态重心燃油余量检测模块Ⅴ计算燃油重量M₂及燃油动态重心

司机重量检测模块Ⅵ计算司机重量M₃及司机动态重心

（六）数据处理模块Ⅶ将所有模块的数据进行处理，并得到整车重量M和整车动态重心位置P(X,Y,Z)。

具体地，本发明的中各部件的动态重心位置以及整车动态重心位置（即上述步骤四和步骤五）通过以下方法实现。

（A）建立铰接式装载机动态重心位置计算坐标系：o_bx_by_bz_b为后车b随动坐标系，坐标原点o_b位于转向销k中心，x_b指向车辆前进方向，y_b指向前进方向的左侧，z_b指向上方；o_ex_ey_ez_e为前车e随动坐标系，初始时刻与o_bx_by_bz_b重合，且在运动过程中会与o_bx_by_bz_b产生绕z_b轴的角度，大小为转向角θ_e；o_ix_iy_iz_i为动臂i随动坐标系，坐标原点位于动臂i与前车e的铰接点中心，且在运动过程中会与o_ex_ey_ez_e产生绕y_e轴的角度θ_i；o_hx_hy_hz_h为铲斗h随动坐标系，坐标原点位于铲斗h与动臂i的铰接点中心，且在运动过程中会与o_ix_iy_iz_i产生绕y_i轴的角度θ_h；o_mx_my_mz_m为后桥m随动坐标系，坐标原点位于摆动销a的中心，且在运动过程中会与o_bx_by_bz_b产生绕x_b轴的角度θ_m。

对于铰接式装载机Ⅷ而言，前车重量M₄，后车重量M₅，后桥重量M₆，铲斗重量M₇和动臂重量M₈以及他们的静态重心位置相对与其各自的自身随动坐标系是不变的。而工作载荷重量M₁，燃油重量M₂和司机重量M₃的大小是变化的，且重心位置不固定，因此需要检测。

以后车b随动坐标系为参考坐标系，那么除后车b之外的其他各部分的重心位置都是可变的，此时的重心位置就是动态重心。因此在计算时，只要将各部分的静态重心进行坐标变换得到关于后车b随动坐标系的动态重心再进行合成即可得到整车相对后车b随动坐标系动态重心。

（B）根据坐标变换的原理，可以得到各坐标系之间的转换矩阵；

将前车e随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵为

$T_e^b=\left(\begin{array}{cc}R_e^b& D_e^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_e& -\sin {\mathrm{\θ}}_e& 0& 0\\ \sin {\mathrm{\θ}}_e& \cos {\mathrm{\θ}}_e& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(1\right)$

式中，

为前车e随动坐标系到后车b随动坐标系的旋转矩阵，

为前车e随动坐标系到后车b随动坐标系的平移矩阵。

将动臂i随动坐标系中的点坐标转换到前车e随动坐标系的转换矩阵为

$T_i^e=\left(\begin{array}{cc}R_i^e& D_i^e\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_i& 0& \sin {\mathrm{\θ}}_i& L_i\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_i& 0& \cos {\mathrm{\θ}}_i& H_i\\ 0& 0& 0& 1\end{array}\right)---\left(2\right)$

式中，

为动臂i随动坐标系到前车e随动坐标系的旋转矩阵，

为动臂i随动坐标系到前车e随动坐标系的平移矩阵，L_i为动臂i随动坐标系原点o_i在前车e随动坐标系中的x_e坐标值，H_i为动臂i随动坐标系原点o_i在前车e随动坐标系中的z_e坐标值。

将铲斗h随动坐标系中的点坐标转换到动臂i随动坐标系的转换矩阵为

$T_h^i=\left(\begin{array}{cc}R_h^i& D_h^i\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_h& 0& \sin {\mathrm{\θ}}_h& L_h\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_h& 0& \cos {\mathrm{\θ}}_h& H_h\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

式中，

为铲斗h随动坐标系到动臂i随动坐标系的旋转矩阵，

为铲斗h随动坐标系到动臂i随动坐标系的平移矩阵，L_h为铲斗h随动坐标系原点o_h在动臂i随动坐标系中的x_i坐标值，H_h为铲斗h随动坐标系原点o_h在动臂i随动坐标系中的z_i坐标值。

将后桥m随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵为

$T_m^b=\left(\begin{array}{cc}R_m^b& D_m^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}1& 0& 0& L_m\\ 0& \cos {\mathrm{\θ}}_m& -\sin {\mathrm{\θ}}_m& 0\\ 0& \sin {\mathrm{\θ}}_m& \cos {\mathrm{\θ}}_m& H_m\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

式中，

为后桥m随动坐标系到后车b随动坐标系的旋转矩阵，

为后桥m随动坐标系到后车b随动坐标系的平移矩阵，L_m为后桥m随动坐标系原点o_m在后车b随动坐标系中的x_b坐标值，H_m为后桥m随动坐标系原点o_m在后车b随动坐标系中的z_b坐标值。

根据式（1）和式（2）可以推知动臂i随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵

$T_i^b=T_e^{b}T_i^e---\left(5\right)$

根据式（3）和式（5）可以推知铲斗h随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵

$T_h^b=T_i^{b}T_h^i=T_e^{b}T_i^{e}T_h^i---\left(6\right)$

（C）计算各部件的重量和各部件的动态重心位置；

（1）计算动臂动态重心

及铲斗动态重心

通过工作装置姿态检测模块Ⅱ可以得到动臂缸j和摇臂缸f的运动状态，同时根据工作装置各铰点间的位置关系，就可以得到动臂i相对前车e的转角θ_i以及铲斗h相对动臂i的转角θ_h，同时已经得到动臂i的重量M_i及重心位置在动臂i随动坐标系中的坐标

和铲斗h的重量M_h及重心位置在铲斗h随动坐标系中的坐标

那么可以得到动臂i的静态重心

转换到后车b随动坐标系中的坐标

$P_i^b=T_i^{b}P_i^i=T_e^{b}T_i^{e}P_i^i---\left(7\right)$

同样可以得到铲斗h的静态重心

转换到后车b随动坐标系中的坐标

$P_h^b=T_h^{b}P_i^i=T_e^{b}T_i^{e}T_h^{i}P_h^h---\left(8\right)$

（2）计算工作载荷的重量M₁以及工作载荷动态重心

通过工作载荷检测模块Ⅰ得到的数据可以得到铲斗h下铰点q的受力F_q及F_q的方向以及摇臂t上与摇臂缸f铰接处p的受力F_p。

通过受力分析得到铲斗h上与拉杆s铰接点r的受力F_h：

F_h＝K_2hF_p        （9）

其中K_2h是根据工作装置姿态检测模块Ⅱ所得的数据以及力矩平衡原理所得，通过几何分析即可求出。

可以认为在运动过程中，工作载荷的重心位置相对铲斗h不发生变化且位于铲斗h的o_hx_hz_h内。那么就可以得到铲斗h绕下铰点q的角速度ω_h和角加速度α_h：

$\left\{\begin{array}{c}{\mathrm{\ω}}_h=K_{\mathrm{\ωh}}\frac{{\mathrm{\ΔS}}_f}{\mathrm{\Δt}}\\ {\mathrm{\α}}_h=K_{\mathrm{\αh}}\frac{{\mathrm{\Δ\ω}}_h}{\mathrm{\Δt}}\end{array}\right.---\left(10\right)$

其中K_ωh是由摇臂缸f的伸缩速度转换到铲斗h绕下铰点q的角速度的系数，K_αh是由摇臂缸f的伸缩加速度转换到铲斗h绕下铰点q的角加速度的系数，K_ωh和K_αh都可以根据几何分析求得；ΔS_f为相邻时刻工作装置姿态检测模块Ⅱ中的摇臂缸直线传感器5所测得数据的差值，Δt为采样时间间隔。

以及动臂i绕上铰点o的角速度ω_i和角加速度α_i：

$\left\{\begin{array}{c}{\mathrm{\ω}}_i=K_{\mathrm{\ωi}}\frac{{\mathrm{\ΔS}}_j}{\mathrm{\Δt}}\\ {\mathrm{\α}}_i=K_{\mathrm{\αi}}\frac{{\mathrm{\Δ\ω}}_i}{\mathrm{\Δt}}\end{array}\right.---\left(11\right)$

其中K_ωi是由动臂缸j的伸缩速度转换到动臂i绕上铰点o的角速度的系数，K_αi是由动臂缸j的伸缩加速度转换到动臂j绕上铰点o的角加速度的系数，K_ωi和K_αi都可以根据几何分析求得。ΔS_j为相邻时刻工作装置姿态检测模块Ⅱ中的动臂缸直线传感器4所测得数据的差值，Δt为采样时间间隔。

由此可以利用矢量合成的方法将铲斗h绕上铰点o的角加速度α₇及工作载荷绕上铰点o的角加速度α₁表示出来：

$\left\{\begin{array}{c}{\mathrm{\α}}_7=K_{a7h}{\mathrm{\α}}_h+K_{a7i}{\mathrm{\α}}_i\\ {\mathrm{\α}}_1=K_{a1h}{\mathrm{\α}}_h+K_{a1i}{\mathrm{\α}}_i\end{array}\right.---\left(12\right)$

式中K_a7h为铲斗h绕下铰点q的角加速度α_h转化为铲斗h绕上铰点o的角加速度的系数，K_a7i为动臂i绕上铰点o的角加速度α_i转化为铲斗h绕上铰点o的角加速度的系数，K_a1h为铲斗h绕下铰点q的角加速度α_h转化为工作载荷绕上铰点o的角加速度的系数，K_a1i为动臂i绕上铰点o的角加速度α_i转化为工作载荷绕上铰点o的角加速度的系数。

那么将铲斗h和工作载荷作为研究对象，列x_b方向上和z_b方向上的力平衡方程及关于下铰点q的力矩平衡方程：

$\left\{\begin{array}{c}\mathrm{\Σ}{F}_{x_b}=0:F_h\cos {\mathrm{\θ}}_s+F_q\cos {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\cos {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\cos {\mathrm{\θ}}_7=0\\ \mathrm{\Σ}{F}_{z_b}=0:F_h\sin {\mathrm{\θ}}_s+F_q\sin {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\sin {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\sin {\mathrm{\θ}}_7+M_{1}g+M_{7}g=0\\ \mathrm{\Σ}{T}_q=0:F_h\cos {\mathrm{\θ}}_s(z_r-z_q)+F_h\sin {\mathrm{\θ}}_s(x_r-x_q)+M_{1}g(x_1-x_q)+M_{7}g(x_7-x_q)\\ +M_1{\mathrm{\α}}_h{D}^2_1^q+M_7{\mathrm{\α}}_h{D}^2_h^q=0\end{array}\right.---\left(13\right)$

其中θ_s为拉杆s与x_b的夹角，θ_q为力F_q与x_b的夹角，M₁为工作载荷的重量，

为工作载荷动态重心

与铰接点q的距离，θ₁为工作载荷动态重心

处的加速度与x_b的夹角，M₇为铲斗h的重量，

为铲斗h动态重心

与铰接点q的距离，θ₇为铲斗动态重心

处的加速度与x_b的夹角，g为重力加速度，z_r为铰接点r在后车随动坐标系中的竖向坐标值，z_q为铰接点q在后车随动坐标系中的竖向坐标值，x_r为铰接点r在后车随动坐标系中的前进方向坐标值，x_q为铰接点q在后车随动坐标系中的前进方向坐标值。

式（12）中的未知数为三个即工作载荷的重量M₁，动态重心的x₁坐标以及z₁坐标。求解即可得到工作载荷的重量M₁以及动态重心

（3）计算前车动态重心

通过前后车体相对位置检测模块Ⅲ可以得到前车e随动坐标系相对后车b随动坐标系的转角θ_e，继而可以根据坐标转换矩阵得到前车e静态重心转换到后车b随动坐标系中的坐标

$P_e^b=T_e^{b}P_e^e---\left(14\right)$

（4）计算燃油重量M₂及燃油动态重心

通过燃油余量检测模块以及燃油密度可以得到燃油的重量M₂。而油箱n为较规则的形状，因此其动态重心

可以根据体积进行估算。

（5）计算司机重量M₃及司机动态重心

通过司机重量检测模块Ⅵ可以得到司机重量M₃，同时根据普通成人的统计数据可以得到司机的动态重心

（6）计算后桥动态重心

通过后桥位置检测模块Ⅵ可以得到后桥m随动坐标系相对后车b随动坐标系的转角θ_m，继而可以根据坐标转换矩阵得到后桥m静态重心转换到后车b随动坐标系中的坐标

$P_m^b=T_m^{b}P_m^m---\left(15\right)$

（D）计算整车重量M和整车动态重心位置P(X,Y,Z)

至此，我们得到了所需要的所有部分的重量及其动态重心位置，就可以得到整车重量M并根据重心合成原理计算整车动态重心位置P(X,Y,Z)。

M＝M₁+M₂+M₃+M₄+M₅+M₆+M₇+M₈           （16）

$\left\{\begin{array}{cc}X=\frac{\mathrm{\Σ}{x}_u{M}_u}{\mathrm{\Σ}{M}_u}& \\ Y=\frac{\mathrm{\Σ}{y}_u{M}_u}{\mathrm{\Σ}{M}_u}& (u=1-8)\\ Z=\frac{\mathrm{\Σ}{z}_u{M}_u}{\mathrm{\Σ}{M}_u}& \end{array}\right.---\left(17\right)$

其中M₁为工作载荷重量，M₂为燃油重量，M₃为司机重量，M₄为前车重量，M₅为后车重量，M₆为后桥重量，M₇为铲斗重量，M₈为动臂重量；x_u，y_u，z_u为对应各部分的重心转换到后车随动坐标系中的坐标值。

本发明的有益效果：本发明可以对铰接式装载机的重心位置进行动态实时检测，克服了只能对其静态重心位置进行检测的局限。同时，提供了模块组成的可行冗余选择，方便了系统的搭建。另外，本发明得到的检测结果是工程车辆主动防倾翻技术实现的必要条件。可见本发明通用性强，节省了专用设备的成本投入，且操作简单，具有很好的应用前景。

附图说明

图1为铰接式装载机动态重心位置检测系统图

图2（a）为铰接式装载机结构简图

图2（b）为铰接式装载机的工作装置示意图

图3为铰接式装载机动态重心位置检测系统安装位置示意图

图4为铰接式装载机动态重心位置计算坐标系

图5为工作载荷重量和重心位置计算简图

图中：

a、摆动销 b、后车 c、座椅 d、驾驶室 e、前车 f、摇臂缸 g、工作装置

h、铲斗 i、动臂 j、动臂缸 k、转向销 L、转向缸 m、后桥 n、油箱 s、拉杆

t、摇臂 o、动臂i与前车e铰接点 p、摇臂缸f与摇臂t铰接点

q、动臂i与铲斗h铰接点 r、拉杆与铲斗铰接点

具体实施方式

下面结合附图所示实施例进一步说明本发明的具体内容和工作过程。

图1为铰接式装载机动态重心位置检测系统图，主要由工作载荷检测模块Ⅰ、工作装置姿态检测模块Ⅱ、前后车体相对位置检测模块Ⅲ、后桥位置检测模块Ⅳ、燃油余量检测模块Ⅴ、司机重量检测模块Ⅵ、数据处理模块Ⅶ和铰接式装载机Ⅷ组成，所述的工作载荷检测模块Ⅰ、工作装置姿态检测模块Ⅱ、前后车体相对位置检测模块Ⅲ、后桥位置检测模块Ⅳ、燃油余量检测模块Ⅴ、司机重量检测模块Ⅵ通过电缆与数据处理模块Ⅶ相连，数据处理模块Ⅶ固定在铰接式装载机Ⅷ的驾驶室d内。

图3为铰接式装载机动态重心位置检测系统安装位置示意图

所述的工作载荷检测模块Ⅰ由铲斗下铰点力传感器1、摇臂缸液压传感器2和A/D转换器甲3组成，所述的铲斗下铰点力传感器1能测量力的大小及方向，并安装在动臂与铲斗相连的下铰点q处，摇臂缸液压传感器2选用输出信号为电压的三线制液压传感器，并固定在摇臂缸f的供油回路上。所述的A/D转换器甲3具有高转换速率特性。

因为各机型的液压系统存在差异，各个液压缸之间的压力有可能有差别，本专利选择多个液压传感器，可以加大其适用范围，增强了通用性。

所述的工作装置姿态检测模块Ⅱ由动臂缸直线位移传感器4、摇臂缸直线位移传感器5和A/D转换器乙6组成，所述的动臂缸直线位移传感器4固定在动臂缸j的缸筒与缸杆之间，摇臂缸直线位移传感器5固定在摇臂缸f的缸筒与缸杆之间。动臂缸直线位移传感器4的轴线与动臂缸j的缸筒轴线平行，摇臂缸直线位移传感器5的轴线与摇臂缸f的缸筒轴线平行。

所述的前后车体相对位置检测模块Ⅲ由转向缸直线位移传感器7和A/D转换器丙8组成，所述的转向缸直线位移传感器7固定在转向缸L的缸筒与缸杆之间。转向缸直线位移传感器7的轴线与转向缸L的缸筒轴线平行。该模块也可以由角位移传感器和A/D转换器组成，所述的角位移传感器安装在前车与后车的角耳上。角位移传感器轴线与转向销轴轴线平行。

所述的后桥位置检测模块Ⅳ由摆动销角位移传感器9和A/D转换器丁10组成，所述的摆动销角位移传感器9安装在摆动销a处。摆动销角位移传感器9的轴线与摆动销a轴线平行。

所述的燃油余量检测模块Ⅴ由直接采用车辆自带的燃油余量检测系统11和信号调理电路12组成，所述的信号调理电路12可以将自带的燃油余量检测系统11的输出信号转化为数据采集模块Ⅶ兼容的数据信号。

所述的司机重量检测模块Ⅵ由司机重力传感器13和A/D转换器戊14组成，所述的司机重力传感器13安装在座椅c上。

所述的数据处理模块Ⅶ由数据采集卡15、计算机16、显示仪17和外壳18组成，所述的数据采集卡15选用多路高频数据采集卡。所述的计算机16配备有高速计算核心，可以及时处理采集到的数据。所述的外壳18将数据采集卡15、计算机16和显示仪17整合在一起，并与铰接式装载机Ⅷ固连。

图2（a）为铰接式装载机结构简图，图2（b）为铰接式装载机的工作装置示意图，所述的铰接式装载机Ⅷ由前车e、后车b、后桥m和工作装置g组成，前车e和后车b之间采用旋转铰接，并通过转向缸L的伸缩改变其相对位置，后车b和后桥m之间采用旋转铰接，工作装置g通过动臂缸j和摇臂缸f与前车e相连，控制动臂缸j和摇臂缸f的伸缩即可改变工作装置g的姿态。

一种铰接式装载机动态重心位置检测方法，包括以下步骤：

（一）将所述的铰接式装载机动态重心位置检测系统中的各模块正确安装在待测的铰接式装载机Ⅷ上的相应位置，并进行连接和标定；

（二）启动铰接式装载机动态重心位置检测系统，并检查各模块工作是否正常；

（三）根据待测的铰接式装载机Ⅷ的型号以及设计参数，可得其以下各部件的重量：前车重量M₄，后车重量M₅，后桥重量M₆，铲斗重量M₇和动臂重量M₈；以及以下各部件的静态重心位置：前车静态重心

后车静态重心

后桥静态重心铲斗静态重心和动臂静态重心

将上述各部件的重量和静态重心位置输入到计算机16中；

（四）启动待测的铰接式装载机Ⅷ；

（五）利用各模块对相应部件进行动态重心的实时计算：工作载荷检测模块Ⅰ联合工作装置姿态检测模块Ⅱ计算动臂动态重心

铲斗动态重心

工作载荷的重量M₁以及工作载荷动态重心前后车体相对位置检测模块Ⅲ计算前车动态重心

后桥位置检测模块Ⅳ计算后桥动态重心

燃油余量检测模块Ⅴ计算燃油重量M₂及燃油动态重心

司机重量检测模块Ⅵ计算司机重量M₃及司机动态重心

（六）数据处理模块Ⅶ将所有模块的数据进行处理，并得到整车重量M和整车动态重心位置P(X,Y,Z)。

具体地，本发明的中各部件的动态重心位置以及整车动态重心位置（即上述步骤四和步骤五）通过以下方法实现。

（A）建立铰接式装载机动态重心位置计算坐标系：图4为铰接式装载机动态重心位置计算坐标系，o_bx_by_bz_b为后车b随动坐标系，坐标原点o_b位于转向销k中心，x_b指向车辆前进方向，y_b指向前进方向的左侧，z_b指向上方；o_ex_ey_ez_e为前车e随动坐标系，初始时刻与o_bx_by_bz_b重合，且在运动过程中会与o_bx_by_bz_b产生绕z_b轴的角度，大小为转向角θ_e；o_ix_iy_iz_i为动臂i随动坐标系，坐标原点位于动臂i与前车e的铰接点中心，且在运动过程中会与o_ex_ey_ez_e产生绕y_e轴的角度θ_i；o_hx_hy_hz_h为铲斗h随动坐标系，坐标原点位于铲斗h与动臂i的铰接点中心，且在运动过程中会与o_ix_iy_iz_i产生绕y_i轴的角度θ_h；o_mx_my_mz_m为后桥m随动坐标系，坐标原点位于摆动销a的中心，且在运动过程中会与o_bx_by_bz_b产生绕x_b轴的角度θ_m。

对于铰接式装载机Ⅷ而言，前车重量M₄，后车重量M₅，后桥重量M₆，铲斗重量M₇和动臂重量M₈以及他们的静态重心位置相对与其各自的自身随动坐标系是不变的。而工作载荷重量M₁，燃油重量M₂和司机重量M₃的大小是变化的，且重心位置不固定，因此需要检测。

以后车b随动坐标系为参考坐标系，那么除后车b之外的其他各部分的重心位置都是可变的，此时的重心位置就是动态重心。因此在计算时，只要将各部分的静态重心进行坐标变换得到关于后车b随动坐标系的动态重心再进行合成即可得到整车相对后车b随动坐标系动态重心。

（B）根据坐标变换的原理，可以得到各坐标系之间的转换矩阵；

将前车e随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵为

$T_e^b=\left(\begin{array}{cc}R_e^b& D_e^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_e& -\sin {\mathrm{\θ}}_e& 0& 0\\ \sin {\mathrm{\θ}}_e& \cos {\mathrm{\θ}}_e& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(1\right)$

式中，

为前车e随动坐标系到后车b随动坐标系的旋转矩阵，

为前车e随动坐标系到后车b随动坐标系的平移矩阵。

将动臂i随动坐标系中的点坐标转换到前车e随动坐标系的转换矩阵为

$T_i^e=\left(\begin{array}{cc}R_i^e& D_i^e\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_i& 0& \sin {\mathrm{\θ}}_i& L_i\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_i& 0& \cos {\mathrm{\θ}}_i& H_i\\ 0& 0& 0& 1\end{array}\right)---\left(2\right)$

式中，

为动臂i随动坐标系到前车e随动坐标系的旋转矩阵，

为动臂i随动坐标系到前车e随动坐标系的平移矩阵，L_i为动臂i随动坐标系原点o_i在前车e随动坐标系中的x_e坐标值，H_i为动臂i随动坐标系原点o_i在前车e随动坐标系中的z_e坐标值。

将铲斗h随动坐标系中的点坐标转换到动臂i随动坐标系的转换矩阵为

$T_h^i=\left(\begin{array}{cc}R_h^i& D_h^i\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_h& 0& \sin {\mathrm{\θ}}_h& L_h\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_h& 0& \cos {\mathrm{\θ}}_h& H_h\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

式中，为铲斗h随动坐标系到动臂i随动坐标系的旋转矩阵，

为铲斗h随动坐标系到动臂i随动坐标系的平移矩阵，L_h为铲斗h随动坐标系原点o_h在动臂i随动坐标系中的x_i坐标值，H_h为铲斗h随动坐标系原点o_h在动臂i随动坐标系中的z_i坐标值。将后桥m随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵为

$T_m^b=\left(\begin{array}{cc}R_m^b& D_m^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}1& 0& 0& L_m\\ 0& \cos {\mathrm{\θ}}_m& -\sin {\mathrm{\θ}}_m& 0\\ 0& \sin {\mathrm{\θ}}_m& \cos {\mathrm{\θ}}_m& H_m\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

式中，为后桥m随动坐标系到后车b随动坐标系的旋转矩阵，

为后桥m随动坐标系到后车b随动坐标系的平移矩阵，L_m为后桥m随动坐标系原点o_m在后车b随动坐标系中的x_b坐标值，H_m为后桥m随动坐标系原点o_m在后车b随动坐标系中的z_b坐标值。

根据式（1）和式（2）可以推知动臂i随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵

$T_i^b=T_e^{b}T_i^e---\left(5\right)$

根据式（3）和式（5）可以推知铲斗h随动坐标系中的点坐标转换到后车b随动坐标系的转换矩阵

$T_h^b=T_i^{b}T_h^i=T_e^{b}T_i^{e}T_h^i---\left(6\right)$

（C）计算各部件的重量和各部件的动态重心位置；

（1）计算动臂动态重心

及铲斗动态重心

通过工作装置姿态检测模块Ⅱ可以得到动臂缸j和摇臂缸f的运动状态，同时根据工作装置各铰点间的位置关系，就可以得到动臂i相对前车e的转角θ_i以及铲斗h相对动臂i的转角θ_h，同时已经得到动臂i的重量M_i及重心位置在动臂i随动坐标系中的坐标

和铲斗h的重量M_h及重心位置在铲斗h随动坐标系中的坐标

那么可以得到动臂i的静态重心

转换到后车b随动坐标系中的坐标

$P_i^b=T_i^{b}P_i^i=T_e^{b}T_i^{e}P_i^i---\left(7\right)$

同样可以得到铲斗h的静态重心

转换到后车b随动坐标系中的坐标

$P_h^b=T_h^{b}P_i^i=T_e^{b}T_i^{e}T_h^{i}P_h^h---\left(8\right)$

（2）计算工作载荷的重量M₁以及工作载荷动态重心

图5为工作载荷重量和重心位置计算简图，通过工作载荷检测模块Ⅰ得到的数据可以得到铲斗h下铰点q的受力F_q及F_q的方向以及摇臂t上与摇臂缸f铰接处p的受力F_p。

通过受力分析得到铲斗h上与拉杆s铰接点r的受力F_h：

F_h＝K_2hF_p        （9）

其中K_2h是根据工作装置姿态检测模块Ⅱ所得的数据以及力矩平衡原理所得，通过几何分析即可求出。

可以认为在运动过程中，工作载荷的重心位置相对铲斗h不发生变化且位于铲斗h的o_hx_hz_h内。那么就可以得到铲斗h绕下铰点q的角速度ω_h和角加速度α_h：

$\left\{\begin{array}{c}{\mathrm{\ω}}_h=K_{\mathrm{\ωh}}\frac{{\mathrm{\ΔS}}_f}{\mathrm{\Δt}}\\ {\mathrm{\α}}_h=K_{\mathrm{\αh}}\frac{{\mathrm{\Δ\ω}}_h}{\mathrm{\Δt}}\end{array}\right.---\left(10\right)$

其中K_ωh是由摇臂缸f的伸缩速度转换到铲斗h绕下铰点q的角速度的系数，K_αh是由摇臂缸f的伸缩加速度转换到铲斗h绕下铰点q的角加速度的系数，K_ωh和K_αh都可以根据几何分析求得；ΔS_f为相邻时刻工作装置姿态检测模块Ⅱ中的摇臂缸直线传感器5所测得数据的差值，Δt为采样时间间隔。

以及动臂i绕上铰点o的角速度ω_i和角加速度α_i：

$\left\{\begin{array}{c}{\mathrm{\ω}}_i=K_{\mathrm{\ωi}}\frac{{\mathrm{\ΔS}}_j}{\mathrm{\Δt}}\\ {\mathrm{\α}}_i=K_{\mathrm{\αi}}\frac{{\mathrm{\Δ\ω}}_i}{\mathrm{\Δt}}\end{array}\right.---\left(11\right)$

其中K_ωi是由动臂缸j的伸缩速度转换到动臂i绕上铰点o的角速度的系数，K_αi是由动臂缸j的伸缩加速度转换到动臂j绕上铰点o的角加速度的系数，K_ωi和K_αi都可以根据几何分析求得。ΔS_j为相邻时刻工作装置姿态检测模块Ⅱ中的动臂缸直线传感器4所测得数据的差值，Δt为采样时间间隔。

由此可以利用矢量合成的方法将铲斗h绕上铰点o的角加速度α₇及工作载荷绕上铰点o的角加速度α₁表示出来：

$\left\{\begin{array}{c}{\mathrm{\α}}_7=K_{a7h}{\mathrm{\α}}_h+K_{a7i}{\mathrm{\α}}_i\\ {\mathrm{\α}}_1=K_{a1h}{\mathrm{\α}}_h+K_{a1i}{\mathrm{\α}}_i\end{array}\right.---\left(11\right)$

式中K_a7h为铲斗h绕下铰点q的角加速度α_h转化为铲斗h绕上铰点o的角加速度的系数，K_a7i为动臂i绕上铰点o的角加速度α_i转化为铲斗h绕上铰点o的角加速度的系数，K_a1h为铲斗h绕下铰点q的角加速度α_h转化为工作载荷绕上铰点o的角加速度的系数，K_a1i为动臂i绕上铰点o的角加速度α_i转化为工作载荷绕上铰点o的角加速度的系数。

那么将铲斗h和工作载荷作为研究对象，列x_b方向上和z_b方向上的力平衡方程及关于下铰点q的力矩平衡方程：

$\left\{\begin{array}{c}\mathrm{\Σ}{F}_{x_b}=0:F_h\cos {\mathrm{\θ}}_s+F_q\cos {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\cos {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\cos {\mathrm{\θ}}_7=0\\ \mathrm{\Σ}{F}_{z_b}=0:F_h\sin {\mathrm{\θ}}_s+F_q\sin {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\sin {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\sin {\mathrm{\θ}}_7+M_{1}g+M_{7}g=0\\ \mathrm{\Σ}{T}_q=0:F_h\cos {\mathrm{\θ}}_s(z_r-z_q)+F_h\sin {\mathrm{\θ}}_s(x_r-x_q)+M_{1}g(x_1-x_q)+M_{7}g(x_7-x_q)\\ +M_1{\mathrm{\α}}_h{D}^2_1^q+M_7{\mathrm{\α}}_h{D}^2_h^q=0\end{array}\right.---\left(12\right)$

其中θ_s为拉杆s与x_b的夹角，θ_q为力F_q与x_b的夹角，M₁为工作载荷的重量，

为工作载荷动态重心

与铰接点q的距离，θ₁为工作载荷动态重心

处的加速度与x_b的夹角，M₇为铲斗h的重量，

为铲斗h动态重心

与铰接点q的距离，θ₇为铲斗动态重心处的加速度与x_b的夹角，g为重力加速度，z_r为铰接点r在后车随动坐标系中的竖向坐标值，z_q为铰接点q在后车随动坐标系中的竖向坐标值，x_r为铰接点r在后车随动坐标系中的前进方向坐标值，x_q为铰接点q在后车随动坐标系中的前进方向坐标值。

式（12）中的未知数为三个即工作载荷的重量M₁，动态重心

的x₁坐标以及z₁坐标。求解即可得到工作载荷的重量M₁以及动态重心

（3）计算前车动态重心

通过前后车体相对位置检测模块Ⅲ可以得到前车e随动坐标系相对后车b随动坐标系的转角θ_e，继而可以根据坐标转换矩阵得到前车e静态重心

转换到后车b随动坐标系中的坐标

$P_e^b=T_e^{b}P_e^e---\left(13\right)$

（4）计算燃油重量M₂及燃油动态重心

通过燃油余量检测模块以及燃油密度可以得到燃油的重量M₂。而油箱n为较规则的形状，因此其动态重心

可以根据体积进行估算。

（5）计算司机重量M₃及司机动态重心

通过司机重量检测模块Ⅵ可以得到司机重量M₃，同时根据普通成人的统计数据可以得到司机的动态重心

（6）计算后桥动态重心

通过后桥位置检测模块Ⅵ可以得到后桥m随动坐标系相对后车b随动坐标系的转角θ_m，继而可以根据坐标转换矩阵得到后桥m静态重心

转换到后车b随动坐标系中的坐标

$P_m^b=T_m^{b}P_m^m---\left(14\right)$

（D）计算整车重量M和整车动态重心位置P(X,Y,Z)

至此，我们得到了所需要的所有部分的重量及其动态重心位置，就可以得到整车重量M并根据重心合成原理计算整车动态重心位置P(X,Y,Z)。

M＝M₁+M₂+M₃+M₄+M₅+M₆+M₇+M₈      （15）

$\left\{\begin{array}{cc}X=\frac{\mathrm{\Σ}{x}_u{M}_u}{\mathrm{\Σ}{M}_u}& \\ Y=\frac{\mathrm{\Σ}{y}_u{M}_u}{\mathrm{\Σ}{M}_u}& (u=1-8)\\ Z=\frac{\mathrm{\Σ}{z}_u{M}_u}{\mathrm{\Σ}{M}_u}& \end{array}\right.---\left(16\right)$

其中M₁为工作载荷重量，M₂为燃油重量，M₃为司机重量，M₄为前车重量，M₅为后车重量，M₆为后桥重量，M₇为铲斗重量，M₈为动臂重量；x_u，y_u，z_u为对应各部分的重心转换到后车随动坐标系中的坐标值。

Claims (8)

1.一种铰接式装载机动态重心位置检测系统，包括有工作载荷检测模块（Ⅰ）、工作装置姿态检测模块（Ⅱ）、前后车体相对位置检测模块（Ⅲ）、后桥位置检测模块（Ⅳ）、燃油余量检测模块（Ⅴ）、司机重量检测模块（Ⅵ）、数据处理模块（Ⅶ）和铰接式装载机（Ⅷ），其特征在于：

所述的工作载荷检测模块（Ⅰ）、工作装置姿态检测模块（Ⅱ）、前后车体相对位置检测模块（Ⅲ）、后桥位置检测模块（Ⅳ）、燃油余量检测模块（Ⅴ）、司机重量检测模块（Ⅵ）通过电缆与数据处理模块（Ⅶ）相连，数据处理模块（Ⅶ）固定在铰接式装载机（Ⅷ）的驾驶室（d）内；

所述的工作载荷检测模块（Ⅰ）由铲斗下铰点力传感器（1）、摇臂缸液压传感器（2）和A/D转换器甲（3）组成，所述的铲斗下铰点力传感器（1）能测量力的大小及方向，并安装在动臂与铲斗相连的下铰点（q）处，摇臂缸液压传感器（2）固定在摇臂缸（f）的供油回路上；

所述的工作装置姿态检测模块（Ⅱ）由动臂缸直线位移传感器（4）、摇臂缸直线位移传感器（5）和A/D转换器乙（6）组成，所述的动臂缸直线位移传感器（4）固定在动臂缸（j）的缸筒与缸杆之间，摇臂缸直线位移传感器（5）固定在摇臂缸（f）的缸筒与缸杆之间，动臂缸直线位移传感器（4）的轴线与动臂缸（j）的缸筒轴线平行，摇臂缸直线位移传感器（5）的轴线与摇臂缸（f）的缸筒轴线平行。

2.如权利要求1所述的一种铰接式装载机动态重心位置检测系统，其特征在于：所述的前后车体相对位置检测模块（Ⅲ）由转向缸直线位移传感器（7）和A/D转换器丙（8）组成，所述的转向缸直线位移传感器（7）固定在转向缸（L）的缸筒与缸杆之间，转向缸直线位移传感器（7）的轴线与转向缸（L）的缸筒轴线平行。

3.如权利要求1所述的一种铰接式装载机动态重心位置检测系统，其特征在于：所述的后桥位置检测模块（Ⅳ）由摆动销角位移传感器（9）和A/D转换器丁（10）组成，所述的摆动销角位移传感器（9）安装在摆动销（a）处，摆动销角位移传感器（9）的轴线与摆动销（a）轴线平行。

4.如权利要求1所述的一种铰接式装载机动态重心位置检测系统，其特征在于：所述的燃油余量检测模块（Ⅴ）由直接采用车辆自带的燃油余量检测系统（11）和信号调理电路（12）组成，所述的信号调理电路（12）可以将自带的燃油余量检测系统（11）的输出信号转化为数据采集模块（Ⅶ）兼容的数据信号。

5.如权利要求1所述的一种铰接式装载机动态重心位置检测系统，其特征在于：所述的司机重量检测模块（Ⅵ）由司机重力传感器（13）和A/D转换器戊（14）组成，所述的司机重力传感器（13）安装在座椅（c）上。

6.如权利要求1所述的一种铰接式装载机动态重心位置检测系统，其特征在于：所述的数据处理模块（Ⅶ）由数据采集卡（15）、计算机（16）、显示仪（17）和外壳（18）组成，所述的外壳（18）将数据采集卡（15）、计算机（16）和显示仪（17）整合在一起，并与铰接式装载机（Ⅷ）固连。

7.一种铰接式装载机动态重心位置检测方法，包括以下步骤：

（一）将权利要求1至6中任一项所述的铰接式装载机动态重心位置检测系统中的各模块正确安装在待测的铰接式装载机（19）上的相应位置，并进行连接和标定；

（二）启动铰接式装载机动态重心位置检测系统，并检查各模块工作是否正常；

（三）根据待测的铰接式装载机（19）的型号以及设计参数，可得其以下各部件的重量：前车重量M₄，后车重量M₅，后桥重量M₆，铲斗重量M₇和动臂重量M₈；以及以下各部件的静态重心位置：前车静态重心

后车静态重心

后桥静态重心

铲斗静态重心

和动臂静态重心将上述各部件的重量和静态重心位置输入到计算机（16）中；

（四）启动待测的铰接式装载机（19）；

（五）利用各模块对相应部件进行动态重心的实时计算：工作载荷检测模块（Ⅰ）联合工作装置姿态检测模块（Ⅱ）计算动臂动态重心

铲斗动态重心

工作载荷的重量M₁以及工作载荷动态重心

前后车体相对位置检测模块（Ⅲ）计算前车动态重心

后桥位置检测模块（Ⅳ）计算后桥动态重心

燃油余量检测模块（Ⅴ）计算燃油重量M₂及燃油动态重心

司机重量检测模块（Ⅵ）计算司机重量M₃及司机动态重心

（六）数据处理模块（Ⅶ）将所有模块的数据进行处理，并得到整车重量M和整车动态重心位置P(X,Y,Z)。

8.如权利要求7所述的一种铰接式装载机动态重心位置检测方法，其特征在于：所述的步骤（五）和步骤（六）通过以下方法实现：

（A）建立铰接式装载机动态重心位置计算坐标系：o_bx_by_bz_b为后车（b）随动坐标系，坐标原点o_b位于转向销（k）中心，x_b指向车辆前进方向，y_b指向前进方向的左侧，z_b指向上方；o_ex_ey_ez_e为前车（e）随动坐标系，初始时刻与o_bx_by_bz_b重合，且在运动过程中会与o_bx_by_bz_b产生绕z_b轴的角度，大小为转向角θ_e；o_ix_iy_iz_i为动臂（i）随动坐标系，坐标原点位于动臂（i）与前车（e）的铰接点中心，且在运动过程中会与o_ex_ey_ez_e产生绕y_e轴的角度θ_i；o_hx_hy_hz_h为铲斗（h）随动坐标系，坐标原点位于铲斗（h）与动臂（i）的铰接点中心，且在运动过程中会与o_ix_iy_iz_i产生绕y_i轴的角度θ_h；o_mx_my_mz_m为后桥（m）随动坐标系，坐标原点位于摆动销（a）的中心，且在运动过程中会与o_bx_by_bz_b产生绕x_b轴的角度θ_m；

（B）根据坐标变换的原理，可得各坐标系之间的转换矩阵；

（1）将前车（e）随动坐标系中的点坐标转换到后车（b）随动坐标系的转换矩阵为

$T_e^b=\left(\begin{array}{cc}R_e^b& D_e^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_e& -\sin {\mathrm{\θ}}_e& 0& 0\\ \sin {\mathrm{\θ}}_e& \cos {\mathrm{\θ}}_e& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)---\left(1\right)$

式中，为前车（e）随动坐标系到后车（b）随动坐标系的旋转矩阵，

为前车（e）随动坐标系到后车（b）随动坐标系的平移矩阵；

（2）将动臂（i）随动坐标系中的点坐标转换到前车（e）随动坐标系的转换矩阵为

$T_i^e=\left(\begin{array}{cc}R_i^e& D_i^e\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_i& 0& \sin {\mathrm{\θ}}_i& L_i\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_i& 0& \cos {\mathrm{\θ}}_i& H_i\\ 0& 0& 0& 1\end{array}\right)---\left(2\right)$

式中，

为动臂（i）随动坐标系到前车（e）随动坐标系的旋转矩阵，为动臂（i）随动坐标系到前车（e）随动坐标系的平移矩阵，L_i为动臂（i）随动坐标系原点o_i在前车（e）随动坐标系中的x_e坐标值，H_i为动臂（i）随动坐标系原点o_i在前车（e）随动坐标系中的z_e坐标值；

（3）将铲斗（h）随动坐标系中的点坐标转换到动臂（i）随动坐标系的转换矩阵为

$T_h^i=\left(\begin{array}{cc}R_h^i& D_h^i\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\mathrm{\θ}}_h& 0& \sin {\mathrm{\θ}}_h& L_h\\ 0& 1& 0& 0\\ -\sin {\mathrm{\θ}}_h& 0& \cos {\mathrm{\θ}}_h& H_h\\ 0& 0& 0& 1\end{array}\right)---\left(3\right)$

式中，

为铲斗（h）随动坐标系到动臂（i）随动坐标系的旋转矩阵，为铲斗（h）随动坐标系到动臂（i）随动坐标系的平移矩阵，L_h为铲斗（h）随动坐标系原点o_h在动臂（i）随动坐标系中的x_i坐标值，H_h为铲斗（h）随动坐标系原点o_h在动臂（i）随动坐标系中的z_i坐标值；

（4）将后桥（m）随动坐标系中的点坐标转换到后车（b）随动坐标系的转换矩阵为

$T_m^b=\left(\begin{array}{cc}R_m^b& D_m^b\\ 0& 1\end{array}\right)=\left(\begin{array}{cccc}1& 0& 0& L_m\\ 0& \cos {\mathrm{\θ}}_m& -\sin {\mathrm{\θ}}_m& 0\\ 0& \sin {\mathrm{\θ}}_m& \cos {\mathrm{\θ}}_m& H_m\\ 0& 0& 0& 1\end{array}\right)---\left(4\right)$

式中，

为后桥（m）随动坐标系到后车（b）随动坐标系的旋转矩阵，

为后桥（m）随动坐标系到后车（b）随动坐标系的平移矩阵，L_m为后桥（m）随动坐标系原点o_m在后车（b）随动坐标系中的x_b坐标值，H_m为后桥（m）随动坐标系原点o_m在后车（b）随动坐标系中的z_b坐标值；

根据式（1）和式（2）可以推知动臂（i）随动坐标系中的点坐标转换到后车（b）随动坐标系的转换矩阵

$T_i^b=T_e^{b}T_i^e---\left(5\right)$

根据式（3）和式（5）可以推知铲斗（h）随动坐标系中的点坐标转换到后车（b）随动坐标系的转换矩阵

$T_h^b=T_i^{b}T_h^i=T_e^{b}T_i^{e}T_h^i---\left(6\right)$

（C）计算各部件的重量和各部件的动态重心位置；

（1）计算动臂动态重心

及铲斗动态重心

通过工作装置姿态检测模块（Ⅱ）可以得到动臂缸（j）和摇臂缸（f）的运动状态，同时根据工作装置各铰点间的位置关系，就可以得到动臂（i）相对前车（e）的转角θ_i以及铲斗（h）相对动臂（i）的转角θ_h，同时已经得到动臂（i）的重量M_i及重心位置在动臂（i）随动坐标系中的坐标

和铲斗（h）的重量M_h及重心位置在铲斗（h）随动坐标系中的坐标

那么可以得到动臂（i）的静态重心

转换到后车（b）随动坐标系中的坐标

$P_i^b=T_i^{b}P_i^i=T_e^{b}T_i^{e}P_i^i---\left(7\right)$

同样可以得到铲斗（h）的静态重心转换到后车（b）随动坐标系中的坐标

$P_h^b=T_h^{b}P_i^i=T_e^{b}T_i^{e}T_h^{i}P_h^h---\left(8\right)$

（2）计算工作载荷的重量M₁以及工作载荷动态重心

通过工作载荷检测模块（Ⅰ）得到的数据可以得到铲斗（h）下铰点（q）的受力F_q及F_q的方向以及摇臂（t）上与摇臂缸（f）铰接处（p）的受力F_p；

通过受力分析得到铲斗（h）上与拉杆（s）铰接点（r）的受力F_h：

F_h＝K_2hF_p       （9）

其中K_2h是根据工作装置姿态检测模块（Ⅱ）所得的数据以及力矩平衡原理所得，通过几何分析即可求出；

可以认为在运动过程中，工作载荷的重心位置相对铲斗（h）不发生变化且位于铲斗（h）的o_hx_hz_h内，那么就可以得到铲斗（h）绕下铰点（q）的角速度ω_h和角加速度α_h：

$\left\{\begin{array}{c}{\mathrm{\ω}}_h=K_{\mathrm{\ωh}}\frac{{\mathrm{\ΔS}}_f}{\mathrm{\Δt}}\\ {\mathrm{\α}}_h=K_{\mathrm{\αh}}\frac{{\mathrm{\Δ\ω}}_h}{\mathrm{\Δt}}\end{array}\right.---\left(10\right)$

其中K_ωh是由摇臂缸（f）的伸缩速度转换到铲斗（h）绕下铰点（q）的角速度的系数，K_αh是由摇臂缸（f）的伸缩加速度转换到铲斗（h）绕下铰点（q）的角加速度的系数，K_ωh和K_αh都可以根据几何分析求得；ΔS_f为相邻时刻工作装置姿态检测模块（Ⅱ）中的摇臂缸直线传感器（5）所测得数据的差值，Δt为采样时间间隔；

以及动臂（i）绕上铰点（o）的角速度ω_i和角加速度α_i：

$\left\{\begin{array}{c}{\mathrm{\ω}}_i=K_{\mathrm{\ωi}}\frac{{\mathrm{\ΔS}}_j}{\mathrm{\Δt}}\\ {\mathrm{\α}}_i=K_{\mathrm{\αi}}\frac{{\mathrm{\Δ\ω}}_i}{\mathrm{\Δt}}\end{array}\right.---\left(11\right)$

其中K_ωi是由动臂缸（j）的伸缩速度转换到动臂（i）绕上铰点（o）的角速度的系数，K_αi是由动臂缸（j）的伸缩加速度转换到动臂（j）绕上铰点（o）的角加速度的系数，K_ωi和K_αi都可以根据几何分析求得，ΔS_j为相邻时刻工作装置姿态检测模块（Ⅱ）中的动臂缸直线传感器（4）所测得数据的差值，Δt为采样时间间隔；

由此可以利用矢量合成的方法将铲斗（h）绕上铰点（o）的角加速度α₇及工作载荷绕上铰点（o）的角加速度α₁表示出来：

$\left\{\begin{array}{c}{\mathrm{\α}}_7=K_{a7h}{\mathrm{\α}}_h+K_{a7i}{\mathrm{\α}}_i\\ {\mathrm{\α}}_1=K_{a1h}{\mathrm{\α}}_h+K_{a1i}{\mathrm{\α}}_i\end{array}\right.---\left(11\right)$

式中K_a7_h为铲斗（h）绕下铰点（q）的角加速度α_h转化为铲斗（h）绕上铰点（o）的角加速度的系数，K_a7i为动臂（i）绕上铰点（o）的角加速度α_i转化为铲斗（h）绕上铰点（o）的角加速度的系数，K_a1h为铲斗（h）绕下铰点（q）的角加速度α_h转化为工作载荷绕上铰点（o）的角加速度的系数，K_a1i为动臂（i）绕上铰点（o）的角加速度α_i转化为工作载荷绕上铰点（o）的角加速度的系数；

那么将铲斗（h）和工作载荷作为研究对象，列x_b方向上和z_b方向上的力平衡方程及关于下铰点（q）的力矩平衡方程：

$\left\{\begin{array}{c}\mathrm{\Σ}{F}_{x_b}=0:F_h\cos {\mathrm{\θ}}_s+F_q\cos {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\cos {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\cos {\mathrm{\θ}}_7=0\\ \mathrm{\Σ}{F}_{z_b}=0:F_h\sin {\mathrm{\θ}}_s+F_q\sin {\mathrm{\θ}}_q+M_1{\mathrm{\α}}_{h}D_1^q\sin {\mathrm{\θ}}_1+M_7{\mathrm{\α}}_{h}D_h^q\sin {\mathrm{\θ}}_7+M_{1}g+M_{7}g=0\\ \mathrm{\Σ}{T}_q=0:F_h\cos {\mathrm{\θ}}_s(z_r-z_q)+F_h\sin {\mathrm{\θ}}_s(x_r-x_q)+M_{1}g(x_1-x_q)+M_{7}g(x_7-x_q)\\ +M_1{\mathrm{\α}}_h{D}^2_1^q+M_7{\mathrm{\α}}_h{D}^2_h^q=0\end{array}\right.---\left(12\right)$

其中θ_s为拉杆（s）与x_b的夹角，θ_q为力F_q与x_b的夹角，M₁为工作载荷的重量，

为工作载荷动态重心

与铰接点（q）的距离，θ₁为工作载荷动态重心

处的加速度与x_b的夹角，M₇为铲斗（h）的重量，

为铲斗（h）动态重心与铰接点（q）的距离，θ₇为铲斗动态重心

处的加速度与x_b的夹角，g为重力加速度，z_r为铰接点（r）在后车随动坐标系中的竖向坐标值，z_q为铰接点（q）在后车随动坐标系中的竖向坐标值，x_r为铰接点（r）在后车随动坐标系中的前进方向坐标值，x_q为铰接点（q）在后车随动坐标系中的前进方向坐标值；

式（12）中的未知数为三个即工作载荷的重量M₁，动态重心

的x₁坐标以及z₁坐标，求解即可得到工作载荷的重量M₁以及动态重心

（3）计算前车动态重心

通过前后车体相对位置检测模块（Ⅲ）可以得到前车（e）随动坐标系相对后车（b）随动坐标系的转角θ_e，继而可以根据坐标转换矩阵得到前车（e）静态重心转换到后车（b）随动坐标系中的坐标

$P_e^b=T_e^{b}P_e^e---\left(13\right)$

（4）计算燃油重量M₂及燃油动态重心

通过燃油余量检测模块以及燃油密度可以得到燃油的重量M₂，而油箱（n）为较规则的形状，因此其动态重心

可以根据体积进行估算；

（5）计算司机重量M₃及司机动态重心

通过司机重量检测模块（Ⅵ）可以得到司机重量M₃，同时根据普通成人的统计数据可以得到司机的动态重心

（6）计算后桥动态重心

通过后桥位置检测模块（Ⅵ）可以得到后桥（m）随动坐标系相对后车（b）随动坐标系的转角θ_m，继而可以根据坐标转换矩阵得到后桥（m）静态重心

转换到后车（b）随动坐标系中的坐标

$P_m^b=T_m^{b}P_m^m---\left(14\right)$

（（D））计算整车重量M和整车动态重心位置P(X,Y,Z)

至此，我们得到了所需要的所有部分的重量及其动态重心位置，就可以得到整车重量M并根据重心合成原理计算整车动态重心位置P(X,Y,Z)：

M＝M₁+M₂+M₃+M₄+M₅+M₆+M₇+M₈       （15）

$\left\{\begin{array}{cc}X=\frac{\mathrm{\Σ}{x}_u{M}_u}{\mathrm{\Σ}{M}_u}& \\ Y=\frac{\mathrm{\Σ}{y}_u{M}_u}{\mathrm{\Σ}{M}_u}& (u=1-8)\\ Z=\frac{\mathrm{\Σ}{z}_u{M}_u}{\mathrm{\Σ}{M}_u}& \end{array}\right.---\left(16\right)$

其中M₁为工作载荷重量，M₂为燃油重量，M₃为司机重量，M₄为前车重量，M₅为后车重量，M₆为后桥重量，M₇为铲斗重量，M₈为动臂重量；x_u，y_u，z_u为对应各部分的重心转换到后车随动坐标系中的坐标值。

Images