專利名稱：一種車輛行駛坡度與相對(duì)高度動(dòng)態(tài)估計(jì)方法
技術(shù)領(lǐng)域：
本發(fā)明屬于車載導(dǎo)航技術(shù)領(lǐng)域，具體涉及一種導(dǎo)航系統(tǒng)中動(dòng)態(tài)檢測(cè)車輛行駛道路坡度和 高度的方法。
背景技術(shù)：
在現(xiàn)有的車載導(dǎo)航系統(tǒng)中，通過GPS可以獲得海拔高度信息，但是由于GPS系統(tǒng)測(cè)量 特性所限，高度信息誤差很大，因此GPS接收機(jī)直接輸出的高度信息不能滿足車輛上下坡判 定，以及行駛相對(duì)高度變化估計(jì)的需要。
當(dāng)前，測(cè)量?jī)A角(即道路坡度)的方法主要有兩種傳統(tǒng)的水泡式框式(條式)水平儀其 檢測(cè)方法依然是"水泡移動(dòng)，肉眼分辨"。這種原始的檢測(cè)方法有諸多缺點(diǎn)，如測(cè)量值因人而 易、功能單一、測(cè)量范圍小等；另外一種是基于加速度傳感器的傾角儀它與傳統(tǒng)的水泡式 框式(條式)水平儀相比具有電子檢測(cè)、測(cè)量精度高、測(cè)量范圍大、使用及攜帶方便等特點(diǎn)， 但是這種方法不能實(shí)現(xiàn)加速運(yùn)動(dòng)情況下角度測(cè)量，應(yīng)用受到較大限制。 發(fā)明 內(nèi)容
本發(fā)明的目的是為了解決車載導(dǎo)航中判斷車輛上下坡的問題，從而實(shí)現(xiàn)更精確定位，提 供了一種動(dòng)態(tài)檢測(cè)車輛行駛道路坡度及行駛相對(duì)高度的方法，該方法釆用了中位值濾波和信 息融合技術(shù)手段，達(dá)到了實(shí)時(shí)判斷車輛上下坡技術(shù)效果。
本發(fā)明在單軸加速度計(jì)和里程儀測(cè)量信息基礎(chǔ)上，利用加速度的分配關(guān)系計(jì)算車輛行駛 坡度角。若單軸加速度計(jì)敏感方向和車輛縱軸方向一致，此單軸加速度計(jì)傳感器測(cè)量車輛縱 軸方向上的加速度accy;里程儀可精確測(cè)量車輛行駛速度，這樣就可以提供車輛加速度信息 a車，由加速度accy車輛加速度和a車兩者之間的幾何關(guān)系，可得到坡度值及高度變化值，然
后由高度變化判斷出車輛上下坡。在具體設(shè)計(jì)過程中要考慮單軸加速度計(jì)安裝誤差、加速度 數(shù)據(jù)的噪聲處理、兩種傳感器數(shù)據(jù)的對(duì)準(zhǔn)以及上下坡的判斷規(guī)則問題。此方法相對(duì)于三軸、 雙軸加速度計(jì)測(cè)量?jī)A角穩(wěn)定性更好。
本發(fā)明提出的一種車輛行駛坡度與相對(duì)高度動(dòng)態(tài)估計(jì)方法是在現(xiàn)有車載導(dǎo)航系統(tǒng)的硬件 設(shè)備基礎(chǔ)上，首先實(shí)時(shí)檢測(cè)出車輛行駛的坡度，然后結(jié)合車輛行駛的距離信息得到行駛相對(duì) 高度。車輛坡度計(jì)算結(jié)果的好壞直接影響著高度的精確性，因此本方法的關(guān)鍵就在于坡度的 計(jì)算。具體步驟為
步驟一、安裝角"裝定。設(shè)備安裝角的獲得方式有兩種
① 根據(jù)裝定的安裝角初始化安裝角《 。
② 測(cè)量安裝角"。當(dāng)無(wú)法獲得裝定時(shí)候的安裝角信息時(shí)，釆用單軸加速度計(jì)方式^即 利用靜態(tài)情況下坡度計(jì)箅公式粗標(biāo)設(shè)備安裝角"，具體如下
單軸加速度計(jì)測(cè)量的加速度accy感應(yīng)的是非引力加速度，即車輛推力F、支持力N和
摩擦力f分別在此方向分量的合力，即有
accy = (F x cosor + sinor -/x cos") / w (1)
式中附為車輛質(zhì)量。
根據(jù)車輛縱軸和橫軸方向上的受力平衡有
a車=(F-f)/m-G0 x sin ^ (2)
式中GO為車輛重力加速度，^為車輛所在位置的坡度。 由式(1)、式(2)得到
accy-a車xcosa + GOxsin夕xcosa + GOxcosexsina (3) accy-a車xcosa+GOxsin(P + or) (4)
由式<4)得到坡度的計(jì)算公式
<formula>formula see original document page 5</formula>(5)
當(dāng)"=0時(shí)，由由式(4)得到
<formula>formula see original document page 5</formula> (6)
此時(shí)的坡度P計(jì)算公式為
<formula>formula see original document page 5</formula>
當(dāng)車輛靜止，不考慮設(shè)備安裝角時(shí)，即靜態(tài)情況下a*=0， a = 0，
<formula>formula see original document page 5</formula>(8)
采集車輛靜止時(shí)的加速度計(jì)輸出數(shù)據(jù)accy,盡量保證車輛在水平面，此時(shí)測(cè)量的坡度0 就是設(shè)備的安裝角""-0-sin"(:)。為下面計(jì)算坡度過程中設(shè)備安裝角的補(bǔ)償提供依
據(jù)。在此過程中盡管不能完全保證車輛靜止在水平面上，但安裝角的這種估計(jì)誤差對(duì)高度變
化小時(shí)誤差不大，不影響判斷上下坡趨勢(shì)。
步驟二、對(duì)單軸加速度計(jì)傳感器輸出的數(shù)據(jù)進(jìn)行中位值濾波處理。 對(duì)加速度計(jì)!Waccy進(jìn)行長(zhǎng)度為n (—般取5)的中位值濾波處理以得到剔除野點(diǎn)后的
數(shù)據(jù)再進(jìn)行后續(xù)工作。本方法中單軸加速度計(jì)敏感車輛縱軸方向的加速度，其數(shù)據(jù)記為accy,
因此對(duì)加速度accy進(jìn)行中位值濾波得到濾波后的結(jié)果記為accy—filted;步驟三、對(duì)加速度計(jì)傳感器和里程儀傳感器傳輸?shù)男畔⑦M(jìn)行時(shí)間對(duì)準(zhǔn)。
利用信息融合方法對(duì)加速度計(jì)傳感器和里程儀傳感器傳輸?shù)男畔⑦M(jìn)行時(shí)間對(duì)準(zhǔn)。同時(shí)利 用兩種傳感器的信息，保證這二者的同步是最為關(guān)鍵的，考慮硬件成本需選擇合適的數(shù)據(jù)傳 輸頻率，將信息量較大的加速度計(jì)的采集頻率設(shè)為nlHz,而停息量較小的里程儀的采樣頻 率設(shè)為n2Hz， n2<nl，本文中取nl-50Hz， n2=l Hz，即里程儀輸出的脈沖數(shù)是ls — 次輸出，而單軸加速度計(jì)輸出的數(shù)據(jù)是ls有50次輸出，因此在時(shí)間對(duì)準(zhǔn)的原則下統(tǒng)一兩部 分?jǐn)?shù)據(jù)。若系統(tǒng)具有GPS，以GPS時(shí)間(也即里程儀時(shí)間)為基準(zhǔn)，對(duì)應(yīng)GPS當(dāng)前時(shí)間n 秒的加速度輸出處理是這樣的對(duì)加速度傳感器的時(shí)間在n和n+l之間(包括n秒的數(shù)據(jù)) 的所有加速度數(shù)據(jù)取均值，把這個(gè)均值作為與GPS時(shí)間n秒對(duì)應(yīng)的加速度數(shù)據(jù) accy—filted_AVG,這樣就實(shí)現(xiàn)了在時(shí)間對(duì)準(zhǔn)原則下加速度計(jì)和里程儀數(shù)據(jù)的融合；若系統(tǒng) 無(wú)GPS,則以加速度計(jì)采集次數(shù)作為與里程儀脈沖對(duì)準(zhǔn)依據(jù)。
最后數(shù)據(jù)格式是每一秒對(duì)應(yīng)有一個(gè)加速度值accy—filted_AVG和一個(gè)里程儀脈沖數(shù) odopulse。
步驟四、計(jì)算車輛的加速度3^。
由里程儀傳感器每秒輸出的脈沖數(shù)odopulse和里程儀的刻度因數(shù)K—od (封裝好的參 數(shù))求得車輛每秒行駛的距離Disj3DM-odopulseXlCod，然后根據(jù)距離差可以計(jì)算出車 輛每秒的加速度％:
a車0) = — O爐(0 — 。/s — O層"—1)。 步驟五、計(jì)算車輛坡度e。
本方法融合了里程儀和慣性傳感器的信息，這樣就避免了單方面信息的不足帶來(lái)的影響, 實(shí)現(xiàn)實(shí)時(shí)檢測(cè)到坡度。
將步驟三中得到的加速度數(shù)據(jù)accy jilted—AVG代入坡度計(jì)算公式(5):
.accy-a車xcosa 0-sm1(-^-(5)
得到車輛坡度^:
^. , ,accy filted AVG-a主xcosor、 …
P = sin—1 (~^=——=-S-) 一 ff (9)
、 GO 7
其中GO為車輛的重力加速度，accy—filted—AVG為融合對(duì)準(zhǔn)后的加速度值。
步驟六、計(jì)箅高度變化
由車輛每秒行駛的距離Dis—ODM和每秒坡度e ，計(jì)算出每秒的高度差Wg/ :
/ 妙=加—OZWxsi, (10)
步驟七、判斷上下坡。
由步驟六可以實(shí)時(shí)得到車輛每秒行駛的高度，由于是進(jìn)行單秒計(jì)算，此高度差即使是在上下坡也不會(huì)很大，其中數(shù)據(jù)誤差、計(jì)算誤差等一些隨機(jī)誤差在此高度差中所占比重就會(huì)比 較大，因此由每秒的高度差判斷上下坡就很不可靠?？蛇x擇分段計(jì)算高度差，降低誤差的影 響，此處取3秒作為一個(gè)時(shí)間段，即毎3秒進(jìn)行積分，計(jì)算出一個(gè)高度差。此積分時(shí)間若太 長(zhǎng)則會(huì)產(chǎn)生積分累積誤差，若太短，則積分后的高度差值比較小容易產(chǎn)生誤判，3秒是由多 組實(shí)驗(yàn)數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析確定的經(jīng)驗(yàn)值。下面以這些分段高度差作為依據(jù)，判斷出車輛是否 上下坡
(1) 分段計(jì)算高度設(shè)置每段時(shí)間為3秒，，即每3秒進(jìn)行一次積分，每3秒計(jì)算出一個(gè)高 度差high—step 。
(2) 設(shè)置上下坡高度閾值上坡閾值Delt—UP-0.4,下坡閾值Delt—DOWN=-0.4。
當(dāng)high—step>Delt—UP時(shí)則認(rèn)為是在上坡，并記錄下滿足此條件的時(shí)刻，構(gòu)造滿足上坡條 件的時(shí)刻序列T1;
當(dāng)high—Step<Delt_DOWN則認(rèn)為是在下坡，并記錄下滿足此條件的時(shí)刻，構(gòu)造滿足下坡條 件的時(shí)刻序列T2;'
(3) 若時(shí)刻序列T1中相鄰元素Tl(n)、 Tl(n+l)間的時(shí)刻差A(yù)f = 71(" + 1)-ri(")^6s則認(rèn)為 此時(shí)間段[Tl(n)， Tl(n+1)]為上坡段，反之則不是。
若時(shí)刻序列T2中相鄰元素T2(n)、 T2(n+l)間的時(shí)刻差A(yù)^r2(" + l)-T2(")二6s則認(rèn)為 此時(shí)間段[T2(n)， T2(n+1)]為下坡段，反之則不是。 本發(fā)明的優(yōu)點(diǎn)在于
(1) 本方法考慮了設(shè)備安裝角以及對(duì)慣性數(shù)據(jù)采用了去噪處理，大大提高了測(cè)量精度；
(2) 本方法利用單軸加速度計(jì)與里程儀信息融合實(shí)現(xiàn)坡度識(shí)別，避免了完全依賴加速度 計(jì)傳感器的弊端，提高了系統(tǒng)的穩(wěn)定性與可靠性；
(3) 本方法實(shí)現(xiàn)了實(shí)時(shí)檢測(cè)車輛坡度和高度，有較高的靈敏度和響應(yīng)速度；
(4) 功耗小，啟動(dòng)快，簡(jiǎn)單易行。


圖l是本發(fā)明方法的流程圖2是車輛行駛在坡度上的受力分析示意圖3是本發(fā)明中車輛行駛坡度計(jì)算的原理示意圖4是判斷上下坡仿真圖5是仿真坡度結(jié)果圖6是仿真高度結(jié)果圖。
具體實(shí)施例方式
下面將結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說(shuō)明。本方法為了提高車輛上下坡度估計(jì)結(jié)果的精度，考慮了安裝角的補(bǔ)償、加速度數(shù)據(jù)的去 噪處理和信息的融合處理，本發(fā)明提供的方法流程如圖1所示，具體通過如下步驟實(shí)現(xiàn) 步驟一、粗標(biāo)設(shè)備安裝角a。
加速度計(jì)被加工安裝在車輛上后，通常存在設(shè)備安裝角"。設(shè)備安裝角《是指綜合的安 裝角，也就是慣性器件安裝平面與水平面的夾角，在進(jìn)行坡度計(jì)算之前必須對(duì)該安裝角a進(jìn) 行標(biāo)定，以便在坡度計(jì)算時(shí)對(duì)該角進(jìn)行補(bǔ)償。設(shè)備安裝角的獲得方式有兩種
(1) 根據(jù)裝定的安裝角初始化安裝角"；
(2) 測(cè)量安裝角cr;若無(wú)法方便提供安裝信息，可采用單軸加速度計(jì)方式，即利用靜 態(tài)情況下坡度計(jì)算公式粗標(biāo)設(shè)備安裝角"，具體如下
單軸加速度計(jì)測(cè)量的加速度accy感應(yīng)的是非引力加速度，即車輛推力F、支持力N和 摩擦力f分別在此方向分量的合力，如圖3所示即有
accy = (/^<:03" +JVxsin" — / xcos")/附 (1)
根據(jù)車輛縱軸和橫軸方向上的受力平衡,如圖2所示有
a車=(F-f)/m-G0 x sin (9 ( 2)
式中GO為重力加速度，m為車輛的質(zhì)量，P為車輛所在^M的坡度。 由式(1)、式(2)得到
accy = a車x cosa + G0 x sin S x cos" + GO x cos 6 x sin or (3)
accy :a車xcosa+GOxsin(^ + a) (4)
由式(4)得到坡度的計(jì)算公式
6 = sin '(-^-)—" (5)
GO
當(dāng)a-0時(shí)，由式(4)得到
accy 二a車+G0x sin 0 (6)
此時(shí)的坡度e計(jì)算公式為
GO , (7)
當(dāng)車輛靜止，不考慮設(shè)備安裝角時(shí)，即靜態(tài)情況下&車=0, " = 0,
e = sin-i(，) (8) GO
采集車輛靜止時(shí)的加速度計(jì)輸出數(shù)據(jù)accy,盡量保證車輛在水平面，此時(shí)測(cè)量的坡度P 就是設(shè)備的安裝角""-^^iiT1(^)。為下面計(jì)算坡度過程中設(shè)備安裝角的補(bǔ)償提供依 腳煙。
步驟二、對(duì)加速度計(jì)傳感器輸出的數(shù)據(jù)進(jìn)行中位值濾波處理。由于慣性器件對(duì)外界環(huán)境的敏感導(dǎo)致其輸出很不穩(wěn)定，因此本發(fā)明中對(duì)加速度計(jì)數(shù)據(jù)進(jìn) 行窗口寬度為n (ri為自然數(shù)， 一般取5)的中位值濾波處理以得到剔除野點(diǎn)后的數(shù)據(jù)再進(jìn) 行后續(xù)工作。本方法中單軸加速度計(jì)敏感車輛縱軸方向的加速度accy,因此對(duì)accy進(jìn)行中 位值濾波得到濾波后的結(jié)果記為accy_filted。
步驟三、對(duì)加速度計(jì)傳感器和里程儀傳感器傳輸?shù)男畔⑦M(jìn)行時(shí)間對(duì)準(zhǔn)。
本發(fā)明通過數(shù)據(jù)融合的方式進(jìn)行時(shí)間對(duì)準(zhǔn)。數(shù)據(jù)融合作為一種數(shù)據(jù)處理方法,其基本思想 是綜合利用系統(tǒng)各個(gè)方面的數(shù)據(jù),最大限度地抽取有關(guān)對(duì)象或環(huán)境的有效信息，以達(dá)到更準(zhǔn)確、 更全面地認(rèn)識(shí)觀測(cè)對(duì)象或環(huán)境的目的。本發(fā)明綜合了單軸加速度計(jì)和里程儀兩方面的信息， 同時(shí)利用二種傳感器的信息，保證這二者的同步是最為關(guān)鍵的，考慮硬件成本需選擇合適的 數(shù)據(jù)傳輸頻率，將信息量較大的加速度計(jì)的采集頻率設(shè)為nl Hz,而信息量較小的里程儀的 采樣頻率設(shè)為n2Hz， n2<nl,本文中取nl-50Hz， n2=l Hz,即里程儀輸出的脈沖數(shù)是 Is —次輸出，而單軸加速度計(jì)輸出的數(shù)據(jù)是Is有50次輸出，因此在時(shí)間對(duì)準(zhǔn)的原則下統(tǒng) 一兩部分?jǐn)?shù)據(jù)。若系統(tǒng)具有GPS,以GPS時(shí)間(也即里程儀時(shí)間)為基準(zhǔn)，對(duì)應(yīng)GPS當(dāng)前 時(shí)間n秒的加速度輸出處理是這樣的對(duì)加速度傳感器的時(shí)間在n和n+1之間(包括n秒 的數(shù)據(jù))的所有加速度數(shù)據(jù)取均值，把這個(gè)均值作為與GPS時(shí)間n秒對(duì)應(yīng)的加速度數(shù)據(jù) accy_filted—AVG,這樣就實(shí)現(xiàn)了在時(shí)間對(duì)準(zhǔn)原則下加速度計(jì)和里程儀數(shù)據(jù)的融合；若系統(tǒng) 無(wú)GPS，可以加速度計(jì)采集次數(shù)作為與里程儀脈沖對(duì)準(zhǔn)依據(jù)。
最后數(shù)據(jù)格式是每一秒對(duì)應(yīng)有一個(gè)加速度值accy_filted_AVG和一個(gè)里程儀脈沖數(shù) odopulse。
步驟四、計(jì)算車輛的加速度％。
由里程儀傳感器每秒輸出的脈沖數(shù)odopulse和里程儀的刻度因數(shù)K_od (封裝好的參 數(shù))求得車輛每秒行駛的距離DisJDDM-odopulseXieod,然后就可以計(jì)算出車輛每秒的 加速度^:
a車(0 = ￡fe _ O服(f)-加—O層0 -1)。 步驟五、計(jì)算車輛坡度e。
計(jì)算每秒的坡度，并對(duì)安裝角進(jìn)行補(bǔ)償，得到坡度如下
八.,，accy filted AVG-a車xeosor、 ，、
"in-'(~^~~^~^-(9)
其中GO為車輛的重力加速度。 步驟六、計(jì)算高度變化。
由車輛每秒行駛的距離Dis—ODM和每秒坡度e計(jì)算出每秒的高度值
=加—ODM x sin(<9)。
9步驟七、判斷上下坡。
由上面可以實(shí)時(shí)得到車輛每秒行駛的高度，由于是進(jìn)行單秒計(jì)算，此高度差即使是在上 下坡也不會(huì)很大，其中數(shù)據(jù)誤差、計(jì)算誤差等一些隨機(jī)誤差在此高度差中所占比重就會(huì)比較 大，因此由每秒的高度差判斷上下坡就很不可靠?？蛇x擇分段計(jì)算高度差，降低誤差的影響， 此處取3秒作為一個(gè)時(shí)間段，即每3秒進(jìn)行積分，計(jì)算出一個(gè)高度差。此積分時(shí)間若太長(zhǎng)則 會(huì)產(chǎn)生積分累積誤差，若太短，則積分后的高度差值比較小容易產(chǎn)生誤判，3秒是由多組實(shí) 驗(yàn)數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分析確定的經(jīng)驗(yàn)值。下步則以這些分段高度差作為依據(jù)，再結(jié)合一些限制規(guī) 則判斷出車輛是否上下坡。
(1)分段計(jì)算高度設(shè)置每段時(shí)間為3秒，即每3秒進(jìn)行一次積分，每3秒計(jì)算出一個(gè)高 度差high—step 。
<2)設(shè)置上下坡高度閾值上坡閾值Delt—UP=0.4，下坡閾值Delt_DOWN=-0.4。
當(dāng)high—Step>Delt—UP時(shí)則認(rèn)為是在上坡，并記錄下滿足此條件的時(shí)刻構(gòu)造滿足上坡 條件的時(shí)刻序列Tl;
當(dāng)highjtep^elLDOWN則認(rèn)為是在下坡，并記錄下滿足此條件的時(shí)刻構(gòu)造滿足下坡 條件的時(shí)刻序列T2;
(3)若時(shí)刻序列T1中相鄰元素T1 (n)、 Tl(n+1)間的時(shí)刻差= +1)-《則認(rèn)為 此時(shí)間段[Tl(n), Tl(n+l)j為上坡段，反之則不是。
若時(shí)刻序列T2中相鄰元素T2(n)、 T2(n+1)間的時(shí)刻差= T2(" +1)-!T2(") 2 則認(rèn)為此 時(shí)間段[T2(n)， T2(n+1)為下坡段，反之則不是。
圖4是車輛行駛在一條上下坡交替道路上的三維軌跡圖，x軸表示的是東向位置，y軸 表示的是北向位置，z軸表示的是高度，點(diǎn)線._:代表平路；粗體實(shí)線一代表是上坡；星線 代表下坡；圖中標(biāo)出了此段軌跡的上坡段和下坡段下以及上下坡的高度，可以看出上下坡 的結(jié)果和實(shí)際是吻合的。
仿真實(shí)驗(yàn)生成兩組數(shù)據(jù)單軸加速度計(jì)輸出的加速度值和里程儀輸出的脈沖數(shù)，這兩組 數(shù)據(jù)是無(wú)噪聲的，但由于實(shí)際情況中加速度計(jì)和里程儀測(cè)量的數(shù)據(jù)都有噪聲，因此在仿真時(shí) 對(duì)理想的加速度值和由理想的脈沖數(shù)獲得的車輛加速度值分別加10%的噪聲，圖5和圖6 分別是通過matlab仿真得到的坡度和高度結(jié)果圖其中圖5中實(shí)線表示的是理想的坡度曲 線，虛線是對(duì)噪聲數(shù)據(jù)應(yīng)用本發(fā)明方法得到的坡度曲線，圖6中實(shí)線表示的是理想的高度曲 線，虛線是對(duì)噪聲數(shù)據(jù)應(yīng)用本發(fā)明方法得到的坡度曲線，從圖中可以看出通過本發(fā)明方法獲 得的坡度值和高度值與無(wú)噪聲的理想值非常接近，坡度誤差在0.4934°以內(nèi)，離度誤差在 L8692m以內(nèi)，達(dá)到了比較好的效果。
權(quán)利要求
1、一種車輛行駛坡度與相對(duì)高度動(dòng)態(tài)估計(jì)方法，其特征在于步驟一、粗標(biāo)設(shè)備安裝角α；(1)根據(jù)裝定的安裝角初始化安裝角α；(2)測(cè)量安裝角α；若無(wú)法提供安裝信息，設(shè)備的安裝角α通過如下公式獲得<maths id="math0001" num="0001" ><math><![CDATA[ <mrow><mi>&alpha;</mi><mo>=</mo><mi>&theta;</mi><mo>=</mo><msup> <mi>sin</mi> <mrow><mo>-</mo><mn>1</mn> </mrow></msup><mrow> <mo>(</mo> <mfrac><mi>accy</mi><mrow> <mi>G</mi> <mn>0</mn></mrow> </mfrac> <mo>)</mo></mrow><mo>,</mo> </mrow>]]></math> id="icf0001" file="A2009100881250002C1.tif" wi="35" he="8" top= "60" left = "136" img-content="drawing" img-format="tif" orientation="portrait" inline="yes"/></maths>其中G0為車輛的重力加速度，accy為單軸加速度計(jì)感應(yīng)的加速度信息，θ為坡度；步驟二、對(duì)單軸加速度計(jì)傳感器輸出的數(shù)據(jù)accy進(jìn)行中位值濾波處理；對(duì)加速度計(jì)數(shù)據(jù)進(jìn)行窗口寬度為n的中位值濾波處理，得到濾波后的結(jié)果記為accy_filted，n為自然數(shù)；步驟三、對(duì)單軸加速度計(jì)傳感器和里程儀傳感器傳輸?shù)男畔⑦M(jìn)行時(shí)間對(duì)準(zhǔn)；若系統(tǒng)具有GPS，以GPS時(shí)間為基準(zhǔn)，對(duì)應(yīng)GPS當(dāng)前時(shí)間n秒的加速度輸出處理是這樣的對(duì)加速度傳感器的時(shí)間在n和n+1之間的所有加速度數(shù)據(jù)取均值，把這個(gè)均值作為與GPS時(shí)間n秒對(duì)應(yīng)的加速度數(shù)據(jù)accy_filted_AVG，這樣就實(shí)現(xiàn)了在時(shí)間對(duì)準(zhǔn)原則下加速度計(jì)和里程儀數(shù)據(jù)的融合；若系統(tǒng)無(wú)GPS，以加速度計(jì)采集次數(shù)作為與里程儀脈沖對(duì)準(zhǔn)依據(jù)；步驟四、計(jì)算車輛的加速度a車；由里程儀傳感器每秒輸出的脈沖數(shù)odopulse和里程儀的刻度因數(shù)K_od求得車輛每秒行駛的距離Dis_ODM＝odopulse×K_od，然后計(jì)算出車輛每秒的加速度a車a車(t)＝Dis_ODM(t)-Dis_ODM(t-1)；步驟五、計(jì)算車輛坡度θ；計(jì)算每秒的坡度，并對(duì)安裝角進(jìn)行補(bǔ)償，得到坡度如下其中G0為車輛的重力加速度；步驟六、計(jì)算高度變化；由車輛每秒行駛的距離Dis_ODM和每秒坡度θ計(jì)算出每秒的高度差highhigh＝Dis_ODM×sin(θ)；步驟七、上下坡判斷；(1)分段計(jì)算高度設(shè)置每段時(shí)間為3秒，即每3秒進(jìn)行一次積分，每3秒計(jì)算出一個(gè)高度差high_step；(2)設(shè)置上下坡高度閾值上坡閾值Delt_UP＝0.4，下坡閾值Delt_DOWN＝-0.4；當(dāng)high_step＞Delt_UP時(shí)則認(rèn)為是在上坡，并記錄下滿足此條件的時(shí)刻，構(gòu)造滿足上坡條件的時(shí)刻序列T1；當(dāng)high_step＜Delt_DOWN則認(rèn)為是在下坡，并記錄下滿足此條件的時(shí)刻，構(gòu)造滿足下坡條件的時(shí)刻序列T2；(3)若時(shí)刻序列T1中相鄰元素T1(n)、T1(n+1)間的時(shí)刻差Δt＝T1(n+1)-T1(n)≤6s則認(rèn)為此時(shí)間段[T1(n)，T1(n+1)]為上坡段，反之則不是；若時(shí)刻序列T2中相鄰元素T2(n)、T2(n+1)間的時(shí)刻差Δt＝T2(n+1)-T2(n)≤6s則認(rèn)為此時(shí)間段[T2(n)，T2(n+1)]為下坡段，反之則不是。
全文摘要
本發(fā)明公開了一種車輛行駛坡度與相對(duì)高度動(dòng)態(tài)估計(jì)方法，該方法是在單軸加速度計(jì)和里程儀硬件基礎(chǔ)上運(yùn)用算法計(jì)算出道路坡度及車輛行駛相對(duì)高度信息。單軸加速度計(jì)傳感器測(cè)量車輛縱軸方向上的加速度accy；里程儀可精確測(cè)量車輛行駛速度，這樣就可以提供車輛加速度信息a_車，由accy和a_車兩者之間的幾何關(guān)系，可得到坡度值及高度變化值，然后由高度變化和限制規(guī)則判斷出車輛上下坡。在具體實(shí)施過程中要考慮單軸加速度計(jì)安裝誤差、加速度數(shù)據(jù)的噪聲處理、兩種傳感器數(shù)據(jù)的對(duì)準(zhǔn)以及上下坡的判斷規(guī)則問題。該方法測(cè)量精度高；提高了系統(tǒng)的穩(wěn)定可靠性；實(shí)現(xiàn)了實(shí)時(shí)檢測(cè)車輛坡度和高度，很高的靈敏度和響應(yīng)速度；功耗小，啟動(dòng)快，簡(jiǎn)單易行。
文檔編號(hào)G01C9/00GK101598549SQ20091008812
公開日2009年12月9日 申請(qǐng)日期2009年7月6日 優(yōu)先權(quán)日2009年7月6日
發(fā)明者倩 劉, 周艷麗, 海 張, 張曉鷗, 沈曉蓉, 范耀祖, 高婷婷 申請(qǐng)人:北京航空航天大學(xué)