[发明专利]起吊工况下救援车整体重心平面位置的动态估计方法

摘要

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

主权项

1.一种起吊工况下救援车整体重心平面位置的动态估计方法，采用平面坐标系，以救援车转台轴心位置作为平面标系的原点o(0 0)，ox轴沿车辆纵轴指向车辆的前方，oy轴沿车辆横轴指向车辆的左侧；采用一个角度传感器、一个拉力传感器、一个加速度传感器、一套长度传感器；角度传感器安装在吊臂转台上，其中传感器的轴心与转台转动轴心一致；加速度传感器安装在吊臂上，且安装方向与吊臂方向一致；拉力传感器安装在吊钩与吊臂连接处；长度传感器安装在吊臂上且与吊臂方向一致；将救援车自身和吊臂重心与被救援车辆重心的平面位置通过组合法获得救援车整体重心的平面位置；利用卡尔曼滤波算法得出精度更高的救援车整体重心平面位置的动态估计；具体各个部分的重量及重心平面位置获取方式如下：(1)救援车的重量及重心平面位置除去吊臂，救援车剩余部分的重量及重心水平位置通过救援车的产品手册或静态测量的方式获得，救援车的重量为M₀，重心的平面位置为S₀(x₀ y₀)；(2)被吊起车辆的重量及重心平面位置被吊起车辆重量M₁通过吊臂顶端拉力传感器测量；由于被救车辆被吊起，根据悬挂法重心的平面位置与吊钩的平面位置相同；吊钩的平面位置，由救援车吊臂上的角度传感器、加速度传感器、拉力传感器，长度传感器，获得吊臂姿态信息，进一步解算出吊钩的平面位置S₁(x₁ y₁)；其中吊臂姿态信息包含：吊臂的回转角α，吊臂与xoy平面的仰角β，吊臂的长度L；其中吊臂的回转角α通过角度传感器直接获取；吊臂的长度L由长度传感器直接测得；考虑到吊臂在起吊的过程中处于静止状态，吊臂的仰角β通过加速度传感器采集的吊臂方向重力的分量A解算出来，具体公式如下：式中，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)，具体计算公式如下：将所计算的救援车的整体重心的平面位置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可表示为对于上述系统状态方程和测量方程，运用卡尔曼滤波理论，建立标准滤波递推过程，该递推过程包括时间更新和测量更新，递推过程的前两步为时间更新，剩余的三步为测量更新；时间更新包括：状态一步预测方程一步预测误差方差阵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)通过上述递推计算后，实时、动态估计起吊工况下救援清障车整体重心的平面位置。