技術特征：

1.一種起吊工況下救援車整體重心平面位置的動態(tài)估計方法，采用平面坐標系，以救援車轉(zhuǎn)臺軸心位置作為平面標系的原點o(0 0)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側(cè)；

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

將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置；利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態(tài)估計；

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

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

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

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

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

其中吊臂姿態(tài)信息包含：吊臂的回轉(zhuǎn)角α，吊臂與xoy平面的仰角β，吊臂的長度L；其中吊臂的回轉(zhuǎn)角α可以通過角度傳感器直接獲??；吊臂的長度L由長度傳感器直接測得；考慮到吊臂在起吊的過程中處于靜止狀態(tài)，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：

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

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

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

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

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

(3)吊臂的重量及重心位置

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

因此，吊臂的重心在xoy平面的坐標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)$

將所計算的救援車的整體重心的平面位置S_m(x_m y_m)作為觀測量，利用卡爾曼濾波算法以求獲得精度更高的整體重心的估計值；

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

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

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

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

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

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

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

時間更新包括：

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

$\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)

狀態(tài)估計

估計誤差方差陣

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

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