郭 威,吳允平,2,3,王廷銀,2,3
1(福建師范大學(xué)光電與信息工程學(xué)院,福州 350007)
2(數(shù)字福建環(huán)境監(jiān)測物聯(lián)網(wǎng)實驗室,福州 350117)
3(福建省光電傳感應(yīng)用工程技術(shù)研究中心,福州 350007)
電梯,已逐漸成為城市生活的基礎(chǔ)設(shè)施,是現(xiàn)代人類出行活動最后50 米的重要交通工具[1].隨著我國城市化進程的發(fā)展,電梯保有量、年產(chǎn)量、年增量均為世界第一[2].與其他發(fā)達國家相比,我國電梯使用呈現(xiàn)負(fù)載高、運量大、隨機性強等特點,電梯事故時有發(fā)生[3].近十年來,通過信息技術(shù)手段如物聯(lián)網(wǎng)對電梯運行等安全性監(jiān)測,使得電梯事故率持續(xù)下降[4].
隨著電梯安全性能提升的同時,電梯舒適性也漸漸成為人們乘梯、用梯的關(guān)注重點.考慮到電梯轎廂異常震動是電梯安全性能的關(guān)鍵,也是電梯乘坐舒適度的關(guān)鍵指標(biāo)之一,為此,2016 年姚長鴻等通過振動傳感器來采集異常震動,將震動的突變嵌入到安全管理[5],給電梯的安全管理注入的新觀點.顧雯雯等[6]從乘客的視覺角度及電梯的運行性能兩方面著手,重點分析了影響電梯乘坐舒適感的主要原因與影響因素.宋涵通過電梯的裝潢、結(jié)構(gòu)、速度控制方式構(gòu)建了基于AHP模糊綜合評價法對電梯的舒適性進行評估的體系[7],通過分析得出各個因素對影響電梯舒適性的程度.這些研究從電梯的構(gòu)成、裝飾等方面嘗試對電梯的安全性和舒適性進行了初步研究.
這些年,MEMS(Micro Electro Mechanical System)技術(shù)呈現(xiàn)體積小、靈敏度高、維度多等發(fā)展趨勢[8],已廣泛應(yīng)用于汽車工業(yè)、地質(zhì)災(zāi)害以及航空航天等眾多領(lǐng)域.九軸傳感器就是MEMS 技術(shù)發(fā)展的典型代表,它采集加速度、磁場和角速度等多維度數(shù)據(jù),為深化行業(yè)應(yīng)用提供了多維數(shù)據(jù)視角.王贇贇等應(yīng)用九軸傳感器采集汽車的數(shù)據(jù),通過互補濾波實現(xiàn)誤差補償,再使用四元數(shù)法進行姿態(tài)解算,得到汽車的姿態(tài)角,進一步研究了汽車運動姿態(tài)和安全行駛之間的相關(guān)性[9].Shi G 等使用九軸傳感器采集汽車的數(shù)據(jù),利用傳統(tǒng)的線性卡爾曼濾波實現(xiàn)四元數(shù)的姿態(tài)解算并提出了新的方法來單獨新更磁力計,消除了對姿態(tài)估計的影響[10].劉飛飛等應(yīng)用九軸傳感器采集危巖的數(shù)據(jù),經(jīng)內(nèi)置運動處理引擎(DMP)處理得到危石四元數(shù)后,解算出危巖危石的姿態(tài)角,來監(jiān)測其滾動和震動的情況[11].張煌軍等通過應(yīng)用九軸傳感器采集無人機數(shù)據(jù),將加速計和磁力計的數(shù)據(jù)通過平方根容積卡爾曼濾波來糾正陀螺儀誤差,再用四元數(shù)進行姿態(tài)解算,從而提高無人機飛行姿態(tài)解算的精度[12].
電梯作為在有限空間內(nèi)導(dǎo)軌上運行的特種設(shè)備,其姿態(tài)、加速度、速度等直接影響乘梯用梯的舒適性以及安全性等體驗,因此,將九軸MEMS 傳感器應(yīng)用于電梯監(jiān)測領(lǐng)域中,解算獲得電梯姿態(tài),無疑有助于開展電梯的安全性評估和舒適性分析,是一項具有重大意義的研究.
九軸MEMS 數(shù)據(jù)采集電路圖如圖1 所示,傳感器內(nèi)部包含了三軸加速計、三軸陀螺儀、三軸磁力計,可以采集加速度、角速度、磁場的數(shù)據(jù),RX 和TX 分別為傳感器的UART 接收端和UART 發(fā)送端,與嵌入式微處理器進行數(shù)據(jù)交互,由嵌入式微處理器對接收到的數(shù)據(jù)進行計算分析、完成姿態(tài)解算后進行持久化存儲.
嵌入式軟件的流程圖如圖2 所示.總體方案是:通過定時器接收傳感器發(fā)送的數(shù)據(jù).首先,對數(shù)據(jù)進行完整性和可靠性校驗,以隊列的方式存儲校驗通過的數(shù)據(jù),然后進行電梯運行姿態(tài)和狀態(tài)的計算.將采集到的加速計和磁力計數(shù)據(jù)利用互補濾波糾正陀螺儀數(shù)據(jù)通過四元數(shù)微分方程進行姿態(tài)角初步求解,最后卡爾曼濾波對初步求解的姿態(tài)角進行數(shù)據(jù)精確化,得到最終的姿態(tài)角.同時,利用加速度對時間進行積分得到電梯的運行姿態(tài).最后進行持久化存儲,方便數(shù)據(jù)的整理分析.
圖1 九軸MEMS 數(shù)據(jù)采集電路圖
圖2 軟件設(shè)計流程圖
通常,通過引入導(dǎo)航坐標(biāo)系(簡稱n系,由Zn、Xn、Yn組成)和載體坐標(biāo)系(簡稱b系,由Zb、Xb、Yb組成)來描述姿態(tài),如圖3 所示.b系可以由n系繞Xn軸、Yn軸、Zn軸轉(zhuǎn)動表示,其中旋轉(zhuǎn)的角分別是俯仰角φ、橫滾角θ、航向角ψ,它們統(tǒng)稱為歐拉角或姿態(tài)角[13].
求解姿態(tài)角方法有歐拉角法、方向余弦法、四元數(shù)法[14],其中,歐拉角法存在奇異點現(xiàn)象無法表示物體的全姿態(tài),方向余弦法則存在計算量大和解算復(fù)雜度高的問題[15],而四元數(shù)法具有計算量小、無奇異點現(xiàn)象等優(yōu)點,因此,主要通過對陀螺儀數(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).
通過一階龍格庫塔法求解四元數(shù)微分方程如式(4),T為采樣周期.求出四元數(shù)得到旋轉(zhuǎn)矩陣可計算出姿態(tài)角如式(5).
由于陀螺儀存在長時間工作會誤差漂移、加速計存在動態(tài)響應(yīng)較慢、磁力計存在數(shù)據(jù)采集容易受環(huán)境影響等因素,在具體應(yīng)用時往往還會將這三者數(shù)據(jù)進行補償從而獲得準(zhǔn)確的姿態(tài)[18-22].圖4 就是一種典型的互補濾波姿態(tài)解算算法[20].
圖4 基于MEMS 的姿態(tài)角互補濾波算法框圖
圖4 中,虛線部分為互補濾波的過程,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系下磁力計理想輸出數(shù)據(jù)[bx,by,bz]T與xnm的關(guān)系為通過旋轉(zhuǎn)矩陣將其轉(zhuǎn)化為b系下表示并與xbm做向量積得到b系磁力計向量誤差,將加速度向量誤差與磁力計向量誤差相加得到總向量誤差e,通過PI 調(diào)節(jié)系統(tǒng)輸出的修正補償量為W;Ubg為b系載體的角速度,Ub表示為經(jīng)過修正后陀螺儀的角速度,kp為比例系數(shù),決定了濾波器的截止頻率和響應(yīng)速度,ki為消除穩(wěn)態(tài)誤差的積分系數(shù).最后通過四元數(shù)微分方程求解出四元數(shù)并轉(zhuǎn)換成姿態(tài)角.
由于MEMS 磁力計數(shù)據(jù)易受周圍金屬影響以及互補濾波的截止頻率不易確定[21,22],解算后電梯姿態(tài)角的誤差較大,精度不高,所以在互補濾波姿態(tài)解算上開展進一步工作,由此,MEMS 四元數(shù)卡爾曼濾波算法的電梯姿態(tài)求解如圖5 所示.首先,在解算俯仰角和橫滾角時,把互補濾波方法中使用的磁力計參數(shù)排除,僅用加速計數(shù)據(jù)計算向量誤差來校正陀螺儀,來避免磁力計數(shù)據(jù)誤差對橫滾角和俯仰角計算的影響;其次,卡爾曼濾波是最優(yōu)遞推估計技術(shù),其中線性化的擴展卡爾曼濾波(EKF)已經(jīng)廣泛用于工業(yè)非線性系統(tǒng)[23],結(jié)合電梯運行規(guī)律以及姿態(tài)估計系統(tǒng)的非線性特點,通過擴展卡爾曼濾波將初步解算的姿態(tài)角作為觀測量對預(yù)測量進行修正,以便得到當(dāng)前時刻的最優(yōu)狀態(tài)估計,從而幫助提高電梯姿態(tài)角的精度.
圖5 四元數(shù)卡爾曼濾波的姿態(tài)解算圖
EKF 是將卡爾曼濾波狀態(tài)方程和測量方程非線性部分線性化,得到線性模型如式(6)和式(7):
預(yù)測更新過程如下:
1)狀態(tài)一步預(yù)測
式中,X(k|k-1)是用k-1 時刻狀態(tài)預(yù)測的結(jié)果,X(k-1|k-1)是k-1 時刻狀態(tài)最優(yōu)結(jié)果.
2)狀態(tài)一步預(yù)測協(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為測量噪聲協(xié)方差.
4)狀態(tài)更新
5)狀態(tài)協(xié)方差更新
式中,X(k)為狀態(tài)值用于狀態(tài)預(yù)測,式(7)中Z(k)為初步解算后的姿態(tài)角,式(8)~式(10)是狀態(tài)估計部分,即當(dāng)前時刻預(yù)測值和協(xié)方差以及增益;式(11)和式(12)是狀態(tài)更新的部分即當(dāng)前時刻的最優(yōu)狀態(tài),然后將當(dāng)前時刻的電梯最優(yōu)姿態(tài)估計用于下一個時刻的預(yù)測,最終得到精確后的電梯姿態(tài)角.
根據(jù)數(shù)學(xué)知識可知,加速度在時間上的積分等于速度,積分的值可無限近似成區(qū)域面積的總和.由于實際采樣時間不趨于0 這樣會有誤差累加的情況出現(xiàn).為了減少誤差,通過以下方法來求得電梯速度:
其中,Sample(n)為當(dāng)前時刻加速度,Sample(n-1)為前一時刻加速度,T為時間間隔.
就互補濾波算法與卡爾曼濾波算法在收斂性進行對比分析,如圖6 所示.卡爾曼濾波的姿態(tài)角比較于互補濾波的姿態(tài)角抖動較小,濾波的收斂效果更好,具有良好的實時性.
圖6 互補濾波和卡爾曼濾波的收斂性分析圖
為驗證本方法的效果,在2015 年安裝使用的某電梯上安裝了本裝置,分別應(yīng)用常規(guī)互補濾波法、本算法進行俯仰角、航向角以及橫滾角結(jié)算,對比如圖7所示.本方法解算的姿態(tài)角相對平滑,抖動小.
為進一步對比驗證本方法的合理性,電梯A 及電梯B 分別安裝了本裝置,對解算的姿態(tài)角進行分析.圖8~圖10 為電梯A 和電梯B 某時段橫滾角、俯仰角、航向角的對比數(shù)據(jù).可以看出,兩部電梯在運行過程中橫滾角和俯仰角的偏離值只有零點幾度.但在航向角的比較中,電梯A 的偏離值為14 而電梯B 的偏離值為8,兩者相差較大,導(dǎo)致電梯A 的晃動感較為明顯.
圖7 互補濾波姿態(tài)解算和本文算法姿態(tài)解算角度對比圖
圖8 電梯A 和B 橫滾角對比圖
圖9 電梯A 和B 俯仰角對比圖
圖11 為電梯A 速度數(shù)據(jù),圖12 為電梯B 速度數(shù)據(jù),速度正值表示電梯為上行姿態(tài),速度負(fù)值表示電梯為下行姿態(tài),速度為零時則表示電梯靜止,通過速度可知電梯的當(dāng)前狀態(tài);可見,電梯B 在上下運行過程中的速度略快于電梯A.
圖10 電梯A 和B 航向角對比圖
圖13 為電梯A 的加速度,圖14 為電梯B 的加速度,可知兩者的加速度數(shù)據(jù)總體正常.統(tǒng)計學(xué)中定義峰度為波形尖峭程度的一個量[24],采用峰度來表示電梯加速度的波形特征,可以較好地比較兩部電梯的舒適性,峰度越大即加速度波形越尖銳,說明電梯在加減速的過程越急速,導(dǎo)致不舒適的乘坐感,峰度越小即加速度波形越平滑電梯在加減速的過程越平緩能帶來良好的乘坐體驗.由表1 和表2 得出,電梯A 的峰度比電梯B 來的大,即電梯A 的舒適性不如電梯B.
因此,綜合電梯的橫滾角、俯仰角、航向角以及加速度峰度可以得出結(jié)論為,電梯A 的舒適性與安全性皆不如電梯B,這和電梯A、電梯B 的實際情況相吻合;電梯A 為2007 年投入使用的貨梯,長時間大負(fù)荷使用導(dǎo)致器械老化,電梯B 為2016 年投入使用的客梯,設(shè)施狀況和維保較好.
圖11 電梯A 的速度圖
圖12 電梯B 的速度圖
圖13 電梯A 的加速度圖
在電梯監(jiān)測中應(yīng)用九軸MEMS 傳感器,在應(yīng)用四元數(shù)算法時需結(jié)合電梯金屬件多、運行突變性較小、姿態(tài)變化率不大等運行特點,首先在求解橫滾角和俯仰角時用加速計數(shù)據(jù)而不是磁力計數(shù)據(jù)來校正陀螺儀,其次,用卡爾曼濾波算法來預(yù)測估計電梯狀態(tài),最終提高了解算電梯姿態(tài)角的精度;通過分析電梯運行時的姿態(tài)角和加速度的峰度以及電梯的速度,為評估對電梯的安全性與舒適性提供了數(shù)據(jù)支持.
圖14 電梯B 的加速度圖
表1 電梯A 加速度波形的峰度
表2 電梯B 加速度波形的峰度