CN106248301B - 基于鲁棒滤波的救援车整体重心平面位置估计方法 - Google Patents

Abstract

本发明公开了一种救援车整体重心平面位置的估计方法。本方法分别测得救援车自身重心与被救援车辆重心的平面位置，然后根据合力矩定理推导获得整体重心的平面位置，在此基础上利用鲁棒滤波算法推算出整体重心平面位置的精确估计值，该方法可用于救援车安全监控及危险工况预警，具有精度高、成本低、实时性好等显著优点。

Description

基于鲁棒滤波的救援车整体重心平面位置估计方法

技术领域

本发明涉及一种起吊工况下救援车整体重心平面位置的估计方法，尤其涉及一种基于H_∞鲁棒滤波的救援车整体重心平面位置的估计方法，其目的在于为救援车提供安全监控及危险姿态预警，属于救援车安全预警领域。

背景技术

近年来，我国的救援车行业得到了高速发展。但是，根据有关部门的统计，救援车发生侧翻或倾翻的事故率也逐年增加，存在着很多未能安全救起被救车辆，而自身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素外，救援车的安全监控装置尚不完善，无法为驾驶员提供准确的状态信息和安全预警也是不可回避的客观因素。

根据救援车的工作需求，救援车需通过吊臂对被救车辆进行起吊等作业。在操作过程中，由于受到被救车辆的影响，整体重心(救援车以及被救车辆的整体重心)会发生变化，甚至可能超出安全范围，进而导致救援车自身发生侧翻或倾翻。如果能够对正在进行起吊作业的救援车的整体重心位置进行实时估计和监控，并在其超出安全范围时给予驾驶员相应的警示信息，则能够避免大量的侧翻或倾翻事故。

为此，本发明专利提出了一种基于H_∞鲁棒滤波的救援车整体重心平面位置估计方法。H_∞滤波是将鲁棒控制设计中引入的性能指标H_∞范数应用于滤波，以解决系统中存在的各种不确定性问题，它将噪声看作是能量有限的随机信号，构建一个滤波器使得从干扰输入到估计误差输出的闭环传递函数的H_∞范数最小或者小于给定的正数γ。因此，H_∞对噪声的不确定性具有很强的鲁棒性。本方法适用于救援车起吊作业时的整体重心平面位置的动态估计，具有低成本、高精度、实时性好的优点，对防止救援过程中出现二次事故，具有重要的现实意义。

发明内容

本发明的目的是提出一种基于H_∞鲁棒滤波的救援车整体重心平面位置估计方法，该方法适用于在路面水平、空旷的环境下进行起吊作业的救援车。该方法使用的传感器数量少，价格低廉，且可以在无法确切知道干扰信号的统计特性的情况下，动态估算出救援车整体重心的平面位置，具有精度高、成本低、实时性好、适用范围广的特点，可以为救援车提供整体重心的实时监控以及安全预警。

本发明采用的技术方案如下：首先分别测得救援车自身重心与被救援车辆重心的平面位置，然后根据合力矩定理推导获得整体重心的平面位置，并在此基础上利用鲁棒滤波算法推算出整体重心平面位置的精确估计值。该方法可以用于无法确切知道干扰信号的统计特性的情况下，精确估计救援车整体重心的平面位置，且仅需一个角度传感器、一个拉力传感器、一个加速度传感器、一套长度传感器。具体步骤包括：

1)建立坐标系

本方法适用在路面水平、空旷的工作环境下，坐标系采用平面坐标系，以救援车转台轴心位置作为坐标系的原点O(0 0)，OX轴沿救援车纵轴指向车辆的前方，OY轴沿救援车横轴指向车辆的左侧。

2)安装传感器

角度传感器安装在吊臂转台上，传感器的轴心与转台转动轴心一致；加速度传感器安装在吊臂上，且安装方向与吊臂方向一致；拉力传感器安装在吊钩与吊臂连接处；长度传感器安装在吊臂上且与吊臂方向一致。

3)获取救援车自身重心平面位置和被救车辆重心平面位置

①救援车自身重心平面位置：救援车的重量及重心水平位置，可以通过救援车的产品手册或静态测量等方式获得，救援车的重量为G₀，重心的平面位置为S₀(x₀ y₀)。鉴于吊臂重量远小于救援车总重量，在此忽略救援车吊臂运动变化对救援车重心的影响。

②被救车辆重心平面位置：吊臂的回转角α(吊臂的回转角范围：0≤α≤360°，α为吊臂与车体坐标系ox轴沿逆时针方向的夹角)，被吊起车辆重量G₁通过吊臂顶端拉力传感器测量；由于被救车辆被吊起，根据悬挂法重心的平面位置与吊钩的平面位置相同；吊臂的长度L(L_min≤L≤L_max，L_min为吊臂长度最小值、L_max为长度最大值)由长度传感器直接测得；考虑到吊臂在起吊的过程中处于静止状态，吊臂的仰角β(β为吊臂与XOY平面的夹角，0≤β≤90°)可通过加速度传感器测得的重力沿吊臂方向的分量A解算出来，具体公式如下：

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

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

4)根据合力矩定理推导整体重心的平面位置

救援车和被救车辆整体的重力即救援车重力和被救车辆重力的合力：G＝G₀+G₁，合力的作用线过整体重心的平面位置S(p q)。根据合力矩定理可推导计算出整体重心的平面位置S(p q)，具体计算公式如下：

5)H_∞鲁棒滤波

①建立H_∞滤波的状态方程和观测方程

离散化后的H_∞滤波状态方程的矩阵形式为：

式中，k表示离散化时刻；X为系统状态向量，且X_k＝[p_k q_k]^T，p_k为平面坐标系下的整体重心OX方向的位置坐标，q_k为平面坐标系下的整体重心OY方向的位置坐标，上角标T表示对矩阵转置；W表示系统噪声向量，且W＝[w₁ w₂]^T，其中w₁、w₂分别表示两个系统噪声分量；状态转移矩阵为这是因为救援车在工作过程中整体的重心位置是缓慢变化的，可以认为上一采样时刻的整体重心坐标等于下一采样时刻的整体重心坐标；Γ_k-1为系统噪声输入矩阵，取Γ_k-1为

离散化的H_∞滤波观测方程的矩阵形式为：

Z_k＝H_k·X_k+V_k (5)

式中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[n_p n_q]，其中n_p是整体重心的平面位置X轴坐标的观测噪声，n_q是整体重心的平面位置Y轴坐标的观测噪声；由于观测向量与状态向量都是指整体重心的平面位置的X轴坐标和Y轴坐标，所以 其中和分别为各个采样时刻初步推算得出的含有噪声的整体重心的平面位置的X轴坐标和Y轴坐标，根据式(3)有：

式中，分别表示各个采样时刻由式(2)初步推算出的被救车辆的重心的平面位置的X轴坐标和Y轴坐标；

②H_∞滤波递推：

对于式(4)和式(5)所描述的系统状态方程和观测方程，建立次优H_∞滤波的递推过程，对于一定的正数γ，次优H_∞滤波递推过程主要包括以下三个步骤：

a)状态线性组合的估计

式中，是Y_k-1的估计值，Y_k-1为系统的待估计的向量，是X_k-1的估计值；L_k-1为给定的状态量线性组合矩阵，由于需要估计的量就是状态变量X_k-1，取L_k-1为

b)时间修正

状态一步预测：

估计误差方差阵：

其中P_k的初始值P₀可选为

c)量测修正

滤波增益矩阵：

状态估计：

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

附图说明

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

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

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

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

具体实施方式

近年来，我国的救援车行业得到了高速发展。但是，根据有关部门的统计，救援车发生侧翻或倾翻的事故率也逐年增加，存在着很多未能安全救起被救车辆，而自身却发生事故的情况。究其原因，除了存在操作者违章作业、经验不足和管理不善等主观因素外，救援车的安全监控装置尚不完善，无法为驾驶员提供准确的状态信息和安全预警也是不可回避的客观因素。

由于救援车的工作需求，救援车需通过吊臂对被救车辆进行起吊等作业。在操作过程中，由于受到被救车辆的影响，整体重心(救援车以及被救车辆的整体重心)的位置会发生变化，甚至可能超出安全范围，进而导致救援车自身发生侧翻或倾翻。如果能够对正在进行起吊作业的救援车的整体重心位置进行实时估计和监控，并在其超出安全范围时给予驾驶员相应的警示信息，则能够避免大量的侧翻或倾翻事故。为此，本专利提出了一种基于H_∞鲁棒滤波的救援车整体重心平面位置估计方法。本方法适用于救援车起吊作业时的整体重心平面位置的动态估计，具有低成本、高精度、实时性好的优点，对防止救援过程中出现二次事故，具有重要的现实意义。

本方法适用在路面水平、空旷的工作环境下。本方法中的坐标系采用平面坐标系，以救援车转台轴心位置作为标系的原点O(0 0)，OX轴沿救援车纵轴指向车辆的前方，OY轴沿救援车横轴指向车辆的左侧。在此坐标系下，分别测得救援车自身重心与被救车辆重心的平面位置，然后根据“合力矩定理”推导求得救援车整体重心的平面位置。

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

救援车和被救车辆的重量及重心平面位置获取方式如下：

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

救援车的重量及重心水平位置，可以通过救援车的产品手册或静态测量等方式获得，救援车的重量为G₀，重心的平面位置为S₀(x₀ y₀)。因为吊臂重量远小于救援车总重量，所以在此忽略救援车吊臂运动变化对救援车重心的影响。

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

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

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

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

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

救援车和被救车辆整体的重力即救援车重力和被救车辆重力的合力：G＝G₀+G₁，合力的作用线过整体重心的平面位置S(p q)。根据合力矩定理可推导计算出整体重心的平面位置S(p q)，具体计算公式如下：

但是考虑到起吊作业时救援车辆工作环境的影响，以及本方法中使用的四种传感器自身存在的不确定的非线性噪声，使得噪声信号很难被准确描述。所以传感器所获得的数据存在较大的随机误差，进而影响获得整体重心平面位置的估计精度，为了得到高精度的整体重心平面位置，需要采用滤波技术对其进行优化估计。在实际应用过程中，不确定的非线性噪声会使噪声统计特性发生变化，从而使得预先设定的噪声模型不准确。

因此，本发明专利提出了一种基于H_∞鲁棒滤波的救援车整体重心平面位置估计方法，本方法对干扰信号统计特性不做任何的假设，只把它看作能量有限的信号，突破了噪声模型不准确的限制，是一种更有效的估计方法。从而得出精度更高的救援车整体重心平面位置的动态估计。

H_∞滤波是将鲁棒控制设计中引入的性能指标H_∞范数应用于滤波，以解决系统中存在的各种不确定性问题，它将噪声看作是能量有限的随机信号，构建一个滤波器使得从干扰输入到估计误差输出的闭环传递函数的H_∞范数最小或者小于给定的正数γ。因此H_∞对噪声的不确定性具有很强的鲁棒性。

本发明专利中将所求的救援车的整体重心的平面位置S(p q)作为状态量，通过H_∞鲁棒滤波获得精度更高的整体重心的估计值。

首先，建立H_∞滤波的状态方程。由于在实际递推过程中，需要采用离散化的H_∞滤波模型，因此要进行离散化处理，得到离散化后的H_∞滤波状态方程的矩阵形式为：

式中，k表示离散化时刻；X为系统状态向量，且X_k＝[p_k q_k]^T，p_k为平面坐标系下的整体重心OX方向的位置坐标，q_k为平面坐标系下的整体重心OY方向的位置坐标，上角标^T表示对矩阵转置；W表示系统噪声向量，且W＝[w₁ w₂]^T，其中w₁、w₂分别表示两个系统噪声分量；状态转移矩阵为这是因为救援车在工作过程中整体的重心位置是缓慢变化的，可以认为上一采样时刻的整体重心坐标等于下一采样时刻的整体重心坐标；Γ_k-1为系统噪声输入矩阵，取Γ_k-1为

下面讨论如何建立其观测方程。由式(3)可得：

其中，和分别为各个采样时刻初步推算得出的含有噪声的整体重心的平面位置的X轴坐标和Y轴坐标。分别表示各个采样时刻由式(2)初步推算出的被救车辆的重心的平面位置的X轴坐标和Y轴坐标。

本发明中，将整体重心的平面位置的X轴坐标和Y轴坐标作为H_∞滤波观测量。由于X轴坐标和Y轴坐标同时又是H_∞滤波的两个状态量，故不难建立该系统的观测方程，其离散化后的矩阵形式为：

Z_k＝H_k·X_k+V_k (6)

式中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[n_p n_q]，其中n_p是整体重心的平面位置X轴坐标的观测噪声，n_q是整体重心的平面位置Y轴坐标的观测噪声；由于观测向量与状态向量都是指整体重心的平面位置的X轴坐标和Y轴坐标，所以

对于式(4)和式(6)所描述的系统状态方程和观测方程，建立次优H_∞滤波的递推过程，对于一定的正数γ，次优H_∞滤波递推过程主要包括以下三个步骤：

a)状态线性组合的估计

式中，是Y_k-1的估计值，Y_k-1为系统的待估计的向量，是X_k-1的估计值；L_k-1为给定的状态量线性组合矩阵，由于需要估计的量就是状态变量X_k-1，所以取L_k-1为

b)时间修正

状态一步预测：

估计误差方差阵：

其中P_k的初始值P₀可选为

c)量测修正

滤波增益矩阵：

状态估计：

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

技术效果

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

(2)本发明利用H_∞鲁棒滤波进行救援车整体重心位置的估计，突破了噪声特性无法准确建模的限制，并且结果可以满足实际应用的精度与实时性要求；

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

Claims (1)

1.一种基于鲁棒滤波的救援车整体重心平面位置估计方法，其特征在于：依据合力矩定理，用测得的救援车自身的重心平面位置和被救车辆的重心平面位置推导出整体重心的平面位置，通过鲁棒滤波算法得出更加精确的整体重心平面位置的估计值，且仅需一个角度传感器、一个拉力传感器、一个加速度传感器、一套长度传感器，具有低成本高精度的特点；

具体步骤包括：

(1)建立坐标系

采用平面坐标系，以救援车转台轴心位置作为坐标系的原点O(0 0)，OX轴沿救援车纵轴指向车辆的前方，OY轴沿救援车横轴指向车辆的左侧；

(2)安装传感器

角度传感器安装在吊臂转台上，传感器的轴心与转台转动轴心一致；加速度传感器安装在吊臂上，且安装方向与吊臂方向一致；拉力传感器安装在吊钩与吊臂连接处；长度传感器安装在吊臂上且与吊臂方向一致；

(3)获取救援车自身重心平面位置和被救车辆重心平面位置

①救援车自身重心平面位置：救援车的重量及重心水平位置，通过救援车的产品手册或静态测量的方式获得，救援车的重量为G₀，重心的平面位置为S₀(x₀ y₀)；忽略救援车吊臂运动变化对救援车重心的影响；

②被救车辆重心平面位置：吊臂的回转角α，被吊起车辆重量G₁通过吊臂顶端拉力传感器测量；根据悬挂法重心的平面位置与吊钩的平面位置相同；吊臂的长度L由长度传感器直接测得，L_min≤L≤L_max，其中L_min为吊臂长度最小值、L_max为长度最大值；吊臂的仰角β通过加速度传感器测得的重力沿吊臂方向的分量A解算出来，β为吊臂与XOY平面的夹角，0≤β≤90°，具体公式如下：

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

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

(4)根据合力矩定理推导整体重心的平面位置

救援车重力和被救车辆重力的合力：G＝G₀+G₁，合力的作用线过整体重心平面位置S(pq)，根据合力矩定理推导计算出整体重心的平面位置S(p q)，具体计算公式如下：

(5)H_∞鲁棒滤波

①建立H_∞滤波的状态方程和观测方程

离散化后的H_∞滤波状态方程的矩阵形式为：

式中，k表示离散化时刻；X为系统状态向量，且X_k＝[p_k q_k]^T，p_k为平面坐标系下的整体重心OX方向的位置坐标，q_k为平面坐标系下的整体重心OY方向的位置坐标，上角标T表示对矩阵转置；W表示系统噪声向量，且W＝[w₁ w₂]^T，其中w₁、w₂分别表示两个系统噪声分量；状态转移矩阵为Γ_k-1为系统噪声输入矩阵，取Γ_k-1为离散化的H_∞滤波观测方程的矩阵形式为：

Z_k＝H_k·X_k+V_k (5)

式中，Z为观测向量，H为观测阵，V表示观测噪声向量，V＝[n_p n_q]，其中n_p是整体重心的平面位置X轴坐标的观测噪声，n_q是整体重心的平面位置Y轴坐标的观测噪声；其中和分别为各个采样时刻初步推算得出的含有噪声的整体重心的平面位置的X轴坐标和Y轴坐标，根据式(3)有：

式中，分别表示各个采样时刻由式(2)初步推算出的被救车辆的重心平面位置的X轴坐标和Y轴坐标；

②H_∞滤波递推

对于式(4)和式(5)所描述的系统状态方程和观测方程，建立次优H_∞滤波的递推过程，对于一定的正数γ，次优H_∞滤波递推过程主要包括以下三个步骤：

a)状态线性组合的估计

式中，为Y_k-1的估计值，Y_k-1为系统的待估计的向量；为X_k-1的估计值；L_k-1为给定的状态量线性组合矩阵，取L_k-1为

b)时间修正

状态一步预测：

估计误差方差阵：

其中，P_k的初始值P₀选为

c)量测修正

滤波增益矩阵：

状态估计：

通过上述递推计算，实时、精确地动态估计起吊工况下救援车整体重心的平面位置。