MEMS四元數卡爾曼濾波算法的電梯姿態估計①

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

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

2(數字福建環境監測物聯網實驗室,福州 350117)

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

1 引言

電梯,已逐漸成為城市生活的基礎設施,是現代人類出行活動最后50 米的重要交通工具[1].隨著我國城市化進程的發展,電梯保有量、年產量、年增量均為世界第一[2].與其他發達國家相比,我國電梯使用呈現負載高、運量大、隨機性強等特點,電梯事故時有發生[3].近十年來,通過信息技術手段如物聯網對電梯運行等安全性監測,使得電梯事故率持續下降[4].

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

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

電梯作為在有限空間內導軌上運行的特種設備,其姿態、加速度、速度等直接影響乘梯用梯的舒適性以及安全性等體驗,因此,將九軸MEMS 傳感器應用于電梯監測領域中,解算獲得電梯姿態,無疑有助于開展電梯的安全性評估和舒適性分析,是一項具有重大意義的研究.

2 數據采集設計

2.1 硬件設計

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

2.2 軟件設計

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

圖1 九軸MEMS 數據采集電路圖

圖2 軟件設計流程圖

3 數據處理

3.1 四元數姿態解算

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

求解姿態角方法有歐拉角法、方向余弦法、四元數法[14],其中,歐拉角法存在奇異點現象無法表示物體的全姿態,方向余弦法則存在計算量大和解算復雜度高的問題[15],而四元數法具有計算量小、無奇異點現象等優點,因此,主要通過對陀螺儀數據用四元數法求解姿態[16].捷聯慣導系統理論定義了四元數q[17]:

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

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

由于陀螺儀存在長時間工作會誤差漂移、加速計存在動態響應較慢、磁力計存在數據采集容易受環境影響等因素,在具體應用時往往還會將這三者數據進行補償從而獲得準確的姿態[18-22].圖4 就是一種典型的互補濾波姿態解算算法[20].

圖4 基于MEMS 的姿態角互補濾波算法框圖

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

3.2 電梯姿態解算

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

圖5 四元數卡爾曼濾波的姿態解算圖

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

預測更新過程如下:

1)狀態一步預測

式中,X(k|k-1)是用k-1 時刻狀態預測的結果,X(k-1|k-1)是k-1 時刻狀態最優結果.

2)狀態一步預測協方差

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

3)卡爾曼增益

式中,R為測量噪聲協方差.

4)狀態更新

5)狀態協方差更新

式中,X(k)為狀態值用于狀態預測,式(7)中Z(k)為初步解算后的姿態角,式(8)～式(10)是狀態估計部分,即當前時刻預測值和協方差以及增益;式(11)和式(12)是狀態更新的部分即當前時刻的最優狀態,然后將當前時刻的電梯最優姿態估計用于下一個時刻的預測,最終得到精確后的電梯姿態角.

3.3 電梯速度計算

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

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

3.4 濾波性能分析

就互補濾波算法與卡爾曼濾波算法在收斂性進行對比分析,如圖6 所示.卡爾曼濾波的姿態角比較于互補濾波的姿態角抖動較小,濾波的收斂效果更好,具有良好的實時性.

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

4 實驗分析

4.1 電梯姿態解算算法優化比對

為驗證本方法的效果,在2015 年安裝使用的某電梯上安裝了本裝置,分別應用常規互補濾波法、本算法進行俯仰角、航向角以及橫滾角結算,對比如圖7所示.本方法解算的姿態角相對平滑,抖動小.

4.2 電梯運行狀態的數據分析

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

圖7 互補濾波姿態解算和本文算法姿態解算角度對比圖

圖8 電梯A 和B 橫滾角對比圖

圖9 電梯A 和B 俯仰角對比圖

圖11 為電梯A 速度數據,圖12 為電梯B 速度數據,速度正值表示電梯為上行姿態,速度負值表示電梯為下行姿態,速度為零時則表示電梯靜止,通過速度可知電梯的當前狀態;可見,電梯B 在上下運行過程中的速度略快于電梯A.

圖10 電梯A 和B 航向角對比圖

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

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

圖11 電梯A 的速度圖

圖12 電梯B 的速度圖

圖13 電梯A 的加速度圖

5 總結

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

圖14 電梯B 的加速度圖

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

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