基于MEMS傳感器的道面坡度提取方法*

蘇志剛,吳柳莉,郝敬堂

(1.中國民航大學 智能信號與圖像處理天津市重點實驗室,天津 300300;2.中國民航大學 中歐航空工程師學院,天津 300300)

由慣性測量單元(Inertial Measurement Unit,IMU)組成的慣性導航系統(Inertial Navigation System,INS)可以精確地定位車輛的水平位置,但對高度和坡度的測量需額外的輔助傳感器提供輔助參數[1]。在車輛行駛過程中若遇到上下坡以及不同坡度的高架橋路面時,導航系統便有可能產生較大的估計偏差。如何較為精確地實現對高度位置和道面坡度的解算,引起業界學者的廣泛關注。

國內外道路坡度識別的方法可從實現原理上分為三類:基于全球定位系統GPS(Global Positioning System)數據的方法、基于車輛動力學模型的方法和基于加速度估計的方法。基于GPS數據的方法,其測量精度取決于GPS的有效使用范圍[2],且易受遮擋物的影響。基于車輛動力學模型的方法是建立車輛與道路坡度之間的動力學關系模型[3],其對傳感設備精度要求較高[4]。基于加速度估計的方法是利用加速度傳感器估計車輛的俯仰角,并提取道路坡度信息[5]。基于加速度估計的方法采用的加速度傳感器大多是便攜式微機械電子系統(Micro-Electro-Mechanical System,MEMS)傳感器。該類傳感器采集的數據精度較低,噪聲干擾相對較大[6],且在解算過程中需要對加速度進行積分處理,引入了累積誤差的問題,同時,在進行坡度提取時一般需要解算運動加速度。因此,此類方法通常需額外的輔助傳感器提供輔助參數。Jang J H等利用輪速傳感器采集的車輛速度數據,并據此解算出軌跡信息[7],Son Y等采用氣壓傳感器來獲得道路的絕對高度信息[8],高婷婷等采用里程儀通過建模得到坡度[9],Norrdine A等利用置于足底的傳感器檢測行人行走過程中腳觸地時短暫的零速狀態實現對累積誤差的修正[10]。

本文采用MEMS傳感器實現對道面坡度的提取,在不增加額外傳感器的基礎上,利用零加速修正方法解決了慣性平臺的誤差累積問題,并提出了一種新的坡度提取方法,根據重力向量在載體坐標系中的變化實現對道面坡度信息的解算。

1 算法框架

利用智能手機中三軸加速度計、三軸角速度計和三軸磁力計構成測量平臺,將智能手機固定在運動平臺上,即運動平臺與智能手機的相對位置關系不變。運動平臺在道面上運動時,道面坡度對運動平臺的影響將直接引起智能手機相對初始狀態的變化,利用智能手機中各個傳感器的實時數據即可實現對道面坡度信息的解算。

道面坡度提取算法的結構如圖1所示,由數據采集、零加速狀態檢測、旋轉四元數估計和位置解算與坡度提取等四部分組成。

圖1 算法框圖

數據采集是利用三軸加速度計、三軸角速度計和三軸磁力計獲得智能手機實時的加速度、角速度和測量當地的磁分量數據,這些數據均以載體坐標系的形式進行表征。在進行道面坡度提取過程中,另外兩個被涉及的坐標系分別是地理坐標系和導航坐標系,三個坐標系的關系如圖2所示。載體坐標系是以載體手機質心為原點,平行于手機短邊、長邊和垂直于手機屏幕向上的方向而建立的坐標系ObXbYbZb(b系)。以道面坡度提取起點為參考點的東北天(ENU)坐標系被定義為地理坐標系。平行于地理坐標系各坐標軸,并與載體坐標系共原點建立導航坐標系ObXnYnZn(n系)。

圖2 坐標系之間的關系

零加速狀態檢測將利用實時的加速度和角速度數據進行判斷智能手機是否處于零加速狀態。旋轉四元數估計則是根據零加速狀態檢測的結果更新旋轉四元數的估計,若手機處于零加速狀態,則根據零加速狀態的特點進行旋轉四元數初始化;若手機處于非零加速狀態,則根據前一時刻的旋轉四元數進行更新處理。位置解算與坡度提取部分則是根據當前的旋轉四元數數據及傳感器的實時數據對手機的位置進行更新,并提取此時的坡度信息。智能手機的位置信息及道面坡度信息均是在地理坐標系中表征。

2 零加速狀態檢測

零加速狀態就是智能手機處于平動加速度和轉動加速度均為零的受力平衡態。此時,雖然速度可能不為零,但載體坐標系與導航坐標系間的位置關系相對固定,可以進行旋轉四元數初始化,從而實現對慣性器件累積誤差的修正。

在零加速狀態時,理論上三軸加速度計的輸出為重力加速度的值,三軸角速度計的輸出為零,但由于噪聲的存在,在進行零加速檢測時,需要對三軸加速度計采集的載體坐標系的加速度向量ab(t)=[axtaytazt]T和三軸角速度計采集的角速度向量ωb(t)=[ωxtωytωzt]T進行范數閾值判決。

利用ab(t)進行加速度檢測。

其中‖·‖表示向量的范數,thamin與thamax為加速度向量ab(t)的范數的上、下閾值。

利用ωb(t)進行角速度檢測。

其中,thωmax為角速度向量ωb(t)的范數的閾值。

如上所述,同時滿足以上兩種條件時判定為零加速狀態,即:

T(t)=T1(t)×T2(t)

(3)

3 旋轉四元數估計

零加速狀態檢測模塊輸出的結果有兩種可能,即檢測出智能手機處于零加速狀態或非零加速狀態。若智能手機處于零加速狀態,則將根據零加速狀態的特點對旋轉四元數初始化;若手機處于非零加速狀態,則將根據前一時刻的旋轉四元數進行旋轉四元數更新。

3.1 旋轉四元數初始化

當智能手機處于零加速狀態時,載體坐標系與導航坐標系之間的位置關系保持不變,此時可以利用傳感器采集的數據直接實現對旋轉四元數進行初始化,從而消除前期累積誤差的影響。

根據圖2定義的坐標系之間的關系,載體坐標系到導航坐標系的變換可以看作載體坐標系繞導航坐標系定義的某單位向量un(t)=[uxtuytuzt]T逆時針旋轉?nt角度得到導航坐標系。因此,載體坐標系下三軸加速度計輸出gb(t)=[gxtgytgzt]T和三軸磁力計輸出hb(t)=[mxtmytmzt]T也可繞單位向量un(t)逆時針旋轉?nt角度得到導航坐標系的重力分量gn=[0 0 -g]T和磁場分量hn=[hxhyhz]T,其中g為當地重力加速度,hx、hy和hz為當地磁力線在地理坐標系的各軸向的分量,可由國際地磁參考場IGRF(International Geomagnetic Reference Field)獲得。由此可知,向量un(t)一定位于gn和gb(t)之間的對稱面上,該對稱面的法向量為:

r1(t)=[gxtgytgzt+g]T

(4)

同理,un(t)也一定落在hn與hb(t)之間的對稱面上,相應對稱面的法向量為:

r2(t)=[mxt-hxmyt-hymzt-hz]T

(5)

所以,un(t)位于式(4)和式(5)定義的兩平面的交線上,該交線的方向向量:

ul(t)=r1(t)×r2(t)

(6)

由此可得ul(t)在導航坐標系各軸向的分量:

ulxt=gyt(mzt-hz)-(gyt+g)(myt-hy)

(7)

ulyt=(gzt+g)(mxt-hx)-gxt(mzt-hz)

(8)

ulzt=gxt(myt-hy)-gyt(mxt-hx)

(9)

對方向向量ul(t)進行歸一化計算:

即可求得單位向量un(t)的三個分量uxt、uyt和uzt。

利用單位向量un(t)和旋轉角度?nt可構成四元數[11]:

其中,i,j和k是虛數單位。顯然范數‖qn(t)‖=1。

假設載體坐標系中的向量rb(t)=[xbtybtzbt]T在導航坐標系中的對應向量為rn(t)=[xntyntznt]T,定義p(·)為將三維向量映射為純四元數的函數,則有:

p[rb(t)]=xbti+ybtj+zbtk

(12)

p[rn(t)]=xnti+yntj+zntk

(13)

式(12)和式(13)中四元數間關系為

對式(14)進行逆運算可獲得由導航坐標系向載體坐標系的映射關系:

將gn和gb(t)代入式(15)并整理得

-gxt/g=(1-cos?nt)uxtuzt-uytsin?nt

(16)

-gyt/g=(1-cos?nt)uytuzt+uxtsin?nt

(17)

(18)

由式(18)可得:

將式(19)可得代入式(17)并整理得:

根據式(19)和式(20)即可確定?nt所在象限及其值。

利用獲得的un(t)的各個分量uxt、uyt、uzt與?nt,由式(11)可確定四元數qn(t)。

3.2 旋轉四元數更新

當智能手機處于非零加速狀態時,三軸加速度計的輸出將不再影響載體坐標系與導航坐標系之間的位置關系,而是三軸角速度傳感器采集的數據向量ωb(t)影響載體坐標系與導航坐標系之間的位置關系。因此,利用ωb(t)實現對旋轉四元數的更新。

如前所述,旋轉向量un(t)和旋轉角度?nt均是在導航坐標系定義的,因此,由un(t)和?nt所確定的旋轉四元數qn(t)也是在導航坐標系下定義的。對旋轉四元數qn(t)的更新需要利用導航坐標系的角速度向量ωn(t)。根據式(14)可得:

在時刻t旋轉四元數qn(t)的變化率為:

因此,相應的旋轉四元數qn(t)的更新為:

4 位置解算與坡度提取

根據獲得的旋轉四元數qn(t)和傳感器采集的數據進行平臺位置的解算和道面坡度信息的提取。假設平臺由靜止狀態開始,則在初始狀態下,導航坐標系的加速度向量an(0)、速度向量vn(0)和位置向量pn(0)均為零向量,根據此時的零加速可以獲得旋轉四元數qn(0)。

在平臺運動過程中,平臺所獲得的加速度向量ab(t)為載體坐標系中的測量值。當進行坡度測量過程中需要將ab(t)進行坐標變換,并用重力加速度向量gn進行補償[12],從而獲得導航坐標系形式an(t)。參考式(14)可得:

速度向量的更新方程為:

v(t+dt)=v(t)+an(t)dt

(25)

其中,v(t)為當前時刻的速度向量,v(t+dt)為速度的更新值。當載體處于零加速狀態時,加速度an(t)為零向量,則式(25)變為:

v(t+dt)=v(t)

(26)

位置向量的更新方程為:

p(t+dt)=p(t)+v(t)dt

(27)

其中,p(t)為當前時刻的位置向量,p(t+dt)為位置向量的更新值。位置向量p(t)的垂直軸向的分量即為平臺相對起始時的相對高度。

常規的傾角儀測量道面坡度的方法,是當載體靜止或以恒定速度運動時,采用安裝在載體上的三軸加速度計測量重力加速度分量,通過各軸向測量信息與重力加速度之間的關系可獲得坡度信息。若是載體處于變加速運動時,則需要采用額外的輔助傳感器提供輔助參數,解算運動加速度的值,如文獻[9]中是由里程儀測量的速度差獲得的,進而通過各軸向測量信息與重力加速度之間的關系獲得坡度信息。本文提出了一種新的坡度提取方法,在不需要額外傳感器提供輔助參數的情況下,利用重力加速度方向不變性,通過載體坐標系中的重力加速度矢量方向的變化實現道面坡度信息提取。

根據圖1所示的坐標系間的關系可知,在坡度測量的過程中,地理坐標系是不動的,導航坐標系只是相對于地理坐標系平移,因此在兩個坐標系中的重力加速度向量gn是不變化的。對于載體坐標系,在坡度測量的過程中是存在相對姿態的變化,即載體坐標系相對導航坐標系發生旋轉,而且這個角度的變化是與平臺所接觸面的坡度有關,因此,在載體坐標系中重力加速度向量gb(t)的方向變化量均反映著平臺所接觸面的坡度變化情況,如圖3所示。

圖3 坡度提取原理圖

假設測量起始時平臺由水平面靜止狀態出發,則起始時刻載體坐標系的重力加速度向量gb(0)為

在時刻t,載體坐標系的重力加速度向量gb(t)為

向量gb(t)與gb(0)間的夾角即為平臺在時刻t所處位置的坡度

5 實驗結果與分析

實驗采用的智能手機為iPhone7,利用其內置的三軸加速度計、三軸陀螺儀和三軸磁力計進行數據采集。手機端MATLAB與電腦端MATLAB通過IP互聯的方法,實現傳感器采集的數據同步到電腦,從而進行數據處理。此方法需將智能手機與電腦置于同一局域網中,且在智能手機終端安裝mobile MATLAB(以下簡稱mobile),在電腦終端安裝MATLAB(R2014a以上版本,以下簡稱MATLAB),同時需安裝插件MATLAB Support Package for IPhone Sensors in MATLAB來匹配手機。該方法實現的過程是首先在MATLAB中與mobile建立連接,然后打開mobile,選擇連接電腦,隨后輸入IP地址、密碼和端口默認值,再點擊Connect與電腦連接。連接成功后,通過在MATLAB中輸入m=mobiledev,可以建立一個采集的對象。通過在命令行中輸入m.Logging=1開始進行采樣,輸入m.Logging=0結束采樣。在采樣結束后,通過在命令行中輸入[a,t]=accellog(m)可以獲得整個采集過程中加速度計的采樣值,獲取其他傳感器輸出的采樣值的過程與此類似。傳感器的數據采樣率可以通過輸入不同的m.SampleRate進行設置,本文實驗的數據采樣率設置為100 Hz。零加速檢測的加速度閾值為thamin=8.0和thamax=10.8,角速度檢測閾值為thωmax=1.3。

為檢驗算法的有效性,第一個實驗場地選擇在可實測角度與尺寸的樓梯上進行,場景模型如圖4所示。此實驗的設定是為了模擬含有若干個坡度的道面,因為在實際的道面中很難找到具有這種效果的路線。將智能手機置于小車頂部,手推小車在樓梯上進行下降四層樓梯的運動,將零加速修正的軌跡與未修正的軌跡以及樓梯的真實軌跡進行比較,并將本文方法與常規傾角儀測量的原始方法提取運動過程中的坡度信息進行對比,如圖5所示。

圖4 樓梯實驗場景模型

圖5 高度與坡度隨位移的變化曲線

由于實驗平臺呈現剛體效果,在臺階的轉折處無法實現理想的90°轉折,存在著一定的角度緩變,如圖5所示。未修正的軌跡與真實軌跡偏離較大,根據實驗數據重建的平臺高度變化情況與實際情況具有較好地契合。由圖5的坡度變化曲線可見,平臺在樓梯平臺上水平移動時坡度角為0°,當小車從一個臺階平臺邊緣下降到另一個臺階平臺上的過程中,小車先從水平狀態偏轉到垂直狀態,然后再逐漸從垂直狀態恢復為水平狀態,坡度從0°減小至-90°,然后再從-90°恢復為0°,具有較好的精度。

第二個實驗場地選擇在可實測尺寸的斜坡道面上進行,斜坡的坡度值可通過測量斜坡的尺寸值進行計算得到,場景模型如圖6所示。此實驗場地是在實際道面上,且是一典型的斜坡道面。將智能手機置于小車頂部,手推小車在斜坡道面上進行下降的運動,將零加速修正的軌跡與未修正的軌跡以及斜坡道面的真實軌跡進行比較,并將本文方法與常規傾角儀測量的原始方法提取運動過程中的坡度信息進行對比,如圖7所示。

圖6 斜坡道面實驗場景模型

圖7 高度與坡度隨位移的變化曲線

由圖7的高度變化曲線可見,未修正的軌跡與真實軌跡偏離較大,根據實驗數據重建的高度變化情況與實際情況具有較好地契合。由圖7的坡度變化曲線可見,實驗平臺在水平面上移動時坡度角為0°。由于斜面的長度相對于小車的尺寸較大,當小車從水平面運動到斜面的過程中,小車先從水平狀態偏轉到傾斜狀態,在車身完全偏轉到斜面之后,可以保持一段時間的傾斜狀態,坡度從0°減小至-29°,然后保持一段時間的-29°。當小車從斜面運動到水平面的過程中,小車從傾斜狀態偏轉到水平狀態,坡度從-29°增大至0°。算法得到的坡度值與實際坡度值的誤差較小,具有較好的精度。

通過樓梯實驗與斜坡道面實驗可以得到,將本文方法與原始方法進行對比,兩種方法的精度差距很小,但本文方法的優點在于不需要使用額外的輔助傳感器解算運動加速度的值,不用區分載體的運動狀態,實現了道面坡度的提取。同時,設備的成本較低,可推廣的范圍更廣。

6 結論

本文提出了一種基于智能手機中MEMS傳感器的道面坡度提取方法,該方法利用檢測出的零加速狀態對旋轉四元數進行重置,克服了慣性器件進行積分所帶來的累積誤差問題,并提出了一種新的坡度提取方法,在不需額外傳感器提供輔助參數的情況下,基于重力加速度方向不變性,利用載體坐標系中重力加速度向量角度變化實現道面坡度信息提取。實驗結果表明該算法可以有效地實現對道面坡度信息的提取和道面坡度位置的定位。