基于重力分量的道路坡度動態測量

王瓊，鮑晨，沈勇*

(同濟大學a)汽車學院;b)智能型新能源汽車協同創新中心，上海　201804)

?

基于重力分量的道路坡度動態測量

王瓊a,b，鮑晨a,b，沈勇a,b*

(同濟大學a)汽車學院;b)智能型新能源汽車協同創新中心，上海201804)

摘要：基于MEMS(微機械)加速度傳感器的測量原理，采用車輛行駛坡道上的重力分量進行縱向坡度的動態測量，并結合陀螺儀的信號建立基于擴展卡爾曼濾波的數據融合模型，推導出數據融合流程。利用標定后的傳感器完成水平道路和坡道的實車試驗，采集實車試驗數據進行離線算法驗證。重力分量法與雙天線GPS測量的坡度差值在±1°上下波動，表明重力分量法能夠獲得實時動態縱向坡度，滿足智能汽車識別環境信息的需求。

關鍵詞：重力分量；數據融合；擴展卡爾曼濾波；道路縱向坡度

自動變速器車輛若使用不當,汽車上、下長坡時可能出現頻繁換擋。為提高安裝自動變速器的汽車的坡道行駛性能，需要實時獲取坡度信息來修正換擋規律[1-3]。未來的智能車需要根據實時的環境信息采取各項控制決策，如需要依據坡度信息判別安全控制決策來進行，因而高效率獲取實時準確的行駛坡度信息成為重要的研究內容[4-5]。

靜態道路坡度測量是道路測繪的重要工作內容之一，在現有道路坡度測量工作中，一般采用傾角儀沿路段逐點測量，具有工作量大、時間久、效率低等缺點[6-7]。動態坡度識別的相關研究大多基于多種數據來源的車輛縱向運動模型，與模型相關的數據大多不易精確測量獲得，可采用區間法、最小二乘法、卡爾曼濾波、狀態觀測器等數據濾波融合算法對模型進行修正以提高精度[8-14]，但坡度估計的偏差仍然較大。文獻[15-17]采用高精度的雙天線GPS或者可以獲得物體三維速度的GPS設備進行坡度估計，但此方法具有GPS信號不穩定、測量數據獲取頻率低且價格昂貴的缺點。雖然有的文獻采用慣性傳感器對缺失的GPS信號進行補償[18]，但運算數據量偏大，誤差較大。

本文采用加速度計和GPS相結合的方法，根據車輛在坡道上的重力分量進行縱向坡度測量，最后運用數據融合進行誤差補償，不需要復雜的車輛縱向運動參數，更適合城市路況的坡度估計。

1道路坡度測量方法

1.1坡度測量原理

市場上絕大部分的MEMS(微機械)加速度傳感器主要是壓電式微加速度計和壓阻式微加速度計，基本原理為當加速度計連同載體一起做加速運動時，質量塊由于慣性作用向相反的方向運動，敏感元件受壓導致輸出的電壓發生變化[19]。

圖1　MEMS加速度傳感器的工作原理

MEMS加速度傳感器的工作原理如圖1所示。當傳感器處于靜止狀態時，質量塊受到重力，敏感元件受壓，此時傳感器的示數應為當地重力加速度，根據上海當地經緯度估計得到重力加速度為9.794 3 m/s2。此數據可以用于傳感器放大系數的標定。

1.2垂向/水平分量測量方法比較

圖2　靜止/運動狀態傳感器受力分析

如圖2所示，假設傳感器安裝于車輛質心位置并與車輛坐標系重合。坡度為α的路面上，重力加速度矢量可以分解為沿斜坡的分量gsinα和垂直于斜坡的分量gcosα。根據垂向分量與當地實際重力加速度的比值可以確定坡度，根據水平分量與當地實際重力加速度的比也可確定坡度。這是傾角儀的2個測量原理[7]。但傾角儀大多用于測量穩態、較大的坡度。同時，由于車輛在爬坡時除了水平運動還伴隨有俯仰運動，此時傾角儀的數據出現跳動和紊亂，導致傾角儀不適合運動狀態的坡度測量。

圖3　傳感器垂向分量波動

當車輛靜止在坡道上時，由于坡度角較小(一般不會超過5°)，g(1-cosα)很小，難以分辨。當車輛在固定坡度路面行駛時，由于路面的振動引起加速度傳感器的垂向分量az波動較大，如圖3所示，即便使用濾波后效果也不好,并且在0～10°范圍內，垂向分量cosα比縱向分量sinα變動的慢，sinα波動更大，更易分辨出。因此采用水平分量的方法較好。

當車輛沿坡道運動時，運動方向的加速度傳感器測量結果是運動方向的真實加速度與重力加速度沿坡道分量的矢量和，此時需要獲取運動方向的真實加速度。采用水平分量獲得坡度的公式為：

gsinθ=asx-acar,

(1)

式中:θ為車輛對水平地面的絕對姿態角;asx為傳感器坡面分量的加速度；acar為汽車的運行加速度。θ與車輛俯仰角的差即為路面的坡度。

一般通過GPS或者輪速測量信息經差分獲得車輛運行加速度。對獲得的真實加速度采用低通濾波器進行濾波，可以獲得較高精度的估計運行加速度。輪速傳感器的數據刷新頻率高于GPS，因而輪速信息的實時性更好，且GPS信號易受環境影響，無法獲得GPS信號時可采用輪速信息。但當滑移率較大時，輪速信息不能表征車輛的實際運行速度(只能采用GPS信息)，此時盡量減少緊急制動。

2基于擴展卡爾曼濾波的數據融合

擴展卡爾曼濾波(Extended Kalmanfilter，EKF)的核心思想是用線性系統近似非線性系統，即將非線性狀態方程展開成泰勒級數，從而線性化[20]。擴展卡爾曼的濾波過程包含以下兩個階段：1) 狀態預測。該階段主要完成對狀態預報值和狀態誤差協方差預報值的計算；2) 時間更新。該階段主要完成對EKF增益的計算，通過對增益的求解實現系統狀態誤差協方差矩陣的更新，同時還要完成預報階段預報狀態值的更新。非線性離散系統的狀態方程

(2)

式中：f為k-1時刻的狀態向量X(k-1)與k時刻狀態向量估計值X(k)的函數關系；Z(k)為系統的m維觀測向量；W(k)為k時刻n維過程噪聲序列；V(k)為k時刻m維觀測噪聲序列。

車輛對水平地面的絕對姿態角θ，陀螺儀的角速度ωy為狀態量，因此狀態向量可表示為X(k)=[acarasxθωy]T，觀測向量可表示為Z(k)=[acarasxωy]T。假設系統k時刻的過程噪聲W(k)和測量噪聲V(k)均為相互獨立且均值為0的高斯白噪聲序列，其協方差矩陣分別為Q(k)、R(k)。

EKF的數據融合流程如下：

1)由上一時刻狀態向量的最優估計值X(k-1|k-1)，得到此刻狀態向量預測值X(k|k-1)。可表示為X(k|k-1)=f(X(k-1|k-1))，根據式(1)，可具體表示為：

2)計算協方差，公式為：

式中：P(k-1|k-1)為X(k-1|k-1)對應的協方差；P(k|k-1)為X(k|k-1)對應的協方差；Jf為Jacobian矩陣，由式(2)中函數關系f對狀態變量X(k|k-1)求偏導數得到，

3)結合預測值X(k|k-1)和測量值Z(k)，可以得到現在狀態的最優化估算值

X(k|k)=(X(k|k-1))+K(k)(Z(k))-HX(k|k-1)),

4)為了使卡爾曼濾波器不斷地運行下去，需要更新現在狀態X(k|k)的協方差

P(k|k)=(I-K(k)H)P(k|k-1),

式中I為單位矩陣。

濾波過程是以不斷“預測—修正”的遞推方式進行計算，首先進行預測值X(k|k-1)的計算，再根據觀測值Z(k)得到的新信息和Kalman增益K(k)，對預測值Z(k)進行修正。

3道路試驗

3.1試驗設計

試驗車為電動汽車，用螺絲將加速度傳感器與陀螺儀的模塊固定安裝在副駕駛座椅處。雙天線GPS接收機分別布置在車頭和車尾。

整個道路試驗分為兩部分：第一部分采用靜止試驗和水平道路試驗對傳感器進行標定。在水平試驗路段進行數據采集，獲取加速度傳感器和陀螺儀的安裝誤差角。第二部分為坡度驗證試驗，選取某一固定坡道進行下坡試驗。在試驗路段進行實時數據采集，為了驗證EKF算法對試驗坡度的估計效果，采用高精度的雙天線GPS的觀測坡度作為實時道路坡度。

3.2試驗結果

本試驗采用的雙天線GPS的靜態傾角測量誤差在0.5°左右，并且車輛在坡道上伴隨有俯仰運動，雙天線GPS測得的傾角為車輛對地面姿態角與坡角的總和。重力分量法的估計坡度也是車輛對地面姿態角與坡角的總和。因此估計坡度受俯仰運動影響處于波動狀態。試驗表明，雙天線GPS需20 min初始化啟動才能獲取穩定值，且穩定性受環境影響較大(如衛星數量等)，重力分量法僅需10 s(傳感器初始化穩定時間和濾波算法的延遲時間)即可獲取穩定值。

圖4　坡度隨時間的變化曲線

同時用雙天線GPS傾角與重力分量法分別測量坡度，得到坡度隨時間變化的曲線，如圖4所示。從圖4可知，在0～7 s時汽車處于水平路面，但是由于路面不平，造成了車身振動，引起測量坡度出現波動偏差(1°左右)，雙天線GPS傾角也顯示出了相應的波動。10～23 s時汽車在固定坡道(坡度為5.6°)上運動，路面沒有凸起和凹陷，測量坡度的波動較小，在1°左右，數據波動的主要原因是俯仰運動，雙天線GPS傾角也有相應的波動顯示。其次，試驗坡道本身為彎道，車輛在轉向時，GPS的觀測車速并不是縱向車速，因而測量坡度相對于雙天線GPS傾角有較大起伏，可以通過汽車航向角對GPS觀測車速進行修正以消除彎道的影響。上述試驗結果驗證了重力分量法可以區分水平路面和小坡度路面，動態估計精度1°左右。綜上可知，重力分量法對于道路坡度的測量結果與雙天線GPS的觀測傾角差值在1°上下波動。試驗坡道的靜態測量坡度為5.6°，本方法的動態測量精度為1°。

4誤差來源分析

4.1MEMS加速度傳感器與MEMS陀螺儀

車輛坐標系原點在車輛質心，x軸沿車身的縱軸指向車輛正前端，y軸指向車輛左側，z軸與x、y軸構成右手直角坐標系。

傳感器坐標系原點在傳感器的質心，與車輛坐標系保持相對靜止，xs軸沿傳感器的縱軸指向傳感器正前端，ys軸指向傳感器左側，zs軸與xs、ys軸構成右手直角坐標系。

圖5　安裝誤差角標定處理流程

假設傳感器安裝在車輛質心，傳感器坐標系與車輛坐標系的偏差為安裝誤差角。

1)安裝誤差角標定

靜止情況，得出z軸的單位向量方向。直線行駛時,傳感器測得的向量減去重力分量得到x軸的單位向量方向。將z軸的單位向量與x軸的單位向量運算得到y軸的單位向量方向。將獲得的三軸的單位向量進行合并，即可得到安裝誤差角的標定矩陣，如圖5所示。

2)靜態噪聲

采集靜態情況下加速度傳感器和陀螺儀測量值進行分析，加速度計xs、ys、zs三軸的靜態輸出的標準差分別為0.004 1、0.005 9、0.004 3 m/s2。陀螺儀xs、ys、zs三軸靜態輸出的標準差分別為0.019 5、0.013 0、0.012 9 (°)/s。

3)傳感器隨溫度的變化

隨著溫度的升高，在短時間內，靜態情況下加速度傳感器的輸出變化并不明顯，噪聲沒有明顯變化。可以認為在試驗階段，加速度傳感器的比例系數不受溫度影響。

綜上可得，MEMS傳感器的誤差對坡度估計結果影響較小。

4.2GPS觀測速度與傾角觀測速度

目前車載GPS大多采用觀測的多普勒頻移得到車速，理論精度約在0.1 km/h，頻率為10 Hz。本文利用靜態情況下GPS接收機收到的速度差值進行分析，得到GPS速度的標準差0.01 m/s。水平靜止情況下GPS觀測傾角的標準差為0.5°。

獲取車輛加速度的方法是對車輛的實時車速進行差分運算，由于差分運算對噪聲比較敏感，所以對速度信號采用先濾波、差分、再濾波的方法。由于車速信息的數據刷新頻率較低，且濾波算法造成時間滯后，與MEMS傳感器存在數據同步性誤差。

4.3俯仰運動

圖6　加速度傳感器與速度濾波差分加速度數值對比

汽車的俯仰運動會造成測量坡度的波動，通過在水平路面上進行試驗，檢驗加速度傳感器獲得的加速度ax和速度濾波差分后的加速度av是否吻合來判斷是否存在俯仰運動造成的傾角估計誤差。試驗結果如圖6所示。從圖6可以看出，汽車在水平路面行駛時，濾波得到的加速度與傳感器得到的加速度趨勢基本吻合，但當加速度絕對值較大時(>2 m/s2)，兩者差異明顯，此差值大小來源于俯仰運動造成車與路面的夾角，俯仰角的估計值在1°左右。

可以采用頻率50 Hz的輪速傳感器，用以提高車速估計的實時性，提高坡度估計角的頻率，進而可以采用濾波算法去除俯仰運動的傾角誤差。這也面臨一定的問題，如輪速傳感器得到的是角加速度，轉換成縱向加速度的話，還要考慮輪胎半徑的影響。而輪胎半徑隨負載、輪胎氣壓、車速的不同而變化。且當滑移率過大時，輪速信號不能表征車速，此時可以通過輪速信號的波動來判斷是否發生打滑。

5結論

1)根據MEMS慣性傳感器的測量原理，提出了基于重力分量的道路縱向坡度估計算法，以GPS和慣性傳感器的數據為基礎，采用擴展卡爾曼濾波建立數據融合模型，并推導出了數據融合流程的表達式。

2)實車試驗數據結果表明:重力分量法可以實現道路縱向坡度動態測量，能區分水平路面和小坡度路面，并且能比雙天線GPS更快初始化，獲得穩定值。由于汽車本身俯仰運動的影響，對坡度的動態測量精度可以達到1°，優于采用縱向動力學模型的坡度估計精度。

3)重力分量法得到的道路動態測量坡度角可用于主動控制系統，還可用于實時智能化測繪道路坡度。提高算法估計結果的實時性和測量俯仰運動的傾角是后續研究工作的重點。

參考文獻：

[1]李磊, 章國勝, 宋健,等. 基于等效坡度的自動手動變速器換檔規律研究[J]. 公路交通科技, 2011, 28(2):144-148.

LI Lei, ZHANG Guosheng, SONG Jian, et al. Research on automated manual transmission shift schedule based on equivalent slope[J].Journal of Highway and Transportation Research and Development, 2011, 28(2):144-148.

[2]王建強, 俞倩雯, 李升波,等. 基于道路坡度實時信息的經濟車速優化方法[J].汽車安全與節能學報, 2014,5(3):257-262.

WANG Jianqiang, YU Qianwen, LI Shengbo, et al. Eco speed optimization based on real-time information of road gradient[J].Automotive Safety and Energy, 2014,5(3):257-262.

[3]楊志剛, 曹長修, 蘇玉剛. 動態識別道路坡度的多傳感器信息融合方法[J]. 重慶交通大學學報(自然科學版), 2002, 21(2):97-102.

YANG Zhigang, CAO Changxiu, SU Yugang. The method of dynamically road grade by means multi-sensor information fusion[J].Journal of Chongqing Jiaotong University (Natural Science), 2002, 21(2):97-102.

[4]張華玉.自主駕駛車輛速度及道路坡度估計研究[D].長春:吉林大學, 2013.

ZHANG Huayu.Estimation road grade and velocity for autonomous driving vehicle[D].Changchun: Jilin University, 2013.

[5]HROVAT D D, TSENG H E, BROWN T A. Method for road grade/vehicle pitch estimation:US6714851[P/OL]. (2004-03-30)[2015-12-01].http://xueshu.baidu.com/s?wd=paperuri%3A%28317cd09b6a2ee140c5f5e762f8479017%29&filter=sc_long_sign&tn=SE_xueshusource_2kduw22v&sc_vurl=http%3A%2F%2Fwww.freepatentsonline.com%2F6714851.html&ie=utf-8.

[6]高婷婷, 張海, 劉倩,等. 一種低成本動態車載導航坡度識別方法[J].系統仿真技術, 2009, 5(4):226-331.

GAO Tingting, ZHANG Hai, LIU Qian, et al. A low-cost dynamic method for slope estimation in vehicle navigation system [J].System Simulation Technology, 2009, 5(4):226-331.

[7]田小芳, 陸起涌, 熊超.基于加速度傳感器的傾角儀設計[J].傳感技術學報, 2006, 19(2):361-363.

TIAN Xiaofang, LU Qiyong, XIONG Chao.Design of tilt-sensor based on accelerometer [J].Chinese Journal of Sensors and Actuators, 2006, 19(2):361-363.

[8]金輝,葛安林,秦貴和,等. 基于縱向動力學的坡道識別方法研究[J].機械工程學報,2002, 38(1) : 79-82.

JIN Hui, GE Anlin, QIN Guihe, et al.Study on slope recognition method based on vehicles′ longitudinal dynamics[J].Chinese Journal of Mechanical Engineering, 2002, 38(1) : 79-82.

[9]BAE H S,RYU J, GERDES C.Road grade and vehicle parameter estimation for longitudinal control using GPS[C]//Proceedings of 2001 IEEE Intelligent Transportation Systems. Oakland, USA: IEEE, 2001:166-171.

[10]KATRANITSAS P F, ORDYS A W, DARNELL P M, et al.Road gradient detection using Kalman filter[C]// EUROCON. Zagreb, Croatia: IEEE, 2013:1492-1497.

[11]MANGAN S, WANG J.Development of a novel sensorless longitudinal road gradient estimation method based on vehicle CAN bus data[J]. IEEE/ASME Transactions on Mechatronics, 2007, 12(3):375-386.

[12]雷雨龍, 付堯, 劉科,等. 基于擴展卡爾曼濾波的車輛質量與道路坡度估計[J]. 農業機械學報, 2014,45(11): 9-13.

LEI Yulong, FU Yao, LIU Ke, et al.Vehicle mass and road grade estimation based on extended Kalman filter[J]. Transactions of the Chinese Society for Agriculture Machinery, 2014, 45(11):9-13.

[13]SAHLHOLM P.A sensor and data fusion algorithm for road grade estimation[J].Symposium on Advances in Automotive Control, 2007, 5(1):215-234.

[14]WINSTEAD V, KOLMANOVSKY I V.Estimation of road grade and vehicle mass via model predictive control[C]// Proceedings of the Control Applications. Toronto, Canada: IEEE, 2005:1588-1593.

[15]KO K H, KWON M J, LEE W H, et al.Altitude prediction using real time GPS data in highway with gradient[M]. Computer Applications for Modeling, Simulation, and Automobile. Berlin Heidelberg: Springer, 2012:179-189.

[16]KORNFELD R P, JOHN H R, DEYST J J. Single-antenna GPS-based aircraft attitude determination[J]. Navigation, 1998, 45(1):51-60.

[17]郭萬祿, 潘玉純, 翟崢嶸. 基于雙天線GPS接收機航向姿態測量方法[J].無線電工程, 2012, 42(5):49-52.

GUO Wanlu, PAN Yuchun, ZHAI Zhengrong.Attitude parameter measurement method based on double antenna GPS receiver system[J].Radio Engineering, 2012, 42(5):49-52.

[18]王慧麗,史忠科.基于GPS/IMU的山區道路參數擬合與行車安全分析[J].中國公路學報,2014, 27(6):27-33.

WANG Huili, SHI Zhongke. Mountainous road parameters fitting and driving safety analysis based on GPS/IMU[J].China Journal of Highway and Transport, 2014, 27(6):27-33.

[19]OH J, CHOI S B. Vehicle roll and pitch angle estimation using a cost-effective six-dimensional inertial measurement unit[J]. Proceedings of the Institution of Mechanical Engineers Part D:Journal of Automobile Engineering, 2013, 227(4):577-590.

[20]SIMANEK J, REINSTEIN M, KUBELKA V. Evaluation of the EKF-based estimation architectures for data fusion in mobile robots[J]. IEEE/ASME Transactions on Mechatronics, 2015, 20(2):985-990.

(責任編輯：郎偉鋒)

Dynamic Measurement on Road Slope Based on Gravity Component

WANGQionga,b,BAOChena,b,SHENYonga,b

(a.SchoolofAutomotiveStudies；b.IntelligentCollaborativeInnovationCenter,TongjiUniversity,Shanghai201804,China)

Abstract:Based on the measuring principle of the MEMS (Micro Mechanical Acceleration Sensor), the gravity component of vehicle driving on the slope is used to acquire dynamic measurement of the longitudinal slope. Firstly, combining with the signal of the gyroscope, the extended Kalman filter data fusion model is established to derive the data fusion process. Then, the field tests over a level road and slope are completed by using the calibrated sensors. Finally, the real car test data is collected to verify the offline algorithm. As a result, the slope margin of error measured by the gravity component method and the double antenna GPS is about ±1°,which indicates that the gravity component method can effectively obtain the real-time dynamic longitudinal gradient and satisfy the demand of the intelligent vehicle identification information.

Key words:gravity component；data fusion；extended Kalman filter；road longitudinal gradient

中圖分類號：U461.1

文獻標志碼：A

文章編號：1672-0032(2016)01-0033-07

DOI：10.3969/j.issn.1672-0032.2016.01.007

作者簡介：王瓊(1991—)，女，河南開封人，碩士研究生，主要研究方向為車載信息技術應用與道路參數識別,E-mail:kf1991wq@163.com.*通訊作者：沈勇(1966—)，男，浙江嘉興人，副教授，博士生導師,主要研究方向為汽車電子-車載信息技術，E-mail:91066@tongji.edu.cn.

基金項目：上海汽車工業科技發展基金(1203)

收稿日期：2015-12-17