專利名稱：車載道路坡度角測量系統(tǒng)及車載道路坡度角測量方法
技術領域：
本發(fā)明涉及一種用于機動車上坡時對道路的坡度角進行測量的車載道路坡度角測量系統(tǒng)及車載道路坡度角測量方法。
背景技術：
為了使機動車在坡路上行駛時能獲得更好的驅(qū)動性能，需要在車身上安裝對道路坡度角進行計算的測量裝置。專利號為6714851的美國專利中公開了一種能夠?qū)Φ缆菲露冉沁M行測量的裝置，但這種裝置結構比較復雜，需要在整個機動車系統(tǒng)中安裝多個傳感器如轉(zhuǎn)向輪轉(zhuǎn)角傳感器、加速度傳感器和車輪轉(zhuǎn)速傳感器，其造價高，并且所測量得出的道路坡度角中包含有機動車的俯仰角。俯仰角是指將機動車在同一條直線上的前后兩個輪子各看成一個質(zhì)點，連接兩質(zhì)點所得到的直線與坡面的夾角即為俯仰角。由于俯仰角不可忽略，因此這種裝置所測量得出的道路坡度值不準確，其精度不高。

發(fā)明內(nèi)容
本發(fā)明的一個目的是提供一種結構簡單、能夠?qū)Φ缆菲露冉沁M行較精確測量的車載道路坡度角測量系統(tǒng)。本發(fā)明的另一目的是提供一種通過所述測量系統(tǒng)對道路坡度角進行測量的方法。
根據(jù)本發(fā)明的第一個方面，提供一種車載道路坡度角測量系統(tǒng)，其包括GPS速度測量儀，其用于測量行駛中的機動車在某一瞬間的垂直速度Vv及水平速度Vh；安裝在機動車前懸架處的前懸架位移傳感器，用于測量所述瞬間時的前懸架位置處的車身沿垂直于道路平面方向的位移Zf；
安裝在機動車后懸架處的后懸架位移傳感器，用于測量所述瞬間時的后懸架位置處的車身沿垂直于道路平面方向的位移Zr；電子控制單(ECU)，其與所述GPS速度測量儀電連接以獲取所述垂直速度Vv和水平速度Vh，與所述前懸架位移傳感器電連接以獲取所述前懸架位置處的車身沿垂直于道路平面方向的位移Zf，與所述后懸架位移傳感器電連接以獲取所述后懸架位置處的車身沿垂直于道路平面方向的位移Zr，并根據(jù)下列公式(1)、(2)和(3)計算出道路坡度角θαGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ(3)式中，γ為機動車行駛在坡度路面時的俯仰角，θ為需要測定的道路坡度角，αGPS為由GPS速度測量儀測定的速度通過公式(1)計算得出的俯仰角與道路坡度角之和，L為前懸架位移傳感器與后懸架位移傳感器之間的距離。
所述GPS速度測量儀優(yōu)選安裝在機動車的車頂上。
所述電子控制單元包括數(shù)據(jù)接收模塊和數(shù)據(jù)處理模塊；數(shù)據(jù)接收模塊用于從所述GPS速度測量儀接收所述垂直速度Vv和水平速度Vh，并從所述前、后懸架位移傳感器分別接收所述前懸架位置處的車身沿垂直于道路平面方向的位移Zf和所述后懸架位置處的車身沿垂直于道路平面方向的位移Zr；所述數(shù)據(jù)處理模塊用于根據(jù)接收的Vv、Vh、Zf、Zr、以及由前、后懸架位移傳感器的安裝位置所確定的L的值，由公式(1)、(2)和(3)計算出道路坡度角θ。
通過測得的道路坡度角，就可以通過機動車的驅(qū)動控制模塊對機動車在坡路上的行駛進行有效控制。
在實際的測量過程中，由于GPS速度測量儀可能存在測量偏差(噪聲)，因此測得的αGPS和實際的α之間的關系可以表達為下列公式(4)αGPS＝α+η (4)式中η為GPS測量過程中的噪聲。
為了更精確地測量道路坡度角θ，根據(jù)本發(fā)明的一種優(yōu)選的實施方式，在機動車的質(zhì)心處還安裝了一個陀螺儀，其與所述電子控制單元(ECU)電連接，用來測量機動車的俯仰角速度q。記陀螺儀的俯仰角速度測量值為qgyro，則存在下列關系式(5)qgyro=α·+qbias+ϵ---(5)]]>式中 表示實際的機動車俯仰角速度，qbias是陀螺儀的偏差，ε是陀螺儀測量過程中的噪聲(高斯白噪聲)。
將(5)式寫成矩陣的形式得α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α為實際的道路坡度角與機動車俯仰角之和， 表示實際的機動車俯仰角速度，qbias是陀螺儀的測量偏差， 表示陀螺儀偏差隨時間的變化率，qgyro是機動車俯仰角速度的陀螺儀測量值，ε是高斯白噪聲。
同樣也可以把(4)式寫成矩陣形式αGPS=10αqbias+η---(7)]]>由式(6)和式(7)可以構造一個俯仰卡爾曼濾波器算法，其狀態(tài)向量x＝[αqbias]T，量測向量為y＝αGPS，狀態(tài)轉(zhuǎn)移矩陣φ＝，控制矩陣ψ＝[10]T，量測矩陣H＝[10]。這樣，就可以較為準確地估計出α角和陀螺儀的偏差qbias。通過由此測得的α角替換公式(3)中的αGPS，同時通過公式(2)算出俯仰角γ，就可以通過公式(3)算出道路坡度角θ。
另外，根據(jù)該優(yōu)選的實施方式，當GPS速度測量儀不能正常工作時，可以設公式(5)中的qbias和ε均為0，這樣就可以通過由陀螺儀測量的俯仰角速度值qgyro粗略地估算出α角，通過將該α角替換公式(3)中的αGPS，同時通過公式(2)算出俯仰角γ，就可以通過公式(3)估算出道路坡度角θ。因此，即使GPS速度測量儀不能正常工作，通過本發(fā)明的該優(yōu)選的實施方式，也可以對道路坡度角進行估算，從而對機動車在坡路上的行駛進行有效控制。
根據(jù)本發(fā)明的第二個方面，提供一種通過所述測量系統(tǒng)對道路坡度角進行測量的方法，該方法包括由GPS速度測量儀測得在坡路上行駛的機動車在某一瞬間的垂直速度Vv及水平速度Vh；由安裝在機動車前懸架處的前懸架位移傳感器測得所述瞬間時的前懸架位置處的車身沿垂直于道路平面方向的位移Zf；由安裝在機動車后懸架處的后懸架位移傳感器測得所述瞬間時的后懸架位置處的車身沿垂直于道路平面方向的位移Zr；由電子控制單元(ECU)接收所述Vv、Vh、Zf、Zr的值，并由下列公式(1)、(2)和(3)計算出道路坡度角θαGPS＝arctan(Vv/Vh) (1)γ＝arctan((Zf-Zr)/L) (2)θ＝αGPS-γ(3)在本發(fā)明方法的一種優(yōu)選的實施方式中，由安裝在機動車質(zhì)心處的陀螺儀測得機動車的俯仰角速度qgyro，由下列公式(6)和(7)通過俯仰卡爾曼濾波器算法計算出實際的α和陀螺儀的偏差qbias，同時通過公式(2)算出俯仰角γ，再通過將該α替換公式(3)中的αGPS而由公式(3)算出道路坡度角θ。
α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>αGPS=10αqbias+η---(7)]]>當GPS速度測量儀不能正常工作時，可以設公式(5)中的qbias和ε均為0，這樣就可以通過由陀螺儀測量的俯仰角速度值qgyro粗略地估算出α角，通過將該α角替換公式(3)中的αGPS，同時通過公式(2)算出俯仰角γ，就可以通過公式(3)估算出道路坡度角θ。因此，即使GPS速度測量儀不能正常工作，通過本發(fā)明的該優(yōu)選的實施方式，也可以對道路坡度角進行估算，從而對機動車在坡路上的行駛進行有效控制。


圖1為根據(jù)本發(fā)明的坡度角測量系統(tǒng)的結構框圖；圖2為機動車在坡路上的狀況及根據(jù)本發(fā)明的坡度角測量系統(tǒng)各部件的安裝位置示意圖；圖3為電子控制單元(ECU)的工作流程圖；圖4為在任一時刻n時的卡爾曼濾波的處理框圖。
具體實施例方式
以下結合附圖對本發(fā)明的具體實施方式
進行更詳細的說明。
如圖1和圖2所示，本發(fā)明的車載道路坡度角測量系統(tǒng)包括一個位于機動車車頂?shù)能囕dGPS速度測量儀20、安裝在機動車質(zhì)心(GC)處的陀螺儀50、安裝在機動車前懸架上的前懸架位移傳感器30和安裝在機動車后懸架上的后懸架位移傳感器40，以及電子控制單元(ECU)60和驅(qū)動控制模塊70。位移傳感器30、40以及陀螺儀50均與電子控制單元(ECU)60電連接。驅(qū)動控制模塊70與電子控制單元電連接，用于根據(jù)電子控制單元(ECU)60提供的道路坡度角對機動車在坡路上的行駛進行有效控制。所述電子控制單元(ECU)60和驅(qū)動控制模塊70可以是集成在機動車主控板上的一部分。
所述GPS速度測量儀以及所述前、后懸架位移傳感器和陀螺儀是現(xiàn)有技術已有的常規(guī)設備，其可以從市售商品獲得，例如Thales Navigation公司的Aquarius 5002 unit或Racelogic公司的VBOX等。
本發(fā)明的工作原理如下如圖2所示，在坡度角為θ的道路上，在行駛方向為u的機動車10的車頂置有GPS速度測量儀20，由該GPS速度測量儀20可以獲得在坡路上行駛的機動車在某一瞬間的垂直速度Vv及水平速度Vh；電子控制單元(ECU)60的數(shù)據(jù)接收模塊獲取該速度后，由其數(shù)據(jù)處理模塊根據(jù)公式(1)計算出GPS速度測量儀的測量值αGPS，該αGPS應該是道路坡度角θ與機動車俯仰角η之和。
αGPS＝arctan(Vv/Vh) (1)式中Vv和Vh分別是GPS速度測量儀測量得到的垂直速度和水平速度。
同時，安裝在機動車前懸架處的前懸架位移傳感器30測得所述瞬間時的前懸架位置處的車身沿垂直于道路平面方向的位移Zf，安裝在機動車后懸架處的后懸架位移傳感器40測得所述瞬間時的后懸架位置處的車身沿垂直于道路平面方向的位移Zr，電子控制單元(ECU)60的數(shù)據(jù)接收模塊接收所述Zf和Zr的值，并且由于由前、后懸架位移傳感器的安裝位置所確定的L的值是已知的，因此電子控制單元(ECU)60的數(shù)據(jù)處理模塊可以根據(jù)下列公式(2)計算出機動車行駛在坡度路面時的俯仰角γγ＝arctan((Zf-Zr)/L) (2)由于αGPS是道路坡度角θ與機動車俯仰角η之和，因此電子控制單元(ECU)60的數(shù)據(jù)處理模塊可以根據(jù)下列公式(3)計算出道路坡度角θ。
θ＝αGPA-γ (3)在實際的測量過程中，由于GPS速度測量儀20可能存在測量偏差(噪聲)，因此測得的αGPS和實際的α之間的關系可以表達為下列公式(4)αGPS＝α+η (4)式中η為GPS測量過程中的噪聲。
為了更精確地測量道路坡度角θ，在機動車的質(zhì)心(GC)處還安裝了一個陀螺儀50，其與所述電子控制單元(ECU)60電連接，用來測量機動車的俯仰角速度q。記陀螺儀的俯仰角速度測量值為qgyro，則存在下列關系式(5)qgyro=α·+qbias+ϵ---(5)]]>式中qbias是陀螺儀的偏差，ε是陀螺儀測量過程中的噪聲。
將(5)式寫成矩陣的形式得α·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>同樣也可以把(4)式寫成矩陣形式得αGPS=10αqbias+η---(7)]]>公式(6)和(7)可以簡寫成如下形式得x·=Φx+Ψu+ϵ---(8)]]>y＝Hx+η (9)將公式(8)和(9)寫成離散化形式得x[n+1]＝Φx[n]+Ψu[n]+ε[n] (10)y[n]＝Hx[n]+η[n] (11)式中x＝[αqbias]T，y＝[αGPS]，Φ=0-100,]]>Ψ＝[10]T，H＝[10]，u＝[qgyro]，ε和η是相互獨立的高斯隨機向量，也就是高斯白噪聲。
對于形如公式(10)與公式(11)構成的單輸入-單輸出的系統(tǒng)狀態(tài)模型，可以用卡爾曼濾波算法對向量x進行最優(yōu)估計。
卡爾曼濾波是一種公知的數(shù)學算法，其數(shù)學原理如下形如公式(10)和(11)的狀態(tài)模型，其卡爾曼濾波滿足以下遞推方程A.測量更新[n|n]＝[n|n-1]+K[n](y[n]-H[n|n-1])(12)K[n]＝P[n|n-1]HT(HP[n|n-1]HT+R[n])-1(13)P[n|n]＝(I-K[n]H)P[n|n-1] (14)B.時間更新[n+1|n]＝Φ[n|n]+Ψu[n] (15)P[n+1|n]＝ΦP[n|n]ΦT+Q[n] (16)以上5個公式中[n|n]表示利用n時刻的量測值y[n]進行更新的x的更新值，也就是在n時刻對x進行的最佳估計，如圖4所示；[n+1|n]表示由y
直到y(tǒng)[n]的量測值估計的n+1時刻的x值；I是單位矩陣；K[n]是卡爾曼增益矩陣；Q[n]是模型噪聲ε[n]的協(xié)方差矩陣，為Q[n]＝E(ε[n]ε[n]T)；R[n]是量測噪聲η[n]的協(xié)方差矩陣，為R[n]＝E(η[n]η[n]T)；P[n|n]和P[n|n-1]是誤差協(xié)方差矩陣，他們的定義為P[n|n]＝E({x[n]-[n|n]}{x[n]-[n|n]T})P[n|n-1]＝E({x[n]-[n|n-1]}{x[n]-[n|n-1]T})其中x[n]表示在n時刻x的實際值，符號E()的含義是求括號中表達式的期望值。
當給定0時刻的初始值x[1|0]和P[1|0]后，就可以反復迭代卡爾曼濾波算法的5個公式，即公式(12)～公式(16)，從而在任一n時刻都可以估計出最佳的x值，如圖4所示。
這樣，就可以經(jīng)過卡爾曼濾波后較為準確地估計出x，即α角和陀螺儀的偏差qbias。通過由此測得的α角替換公式(3)中的αGPS，同時通過公式(2)算出俯仰角γ，就可以通過公式(3)算出道路坡度角θ。
另外，當GPS速度測量儀不能正常工作時，可以設公式(5)中的qbias和ε均為0，這樣就可以通過由陀螺儀測量的俯仰角速度值qgyro粗略地估算出α角，通過將該α角替換公式(3)中的αGPS，同時通過公式(2)算出俯仰角γ，就可以通過公式(3)估算出道路坡度角θ。因此，即使GPS速度測量儀不能正常工作，通過本發(fā)明提供的陀螺儀，也可以對道路坡度角進行估算。
通過得到的道路坡度角θ，驅(qū)動控制模塊70可以根據(jù)機動車動力學方程調(diào)節(jié)驅(qū)動力，使機動車能夠在坡道上也能夠獲得很好的驅(qū)動性能。
圖3為電子控制單元(ECU)的工作流程圖。在圖3的步驟S200中，ECU 60從GPS速度測量儀20、陀螺儀50、前懸架位移傳感器30和后懸架位移傳感器40獲取信號；在步驟S300中根據(jù)公式(1)計算α角的GPS測量值αGPS；在步驟S400中，利用步驟S300中的結果和俯仰卡爾曼濾波器算法由公式(6)和公式(7)計算α角的估計值；在步驟S500中，根據(jù)公式(2)計算汽車的俯仰角γ；在步驟S600中，根據(jù)公式(3)計算出道路的坡度角θ。
權利要求
1.一種車載道路坡度角測量系統(tǒng)，其包括GPS速度測量儀，其用于測量行駛中的機動車在某一瞬間的垂直速度Vv及水平速度Vh；安裝在機動車前懸架處的前懸架位移傳感器，用于測量所述瞬間時的前懸架位置處的車身沿垂直于道路平面方向的位移Zf；安裝在機動車后懸架處的后懸架位移傳感器，用于測量所述瞬間時的后懸架位置處的車身沿垂直于道路平面方向的位移Zr；電子控制單元，其與所述GPS速度測量儀電連接以獲取所述垂直速度Vv和水平速度Vh，與所述前懸架位移傳感器電連接以獲取所述前懸架位置處的車身沿垂直于道路平面方向的位移Zf，與所述后懸架位移傳感器電連接以獲取所述后懸架位置處的車身沿垂直于道路平面方向的位移Zr，并根據(jù)下列公式(1)、(2)和(3)計算出道路坡度角θαGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ(3)式中，γ為機動車行駛在坡度路面時的俯仰角，θ為需要測定的道路坡度角，αGPS為由GPS速度測量儀測定的所述速度通過公式(1)計算得出的機動車俯仰角與道路坡度角之和，L為前懸架位移傳感器與后懸架位移傳感器之間的距離。
2.根據(jù)權利要求1所述的車載道路坡度角測量系統(tǒng)，其中所述GPS速度測量儀安裝在機動車的車頂上。
3.根據(jù)權利要求1所述的車載道路坡度角測量系統(tǒng)，其中所述電子控制單元包括數(shù)據(jù)接收模塊和數(shù)據(jù)處理模塊；所述數(shù)據(jù)接收模塊用于從所述GPS速度測量儀接收所述垂直速度Vv和水平速度Vh，并從所述前、后懸架位移傳感器分別接收所述前懸架位置處的車身沿垂直于道路平面方向的位移Zf和所述后懸架位置處的車身沿垂直于道路平面方向的位移Zr；所述數(shù)據(jù)處理模塊用于根據(jù)接收的Vv、Vh、Zf、Zr、以及由前、后懸架位移傳感器的安裝位置所確定的L的值，由公式(1)、(2)和(3)計算出道路坡度角θ。
4.根據(jù)權利要求1所述的車載道路坡度角測量系統(tǒng)，還包括安裝在機動車質(zhì)心處的陀螺儀，其與所述電子控制單元電連接，用于測量機動車的俯仰角速度qgyro，由下列公式(6)和(7)通過俯仰卡爾曼濾波器算法計算出實際的α和陀螺儀的偏差qbias，同時通過公式(2)算出俯仰角γ，再通過將該α替換公式(3)中的αGPS而由公式(3)算出道路坡度角θα·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α為實際的道路坡度角與機動車俯仰角之和， 表示實際的機動車俯仰角速度，qbias是陀螺儀的測量偏差， 表示陀螺儀偏差隨時間的變化率，qgyro是機動車俯仰角速度的陀螺儀測量值，ε是高斯白噪聲；αGPS=10αqbias+η---(7)]]>式中η是1×1高斯白噪聲。
5.一種通過權利要求1所述測量系統(tǒng)對道路坡度角進行測量的方法，該方法包括由GPS速度測量儀測得在坡路上行駛的機動車在某一瞬間的垂直速度Vv及水平速度Vh；由安裝在機動車前懸架處的前懸架位移傳感器測得所述瞬間時的前懸架位置處的車身沿垂直于道路平面方向的位移Zf；由安裝在機動車后懸架處的后懸架位移傳感器測得所述瞬間時的后懸架位置處的車身沿垂直于道路平面方向的位移Zr；由電子控制單元接收所述Vv、Vh、Zf、Zr的值，并由下列公式(1)、(2)和(3)計算出道路坡度角θ。αGPS＝arctan(Vv/Vh)(1)γ＝arctan((Zf-Zr)/L)(2)θ＝αGPS-γ (3)
6.根據(jù)權利要求5所述的對道路坡度角進行測量的方法，其特征在于，該方法還包括由安裝在機動車質(zhì)心處的陀螺儀測得機動車的俯仰角速度qgyro，由下列公式(6)和(7)通過俯仰卡爾曼濾波器算法計算出實際的α和陀螺儀的偏差qbias，同時通過公式(2)算出俯仰角γ，再通過將該α替換公式(3)中的αGPS而由公式(3)算出道路坡度角θα·q·bias=0-100αqbias+10qgyro+ϵ---(6)]]>式中α為實際的道路坡度角與機動車俯仰角之和， 表示實際的機動車俯仰角速度，qbias是陀螺儀的測量偏差， 表示陀螺儀偏差隨時間的變化率，qgyro是機動車俯仰角速度的陀螺儀測量值，ε是高斯白噪聲；αGPS=10αqbias+η---(7)]]>式中η是1×1高斯白噪聲。
7.根據(jù)權利要求6所述的對道路坡度角進行測量的方法，其特征在于，當GPS速度測量儀不能正常工作時，由公式qgyro=α·]]>估算出α角，同時通過公式(2)算出俯仰角γ，再通過將該α角替換公式(3)中的αGPS而由公式(3)估算出道路坡度角θ。
全文摘要
本發(fā)明提供一種車載道路坡度角測量系統(tǒng)及車載道路坡度角測量方法。其中所述車載道路坡度角測量系統(tǒng)包括GPS速度測量儀，其用于測量行駛中的機動車在某一瞬間的垂直速度V
文檔編號B60R21/01GK1800780SQ20041010403
公開日2006年7月12日 申請日期2004年12月31日 優(yōu)先權日2004年12月31日
發(fā)明者宋佑川 申請人:比亞迪股份有限公司