本發(fā)明涉及一種起吊工況下救援車整體重心平面位置的動態(tài)估計方法，可用于救援清障車姿態(tài)監(jiān)控及危險姿態(tài)預(yù)警，屬于救援清障車安全預(yù)警領(lǐng)域。

背景技術(shù)：

據(jù)有關(guān)部門統(tǒng)計，近幾年救援車翻車事故頻發(fā)，救援過程中往往出現(xiàn)未能安全救起被救車輛，而救援車自身發(fā)生側(cè)翻或傾翻事故(二次事故)。究其原因，除了操作者違章作業(yè)、經(jīng)驗不足和管理不善等主觀因素外，救援車缺乏安全監(jiān)控裝置，無法為操作人員提供準(zhǔn)確的救援車實時狀態(tài)信息也是一個重要客觀因素。

根據(jù)救援車的工作特點，救援車通過吊臂對救援車輛進(jìn)行起吊等施救作業(yè)，考慮到被救車輛以及救援車支起的吊臂的影響，救援車整體重心(救援車和吊臂以及被救車輛的整體重心)就可能超出安全范圍的情況，進(jìn)而導(dǎo)致救援車自身發(fā)生側(cè)翻或傾翻事故。如果能夠針對起吊工況下(救援車進(jìn)行起吊作業(yè))救援車整體重心位置信息進(jìn)行實時估計和監(jiān)控，當(dāng)其超出安全范圍時給予駕駛員相應(yīng)的警示信息，則能夠避免大量的傾翻事故，但目前相關(guān)技術(shù)目前尚未見研究。所以，本專利提出了一種起吊工況下救援車整體重心動態(tài)平面位置估計方法(本方法只考慮重心的平面位置的估計)，具有低成本、高精度、實時性好的優(yōu)點，防止救援過程中出現(xiàn)二次事故，具有重要的現(xiàn)實意義。

技術(shù)實現(xiàn)要素：

本發(fā)明提出了起吊工況下救援車整體重心平面位置的動態(tài)估計方法，已解決目前缺乏對起吊工況下救援車整體動態(tài)重心實時監(jiān)控、預(yù)警方法的問題。

本方法中坐標(biāo)系采用平面坐標(biāo)系，以救援車轉(zhuǎn)臺軸心位置作為標(biāo)系的原點o(0 0)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側(cè)。本方法適用在路面水平、空曠的環(huán)境下，救援車進(jìn)行起吊作業(yè)時對整體重心的平面位置動態(tài)估計。

本方法采用了一個角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度傳感器。各個傳感器的安裝位置如下：角度傳感器安裝在吊臂轉(zhuǎn)臺上，其中傳感器的軸心與轉(zhuǎn)臺轉(zhuǎn)動軸心一直；加速度傳感器安裝在吊臂上，且安裝方向與吊臂方向一致；拉力傳感器安裝在吊鉤與吊臂連接處；長度傳感器安裝在吊臂上且與吊臂方向一致。

本發(fā)明公開了一種起吊工況下救援車整體重心平面位置的動態(tài)估計方法，將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置?？紤]到傳感器的誤差以及救援過程中產(chǎn)生的噪聲，本發(fā)明利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態(tài)估計，該方法可用于救援車安全監(jiān)控及危險工況預(yù)警，具有精度高、成本低、實時性好等顯著優(yōu)點。

具體各個部分的重量及重心平面位置獲取方式如下：

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

救援車的重量及重心水平位置(不包含吊臂)，可以通過救援車的產(chǎn)品手冊或靜態(tài)測量等方式獲得，救援車的重量為M₀，重心的平面位置為S₀(x₀ y₀)。

2、被吊起車輛的重量及重心平面位置

被吊起車輛重量M₁通過吊臂頂端拉力傳感器測量。由于被救車輛被吊起，根據(jù)懸掛法重心的平面位置與吊鉤的平面位置相同。吊鉤的平面位置，可由救援車吊臂上的角度傳感器、加速度傳感器、拉力傳感器，長度傳感器，獲得吊臂姿態(tài)信息，進(jìn)一步解算出吊鉤的平面位置。

其中吊臂姿態(tài)信息包含：吊臂的回轉(zhuǎn)角α(吊臂的回轉(zhuǎn)角范圍：0≤α≤360°，α為吊臂與車體坐標(biāo)系ox軸沿逆時針方向的夾角)，吊臂與xoy平面的仰角β(吊臂仰角范圍：0≤β≤90°)，吊臂的長度L(吊臂長度L_min≤L≤L_max)，其中L_min為吊臂長度最小值、L_max為長度最大值(L_min、L_max通過吊臂的產(chǎn)品手冊獲取)。其中吊臂的回轉(zhuǎn)角α可以通過角度傳感器直接獲?。坏醣鄣拈L度L由長度傳感器直接測得。考慮到吊臂在起吊的過程中處于靜止?fàn)顟B(tài)，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：

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

式中，g表示重力加速度常量，sin^-1表示正弦函數(shù)的反函數(shù)。

綜上被吊起車輛重心在xoy平面的坐標(biāo)S₁(x₁ y₁)的具體計算公式如下：

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

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

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

吊臂的質(zhì)量M₂可以通過說明書或者靜態(tài)測量獲取。吊臂重心距吊臂起始端(吊臂與救援車輛轉(zhuǎn)臺連接處)的距離D隨著吊臂的長度L的變化而變化。本方法兼顧計算的復(fù)雜度和擬合的精度，選擇二次曲線擬合吊臂重心距吊臂起始端的距離D隨著吊臂的長度L的函數(shù)D＝A(L)，其中S(L)＝A₂L²+A₁L+A₀。通過靜態(tài)測量的方式獲得吊臂長度為L_min、L_mid、L_max以及對應(yīng)的吊臂重心距吊臂起始端的距離D_min、D_mid、D_max，其中L_mid＝0.5×(L_min+L_max)。然后通過以上獲得的數(shù)據(jù)解算出二次曲線的系數(shù)A₂、A₁、A₀的值，根據(jù)測得吊臂的長度L即可獲得吊臂重心距吊臂起始端的距離D的值。

因此吊臂的重心在xoy平面的坐標(biāo)S₂(x₂ y₂)具體計算公式如下：

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

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

其中，α、β意義與上文一致，表示吊臂的回轉(zhuǎn)角和俯仰角。

利用已知的救援車和吊臂以及被吊起的物體的重心平面位置、重量，通過組合法即可計算出整體重心的平面位置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)$

考慮到起吊作業(yè)下救援車輛處于動態(tài)，傳感器所獲得的數(shù)據(jù)存在較大的誤差隨機，進(jìn)而影響獲得整體重心平面位置的動態(tài)估計精度，所以本方法將所計算的救援車的整體重心的平面位置S_m(x_m y_m)作為觀測量，利用卡爾曼濾波算法以求獲得精度更高的整體重心的估計值。

卡爾曼濾波算法是以最小均方差為準(zhǔn)則的最優(yōu)狀態(tài)估計濾波算法，它不需要儲存過去的測量值，只根據(jù)當(dāng)前的觀測值和前一時刻的估計值進(jìn)行遞推計算，便可實現(xiàn)對實時信號的估計，具有數(shù)據(jù)存儲量小、算法簡便的特點。將該方法應(yīng)用于救援清障車起吊工況下的整體重心估計時，在保證實時性的同時，可以有效提高估計精度。

下面建立卡爾曼濾波的狀態(tài)方程和觀測方程。

離散化后的卡爾曼濾波的狀態(tài)方程的矩陣形式表示為：

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

式中，k表示離散化時刻；系統(tǒng)狀態(tài)向量為X＝[p1 p2]其中p1為平面坐標(biāo)系下的整體重心ox方向的位置坐標(biāo)，p2為車體坐標(biāo)系下的整體重心oy方向的位置坐標(biāo)，本發(fā)明中上角標(biāo)’表示對矩陣轉(zhuǎn)置；W表示零均值的系統(tǒng)高斯白噪聲向量且W＝[w₁ w₂]，其中w₁、w₂分別表示兩個系統(tǒng)高斯白噪聲分量，W對應(yīng)的系統(tǒng)噪聲協(xié)方差陣其中分別表示系統(tǒng)高斯白噪聲w₁、w₂對應(yīng)的方差；狀態(tài)轉(zhuǎn)移矩陣為這是因為救援清障車在工況下整體重心位置是緩慢變化的，可以認(rèn)為上一采樣時刻的整體重心坐標(biāo)等于下一采樣時刻的整體重心坐標(biāo)。

卡爾曼濾波觀測方程的離散化矩陣形式為：

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

式中，Z為觀測向量，H為觀測陣，V表示與W互不相關(guān)的零均值觀測白噪聲向量。觀測量取組合法計算出整體重心ox、oy軸方向坐標(biāo)值x_m、y_m，由于觀測向量與狀態(tài)向量都是指整體重心ox、oy軸方向坐標(biāo)值，所以表示整體重心ox方向坐標(biāo)的觀測噪聲且均值為0、方差為高斯白噪聲，表示通過式計算獲得的整體重心oy方向坐標(biāo)值的觀測噪聲且均值為0、方差為的高斯白噪聲；V對應(yīng)的觀測噪聲方差陣R可表示為

對于上述所描述的系統(tǒng)狀態(tài)方程和測量方程，運用卡爾曼濾波理論，建立下面的標(biāo)準(zhǔn)濾波遞推過程，該遞推過程包括時間更新和測量更新，下面遞推過程的前兩步為時間更新，剩余的三步為測量更新。

時間更新包括：

狀態(tài)一步預(yù)測方程

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

一步預(yù)測誤差方差陣

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)

狀態(tài)估計

估計誤差方差陣

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

經(jīng)過上述遞推計算后，可實時、動態(tài)估計起吊工況下救援清障車整體重心的平面位置。

附圖說明

圖1為本發(fā)明所提方法的流程圖

圖2為本發(fā)明所提吊臂回轉(zhuǎn)角示意圖

圖3為本發(fā)明所提吊臂仰角示意圖

圖4為本發(fā)明所提吊臂仰角估計模型示意圖

具體實施方式

據(jù)有關(guān)部門統(tǒng)計，近幾年救援車翻車事故頻發(fā)，救援過程中往往出現(xiàn)未能安全救起被救車輛，而救援車自身發(fā)生側(cè)翻或傾翻事故(二次事故)。究其原因，除了操作者違章作業(yè)、經(jīng)驗不足和管理不善等主觀因素外，救援車缺乏安全監(jiān)控裝置，無法為操作人員提供準(zhǔn)確的救援車狀態(tài)信息也是一個重要客觀因素。

根據(jù)救援車的工作特點，救援車通過吊臂對救援車輛進(jìn)行起吊等施救作業(yè)，考慮到被救車輛以及救援車支起的吊臂的影響，救援車整體重心(救援車和吊臂以及被救車輛的整體重心)就可能超出安全范圍的情況，進(jìn)而導(dǎo)致救援車自身發(fā)生側(cè)翻或傾翻事故。如果能夠針對起吊工況下(救援車進(jìn)行起吊作業(yè))救援車整體重心位置信息進(jìn)行實時估計和監(jiān)控，當(dāng)其超出安全范圍時給予駕駛員相應(yīng)的警示信息，則能夠避免大量的傾翻事故，但目前相關(guān)技術(shù)目前尚未見研究。所以本專利提出了一種起吊工況下救援車整體重心動態(tài)平面位置估計方法(本方法只考慮重心的平面位置的估計)，具有低成本、高精度、實時性好的優(yōu)點，防止救援過程中出現(xiàn)二次事故，具有重要的現(xiàn)實意義。

本方法中坐標(biāo)系采用平面坐標(biāo)系，以救援車轉(zhuǎn)臺軸心位置作為標(biāo)系的原點o(0 0)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側(cè)。本方法適用在路面水平、空曠的環(huán)境下，救援車進(jìn)行起吊作業(yè)時對整體重心的平面位置動態(tài)估計。

本方法采用了一個角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度傳感器。各個傳感器的安裝位置如下：角度傳感器安裝在吊臂轉(zhuǎn)臺上，其中傳感器的軸心與轉(zhuǎn)臺轉(zhuǎn)動軸心一直；加速度傳感器安裝在吊臂上，且安裝方向與吊臂方向一致；拉力傳感器安裝在吊鉤與吊臂連接處；長度傳感器安裝在吊臂上且與吊臂方向一致。

本發(fā)明公開了一種起吊工況下救援車整體重心平面位置的動態(tài)估計方法，將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置?？紤]到傳感器的誤差以及救援過程中產(chǎn)生的噪聲，本發(fā)明利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態(tài)估計，該方法可用于救援車安全監(jiān)控及危險工況預(yù)警，具有精度高、成本低、實時性好等顯著優(yōu)點。

具體各個部分的重量及重心平面位置獲取方式如下：

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

救援車的重量及重心水平位置(不包含吊臂)，可以通過救援車的產(chǎn)品手冊或靜態(tài)測量等方式獲得，救援車的重量為M₀，重心的平面位置為S₀(x₀ y₀)。

2、被吊起車輛的重量及重心平面位置

被吊起車輛重量M₁通過吊臂頂端拉力傳感器測量。由于被救車輛被吊起，根據(jù)懸掛法重心的平面位置與吊鉤的平面位置相同。吊鉤的平面位置，可由救援車吊臂上的角度傳感器、加速度傳感器、拉力傳感器，長度傳感器，獲得吊臂姿態(tài)信息，進(jìn)一步解算出吊鉤的平面位置。

其中吊臂姿態(tài)信息包含：吊臂的回轉(zhuǎn)角α(吊臂的回轉(zhuǎn)角范圍：0≤α≤360°，α為吊臂與車體坐標(biāo)系ox軸沿逆時針方向的夾角)，吊臂與xoy平面的仰角β(吊臂仰角范圍：0≤β≤90°)，吊臂的長度L(吊臂長度L_min≤L≤L_max)，其中L_min為吊臂長度最小值、L_max為長度最大值(L_min、L_max通過吊臂的產(chǎn)品手冊獲取)。其中吊臂的回轉(zhuǎn)角α可以通過角度傳感器直接獲?。坏醣鄣拈L度L由長度傳感器直接測得。考慮到吊臂在起吊的過程中處于靜止?fàn)顟B(tài)，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：

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

式中，g表示重力加速度常量，sin^-1表示正弦函數(shù)的反函數(shù)。

綜上被吊起車輛重心在xoy平面的坐標(biāo)S₁(x₁ y₁)的具體計算公式如下：

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

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

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

吊臂的質(zhì)量M₂可以通過說明書或者靜態(tài)測量獲取。吊臂重心距吊臂起始端(吊臂與救援車輛轉(zhuǎn)臺連接處)的距離D隨著吊臂的長度L的變化而變化。本方法兼顧計算的復(fù)雜度和擬合的精度，選擇二次曲線擬合吊臂重心距吊臂起始端的距離D隨著吊臂的長度L的函數(shù)D＝S(L)，其中S(L)＝A₂L²+A₁L+A₀。通過靜態(tài)測量的方式獲得吊臂長度為L_min、L_mid、L_max以及對應(yīng)的吊臂重心距吊臂起始端的距離D_min、D_mid、D_max，其中L_mid＝0.5×(L_min+L_max)。然后通過以上獲得的數(shù)據(jù)解算出二次曲線的系數(shù)A₂、A₁、A₀的值，根據(jù)測得吊臂的長度L即可獲得吊臂重心距吊臂起始端的距離D的值。

因此吊臂的重心在xoy平面的坐標(biāo)S₂(x₂ y₂)具體計算公式如下：

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

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

其中，α、β意義與上文一致，表示吊臂的回轉(zhuǎn)角和俯仰角。

利用已知的救援車和吊臂以及被吊起的物體的重心平面位置、重量，通過組合法即可計算出整體重心的平面位置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)$

考慮到起吊作業(yè)下救援車輛處于動態(tài)，傳感器所獲得的數(shù)據(jù)存在較大的誤差隨機，進(jìn)而影響獲得整體重心平面位置的動態(tài)估計精度，所以本專利將所計算的救援車的整體重心的平面位置S_m(x_m y_m)作為觀測量，利用卡爾曼濾波算法以求獲得精度更高的整體重心的估計值。

卡爾曼濾波算法是以最小均方差為準(zhǔn)則的最優(yōu)狀態(tài)估計濾波算法，它不需要儲存過去的測量值，只根據(jù)當(dāng)前的觀測值和前一時刻的估計值進(jìn)行遞推計算，便可實現(xiàn)對實時信號的估計，具有數(shù)據(jù)存儲量小、算法簡便的特點。將該方法應(yīng)用于救援清障車起吊工況下整體重心估計時，在保證實時性的同時，可以有效提高估計精度。

下面建立卡爾曼濾波的狀態(tài)方程和觀測方程。

離散化后的卡爾曼濾波的狀態(tài)方程的矩陣形式表示為：

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

式中，k表示離散化時刻；系統(tǒng)狀態(tài)向量為X＝[p1 p2]其中p1為平面坐標(biāo)系下的整體重心ox方向的位置坐標(biāo)，p2為車體坐標(biāo)系下的整體重心oy方向的位置坐標(biāo)，本發(fā)明中上角標(biāo)’表示對矩陣轉(zhuǎn)置；W表示零均值的系統(tǒng)高斯白噪聲向量且W＝[w₁ w₂]，其中w₁、w₂分別表示兩個系統(tǒng)高斯白噪聲分量，W對應(yīng)的系統(tǒng)噪聲協(xié)方差陣其中分別表示系統(tǒng)高斯白噪聲w₁、w₂對應(yīng)的方差；狀態(tài)轉(zhuǎn)移矩陣為這是因為救援清障車在工況下整體重心位置是緩慢變化的，可以認(rèn)為上一采樣時刻的整體重心坐標(biāo)等于下一采樣時刻的整體重心坐標(biāo)。

卡爾曼濾波觀測方程的離散化矩陣形式為：

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

式中，Z為觀測向量，H為觀測陣，V表示與W互不相關(guān)的零均值觀測白噪聲向量。觀測量取組合法計算出整體重心ox、oy軸方向坐標(biāo)值x_m、y_m，由于觀測向量與狀態(tài)向量都是指整體重心ox、oy軸方向坐標(biāo)值，所以表示整體重心ox方向坐標(biāo)的觀測噪聲且均值為0、方差為高斯白噪聲，表示通過式計算獲得的整體重心oy方向坐標(biāo)值的觀測噪聲且均值為0、方差為的高斯白噪聲；V對應(yīng)的觀測噪聲方差陣R可表示為

對于上述所描述的系統(tǒng)狀態(tài)方程和測量方程，運用卡爾曼濾波理論，建立下面的標(biāo)準(zhǔn)濾波遞推過程，該遞推過程包括時間更新和測量更新，下面遞推過程的前兩步為時間更新，剩余的三步為測量更新。

時間更新包括：

狀態(tài)一步預(yù)測方程

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

一步預(yù)測誤差方差陣

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)

狀態(tài)估計

估計誤差方差陣

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

經(jīng)過上述遞推計算后，可實時、動態(tài)估計起吊工況下救援清障車整體重心的平面位置。

技術(shù)效果

(1)本發(fā)明提出了一種低成本、高精度、實時性好的起吊工況下救援車整體重心平面位置估計方法，可用于救援車安全監(jiān)控和危險預(yù)警的需要；

(2)本發(fā)明根據(jù)救援清障車工作特點，對救援過程中整體重心模型進(jìn)行合理簡化，并利用卡爾曼濾波進(jìn)行救援車整體重心位置的估計，保障了其估計精度與實時性；

(3)本發(fā)明僅需一個低成本角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度感器，具有成本低的優(yōu)點，便于大規(guī)模推廣。