起吊工況下救援車整體重心平面位置的動態估計方法技術

本發明專利技術公開了一種起吊工況下救援車整體重心平面位置的動態估計方法，將救援車自身及吊臂重心與被救援車輛重心的平面位置通過組合法獲得整體重心的平面位置，然后利用卡爾曼濾波算法得出動態整體重心平面位置的估計，該方法可用于救援車安全監控及危險工況預警，具有精度高、成本低、實時性好等顯著優點。

【技術實現步驟摘要】

本專利技術涉及一種起吊工況下救援車整體重心平面位置的動態估計方法，可用于救援清障車姿態監控及危險姿態預警，屬于救援清障車安全預警領域。
技術介紹
據有關部門統計，近幾年救援車翻車事故頻發，救援過程中往往出現未能安全救起被救車輛，而救援車自身發生側翻或傾翻事故(二次事故)。究其原因，除了操作者違章作業、經驗不足和管理不善等主觀因素外，救援車缺乏安全監控裝置，無法為操作人員提供準確的救援車實時狀態信息也是一個重要客觀因素。根據救援車的工作特點，救援車通過吊臂對救援車輛進行起吊等施救作業，考慮到被救車輛以及救援車支起的吊臂的影響，救援車整體重心(救援車和吊臂以及被救車輛的整體重心)就可能超出安全范圍的情況，進而導致救援車自身發生側翻或傾翻事故。如果能夠針對起吊工況下(救援車進行起吊作業)救援車整體重心位置信息進行實時估計和監控，當其超出安全范圍時給予駕駛員相應的警示信息，則能夠避免大量的傾翻事故，但目前相關技術目前尚未見研究。所以，本專利提出了一種起吊工況下救援車整體重心動態平面位置估計方法(本方法只考慮重心的平面位置的估計)，具有低成本、高精度、實時性好的優點，防止救援過程中出現二次事故，具有重要的現實意義。
技術實現思路
本專利技術提出了起吊工況下救援車整體重心平面位置的動態估計方法，已解決目前缺乏對起吊工況下救援車整體動態重心實時監控、預警方法的問題。本方法中坐標系采用平面坐標系，以救援車轉臺軸心位置作為標系的原點o(00)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側。本方法適用在路面水平、空曠的環境下，救援車進行起吊作業時對整體重心的平面位置動態估計。本方法采用了一個角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度傳感器。各個傳感器的安裝位置如下：角度傳感器安裝在吊臂轉臺上，其中傳感器的軸心與轉臺轉動軸心一直；加速度傳感器安裝在吊臂上，且安裝方向與吊臂方向一致；拉力傳感器安裝在吊鉤與吊臂連接處；長度傳感器安裝在吊臂上且與吊臂方向一致。本專利技術公開了一種起吊工況下救援車整體重心平面位置的動態估計方法，將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置。考慮到傳感器的誤差以及救援過程中產生的噪聲，本專利技術利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態估計，該方法可用于救援車安全監控及危險工況預警，具有精度高、成本低、實時性好等顯著優點。具體各個部分的重量及重心平面位置獲取方式如下：1、救援車的重量及重心平面位置救援車的重量及重心水平位置(不包含吊臂)，可以通過救援車的產品手冊或靜態測量等方式獲得，救援車的重量為M0，重心的平面位置為S0(x0y0)。2、被吊起車輛的重量及重心平面位置被吊起車輛重量M1通過吊臂頂端拉力傳感器測量。由于被救車輛被吊起，根據懸掛法重心的平面位置與吊鉤的平面位置相同。吊鉤的平面位置，可由救援車吊臂上的角度傳感器、加速度傳感器、拉力傳感器，長度傳感器，獲得吊臂姿態信息，進一步解算出吊鉤的平面位置。其中吊臂姿態信息包含：吊臂的回轉角α(吊臂的回轉角范圍：0≤α≤360°，α為吊臂與車體坐標系ox軸沿逆時針方向的夾角)，吊臂與xoy平面的仰角β(吊臂仰角范圍：0≤β≤90°)，吊臂的長度L(吊臂長度Lmin≤L≤Lmax)，其中Lmin為吊臂長度最小值、Lmax為長度最大值(Lmin、Lmax通過吊臂的產品手冊獲取)。其中吊臂的回轉角α可以通過角度傳感器直接獲取；吊臂的長度L由長度傳感器直接測得。考慮到吊臂在起吊的過程中處于靜止狀態，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：β=sin-1(Ag)---(1)]]>式中，g表示重力加速度常量，sin-1表示正弦函數的反函數。綜上被吊起車輛重心在xoy平面的坐標S1(x1y1)的具體計算公式如下：x1＝cosβ×L×cosα(2)y1＝cosβ×L×sinα(3)3、吊臂的重量及重心位置吊臂的質量M2可以通過說明書或者靜態測量獲取。吊臂重心距吊臂起始端(吊臂與救援車輛轉臺連接處)的距離D隨著吊臂的長度L的變化而變化。本方法兼顧計算的復雜度和擬合的精度，選擇二次曲線擬合吊臂重心距吊臂起始端的距離D隨著吊臂的長度L的函數D＝A(L)，其中S(L)＝A2L2+A1L+A0。通過靜態測量的方式獲得吊臂長度為Lmin、Lmid、Lmax以及對應的吊臂重心距吊臂起始端的距離Dmin、Dmid、Dmax，其中Lmid＝0.5×(Lmin+Lmax)。然后通過以上獲得的數據解算出二次曲線的系數A2、A1、A0的值，根據測得吊臂的長度L即可獲得吊臂重心距吊臂起始端的距離D的值。因此吊臂的重心在xoy平面的坐標S2(x2y2)具體計算公式如下：x2＝cosβ×D×cosα(4)y2＝cosβ×D×sinα(5)其中，α、β意義與上文一致，表示吊臂的回轉角和俯仰角。利用已知的救援車和吊臂以及被吊起的物體的重心平面位置、重量，通過組合法即可計算出整體重心的平面位置Sm(xmym)，具體計算公式如下：xm=M0x0+M1x1+M2x2M0+M1+M2---(6)]]>ym=M0y0+M1y1+M2y2M0+M1+M2---(7)]]>考慮到起吊作業下救援車輛處于動態，傳感器所獲得的數據存在較大的誤差隨機，進而影響獲得整體重心平面位置的動態估計精度，所以本方法將所計算的救援車的整體重心的平面位置Sm(xmym)作為觀測量，利用卡爾曼濾波算法以求獲得精度更高的整體重心的估計值。卡爾曼濾波算法是以最小均方差為準則的最優狀態估計濾波算法，它不需要儲存過去的測量值，只根據當前的觀測值和前一時刻的估計值進行遞推計算，便可實現對實時信號的估計，具有數據存儲量小、算法簡便的特點。將該方法應用于救援清障車起吊工況下的整體重心估計時，在保證實時性的同時，可以有效提高估計精度。下面建立卡爾曼濾波的狀態方程和觀測方程。離散化后的卡爾曼濾波的狀態方程的矩陣形式表示為：X(k)＝F(k，k-1)X(k-1)+W(k-1)(8)式中，k表示離散化時刻；系統狀態向量為X＝[p1p2]其中p1為平面坐標系下的整體重心ox方向的位置坐標，p2為車體坐標系下的整體重心oy方向的位置坐標，本專利技術中上角標’表示對矩陣轉置；W表示零均值的系統高斯白噪聲向量且W＝[w1w2]，其中w1、w2分別表示兩個系統高斯白噪聲分量，W對應的系統噪聲協方差陣其中分別表示系統高斯白噪聲w1、w2對應的方差；狀態轉移矩陣為這是因為救援清障車在工況下整體重心位置是緩慢變化的，可以認為上一采樣時刻的整體重心坐標等于下一采樣時刻的整體重心坐標。卡爾曼濾波觀測方程的離散化矩陣形式為：Z(k)＝H(k)X(k)+V(k)(9)式中，Z為觀測向量，H為觀測陣，V表示與W互不相關的零均值觀測白噪聲向量。觀測量取組合法計算出整體重心ox、oy軸方向坐標值xm、ym，由于觀測向量與狀態向量都是指整體重心ox、oy軸方向坐標值，所以表示整體重心ox方向坐標的觀測噪聲且均值為0、方差為高斯白噪聲，表示通過式計算獲得的整體重心oy方向坐標值的觀測噪聲且均值為0、方差為的高斯白噪聲；V對應的觀測噪聲方差陣R可表示為對于上述所描本文檔來自技高網...

【技術保護點】
一種起吊工況下救援車整體重心平面位置的動態估計方法，采用平面坐標系，以救援車轉臺軸心位置作為平面標系的原點o(0?0)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側；采用一個角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度傳感器；角度傳感器安裝在吊臂轉臺上，其中傳感器的軸心與轉臺轉動軸心一致；加速度傳感器安裝在吊臂上，且安裝方向與吊臂方向一致；拉力傳感器安裝在吊鉤與吊臂連接處；長度傳感器安裝在吊臂上且與吊臂方向一致；將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置；利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態估計；具體各個部分的重量及重心平面位置獲取方式如下：(1)救援車的重量及重心平面位置救援車的重量及重心水平位置(不包含吊臂)，可以通過救援車的產品手冊或靜態測量等方式獲得，救援車的重量為M0，重心的平面位置為S0(x0?y0)；(2)被吊起車輛的重量及重心平面位置被吊起車輛重量M1通過吊臂頂端拉力傳感器測量；由于被救車輛被吊起，根據懸掛法重心的平面位置與吊鉤的平面位置相同；吊鉤的平面位置，可由救援車吊臂上的角度傳感器、加速度傳感器、拉力傳感器，長度傳感器，獲得吊臂姿態信息，進一步解算出吊鉤的平面位置S1(x1?y1)；其中吊臂姿態信息包含：吊臂的回轉角α，吊臂與xoy平面的仰角β，吊臂的長度L；其中吊臂的回轉角α可以通過角度傳感器直接獲取；吊臂的長度L由長度傳感器直接測得；考慮到吊臂在起吊的過程中處于靜止狀態，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：β=sin-1(Ag)---(1)]]>式中，g表示重力加速度常量，sin?1表示正弦函數的反函數；綜上被吊起車輛重心在xoy平面的坐標S1(x1?y1)的具體計算公式如下：x1＝cosβ×L×cosα???(2)y1＝cosβ×L×sinα???(3)(3)吊臂的重量及重心位置吊臂的質量M2可以通過說明書或者靜態測量獲取；吊臂重心距吊臂起始端即吊臂與救援車輛轉臺連接處的距離D隨著吊臂的長度L的變化而變化，選擇二次曲線擬合吊臂重心距吊臂起始端的距離D隨著吊臂的長度L的函數D＝S(L)，其中S(L)＝A2L2+A1L+A0。通過靜態測量的方式獲得吊臂長度為Lmin、Lmid、Lmax以及對應的吊臂重心距吊臂起始端的距離Dmin、Dmid、Dmax，其中Lmid＝0.5×(Lmin+Lmax)；通過以上獲得的數據解算出二次曲線的系數A2、A1、A0的值，根據測得吊臂的長度L即可獲得吊臂重心距吊臂起始端的距離D的值。因此，吊臂的重心在xoy平面的坐標S2(x2?y2)具體計算公式如下：x2＝cosβ×D×cosα???(4)y2＝cosβ×D×sinα???(5)其中，α、β表示吊臂的回轉角和俯仰角。利用已知的救援車和吊臂以及被吊起的物體的重心平面位置、重量，通過組合法即可計算出整體重心的平面位置Sm(xm?ym)，具體計算公式如下：xm=M0x0+M1x1+M2x2M0+M1+M2---(6)]]>ym=M0y0+M1y1+M2y2M0+M1+M2---(7)]]>將所計算的救援車的整體重心的平面位置Sm(xm?ym)作為觀測量，利用卡爾曼濾波算法以求獲得精度更高的整體重心的估計值；離散化后的卡爾曼濾波的狀態方程的矩陣形式表示為：X(k)＝F(k，k?1)X(k?1)+W(k?1)???(8)式中，k表示離散化時刻；系統狀態向量為X＝[p1?p2]′其中p1為平面坐標系下的整體重心ox方向的位置坐標，p2為車體坐標系下的整體重心oy方向的位置坐標上角標’表示對矩陣轉置；W表示零均值的系統高斯白噪聲向量且W＝[w1?w2]′，其中w1、w2分別表示兩個系統高斯白噪聲分量，W對應的系統噪聲協方差陣其中分別表示系統高斯白噪聲w1、w2對應的方差；狀態轉移矩陣為救援清障車在工況下整體重心位置是緩慢變化的，上一采樣時刻的整體重心坐標等于下一采樣時刻的整體重心坐標；卡爾曼濾波觀測方程的離散化矩陣形式為：Z(k)＝H(k)X(k)+V(k)???(9)式中，Z為觀測向量，H為觀測陣，V表示與W互不相關的零均值觀測白噪聲向量；觀測量取組合法計算出整體重心ox、oy軸方向坐標值xm、ym，由于觀測向量與狀態向量都是指整體重心ox、oy軸方向坐標值，所以表示整體重心ox方向坐標的觀測噪聲且均值為0、方差為高斯白噪聲，表示通過式計算獲得的整體重心oy方向坐標值的觀測噪聲且均值為0、方差為的高斯白噪聲；V對應的觀測噪聲方差陣R可表示為對于上述系統狀態方程和測量方程，運用卡爾曼濾波理論，建立標準濾波遞推過程，該遞推過程包括時間更新和測量更新，遞推過程的前兩步為時間更新，剩余的三步為...

【技術特征摘要】
1.一種起吊工況下救援車整體重心平面位置的動態估計方法，采用平面坐標系，以救援車轉臺軸心位置作為平面標系的原點o(00)，ox軸沿車輛縱軸指向車輛的前方，oy軸沿車輛橫軸指向車輛的左側；采用一個角度傳感器、一個拉力傳感器、一個加速度傳感器、一套長度傳感器；角度傳感器安裝在吊臂轉臺上，其中傳感器的軸心與轉臺轉動軸心一致；加速度傳感器安裝在吊臂上，且安裝方向與吊臂方向一致；拉力傳感器安裝在吊鉤與吊臂連接處；長度傳感器安裝在吊臂上且與吊臂方向一致；將救援車自身和吊臂重心與被救援車輛重心的平面位置通過組合法獲得救援車整體重心的平面位置；利用卡爾曼濾波算法得出精度更高的救援車整體重心平面位置的動態估計；具體各個部分的重量及重心平面位置獲取方式如下：(1)救援車的重量及重心平面位置救援車的重量及重心水平位置(不包含吊臂)，可以通過救援車的產品手冊或靜態測量等方式獲得，救援車的重量為M0，重心的平面位置為S0(x0y0)；(2)被吊起車輛的重量及重心平面位置被吊起車輛重量M1通過吊臂頂端拉力傳感器測量；由于被救車輛被吊起，根據懸掛法重心的平面位置與吊鉤的平面位置相同；吊鉤的平面位置，可由救援車吊臂上的角度傳感器、加速度傳感器、拉力傳感器，長度傳感器，獲得吊臂姿態信息，進一步解算出吊鉤的平面位置S1(x1y1)；其中吊臂姿態信息包含：吊臂的回轉角α，吊臂與xoy平面的仰角β，吊臂的長度L；其中吊臂的回轉角α可以通過角度傳感器直接獲取；吊臂的長度L由長度傳感器直接測得；考慮到吊臂在起吊的過程中處于靜止狀態，吊臂的仰角β可通過加速度傳感器采集的吊臂方向重力的分量A解算出來，具體公式如下：β=sin-1(Ag)---(1)]]>式中，g表示重力加速度常量，sin-1表示正弦函數的反函數；綜上被吊起車輛重心在xoy平面的坐標S1(x1y1)的具體計算公式如下：x1＝cosβ×L×cosα(2)y1＝cosβ×L×sinα(3)(3)吊臂的重量及重心位置吊臂的質量M2可以通過說明書或者靜態測量獲取；吊臂重心距吊臂起始端即吊臂與救援車輛轉臺連接處的距離D隨著吊臂的長度L的變化而變化，選擇二次曲線擬合吊臂重心距吊臂起始端的距離D隨著吊臂的長度L的函數D＝S(L)，其中S(L)＝A2L2+A1L+A0。通過靜態測量的方式獲得吊臂長度為Lmin、Lmid、Lmax以及對應的吊臂重心距吊臂起始端的距離Dmin、Dmid、Dmax，其中Lmid＝0.5×(Lmin+Lmax)；通過以上獲得的數據解算出二次曲線的系數A2、A1、A0的值，根據測得吊臂的長度L即可獲得吊臂重心距吊臂起始端的距離D的值。因此，吊臂...

【專利技術屬性】
技術研發人員：張國勝，周煒，唐歌騰，任春曉，黃李原，
申請(專利權)人：交通運輸部公路科學研究所，
類型：發明
國別省市：北京;11