本發(fā)明涉及一種基于無損卡爾曼濾波的救援清障車姿態(tài)角估計方法，其目的在于通過無損變換使救援清障車姿態(tài)角估計的非線性模型適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系，從而獲得準(zhǔn)確的救援清障車姿態(tài)角估計值，可用于救援清障車姿態(tài)監(jiān)控及危險姿態(tài)預(yù)警，屬于救援清障車安全預(yù)警領(lǐng)域。
背景技術(shù)：
：隨著我國經(jīng)濟(jì)的快速增長，道路交通基礎(chǔ)設(shè)施條件逐漸改善，機(jī)動車保有量逐年增長，道路車輛事故救援、城市違章車輛處理以及自然災(zāi)害搶險救援等社會需求也隨著快速增長。與此同時，作為汽車后市場的救援清障行業(yè)也逐漸被重視，得到了快速的發(fā)展。根據(jù)相關(guān)數(shù)據(jù)顯示，救援清障車在作業(yè)過程中發(fā)生側(cè)翻或傾翻的事故率也在逐年增加，往往存在著未能正常作業(yè)卻自身發(fā)生事故的情況。究其原因，一方面存在救援清障車操作人員未按規(guī)定作業(yè)、處理事故經(jīng)驗(yàn)不足等主觀因素；另一方面車輛的姿態(tài)監(jiān)控裝置尚未完善，不能為操作人員提供準(zhǔn)確的姿態(tài)信息以及安全預(yù)警也是不可回避的客觀因素。目前，已有專利文獻(xiàn)對救援清障車姿態(tài)角進(jìn)行研究：專利文獻(xiàn)CN103616013A公開了一種基于遞歸最小二乘的救援清障車姿態(tài)角估計方法，該方法填補(bǔ)了國內(nèi)該領(lǐng)域的空白，但是僅適用于橫向坡度和縱向坡度較小的路面(即可采用arcsinδ≈δ進(jìn)行約等于處理的情況，其中δ為較小的坡度值)，應(yīng)用工況受限；專利文獻(xiàn)CN104061899A公開了一種基于卡爾曼濾波的車輛姿態(tài)角估計方法，可應(yīng)用于較為復(fù)雜的工況，但是該方法是在對測量數(shù)據(jù)計算后進(jìn)行濾波，并未采用基于非線性濾波的估計方法，濾波精度和效率存在一定的不足。為了解決上述技術(shù)在實(shí)際應(yīng)用中的不足，本發(fā)明提供了一種基于無損卡爾曼濾波的救援清障車姿態(tài)角估計方法。該方法采用較少的低成本MEMS(Micro-Electro-MechanicalSystem)加速度傳感器采集救援清障車的狀態(tài)信息，根據(jù)救援清障車的工作特點(diǎn)，基于車輛運(yùn)動學(xué)模型對車輛姿態(tài)角進(jìn)行估計，并且舍棄了擴(kuò)展卡爾曼濾波算法在解決非線性系統(tǒng)將其近似線性化的思路，而是通過無損變換使非線性系統(tǒng)適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系，提高了濾波精度和效率，從而得到更為準(zhǔn)確的姿態(tài)角估計值，為救援清障車提供姿態(tài)安全預(yù)警。技術(shù)實(shí)現(xiàn)要素：本發(fā)明的目的是提供一種基于無損卡爾曼濾波的救援清障車姿態(tài)角估計方法，該方法使用較少的MEMS加速度傳感器，基于車輛運(yùn)動學(xué)模型對姿態(tài)角進(jìn)行估計，可以為救援清障車提供精確可靠的姿態(tài)信息并提供有效的安全預(yù)警。本發(fā)明采用的技術(shù)方案如下：一種基于無損卡爾曼濾波的救援清障車姿態(tài)角估計方法，其特征在于：本發(fā)明采用兩個低成本的MEMS加速度傳感器采集救援清障車的狀態(tài)信息，根據(jù)救援清障車的工作特點(diǎn)，基于車輛運(yùn)動學(xué)模型對姿態(tài)角進(jìn)行估計，并且舍棄了擴(kuò)展卡爾曼濾波算法在解決非線性系統(tǒng)將其近似線性化的思路，而是通過無損變換使非線性系統(tǒng)適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系，提高了濾波的精度和效率，從而提供更為可靠的救援清障車姿態(tài)角估計值。具體步驟包括：1)建立救援清障車的運(yùn)動學(xué)模型忽略地球旋轉(zhuǎn)速度，車輛運(yùn)動學(xué)方程為：v·x=ax+wzvy-wyvz+gsinθv·y=ay-wzvx+wxvz-gsinφcosθv·z=az+wyvx-wxvy-gcosφcosθ---(1)]]>式(1)中，wx、wy和wz分別表示圍繞車體坐標(biāo)系縱軸、橫軸以及垂向軸的角速度，vx、vy和vz分別表示沿車體坐標(biāo)系縱軸、橫軸以及垂向軸的線速度，ax、ay和az分別表示沿車體坐標(biāo)系縱軸、橫軸以及垂向軸的加速度；θ、φ分別表示俯仰角、側(cè)傾角；g表示重力加速度；上標(biāo)“·”表示微分，如表示vx的微分。由于救援清障車在工作時，僅利用吊臂對事故車輛開展施救工作，車身部分相對于地面處于靜止?fàn)顟B(tài)，即可以合理地認(rèn)為vx、vy、vz、wx、wy和wz均為零，則救援清障車的運(yùn)動學(xué)模型可合理地簡化為：ax＝-gsinθ(2)ay＝gsinφcosθ2)所需車載傳感器安裝由式(2)可以看出，只需測得車輛縱向加速度與橫向加速度，即可對救援清障車的姿態(tài)角進(jìn)行估計。因此，兩個低成本MEMS加速度傳感器即可滿足救援清障車姿態(tài)角的測量要求。兩個低成本MEMS加速度傳感器應(yīng)安裝于車身(即除吊臂外救援清障車的其他部分)質(zhì)心位置附近，一個與車體坐標(biāo)系縱軸平行，用于測量縱向加速度，記為ax_m；另一個與車體坐標(biāo)系橫軸平行，用于測量成橫向加速度，記為ay_m。3)建立無損卡爾曼濾波的狀態(tài)方程和觀測方程無損卡爾曼濾波狀態(tài)方程為：xk＝f(xk-1)+Wk-1(3)式(3)中，k表示離散化時刻，xk＝[θφ]T，其中xk為系統(tǒng)狀態(tài)向量，θ、φ是系統(tǒng)的待估參數(shù)；由于救援清障車在工作過程中，側(cè)傾角與俯仰角是連續(xù)緩慢變化的，可認(rèn)為當(dāng)前采樣時刻的側(cè)傾角與俯仰角等于下一采樣時刻的側(cè)傾角與俯仰角，所以f(xk-1)＝xk-1；Wk-1表示對應(yīng)k-1時刻零均值的系統(tǒng)高斯白噪聲向量且W＝[w1w2]T，其中w1、w2分別表示兩個系統(tǒng)高斯白噪聲分量，Wk-1對應(yīng)的系統(tǒng)高斯白噪聲協(xié)方差陣Qk-1=]]>σw1200σw22,]]>其中分別表示系統(tǒng)高斯白噪聲w1、w2對應(yīng)的方差；無損卡爾曼濾波觀測方程為：yk＝h(xk)+Vk(4)式(4)中，yk＝[ax_may_m]T；由式(2)可知h(xk)＝[-gsinθgsinφcosθ]T；Vk表示與Wk互不相關(guān)的零均值測量高斯白噪聲向量且V＝[V1V2]T，其中v1、v2分別表示兩個測量高斯白噪聲分量，Vk對應(yīng)的測量高斯白噪聲協(xié)方差陣Rk=σv1200σv22,]]>其中分別表示測量高斯白噪聲v1、v2對應(yīng)的方差。4)無損卡爾曼濾波算法：根據(jù)式(3)和式(4)所描述的系統(tǒng)狀態(tài)方程和觀測方程，結(jié)合無損變換，建立無損卡爾曼濾波遞推過程，如下述步驟所示：步驟1：輸入變量初始化x^0=E[x0]P0=E[(x0-x^0)(x0-x^0)T]---(5)]]>步驟2：計算Sigma采樣點(diǎn)采用對稱采樣策略，產(chǎn)生2n+1個均值為方差為P的Sigma點(diǎn)集，n為狀態(tài)向量維數(shù)，本發(fā)明中n＝2；χk-10=x^k-1χk-1i=x^k-1+((n+λ)Pk-1)i,i=1,2χk-1i=x^k-1-((n+λ)Pk-1)i-n,i=3,4---(6)]]>式(6)中，為5個Sigma點(diǎn)；λ＝α2(n+κ)-n為調(diào)節(jié)Sigma點(diǎn)和的距離參數(shù)，調(diào)節(jié)它可提高逼近精度，其中α決定Sigma點(diǎn)圍繞均值的散布，通常取一小正數(shù)(如：1e-4≤α≤1)，本文取0.01；κ為比例因數(shù)，本發(fā)明取0；為加權(quán)協(xié)方差矩陣平方根的第i列；步驟3：無損卡爾曼濾波的時間更新方程xk|k-1i=f(χk-1i)---(7)]]>x^k-=Σi=02nWimχk|k-1i---(8)]]>Px,k-=Σi=02nWic(χk|k-1i-x^k-)(χk|k-1i-x^k-)T+Qk---(9)]]>γk|k-1i=h(χk|k-1i)---(10)]]>y^k-=Σi=02nWimγk|k-1i---(11)]]>相應(yīng)的權(quán)值定義如下：Wim=λn+λ,i=0Wic=λn+λ+1-α2+β,i=0Wim=Wic=12(n+λ),i=1,...,4---(12)]]>其中，β為比例因數(shù)，對于高斯分布，其最優(yōu)值為2；為均值的權(quán)值，為方差的權(quán)值，Σi=02nWic=1;]]>步驟4：無損卡爾曼濾波的測量更新方程Py,k=Σi=02nWic(γk|k-1i-y^k-)(γk|k-1i-y^k-)T+Rk---(13)]]>Pxy,k=Σi=02nWic(χk|k-1i-x^k-)(γk|k-1i-y^k-)T---(14)]]>Kk＝Pxy，k(Py，k)-1(15)x^k=x^k-+Kk(yk-y^k-)---(16)]]>Px,k=Px,k--KkPy,k(Kk)T---(17)]]>經(jīng)過上述步驟的遞推計算，可以準(zhǔn)確實(shí)時地估計出救援清障車在每個時刻的側(cè)傾角與俯仰角。具體實(shí)施方式隨著我國經(jīng)濟(jì)的快速增長，道路交通基礎(chǔ)設(shè)施條件逐漸改善，機(jī)動車保有量逐年增長，道路車輛事故救援、城市違章車輛處理以及自然災(zāi)害搶險救援等社會需求也隨著快速增長。與此同時，作為汽車后市場的救援清障行業(yè)也逐漸被重視，得到了快速的發(fā)展。根據(jù)相關(guān)數(shù)據(jù)顯示，救援清障車在作業(yè)過程中發(fā)生側(cè)翻或傾翻的事故率也在逐年增加，往往存在著未能正常作業(yè)卻自身發(fā)生事故的情況。究其原因，一方面存在救援清障車操作人員未按規(guī)定作業(yè)、處理事故經(jīng)驗(yàn)不足等主觀因素；另一方面車輛的姿態(tài)監(jiān)控裝置尚未完善，不能為操作人員提供準(zhǔn)確的姿態(tài)信息以及安全預(yù)警也是不可回避的客觀因素。目前，已有專利文獻(xiàn)對救援清障車姿態(tài)角進(jìn)行研究：專利文獻(xiàn)CN103616013A公開了一種基于遞歸最小二乘的救援清障車姿態(tài)角估計方法，該方法填補(bǔ)了國內(nèi)該領(lǐng)域的空白，但是僅適用于橫向坡度和縱向坡度較小的路面(即可采用arcsinδ≈δ進(jìn)行約等于處理的情況，其中δ為較小的坡度值)，應(yīng)用工況受限；專利文獻(xiàn)CN104061899A公開了一種基于卡爾曼濾波的車輛姿態(tài)角估計方法，可應(yīng)用于較為復(fù)雜的工況，但是該方法是在對測量數(shù)據(jù)計算后進(jìn)行濾波，并未采用基于非線性濾波的估計方法，濾波精度和效率存在一定的不足。為了解決上述技術(shù)在實(shí)際應(yīng)用中的不足，本發(fā)明提供了一種基于無損卡爾曼濾波的救援清障車姿態(tài)角估計方法。該方法采用較少的低成本MEMS(Micro-Electro-MechanicalSystem)加速度傳感器采集救援清障車的狀態(tài)信息，根據(jù)救援清障車的工作特點(diǎn)，基于車輛運(yùn)動學(xué)模型對車輛姿態(tài)角進(jìn)行估計，并且舍棄了擴(kuò)展卡爾曼濾波算法在解決非線性系統(tǒng)將其近似線性化的思路，而是通過無損變換使非線性系統(tǒng)適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼濾波體系，提高了濾波精度和效率，從而得到更為準(zhǔn)確的姿態(tài)角估計值，為救援清障車提供姿態(tài)安全預(yù)警。對于普通車輛而言，通常用來確定姿態(tài)角信息的方法是使用完整的六維慣性測量單元IMU(InertialMeasurementUnit)，該IMU包括3個加速度計和3個角速度陀螺儀，利用其輸出量和角度信息的微分之間的運(yùn)動學(xué)關(guān)系，并忽略地球旋轉(zhuǎn)速度，車輛運(yùn)動學(xué)過程可建模為[此處可參考文獻(xiàn)：H.EricTseng，LiXu，DavorHrovat，Estimationoflandvehiclerollandpitchangles[J].VehicleSystemDynamics：InternationalJournalofVehicleMechanicsandMobility，2007，45(5)：433-443.]：v·x=ax+wzvy-wyvz+gsinθv·y=ay-wzvx+wxvz-gsinφcosθv·z=az+wyvx-wxvy-gcosφcosθ---(1)]]>式(1)中，wx、wy和wz分別表示圍繞車體坐標(biāo)系縱軸、橫軸以及垂向軸的角速度，vx、vy和vz分別表示沿車體坐標(biāo)系縱軸、橫軸以及垂向軸的線速度，ax、ay和az分別表示沿車體坐標(biāo)系縱軸、橫軸以及垂向軸的加速度；θ、φ分別表示俯仰角、側(cè)傾角；g表示重力加速度；上標(biāo)“-”表示微分，如表示vx的微分。由式(1)可得，車輛的姿態(tài)角信息可以通過六維IMU的導(dǎo)航解算算法得出，這一類方法在諸多相關(guān)研究文獻(xiàn)中都有涉及。然而六維IMU價格較為昂貴，出于成本因素的考慮在實(shí)際應(yīng)用中難以大規(guī)模推廣。雖然MEMS加速度傳感器的測量精度相對較低，但是由于其具有體積小、耐用性好、價格低廉、性能穩(wěn)定等特點(diǎn)，自八十年代中后期發(fā)展極為迅速。本發(fā)明結(jié)合救援清障車的運(yùn)動學(xué)模型及工作特點(diǎn)，研究如何利用較少的MEMS加速度傳感器并通過無損卡爾曼濾波來進(jìn)行救援清障車的姿態(tài)角估計，以達(dá)到較高的穩(wěn)定性和精度。由于救援清障車在工作時，僅利用吊臂對事故車輛開展施救工作，車身部分相對于地面處于靜止?fàn)顟B(tài)，即可以合理地認(rèn)為vx、vy、vz、wx、wy和wz均為零，則救援清障車的運(yùn)動學(xué)模型可合理地簡化為：ax＝-gsinθ(2)ay＝gsinφcosθ由式(2)可以看出，只需測得車輛縱向加速度與橫向加速度，即可對救援清障車的姿態(tài)角進(jìn)行估計。因此，兩個低成本MEMS加速度傳感器即可滿足救援清障車姿態(tài)角的測量要求。兩個低成本MEMS加速度傳感器應(yīng)安裝于車身(即除吊臂外救援清障車的其他部分)質(zhì)心位置附近，一個與車體坐標(biāo)系縱軸平行，用于測量縱向加速度，記為ax_m；另一個與車體坐標(biāo)系橫軸平行，用于測量成橫向加速度，記為ay_m。綜合救援清障車的工作特點(diǎn)，根據(jù)公式(1)建立的運(yùn)動學(xué)模型及其簡化形式，基于無損卡爾曼濾波算法，無損卡爾曼濾波狀態(tài)方程為：xk＝f(xk-1)+Wk-1(3)式(3)中，k表示離散化時刻，xk＝[θφ]T，其中xk為系統(tǒng)狀態(tài)向量，θ、φ是系統(tǒng)的待估參數(shù)；由于救援清障車在工作過程中，側(cè)傾角與俯仰角是連續(xù)緩慢變化的，可認(rèn)為當(dāng)前采樣時刻的側(cè)傾角與俯仰角等于下一采樣時刻的側(cè)傾角與俯仰角，所以f(xk-1)＝xk-1；Wk-1表示對應(yīng)k-1時刻零均值的系統(tǒng)高斯白噪聲向量且W＝[w1w2]T，其中w1、w2分別表示兩個系統(tǒng)高斯白噪聲分量，Wk-1對應(yīng)的系統(tǒng)高斯白噪聲協(xié)方差陣Qk-1=]]>σw1200σw22,]]>其中分別表示系統(tǒng)高斯白噪聲w1、w2對應(yīng)的方差；無損卡爾曼濾波觀測方程為：yk＝h(xk)+Vk(4)式(4)中，yk＝[ax_may_m]T；由式(2)可知h(xk)＝[-gsinθgsinφcosθ]T；Vk表示與Wk互不相關(guān)的零均值測量高斯白噪聲向量且V＝[V1V2]T，其中v1、v2分別表示兩個測量高斯白噪聲分量，Vk對應(yīng)的測量高斯白噪聲協(xié)方差陣Rk=σv1200σv22,]]>其中分別表示測量高斯白噪聲v1、v2對應(yīng)的方差。無損卡爾曼濾波的具體算法步驟如下：步驟1：輸入變量初始化x^0=E[x0]P0=E[(x0-x^0)(x0-x^0)T]---(5)]]>為確保算法收斂，初始誤差方陣P0設(shè)為二階單位矩陣；步驟2：計算Sigma采樣點(diǎn)采用對稱采樣策略，產(chǎn)生2n+1個均值為方差為P的Sigma點(diǎn)集，n為狀態(tài)向量維數(shù)，本發(fā)明中n＝2；χk-10=x^k-1χk-1i=x^k-1+((n+λ)Pk-1)i,i=1,2χk-1i=x^k-1-((n+λ)Pk-1)i-n,i=3,4---(6)]]>式(6)中，為5個Sigma點(diǎn)；λ＝α2(n+κ)-n為調(diào)節(jié)Sigma點(diǎn)和的距離參數(shù)，調(diào)節(jié)它可提高逼近精度，其中α決定Sigma點(diǎn)圍繞均值的散布，通常取一小正數(shù)(如：1e-4≤α≤1)，本文取0.01；κ為比例因數(shù)，本發(fā)明取0；為加權(quán)協(xié)方差矩陣平方根的第i列，為加權(quán)協(xié)方差矩陣平方根的第i-n列；步驟3：無損卡爾曼濾波的時間更新方程由系統(tǒng)狀態(tài)方程對各個Sigma點(diǎn)進(jìn)行變換；xk|k-1i=f(χk-1i)---(7)]]>對變換后的Sigma點(diǎn)集進(jìn)行加權(quán)處理，從而得到狀態(tài)的一步預(yù)測值；x^k-=Σi=02nWimχk|k-1i---(8)]]>通過加權(quán)處理得到方差陣的一步預(yù)測值；Px,k-=Σi=02nWic(χk|k-1i-x^k-)(χk|k-1i-x^k-)T+Qk---(9)]]>由系統(tǒng)觀測方程對變換后的Sigma點(diǎn)集進(jìn)行非線性變換；γk|k-1i=h(χk|k-1i)---(10)]]>使用加權(quán)求和計算得到系統(tǒng)的一步預(yù)測觀測值；y^k-=Σi=02nWimγk|k-1i---(11)]]>相應(yīng)的權(quán)值定義如下：Wim=λn+λ,i=0Wic=λn+λ+1-α2+β,i=0Wim=Wic=12(n+λ),i=1,...,4---(12)]]>其中，β為比例因數(shù)，對于高斯分布，其最優(yōu)值為2；為均值的權(quán)值，為方差的權(quán)值，Σi=02nWic=1;]]>步驟4：無損卡爾曼濾波的測量更新方程Py,k=Σi=02nWic(γk|k-1i-y^k-)(γk|k-1i-y^k-)T+Rk---(13)]]>Pxy,k=Σi=02nWic(χk|k-1i-x^k-)(γk|k-1i-y^k-)T---(14)]]>其中，Py，k為輸出預(yù)測值協(xié)方差；Pxy，k為狀態(tài)值與測量值的協(xié)方差；濾波更新：Kk＝Pxy，k(Py，k)-1(15)x^k=x^k-+Kk(yk-y^k-)---(16)]]>Px,k=Px,k--KkPy,k(Kk)T---(17)]]>其中，Kk為無損卡爾曼濾波增益；為更新后的狀態(tài)向量估計值；Px，k為對應(yīng)的后驗(yàn)方差；經(jīng)過上述步驟的遞推計算，可以準(zhǔn)確實(shí)時地估計出救援清障車在每個時刻的側(cè)傾角與俯仰角。當(dāng)前第1頁1 2 3