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

蘇志剛,吳柳莉,郝敬堂

(1.中國(guó)民航大學(xué) 智能信號(hào)與圖像處理天津市重點(diǎn)實(shí)驗(yàn)室,天津 300300;2.中國(guó)民航大學(xué) 中歐航空工程師學(xué)院,天津 300300)

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

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

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

1 算法框架

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

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

圖1 算法框圖

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

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

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

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

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

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

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

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

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

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

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

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

(3)

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

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

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

當(dāng)智能手機(jī)處于零加速狀態(tài)時(shí),載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的位置關(guān)系保持不變,此時(shí)可以利用傳感器采集的數(shù)據(jù)直接實(shí)現(xiàn)對(duì)旋轉(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逆時(shí)針旋轉(zhuǎn)?nt角度得到導(dǎo)航坐標(biāo)系。因此,載體坐標(biāo)系下三軸加速度計(jì)輸出gb(t)=[gxtgytgzt]T和三軸磁力計(jì)輸出hb(t)=[mxtmytmzt]T也可繞單位向量un(t)逆時(shí)針旋轉(zhuǎn)?nt角度得到導(dǎo)航坐標(biāo)系的重力分量gn=[0 0 -g]T和磁場(chǎng)分量hn=[hxhyhz]T,其中g(shù)為當(dāng)?shù)刂亓铀俣?hx、hy和hz為當(dāng)?shù)卮帕€在地理坐標(biāo)系的各軸向的分量,可由國(guó)際地磁參考場(chǎng)IGRF(International Geomagnetic Reference Field)獲得。由此可知,向量un(t)一定位于gn和gb(t)之間的對(duì)稱面上,該對(duì)稱面的法向量為:

r1(t)=[gxtgytgzt+g]T

(4)

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

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)

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

即可求得單位向量un(t)的三個(gè)分量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)系中的對(duì)應(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)系為

對(duì)式(14)進(jìn)行逆運(yù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)的各個(gè)分量uxt、uyt、uzt與?nt,由式(11)可確定四元數(shù)qn(t)。

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

當(dāng)智能手機(jī)處于非零加速狀態(tài)時(shí),三軸加速度計(jì)的輸出將不再影響載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的位置關(guān)系,而是三軸角速度傳感器采集的數(shù)據(jù)向量ωb(t)影響載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間的位置關(guān)系。因此,利用ωb(t)實(shí)現(xiàn)對(duì)旋轉(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)系下定義的。對(duì)旋轉(zhuǎn)四元數(shù)qn(t)的更新需要利用導(dǎo)航坐標(biāo)系的角速度向量ωn(t)。根據(jù)式(14)可得:

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

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

4 位置解算與坡度提取

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

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

速度向量的更新方程為:

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

(25)

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

v(t+dt)=v(t)

(26)

位置向量的更新方程為:

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

(27)

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

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

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

圖3 坡度提取原理圖

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

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

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

5 實(shí)驗(yàn)結(jié)果與分析

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

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

圖4 樓梯實(shí)驗(yàn)場(chǎng)景模型

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

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

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

圖6 斜坡道面實(shí)驗(yàn)場(chǎng)景模型

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

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

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

6 結(jié)論

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