蘇志剛,吳柳莉,郝敬堂

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

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

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

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

1 算法框架

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

道面坡度提取算法的結(jié)構(gòu)如圖1所示,由數(shù)據(jù)采集、零加速狀態(tài)檢測、旋轉(zhuǎn)四元數(shù)估計和位置解算與坡度提取等四部分組成。

圖1 算法框圖

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

圖2 坐標(biāo)系之間的關(guān)系

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

2 零加速狀態(tài)檢測

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

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

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

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

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

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

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

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

(3)

3 旋轉(zhuǎn)四元數(shù)估計

零加速狀態(tài)檢測模塊輸出的結(jié)果有兩種可能,即檢測出智能手機處于零加速狀態(tài)或非零加速狀態(tài)。若智能手機處于零加速狀態(tài),則將根據(jù)零加速狀態(tài)的特點對旋轉(zhuǎn)四元數(shù)初始化;若手機處于非零加速狀態(tài),則將根據(jù)前一時刻的旋轉(zhuǎn)四元數(shù)進(jìn)行旋轉(zhuǎn)四元數(shù)更新。

3.1 旋轉(zhuǎn)四元數(shù)初始化

當(dāng)智能手機處于零加速狀態(tài)時,載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的位置關(guān)系保持不變,此時可以利用傳感器采集的數(shù)據(jù)直接實現(xiàn)對旋轉(zhuǎn)四元數(shù)進(jìn)行初始化,從而消除前期累積誤差的影響。

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

r1(t)=[gxtgytgzt+g]T

(4)

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

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

(5)

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

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

(6)

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

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)進(jìn)行歸一化計算:

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

利用單位向量un(t)和旋轉(zhuǎn)角度?nt可構(gòu)成四元數(shù)[11]:

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

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

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

(12)

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

(13)

式(12)和式(13)中四元數(shù)間關(guān)系為

對式(14)進(jìn)行逆運算可獲得由導(dǎo)航坐標(biāo)系向載體坐標(biāo)系的映射關(guān)系:

將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)并整理得:

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

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

3.2 旋轉(zhuǎn)四元數(shù)更新

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

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

在時刻t旋轉(zhuǎn)四元數(shù)qn(t)的變化率為:

因此,相應(yīng)的旋轉(zhuǎn)四元數(shù)qn(t)的更新為:

4 位置解算與坡度提取

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

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

速度向量的更新方程為:

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

(25)

其中,v(t)為當(dāng)前時刻的速度向量,v(t+dt)為速度的更新值。當(dāng)載體處于零加速狀態(tài)時,加速度an(t)為零向量,則式(25)變?yōu)?

v(t+dt)=v(t)

(26)

位置向量的更新方程為:

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

(27)

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

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

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

圖3 坡度提取原理圖

假設(shè)測量起始時平臺由水平面靜止?fàn)顟B(tài)出發(fā),則起始時刻載體坐標(biāo)系的重力加速度向量gb(0)為

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

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

5 實驗結(jié)果與分析

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

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

圖4 樓梯實驗場景模型

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

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

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

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

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

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

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

6 結(jié)論

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