MEMS四元數(shù)卡爾曼濾波算法的電梯姿態(tài)估計(jì)①

郭 威,吳允平,2,3,王廷銀,2,3

1(福建師范大學(xué)光電與信息工程學(xué)院,福州 350007)

2(數(shù)字福建環(huán)境監(jiān)測(cè)物聯(lián)網(wǎng)實(shí)驗(yàn)室,福州 350117)

3(福建省光電傳感應(yīng)用工程技術(shù)研究中心,福州 350007)

1 引言

電梯,已逐漸成為城市生活的基礎(chǔ)設(shè)施,是現(xiàn)代人類出行活動(dòng)最后50 米的重要交通工具[1].隨著我國(guó)城市化進(jìn)程的發(fā)展,電梯保有量、年產(chǎn)量、年增量均為世界第一[2].與其他發(fā)達(dá)國(guó)家相比,我國(guó)電梯使用呈現(xiàn)負(fù)載高、運(yùn)量大、隨機(jī)性強(qiáng)等特點(diǎn),電梯事故時(shí)有發(fā)生[3].近十年來,通過信息技術(shù)手段如物聯(lián)網(wǎng)對(duì)電梯運(yùn)行等安全性監(jiān)測(cè),使得電梯事故率持續(xù)下降[4].

隨著電梯安全性能提升的同時(shí),電梯舒適性也漸漸成為人們乘梯、用梯的關(guān)注重點(diǎn).考慮到電梯轎廂異常震動(dòng)是電梯安全性能的關(guān)鍵,也是電梯乘坐舒適度的關(guān)鍵指標(biāo)之一,為此,2016 年姚長(zhǎng)鴻等通過振動(dòng)傳感器來采集異常震動(dòng),將震動(dòng)的突變嵌入到安全管理[5],給電梯的安全管理注入的新觀點(diǎn).顧雯雯等[6]從乘客的視覺角度及電梯的運(yùn)行性能兩方面著手,重點(diǎn)分析了影響電梯乘坐舒適感的主要原因與影響因素.宋涵通過電梯的裝潢、結(jié)構(gòu)、速度控制方式構(gòu)建了基于AHP模糊綜合評(píng)價(jià)法對(duì)電梯的舒適性進(jìn)行評(píng)估的體系[7],通過分析得出各個(gè)因素對(duì)影響電梯舒適性的程度.這些研究從電梯的構(gòu)成、裝飾等方面嘗試對(duì)電梯的安全性和舒適性進(jìn)行了初步研究.

這些年,MEMS(Micro Electro Mechanical System)技術(shù)呈現(xiàn)體積小、靈敏度高、維度多等發(fā)展趨勢(shì)[8],已廣泛應(yīng)用于汽車工業(yè)、地質(zhì)災(zāi)害以及航空航天等眾多領(lǐng)域.九軸傳感器就是MEMS 技術(shù)發(fā)展的典型代表,它采集加速度、磁場(chǎng)和角速度等多維度數(shù)據(jù),為深化行業(yè)應(yīng)用提供了多維數(shù)據(jù)視角.王贇贇等應(yīng)用九軸傳感器采集汽車的數(shù)據(jù),通過互補(bǔ)濾波實(shí)現(xiàn)誤差補(bǔ)償,再使用四元數(shù)法進(jìn)行姿態(tài)解算,得到汽車的姿態(tài)角,進(jìn)一步研究了汽車運(yùn)動(dòng)姿態(tài)和安全行駛之間的相關(guān)性[9].Shi G 等使用九軸傳感器采集汽車的數(shù)據(jù),利用傳統(tǒng)的線性卡爾曼濾波實(shí)現(xiàn)四元數(shù)的姿態(tài)解算并提出了新的方法來單獨(dú)新更磁力計(jì),消除了對(duì)姿態(tài)估計(jì)的影響[10].劉飛飛等應(yīng)用九軸傳感器采集危巖的數(shù)據(jù),經(jīng)內(nèi)置運(yùn)動(dòng)處理引擎(DMP)處理得到危石四元數(shù)后,解算出危巖危石的姿態(tài)角,來監(jiān)測(cè)其滾動(dòng)和震動(dòng)的情況[11].張煌軍等通過應(yīng)用九軸傳感器采集無人機(jī)數(shù)據(jù),將加速計(jì)和磁力計(jì)的數(shù)據(jù)通過平方根容積卡爾曼濾波來糾正陀螺儀誤差,再用四元數(shù)進(jìn)行姿態(tài)解算,從而提高無人機(jī)飛行姿態(tài)解算的精度[12].

電梯作為在有限空間內(nèi)導(dǎo)軌上運(yùn)行的特種設(shè)備,其姿態(tài)、加速度、速度等直接影響乘梯用梯的舒適性以及安全性等體驗(yàn),因此,將九軸MEMS 傳感器應(yīng)用于電梯監(jiān)測(cè)領(lǐng)域中,解算獲得電梯姿態(tài),無疑有助于開展電梯的安全性評(píng)估和舒適性分析,是一項(xiàng)具有重大意義的研究.

2 數(shù)據(jù)采集設(shè)計(jì)

2.1 硬件設(shè)計(jì)

九軸MEMS 數(shù)據(jù)采集電路圖如圖1 所示,傳感器內(nèi)部包含了三軸加速計(jì)、三軸陀螺儀、三軸磁力計(jì),可以采集加速度、角速度、磁場(chǎng)的數(shù)據(jù),RX 和TX 分別為傳感器的UART 接收端和UART 發(fā)送端,與嵌入式微處理器進(jìn)行數(shù)據(jù)交互,由嵌入式微處理器對(duì)接收到的數(shù)據(jù)進(jìn)行計(jì)算分析、完成姿態(tài)解算后進(jìn)行持久化存儲(chǔ).

2.2 軟件設(shè)計(jì)

嵌入式軟件的流程圖如圖2 所示.總體方案是:通過定時(shí)器接收傳感器發(fā)送的數(shù)據(jù).首先,對(duì)數(shù)據(jù)進(jìn)行完整性和可靠性校驗(yàn),以隊(duì)列的方式存儲(chǔ)校驗(yàn)通過的數(shù)據(jù),然后進(jìn)行電梯運(yùn)行姿態(tài)和狀態(tài)的計(jì)算.將采集到的加速計(jì)和磁力計(jì)數(shù)據(jù)利用互補(bǔ)濾波糾正陀螺儀數(shù)據(jù)通過四元數(shù)微分方程進(jìn)行姿態(tài)角初步求解,最后卡爾曼濾波對(duì)初步求解的姿態(tài)角進(jìn)行數(shù)據(jù)精確化,得到最終的姿態(tài)角.同時(shí),利用加速度對(duì)時(shí)間進(jìn)行積分得到電梯的運(yùn)行姿態(tài).最后進(jìn)行持久化存儲(chǔ),方便數(shù)據(jù)的整理分析.

圖1 九軸MEMS 數(shù)據(jù)采集電路圖

圖2 軟件設(shè)計(jì)流程圖

3 數(shù)據(jù)處理

3.1 四元數(shù)姿態(tài)解算

通常,通過引入導(dǎo)航坐標(biāo)系(簡(jiǎn)稱n系,由Zn、Xn、Yn組成)和載體坐標(biāo)系(簡(jiǎn)稱b系,由Zb、Xb、Yb組成)來描述姿態(tài),如圖3 所示.b系可以由n系繞Xn軸、Yn軸、Zn軸轉(zhuǎn)動(dòng)表示,其中旋轉(zhuǎn)的角分別是俯仰角φ、橫滾角θ、航向角ψ,它們統(tǒng)稱為歐拉角或姿態(tài)角[13].

求解姿態(tài)角方法有歐拉角法、方向余弦法、四元數(shù)法[14],其中,歐拉角法存在奇異點(diǎn)現(xiàn)象無法表示物體的全姿態(tài),方向余弦法則存在計(jì)算量大和解算復(fù)雜度高的問題[15],而四元數(shù)法具有計(jì)算量小、無奇異點(diǎn)現(xiàn)象等優(yōu)點(diǎn),因此,主要通過對(duì)陀螺儀數(shù)據(jù)用四元數(shù)法求解姿態(tài)[16].捷聯(lián)慣導(dǎo)系統(tǒng)理論定義了四元數(shù)q[17]:

q0為四元數(shù)標(biāo)量部分,q1、q2、q3為四元數(shù)矢量部分;還定義了陀螺儀和四元數(shù)的微分關(guān)系以及四元數(shù)與n、b兩系之間旋轉(zhuǎn)矩陣的關(guān)系,如式(2)和式(3).

通過一階龍格庫(kù)塔法求解四元數(shù)微分方程如式(4),T為采樣周期.求出四元數(shù)得到旋轉(zhuǎn)矩陣可計(jì)算出姿態(tài)角如式(5).

由于陀螺儀存在長(zhǎng)時(shí)間工作會(huì)誤差漂移、加速計(jì)存在動(dòng)態(tài)響應(yīng)較慢、磁力計(jì)存在數(shù)據(jù)采集容易受環(huán)境影響等因素,在具體應(yīng)用時(shí)往往還會(huì)將這三者數(shù)據(jù)進(jìn)行補(bǔ)償從而獲得準(zhǔn)確的姿態(tài)[18-22].圖4 就是一種典型的互補(bǔ)濾波姿態(tài)解算算法[20].

圖4 基于MEMS 的姿態(tài)角互補(bǔ)濾波算法框圖

圖4 中,虛線部分為互補(bǔ)濾波的過程,xba為b系載體的加速度,n系中標(biāo)準(zhǔn)重力加速度gn=[0,0,1]T,通過旋轉(zhuǎn)矩陣表示為b系標(biāo)準(zhǔn)重力加速度并與xba做向量積,得到b系下加速度向量誤差;xbm為b系載體的磁力,通過旋轉(zhuǎn)矩陣將xbm轉(zhuǎn)到n系下表示xnm=[hx,hy,hz]T,n系下磁力計(jì)理想輸出數(shù)據(jù)[bx,by,bz]T與xnm的關(guān)系為通過旋轉(zhuǎn)矩陣將其轉(zhuǎn)化為b系下表示并與xbm做向量積得到b系磁力計(jì)向量誤差,將加速度向量誤差與磁力計(jì)向量誤差相加得到總向量誤差e,通過PI 調(diào)節(jié)系統(tǒng)輸出的修正補(bǔ)償量為W;Ubg為b系載體的角速度,Ub表示為經(jīng)過修正后陀螺儀的角速度,kp為比例系數(shù),決定了濾波器的截止頻率和響應(yīng)速度,ki為消除穩(wěn)態(tài)誤差的積分系數(shù).最后通過四元數(shù)微分方程求解出四元數(shù)并轉(zhuǎn)換成姿態(tài)角.

3.2 電梯姿態(tài)解算

由于MEMS 磁力計(jì)數(shù)據(jù)易受周圍金屬影響以及互補(bǔ)濾波的截止頻率不易確定[21,22],解算后電梯姿態(tài)角的誤差較大,精度不高,所以在互補(bǔ)濾波姿態(tài)解算上開展進(jìn)一步工作,由此,MEMS 四元數(shù)卡爾曼濾波算法的電梯姿態(tài)求解如圖5 所示.首先,在解算俯仰角和橫滾角時(shí),把互補(bǔ)濾波方法中使用的磁力計(jì)參數(shù)排除,僅用加速計(jì)數(shù)據(jù)計(jì)算向量誤差來校正陀螺儀,來避免磁力計(jì)數(shù)據(jù)誤差對(duì)橫滾角和俯仰角計(jì)算的影響;其次,卡爾曼濾波是最優(yōu)遞推估計(jì)技術(shù),其中線性化的擴(kuò)展卡爾曼濾波(EKF)已經(jīng)廣泛用于工業(yè)非線性系統(tǒng)[23],結(jié)合電梯運(yùn)行規(guī)律以及姿態(tài)估計(jì)系統(tǒng)的非線性特點(diǎn),通過擴(kuò)展卡爾曼濾波將初步解算的姿態(tài)角作為觀測(cè)量對(duì)預(yù)測(cè)量進(jìn)行修正,以便得到當(dāng)前時(shí)刻的最優(yōu)狀態(tài)估計(jì),從而幫助提高電梯姿態(tài)角的精度.

圖5 四元數(shù)卡爾曼濾波的姿態(tài)解算圖

EKF 是將卡爾曼濾波狀態(tài)方程和測(cè)量方程非線性部分線性化,得到線性模型如式(6)和式(7):

預(yù)測(cè)更新過程如下:

1)狀態(tài)一步預(yù)測(cè)

式中,X(k|k-1)是用k-1 時(shí)刻狀態(tài)預(yù)測(cè)的結(jié)果,X(k-1|k-1)是k-1 時(shí)刻狀態(tài)最優(yōu)結(jié)果.

2)狀態(tài)一步預(yù)測(cè)協(xié)方差

式中,P(k|k-1)是X(k|k-1)的協(xié)方差,P(k-1|k-1)是X(k-1|k-1)的協(xié)方差,Q為系統(tǒng)噪聲協(xié)方差.

3)卡爾曼增益

式中,R為測(cè)量噪聲協(xié)方差.

4)狀態(tài)更新

5)狀態(tài)協(xié)方差更新

式中,X(k)為狀態(tài)值用于狀態(tài)預(yù)測(cè),式(7)中Z(k)為初步解算后的姿態(tài)角,式(8)～式(10)是狀態(tài)估計(jì)部分,即當(dāng)前時(shí)刻預(yù)測(cè)值和協(xié)方差以及增益;式(11)和式(12)是狀態(tài)更新的部分即當(dāng)前時(shí)刻的最優(yōu)狀態(tài),然后將當(dāng)前時(shí)刻的電梯最優(yōu)姿態(tài)估計(jì)用于下一個(gè)時(shí)刻的預(yù)測(cè),最終得到精確后的電梯姿態(tài)角.

3.3 電梯速度計(jì)算

根據(jù)數(shù)學(xué)知識(shí)可知,加速度在時(shí)間上的積分等于速度,積分的值可無限近似成區(qū)域面積的總和.由于實(shí)際采樣時(shí)間不趨于0 這樣會(huì)有誤差累加的情況出現(xiàn).為了減少誤差,通過以下方法來求得電梯速度:

其中,Sample(n)為當(dāng)前時(shí)刻加速度,Sample(n-1)為前一時(shí)刻加速度,T為時(shí)間間隔.

3.4 濾波性能分析

就互補(bǔ)濾波算法與卡爾曼濾波算法在收斂性進(jìn)行對(duì)比分析,如圖6 所示.卡爾曼濾波的姿態(tài)角比較于互補(bǔ)濾波的姿態(tài)角抖動(dòng)較小,濾波的收斂效果更好,具有良好的實(shí)時(shí)性.

圖6 互補(bǔ)濾波和卡爾曼濾波的收斂性分析圖

4 實(shí)驗(yàn)分析

4.1 電梯姿態(tài)解算算法優(yōu)化比對(duì)

為驗(yàn)證本方法的效果,在2015 年安裝使用的某電梯上安裝了本裝置,分別應(yīng)用常規(guī)互補(bǔ)濾波法、本算法進(jìn)行俯仰角、航向角以及橫滾角結(jié)算,對(duì)比如圖7所示.本方法解算的姿態(tài)角相對(duì)平滑,抖動(dòng)小.

4.2 電梯運(yùn)行狀態(tài)的數(shù)據(jù)分析

為進(jìn)一步對(duì)比驗(yàn)證本方法的合理性,電梯A 及電梯B 分別安裝了本裝置,對(duì)解算的姿態(tài)角進(jìn)行分析.圖8～圖10 為電梯A 和電梯B 某時(shí)段橫滾角、俯仰角、航向角的對(duì)比數(shù)據(jù).可以看出,兩部電梯在運(yùn)行過程中橫滾角和俯仰角的偏離值只有零點(diǎn)幾度.但在航向角的比較中,電梯A 的偏離值為14 而電梯B 的偏離值為8,兩者相差較大,導(dǎo)致電梯A 的晃動(dòng)感較為明顯.

圖7 互補(bǔ)濾波姿態(tài)解算和本文算法姿態(tài)解算角度對(duì)比圖

圖8 電梯A 和B 橫滾角對(duì)比圖

圖9 電梯A 和B 俯仰角對(duì)比圖

圖11 為電梯A 速度數(shù)據(jù),圖12 為電梯B 速度數(shù)據(jù),速度正值表示電梯為上行姿態(tài),速度負(fù)值表示電梯為下行姿態(tài),速度為零時(shí)則表示電梯靜止,通過速度可知電梯的當(dāng)前狀態(tài);可見,電梯B 在上下運(yùn)行過程中的速度略快于電梯A.

圖10 電梯A 和B 航向角對(duì)比圖

圖13 為電梯A 的加速度,圖14 為電梯B 的加速度,可知兩者的加速度數(shù)據(jù)總體正常.統(tǒng)計(jì)學(xué)中定義峰度為波形尖峭程度的一個(gè)量[24],采用峰度來表示電梯加速度的波形特征,可以較好地比較兩部電梯的舒適性,峰度越大即加速度波形越尖銳,說明電梯在加減速的過程越急速,導(dǎo)致不舒適的乘坐感,峰度越小即加速度波形越平滑電梯在加減速的過程越平緩能帶來良好的乘坐體驗(yàn).由表1 和表2 得出,電梯A 的峰度比電梯B 來的大,即電梯A 的舒適性不如電梯B.

因此,綜合電梯的橫滾角、俯仰角、航向角以及加速度峰度可以得出結(jié)論為,電梯A 的舒適性與安全性皆不如電梯B,這和電梯A、電梯B 的實(shí)際情況相吻合;電梯A 為2007 年投入使用的貨梯,長(zhǎng)時(shí)間大負(fù)荷使用導(dǎo)致器械老化,電梯B 為2016 年投入使用的客梯,設(shè)施狀況和維保較好.

圖11 電梯A 的速度圖

圖12 電梯B 的速度圖

圖13 電梯A 的加速度圖

5 總結(jié)

在電梯監(jiān)測(cè)中應(yīng)用九軸MEMS 傳感器,在應(yīng)用四元數(shù)算法時(shí)需結(jié)合電梯金屬件多、運(yùn)行突變性較小、姿態(tài)變化率不大等運(yùn)行特點(diǎn),首先在求解橫滾角和俯仰角時(shí)用加速計(jì)數(shù)據(jù)而不是磁力計(jì)數(shù)據(jù)來校正陀螺儀,其次,用卡爾曼濾波算法來預(yù)測(cè)估計(jì)電梯狀態(tài),最終提高了解算電梯姿態(tài)角的精度;通過分析電梯運(yùn)行時(shí)的姿態(tài)角和加速度的峰度以及電梯的速度,為評(píng)估對(duì)電梯的安全性與舒適性提供了數(shù)據(jù)支持.

圖14 電梯B 的加速度圖

表1 電梯A 加速度波形的峰度

表2 電梯B 加速度波形的峰度