起吊工况下救援车整体重心平面位置的动态估计方法与流程

本发明涉及一种起吊工况下救援车整体重心平面位置的动态估计方法，可用于救援清障车姿态监控及危险姿态预警，属于救援清障车安全预警领域。

背景技术：

据有关部门统计，近几年救援车翻车事故频发，救援过程中往往出现未能安全救起被救车辆，而救援车自身发生侧翻或倾翻事故(二次事故)。究其原因，除了操作者违章作业、经验不足和管理不善等主观因素外，救援车缺乏安全监控装置，无法为操作人员提供准确的救援车实时状态信息也是一个重要客观因素。

根据救援车的工作特点，救援车通过吊臂对救援车辆进行起吊等施救作业，考虑到被救车辆以及救援车支起的吊臂的影响，救援车整体重心(救援车和吊臂以及被救车辆的整体重心)就可能超出安全范围的情况，进而导致救援车自身发生侧翻或倾翻事故。如果能够针对起吊工况下(救援车进行起吊作业)救援车整体重心位置信息进行实时估计和监控，当其超出安全范围时给予驾驶员相应的警示信息，则能够避免大量的倾翻事故，但目前相关技术目前尚未见研究。所以，本专利提出了一种起吊工况下救援车整体重心动态平面位置估计方法(本方法只考虑重心的平面位置的估计)，具有低成本、高精度、实时性好的优点，防止救援过程中出现二次事故，具有重要的现实意义。

技术实现要素：

本发明提出了起吊工况下救援车整体重心平面位置的动态估计方法，已解决目前缺乏对起吊工况下救援车整体动态重心实时监控、预警方法的问题。

本方法中坐标系采用平面坐标系，以救援车转台轴心位置作为标系的原点o(0 0)，ox轴沿车辆纵轴指向车辆的前方，oy轴沿车辆横轴指向车辆的左侧。本方法适用在路面水平、空旷的环境下，救援车进行起吊作业时对整体重心的平面位置动态估计。

本方法采用了一个角度传感器、一个拉力传感器、一个加速度传感器、一套长度传感器。各个传感器的安装位置如下：角度传感器安装在吊臂转台上，其中传感器的轴心与转台转动轴心一直；加速度传感器安装在吊臂上，且安装方向与吊臂方向一致；拉力传感器安装在吊钩与吊臂连接处；长度传感器安装在吊臂上且与吊臂方向一致。

本发明公开了一种起吊工况下救援车整体重心平面位置的动态估计方法，将救援车自身和吊臂重心与被救援车辆重心的平面位置通过组合法获得救援车整体重心的平面位置。考虑到传感器的误差以及救援过程中产生的噪声，本发明利用卡尔曼滤波算法得出精度更高的救援车整体重心平面位置的动态估计，该方法可用于救援车安全监控及危险工况预警，具有精度高、成本低、实时性好等显著优点。

具体各个部分的重量及重心平面位置获取方式如下：

1、救援车的重量及重心平面位置

救援车的重量及重心水平位置(不包含吊臂)，可以通过救援车的产品手册或静态测量等方式获得，救援车的重量为M₀，重心的平面位置为S₀(x₀ y₀)。

2、被吊起车辆的重量及重心平面位置

被吊起车辆重量M₁通过吊臂顶端拉力传感器测量。由于被救车辆被吊起，根据悬挂法重心的平面位置与吊钩的平面位置相同。吊钩的平面位置，可由救援车吊臂上的角度传感器、加速度传感器、拉力传感器，长度传感器，获得吊臂姿态信息，进一步解算出吊钩的平面位置。

其中吊臂姿态信息包含：吊臂的回转角α(吊臂的回转角范围：0≤α≤360°，α为吊臂与车体坐标系ox轴沿逆时针方向的夹角)，吊臂与xoy平面的仰角β(吊臂仰角范围：0≤β≤90°)，吊臂的长度L(吊臂长度L_min≤L≤L_max)，其中L_min为吊臂长度最小值、L_max为长度最大值(L_min、L_max通过吊臂的产品手册获取)。其中吊臂的回转角α可以通过角度传感器直接获取；吊臂的长度L由长度传感器直接测得。考虑到吊臂在起吊的过程中处于静止状态，吊臂的仰角β可通过加速度传感器采集的吊臂方向重力的分量A解算出来，具体公式如下：

$\mathrm{\β}={\sin }^{-1}\left(\frac{A}{g}\right)---\left(1\right)$

式中，g表示重力加速度常量，sin^-1表示正弦函数的反函数。

综上被吊起车辆重心在xoy平面的坐标S₁(x₁ y₁)的具体计算公式如下：

x₁＝cosβ×L×cosα (2)

y₁＝cosβ×L×sinα (3)

3、吊臂的重量及重心位置

吊臂的质量M₂可以通过说明书或者静态测量获取。吊臂重心距吊臂起始端(吊臂与救援车辆转台连接处)的距离D随着吊臂的长度L的变化而变化。本方法兼顾计算的复杂度和拟合的精度，选择二次曲线拟合吊臂重心距吊臂起始端的距离D随着吊臂的长度L的函数D＝A(L)，其中S(L)＝A₂L²+A₁L+A₀。通过静态测量的方式获得吊臂长度为L_min、L_mid、L_max以及对应的吊臂重心距吊臂起始端的距离D_min、D_mid、D_max，其中L_mid＝0.5×(L_min+L_max)。然后通过以上获得的数据解算出二次曲线的系数A₂、A₁、A₀的值，根据测得吊臂的长度L即可获得吊臂重心距吊臂起始端的距离D的值。

因此吊臂的重心在xoy平面的坐标S₂(x₂ y₂)具体计算公式如下：

x₂＝cosβ×D×cosα (4)

y₂＝cosβ×D×sinα (5)

其中，α、β意义与上文一致，表示吊臂的回转角和俯仰角。

利用已知的救援车和吊臂以及被吊起的物体的重心平面位置、重量，通过组合法即可计算出整体重心的平面位置S_m(x_m y_m)，具体计算公式如下：

$x_m=\frac{M_0{x}_0+M_1{x}_1+M_2{x}_2}{M_0+M_1+M_2}---\left(6\right)$

$y_m=\frac{M_0{y}_0+M_1{y}_1+M_2{y}_2}{M_0+M_1+M_2}---\left(7\right)$

考虑到起吊作业下救援车辆处于动态，传感器所获得的数据存在较大的误差随机，进而影响获得整体重心平面位置的动态估计精度，所以本方法将所计算的救援车的整体重心的平面位置S_m(x_m y_m)作为观测量，利用卡尔曼滤波算法以求获得精度更高的整体重心的估计值。

卡尔曼滤波算法是以最小均方差为准则的最优状态估计滤波算法，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。将该方法应用于救援清障车起吊工况下的整体重心估计时，在保证实时性的同时，可以有效提高估计精度。

下面建立卡尔曼滤波的状态方程和观测方程。

离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X(k)＝F(k，k-1)X(k-1)+W(k-1) (8)

式中，k表示离散化时刻；系统状态向量为X＝[p1 p2]其中p1为平面坐标系下的整体重心ox方向的位置坐标，p2为车体坐标系下的整体重心oy方向的位置坐标，本发明中上角标’表示对矩阵转置；W表示零均值的系统高斯白噪声向量且W＝[w₁ w₂]，其中w₁、w₂分别表示两个系统高斯白噪声分量，W对应的系统噪声协方差阵其中分别表示系统高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是因为救援清障车在工况下整体重心位置是缓慢变化的，可以认为上一采样时刻的整体重心坐标等于下一采样时刻的整体重心坐标。

卡尔曼滤波观测方程的离散化矩阵形式为：

Z(k)＝H(k)X(k)+V(k) (9)

式中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。观测量取组合法计算出整体重心ox、oy轴方向坐标值x_m、y_m，由于观测向量与状态向量都是指整体重心ox、oy轴方向坐标值，所以表示整体重心ox方向坐标的观测噪声且均值为0、方差为高斯白噪声，表示通过式计算获得的整体重心oy方向坐标值的观测噪声且均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为

对于上述所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新。

时间更新包括：

状态一步预测方程

$\hat{X}(k,k-1)=F(k,k-1)\hat{X}(k-1)---\left(10\right)$

一步预测误差方差阵

P(k，k-1)＝F(k，k-1)P(k-1)F′(k，k-1)+Q(k-1) (11)

测量更新包括：

滤波增益矩阵

K(k)＝P(k，k-1)H′(k)[H(k)P(k，k-1)H′(k)+R(k)]^-1 (12)

状态估计

估计误差方差阵

P(k)＝[I-K(k)H(k)]P(k，k-1) (14)

经过上述递推计算后，可实时、动态估计起吊工况下救援清障车整体重心的平面位置。

附图说明

图1为本发明所提方法的流程图

图2为本发明所提吊臂回转角示意图

图3为本发明所提吊臂仰角示意图

图4为本发明所提吊臂仰角估计模型示意图

具体实施方式

据有关部门统计，近几年救援车翻车事故频发，救援过程中往往出现未能安全救起被救车辆，而救援车自身发生侧翻或倾翻事故(二次事故)。究其原因，除了操作者违章作业、经验不足和管理不善等主观因素外，救援车缺乏安全监控装置，无法为操作人员提供准确的救援车状态信息也是一个重要客观因素。

根据救援车的工作特点，救援车通过吊臂对救援车辆进行起吊等施救作业，考虑到被救车辆以及救援车支起的吊臂的影响，救援车整体重心(救援车和吊臂以及被救车辆的整体重心)就可能超出安全范围的情况，进而导致救援车自身发生侧翻或倾翻事故。如果能够针对起吊工况下(救援车进行起吊作业)救援车整体重心位置信息进行实时估计和监控，当其超出安全范围时给予驾驶员相应的警示信息，则能够避免大量的倾翻事故，但目前相关技术目前尚未见研究。所以本专利提出了一种起吊工况下救援车整体重心动态平面位置估计方法(本方法只考虑重心的平面位置的估计)，具有低成本、高精度、实时性好的优点，防止救援过程中出现二次事故，具有重要的现实意义。

本方法中坐标系采用平面坐标系，以救援车转台轴心位置作为标系的原点o(0 0)，ox轴沿车辆纵轴指向车辆的前方，oy轴沿车辆横轴指向车辆的左侧。本方法适用在路面水平、空旷的环境下，救援车进行起吊作业时对整体重心的平面位置动态估计。

本方法采用了一个角度传感器、一个拉力传感器、一个加速度传感器、一套长度传感器。各个传感器的安装位置如下：角度传感器安装在吊臂转台上，其中传感器的轴心与转台转动轴心一直；加速度传感器安装在吊臂上，且安装方向与吊臂方向一致；拉力传感器安装在吊钩与吊臂连接处；长度传感器安装在吊臂上且与吊臂方向一致。

本发明公开了一种起吊工况下救援车整体重心平面位置的动态估计方法，将救援车自身和吊臂重心与被救援车辆重心的平面位置通过组合法获得救援车整体重心的平面位置。考虑到传感器的误差以及救援过程中产生的噪声，本发明利用卡尔曼滤波算法得出精度更高的救援车整体重心平面位置的动态估计，该方法可用于救援车安全监控及危险工况预警，具有精度高、成本低、实时性好等显著优点。

具体各个部分的重量及重心平面位置获取方式如下：

1、救援车的重量及重心平面位置

救援车的重量及重心水平位置(不包含吊臂)，可以通过救援车的产品手册或静态测量等方式获得，救援车的重量为M₀，重心的平面位置为S₀(x₀ y₀)。

2、被吊起车辆的重量及重心平面位置

被吊起车辆重量M₁通过吊臂顶端拉力传感器测量。由于被救车辆被吊起，根据悬挂法重心的平面位置与吊钩的平面位置相同。吊钩的平面位置，可由救援车吊臂上的角度传感器、加速度传感器、拉力传感器，长度传感器，获得吊臂姿态信息，进一步解算出吊钩的平面位置。

其中吊臂姿态信息包含：吊臂的回转角α(吊臂的回转角范围：0≤α≤360°，α为吊臂与车体坐标系ox轴沿逆时针方向的夹角)，吊臂与xoy平面的仰角β(吊臂仰角范围：0≤β≤90°)，吊臂的长度L(吊臂长度L_min≤L≤L_max)，其中L_min为吊臂长度最小值、L_max为长度最大值(L_min、L_max通过吊臂的产品手册获取)。其中吊臂的回转角α可以通过角度传感器直接获取；吊臂的长度L由长度传感器直接测得。考虑到吊臂在起吊的过程中处于静止状态，吊臂的仰角β可通过加速度传感器采集的吊臂方向重力的分量A解算出来，具体公式如下：

$\mathrm{\β}={\sin }^{-1}\left(\frac{A}{g}\right)---\left(1\right)$

式中，g表示重力加速度常量，sin^-1表示正弦函数的反函数。

综上被吊起车辆重心在xoy平面的坐标S₁(x₁ y₁)的具体计算公式如下：

x₁＝cosβ×L×cosα (2)

y₁＝cosβ×L×sinα (3)

3、吊臂的重量及重心位置

吊臂的质量M₂可以通过说明书或者静态测量获取。吊臂重心距吊臂起始端(吊臂与救援车辆转台连接处)的距离D随着吊臂的长度L的变化而变化。本方法兼顾计算的复杂度和拟合的精度，选择二次曲线拟合吊臂重心距吊臂起始端的距离D随着吊臂的长度L的函数D＝S(L)，其中S(L)＝A₂L²+A₁L+A₀。通过静态测量的方式获得吊臂长度为L_min、L_mid、L_max以及对应的吊臂重心距吊臂起始端的距离D_min、D_mid、D_max，其中L_mid＝0.5×(L_min+L_max)。然后通过以上获得的数据解算出二次曲线的系数A₂、A₁、A₀的值，根据测得吊臂的长度L即可获得吊臂重心距吊臂起始端的距离D的值。

因此吊臂的重心在xoy平面的坐标S₂(x₂ y₂)具体计算公式如下：

x₂＝cosβ×D×cosα (4)

y₂＝cosβ×D×sinα (5)

其中，α、β意义与上文一致，表示吊臂的回转角和俯仰角。

利用已知的救援车和吊臂以及被吊起的物体的重心平面位置、重量，通过组合法即可计算出整体重心的平面位置S_m(x_m y_m)，具体计算公式如下：

$x_m=\frac{M_0{x}_0+M_1{x}_1+M_2{x}_2}{M_0+M_1+M_2}---\left(6\right)$

$y_m=\frac{M_0{y}_0+M_1{y}_1+M_2{y}_2}{M_0+M_1+M_2}---\left(7\right)$

考虑到起吊作业下救援车辆处于动态，传感器所获得的数据存在较大的误差随机，进而影响获得整体重心平面位置的动态估计精度，所以本专利将所计算的救援车的整体重心的平面位置S_m(x_m y_m)作为观测量，利用卡尔曼滤波算法以求获得精度更高的整体重心的估计值。

卡尔曼滤波算法是以最小均方差为准则的最优状态估计滤波算法，它不需要储存过去的测量值，只根据当前的观测值和前一时刻的估计值进行递推计算，便可实现对实时信号的估计，具有数据存储量小、算法简便的特点。将该方法应用于救援清障车起吊工况下整体重心估计时，在保证实时性的同时，可以有效提高估计精度。

下面建立卡尔曼滤波的状态方程和观测方程。

离散化后的卡尔曼滤波的状态方程的矩阵形式表示为：

X(k)＝F(k，k-1)X(k-1)+W(k-1) (8)

式中，k表示离散化时刻；系统状态向量为X＝[p1 p2]其中p1为平面坐标系下的整体重心ox方向的位置坐标，p2为车体坐标系下的整体重心oy方向的位置坐标，本发明中上角标’表示对矩阵转置；W表示零均值的系统高斯白噪声向量且W＝[w₁ w₂]，其中w₁、w₂分别表示两个系统高斯白噪声分量，W对应的系统噪声协方差阵其中分别表示系统高斯白噪声w₁、w₂对应的方差；状态转移矩阵为这是因为救援清障车在工况下整体重心位置是缓慢变化的，可以认为上一采样时刻的整体重心坐标等于下一采样时刻的整体重心坐标。

卡尔曼滤波观测方程的离散化矩阵形式为：

Z(k)＝H(k)X(k)+V(k) (9)

式中，Z为观测向量，H为观测阵，V表示与W互不相关的零均值观测白噪声向量。观测量取组合法计算出整体重心ox、oy轴方向坐标值x_m、y_m，由于观测向量与状态向量都是指整体重心ox、oy轴方向坐标值，所以表示整体重心ox方向坐标的观测噪声且均值为0、方差为高斯白噪声，表示通过式计算获得的整体重心oy方向坐标值的观测噪声且均值为0、方差为的高斯白噪声；V对应的观测噪声方差阵R可表示为

对于上述所描述的系统状态方程和测量方程，运用卡尔曼滤波理论，建立下面的标准滤波递推过程，该递推过程包括时间更新和测量更新，下面递推过程的前两步为时间更新，剩余的三步为测量更新。

时间更新包括：

状态一步预测方程

$\hat{X}(k,k-1)=F(k,k-1)\hat{X}(k-1)---\left(10\right)$

一步预测误差方差阵

P(k，k-1)＝F(k，k-1)P(k-1)F′(k，k-1)+Q(k-1) (11)

测量更新包括：

滤波增益矩阵

K(k)＝P(k，k-1)H′(k)[H(k)P(k，k-1)H′(k)+R(k)]^-1 (12)

状态估计

估计误差方差阵

P(k)＝[I-K(k)H(k)]P(k，k-1) (14)

经过上述递推计算后，可实时、动态估计起吊工况下救援清障车整体重心的平面位置。

技术效果

(1)本发明提出了一种低成本、高精度、实时性好的起吊工况下救援车整体重心平面位置估计方法，可用于救援车安全监控和危险预警的需要；

(2)本发明根据救援清障车工作特点，对救援过程中整体重心模型进行合理简化，并利用卡尔曼滤波进行救援车整体重心位置的估计，保障了其估计精度与实时性；

(3)本发明仅需一个低成本角度传感器、一个拉力传感器、一个加速度传感器、一套长度感器，具有成本低的优点，便于大规模推广。