基于IMU智能安全帽姿態(tài)估計(jì)

馬明剛 嚴(yán)家常 鄭賢喜 金超 黃彥慶

(1.國(guó)網(wǎng)新源浙江寧海抽水蓄能有限公司 浙江寧波 315000； 2.四川超影科技有限公司 成都 610065)

0 引言

安全帽是防止物體碰到頭部的保護(hù)裝置，在現(xiàn)代工程中是非常重要的安全保護(hù)措施之一。在電力、地質(zhì)、林業(yè)和礦業(yè)等工作環(huán)境有危險(xiǎn)的領(lǐng)域都有很廣泛的應(yīng)用。隨著現(xiàn)代工程中的安全需求越來(lái)越高，新型智能安全帽應(yīng)運(yùn)而生。新型智能安全帽一般包括位置定位、安全感知、安全預(yù)警和遠(yuǎn)程通信等功能。而安全帽的姿態(tài)估計(jì)是安全預(yù)警的前提條件之一。

近年來(lái)，不少學(xué)者使用緊湊型MEMS-IMU(微機(jī)電系統(tǒng)-慣性測(cè)量單元)進(jìn)行了關(guān)于姿態(tài)估計(jì)的廣泛研究。MEMS-IMU主要由三軸陀螺儀(測(cè)量角速度)、三軸加速度計(jì)(測(cè)量外部加速度和重力之和)和三軸磁傳感器(測(cè)量地球磁場(chǎng))組成。這些傳感器中的每一個(gè)都可以單獨(dú)使用，以相對(duì)較低的精度推斷出姿態(tài)[1]。因此，如何克服每個(gè)傳感器單獨(dú)使用時(shí)出現(xiàn)的缺點(diǎn)以及如何有針對(duì)性地結(jié)合慣性和磁傳感器測(cè)量[2]，是設(shè)計(jì)姿態(tài)估計(jì)方法時(shí)要解決的關(guān)鍵問(wèn)題。

物體姿態(tài)估計(jì)一般分為以下兩種思路。第一種使用磁力計(jì)和加速度計(jì)，并包括確定性方法[3]，找到姿態(tài)矩陣即可。這些方法是通過(guò)運(yùn)動(dòng)是靜態(tài)的或準(zhǔn)靜態(tài)的還原假設(shè)來(lái)設(shè)計(jì)的。在可以觀察到顯著的外部加速度的情況下，這種假設(shè)是無(wú)效的。在這種情況下，由于不能直接分離外部加速度和重力，因此難以高精度地檢索姿態(tài)。第二種思路是結(jié)合陀螺儀、加速度計(jì)和磁力計(jì)，包括使用基于卡爾曼濾波器(KF)的非線性觀測(cè)器的經(jīng)典濾波方法[4]。一種使用線性觀測(cè)模型的新型四元數(shù)卡爾曼濾波器(QKF)被提出，其測(cè)量噪聲的協(xié)方差中添加了基于函數(shù)噪聲的對(duì)角矩陣[5]。

本文智能安全帽姿態(tài)估計(jì)采用第二種思路。在實(shí)驗(yàn)平臺(tái)智能安全帽中安裝MEMS-IMU，通過(guò)其包含的三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)得到相關(guān)數(shù)據(jù)。物體的加速度數(shù)據(jù)和陀螺儀測(cè)得的在頻域上能夠?qū)崿F(xiàn)特性互補(bǔ)[6]，根據(jù)磁力計(jì)傳感器的數(shù)據(jù)反饋，可以提高系統(tǒng)的精度。將3個(gè)傳感器的數(shù)據(jù)融合成四元數(shù)，再將四元數(shù)轉(zhuǎn)換成歐拉角，得到智能安全帽的姿態(tài)信息。

1 智能安全帽的姿態(tài)

1.1 歐拉角描述姿態(tài)

歐拉角是一種常見(jiàn)的姿態(tài)描述方式，其可以體現(xiàn)出一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換方式，通過(guò)繞不同坐標(biāo)軸的3次連續(xù)轉(zhuǎn)動(dòng)來(lái)實(shí)現(xiàn)[7]。歐拉角是這3次轉(zhuǎn)動(dòng)角度的統(tǒng)一稱呼。為了更好地描述文中實(shí)驗(yàn)平臺(tái)的方位及位置信息，定義了坐標(biāo)系如圖1所示。圖中包括大地坐標(biāo)系O-XYZ和安全帽坐標(biāo)系P-UVW。大地坐標(biāo)系是以地球?yàn)閰⒖嫉摹?/p>

研究使用航空次序歐拉角，定義其轉(zhuǎn)動(dòng)順序?yàn)閆-Y-X，繞Z軸轉(zhuǎn)動(dòng)為偏航角(Yaw)，繞Y軸轉(zhuǎn)動(dòng)為俯仰角(Pitch)，繞X軸轉(zhuǎn)動(dòng)為橫滾角(Roll)。

偏航角(Yaw)：智能安全帽坐標(biāo)系X軸投影到水平面與參考系X軸的夾角，規(guī)定順時(shí)針旋轉(zhuǎn)為正。俯仰角(Pitch)：智能安全帽坐標(biāo)系X軸與水平面的夾角，規(guī)定仰角為正。橫滾角(Roll)：安全帽系Z軸與通過(guò)安全帽系X軸的鉛垂面間的夾角，規(guī)定安全帽右旋為正。

圖1 大地坐標(biāo)系和智能安全帽坐標(biāo)系相對(duì)位置

1.2 四元數(shù)描述物體姿態(tài)

旋轉(zhuǎn)矩陣可以直觀地表示出物體的姿態(tài)，但是旋轉(zhuǎn)矩陣有9個(gè)分量，顯得十分冗余。同樣可以表征旋轉(zhuǎn)的還有歐拉角，但是歐拉角具有萬(wàn)向節(jié)鎖問(wèn)題[8]，給計(jì)算解析造成很大麻煩。本研究選用在慣性導(dǎo)航中物體的空間姿態(tài)表示方法四元數(shù)，其一般由3個(gè)虛部和一個(gè)實(shí)部構(gòu)成，一般形式為

Q=[qW,qX,qY,qZ]=qW+qXi+qYj+qZk(1)

Q=[Nsin(α/2)，cos(α/2)] (2)

四元數(shù)乘法和多項(xiàng)式相乘很類似，可以理解為再作一次上述定點(diǎn)轉(zhuǎn)動(dòng)變換，用Q'表示，即可以等效為一個(gè)轉(zhuǎn)動(dòng)四元數(shù)。

Q0=Q?Q'(3)

1.3 相關(guān)角度的測(cè)量

當(dāng)智能安全帽處于靜止?fàn)顟B(tài)時(shí)，加速度計(jì)只受到重力這一個(gè)作用力。當(dāng)安全帽與水平面完全平行時(shí)，加速度計(jì)的理論輸出值是[0,0,g](g是所在地區(qū)重力加速度大小)。當(dāng)智能安全帽以一定角速度傾斜于水平面時(shí)，其各方向上輸出值不一樣，但其合成模值為1 G。

實(shí)際上，在智能安全帽處于靜止?fàn)顟B(tài)且有傾斜角度時(shí)，如圖2所示，加速度計(jì)此時(shí)所測(cè)量的數(shù)據(jù)是重力加速度在其坐標(biāo)系下的投影。借助于三角函數(shù)關(guān)系，可以計(jì)算出智能安全帽的姿態(tài)角。

圖2 物體姿態(tài)角和加速度之間的三角函數(shù)關(guān)系

2 四元數(shù)法姿態(tài)估計(jì)

通過(guò)四元數(shù)的分解進(jìn)行物體的準(zhǔn)靜態(tài)姿態(tài)估計(jì)。該算法從四元數(shù)的物理意義出發(fā)，把載體的轉(zhuǎn)動(dòng)分解并合成以求得其姿態(tài)四元數(shù)。一般都是使用三參數(shù)法推導(dǎo)四元數(shù)，這會(huì)導(dǎo)致結(jié)果出現(xiàn)奇異值問(wèn)題[9]。針對(duì)這個(gè)問(wèn)題，提出使用轉(zhuǎn)軸補(bǔ)償?shù)姆椒ǜ恼娈愔?，可以?shí)現(xiàn)智能安全帽的全姿態(tài)估計(jì)，同時(shí)提高其姿態(tài)估計(jì)的準(zhǔn)確率。

2.1 分解四元數(shù)

假設(shè)智能安全帽與地球坐標(biāo)系重合，設(shè)繞Z軸轉(zhuǎn)動(dòng)的，稱為航向角(Yaw)，記為φ；繞Y軸轉(zhuǎn)動(dòng)的，稱為俯仰角，記為θ；繞過(guò)X軸轉(zhuǎn)動(dòng)的，稱為橫滾角(Roll),記為γ。這樣智能安全帽相對(duì)于地球坐標(biāo)系的任意姿勢(shì)都可以用對(duì)應(yīng)的歐拉角表示出來(lái)，分別求出各個(gè)方向的歐拉角四元數(shù)，再合成姿態(tài)四元數(shù)即是智能安全帽的姿態(tài)描述。

2.1.1 航向角四元數(shù)

求航向角之前，先要轉(zhuǎn)換到水平平面上，根據(jù)式(4)可以求得智能安全帽所在地等效的磁感性強(qiáng)度。令智能安全帽航向角為φ時(shí)，其所在地的磁感應(yīng)強(qiáng)度mb=[mbX,mbY,mbZ]。這時(shí)，智能安全帽和地球磁場(chǎng)在水平方向的磁感應(yīng)強(qiáng)度關(guān)系為

因φ∈(-π，π]，則得到航向角的旋轉(zhuǎn)四元數(shù)為

(6)

2.1.2 俯仰角四元數(shù)

一般來(lái)說(shuō)，物體水平方向的旋轉(zhuǎn)不會(huì)影響其垂直方向的姿態(tài)。在智能安全帽靜止時(shí)，測(cè)得的加速度a=[0,0,g]T,若智能安全帽繞Y軸旋轉(zhuǎn)一定角度θ，則加速度在Y軸的分量仍為0，即為

aX=-gsinθ,aZ=gcosθ(7)

應(yīng)用式(8)將加速度數(shù)據(jù)歸一化為

根據(jù)歸一化后的三角函數(shù)關(guān)系，有

式中，sign(a)為取符函數(shù)。

2.1.3 橫滾角四元數(shù)

橫滾角四元數(shù)X軸分量aX不變，aY,aZ為

aY=gcosθsinγ,aZ=gcosθcosγ

(12)

這時(shí)橫滾角的正、余弦值分別為

橫滾角γ∈(-π，π]，則橫滾角的四元數(shù)為

在求出智能安全帽的俯仰角、橫滾角和航向角的3個(gè)四元數(shù)之后，將這3個(gè)四元數(shù)分量進(jìn)行合成后便可得到智能安全帽姿態(tài)四元數(shù)，合成公式為

qm=qφ?qθ?qγ(15)

在傳統(tǒng)的靜態(tài)姿態(tài)估計(jì)算法中，常常會(huì)由于磁感應(yīng)模型的非精確匹配造成物體豎直方向姿態(tài)估計(jì)產(chǎn)生誤差，文中上述方法能有效減小該誤差。

3 補(bǔ)償轉(zhuǎn)軸方法

文中介紹的分解-合成四元數(shù)算法是利用智能安全帽固有的重力加速度g以及智能安全帽所在地的磁場(chǎng)強(qiáng)度作為參考的，可以在一定程度上消除磁感應(yīng)模型非精確匹配造成的估計(jì)誤差[10]。但是過(guò)程中在一定情況下會(huì)出現(xiàn)奇異值點(diǎn)問(wèn)題，這里需要采取一定措施予以修正。

根據(jù)式(13)求俯仰角時(shí)，就有可能會(huì)出現(xiàn)分母為0的情況，此時(shí)通過(guò)分解四元數(shù)來(lái)進(jìn)行智能安全帽姿態(tài)估計(jì)是無(wú)法進(jìn)行的。針對(duì)這個(gè)問(wèn)題，主流的解決方法是借角法[11]。其基本過(guò)程是在俯仰角θ無(wú)限接近于±π/2時(shí)，將俯仰角再轉(zhuǎn)過(guò)一定角度φ，這時(shí)的等效俯仰角即為θ-φ。在分解四元數(shù)時(shí)，智能安全帽相應(yīng)的原始加速度和其所在地磁感應(yīng)強(qiáng)度數(shù)據(jù)也作相應(yīng)四元數(shù)轉(zhuǎn)化。這樣得到相應(yīng)的四元數(shù)后，再轉(zhuǎn)化為實(shí)際的姿態(tài)四元數(shù)。

借角法能解決上述的奇異值問(wèn)題，但是其不足之處在于會(huì)增加大量運(yùn)算，加大時(shí)間損耗，不利于實(shí)現(xiàn)算法的實(shí)時(shí)性。文中提出補(bǔ)償轉(zhuǎn)軸方法來(lái)修正奇異值，以實(shí)現(xiàn)智能安全帽的實(shí)時(shí)全姿態(tài)估計(jì)。

首先將加速度數(shù)據(jù)和智能安全帽所在地的磁感應(yīng)強(qiáng)度數(shù)據(jù)轉(zhuǎn)軸匹配為智能安全帽在另一側(cè)水平面的數(shù)據(jù)；然后將俯仰角設(shè)為0，根據(jù)前面公式計(jì)算橫滾角和航向角所對(duì)應(yīng)的四元數(shù)；接著把原本的俯仰角所對(duì)應(yīng)的四元數(shù)代入式(15)，合成新的四元數(shù)。

當(dāng)實(shí)驗(yàn)平臺(tái)智能安全帽是由水平正面向上沿俯仰角方向轉(zhuǎn)至豎立(左側(cè)正面，右側(cè)反面)時(shí)會(huì)出現(xiàn)奇異值問(wèn)題。這時(shí)，應(yīng)用上述的補(bǔ)償轉(zhuǎn)軸方法，即將處于豎直狀態(tài)的智能安全帽慣性數(shù)據(jù)匹配至反面位置，可以推算出反面位置中X軸數(shù)據(jù)應(yīng)該是豎立位置中Z軸測(cè)得數(shù)據(jù)的相反數(shù)，并且Z軸數(shù)據(jù)為豎立位置中X軸測(cè)得數(shù)據(jù)。這時(shí)將豎立位置的慣性數(shù)據(jù)轉(zhuǎn)軸匹配，分解四元數(shù)即得到智能安全帽的奇異值點(diǎn)的姿態(tài)信息。

補(bǔ)償轉(zhuǎn)軸的本質(zhì)是目標(biāo)物體所在坐標(biāo)系的轉(zhuǎn)變。上述方法即是將智能安全帽從之前的Z-Y-X軸轉(zhuǎn)換為Z-X-Y軸，這并不能從根本上消除奇異值點(diǎn)，而是將奇異值點(diǎn)轉(zhuǎn)移至不影響運(yùn)算的位置。

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

通過(guò)智能安全帽實(shí)驗(yàn)平臺(tái)(見(jiàn)圖3)驗(yàn)證文中姿態(tài)估計(jì)方法可行性。智能安全帽中的MEMS-IMU系統(tǒng)架構(gòu)如圖4所示。智能安全帽姿態(tài)估計(jì)驗(yàn)證系統(tǒng)主要是由MEMS傳感器、微處理器和上位機(jī)組成。軟件部分采用Matlab2016b作為算法驗(yàn)證平臺(tái)，以便于實(shí)現(xiàn)補(bǔ)償轉(zhuǎn)軸和分解四元數(shù)算法。

在實(shí)驗(yàn)過(guò)程中，由于四元數(shù)不能直觀地反映出智能安全帽的姿態(tài)信息，因此在實(shí)驗(yàn)結(jié)果中將四元數(shù)轉(zhuǎn)換為歐拉角顯示出來(lái)，并且將航向角由-180°～180°的表示方式改為0°～360°來(lái)顯示。

將智能安全帽放置水平在面上，沿其自身所在坐標(biāo)系正向旋轉(zhuǎn)，每次旋轉(zhuǎn)一圈后都在某個(gè)點(diǎn)停留幾秒，之后繼續(xù)旋轉(zhuǎn)，重復(fù)7次。部分實(shí)驗(yàn)結(jié)果可視化后如圖5～圖7所示。

圖3 智能安全帽實(shí)驗(yàn)平臺(tái)

圖4 智能安全帽驗(yàn)證系統(tǒng)架構(gòu)

圖5 航向角變化可視化結(jié)果

圖6 俯仰角變化可視化結(jié)果

圖7 橫滾角變化可視化結(jié)果

由于智能安全帽的主要應(yīng)用場(chǎng)景是水平方向，分解四元數(shù)法將磁感應(yīng)數(shù)用于水平方向上的航向角的姿態(tài)估計(jì)，但并不涉及到垂直方向上俯仰角和橫滾角的姿態(tài)計(jì)算，在一定程度上可以減小磁感應(yīng)模型的干擾所帶來(lái)的誤差。

5 結(jié)論

提出以MEMS-IMU為基礎(chǔ)的智能安全帽姿態(tài)估計(jì)算法，根據(jù)智能安全帽的加速度和磁感應(yīng)強(qiáng)度數(shù)據(jù)的四元數(shù)分解，以及和角速度的微分關(guān)系可以得到智能安全帽的姿態(tài)。同時(shí)，針對(duì)姿態(tài)估計(jì)過(guò)程中存在奇異值點(diǎn)的問(wèn)題，提出補(bǔ)償轉(zhuǎn)軸的方法，可以實(shí)現(xiàn)智能安全帽的全姿態(tài)估計(jì)，這對(duì)于智能安全帽在工業(yè)安全預(yù)警中的應(yīng)用很有幫助。實(shí)驗(yàn)結(jié)果顯示，文中算法可以滿足實(shí)時(shí)應(yīng)用的要求。