基于改進擴展卡爾曼濾波的爬架運行安全姿態估計

周豪, 韓志剛*, 胡錦仁

(1.中南林業科技大學機電工程學院, 長沙 410000; 2.湖南中模云建筑科技有限公司, 株洲 412200)

爬架(附著式腳手架)作為高層建筑施工的多節點承載平臺,安全性是其運行過程中最重要的指標,一直以來是設計和使用過程中的基準[1]。目前爬架主要通過各個機位載荷和水平高差限值對架體運行的均衡程度進行安全判定和同步控制。在爬架系統整體升降過程中,由于受電機個體差異或故障、外部阻力、風載荷等因素綜合影響,除了可能出現水平高差外,還可能會出現內外傾斜,利用載荷或水平高差限值可以保證架體的基本運行安全,但對架體的整個運行過程無法進行有效監管,對風載荷等側向作用的載荷也無法進行監測,不能完全滿足施工安全性和智能化的需要[2]。姿態監測是導航和制導領域常用的一種高精度安全監測方法,在相關工程領域有著廣泛的利用。杜署明[3]將捷聯慣性傳感技術引入吊鉤姿態估計系統中,在分析吊鉤三維運動模型基礎上,采用數據融合算法,實現姿態實時檢測;陸晨等[4]基于MEMS傳感器設計了一種軌道坍塌監測系統,用卡爾曼濾算法進行傳感器的數據融合,用于監測采礦區軌道塌陷,保證安全生產;曹麗琴等[5]設計了桿塔傾斜姿態實時監測方法,通過卡爾曼濾波對MEMS傳感器進行融合得到桿塔傾斜姿態角。卡爾曼濾波算法已成為姿態估計中的常用方法,但如果在進行融合濾波時未考慮時變噪聲對MEMS傳感器的影響,在復雜的環境中會出現測量精度下降的問題。

擴展卡爾曼濾波(extended Kalman filter,EKF)在對MEMS傳感器進行數據解算融合時認為噪聲是高斯白噪聲,而實際的噪聲是時變的,當系統的噪聲協方差矩陣與實際不相符合時會出現濾波發散。因此,針對復雜噪聲干擾下的爬架姿態估計問題,現提出一種改進的Sage-Husa擴展卡爾曼濾波算法,引入爬架實時運動速度改進濾波異常判定條件,提高濾波算法的自適應性,以期實現在時變噪聲下姿態準確估計。

1 爬架運行姿態模型

1.1 姿態描述

MEMS傳感器是一種應用于機器人、飛行器、桿塔傾斜、空中作業平臺等姿態監測的新型傳感器,具有體積小、成本低、精度高等特點[6]。爬架架體是由立桿、橫桿、斜撐等組成的框架結構,在工作時受多種干擾的影響會出現傾覆、扭轉、不同步等危險因素,利用MEMS傳感器監測爬架工作過程中三維空間姿態角的變化,從而精準的反饋架體運行時的姿態變化,能有效提高爬架運行過程的安全性。

如圖1所示,定義爬架的坐標系(b系)為載體坐標系,其中zb軸方向為爬架上升方向,yb軸指向右側,xb軸與zboyb組成的平面垂直,地理坐標系(n系)zn軸方向指向天空,yn軸向右,xn軸與znoyn組成的平面垂直。俯仰角θ表示傾覆程度;橫滾角φ表示運行出現的不同步程度;偏航角ψ表示運行時出現的架體扭轉情況。

圖1 坐標系與姿態角定義Fig.1 Definition of coordinate system and attitude angle

載體坐標系與地理坐標系的轉換關系為

(2)

式(2)中:XbYbZb表示地理坐標系;XnYnZn表示載體坐標系。定義四元數為q,表達式為

(3)

(4)

由式(1)～式(4)可得姿態角為

(5)

(1)

(6)

(7)

式(7)中:ωx為x軸方向角速度分量;ωy為y軸方向角速度分量;ωz為z軸方向角速度分量。

由式(6)和式(7)可得

(8)

采用一階龍格庫塔法對方程進行求解可得

(9)

經過更新后得到的四元數為

(10)

1.2 傳感器誤差模型

對爬架進行姿態估計時,由于爬架的安裝誤差時隨機的,在安裝MEMS傳感器時對傳感器進行了調零校準,因此忽略架體安裝誤差,只考慮傳感器的誤差。常用的MEMS傳感器為陀螺儀、加速度計、和磁力計。陀螺儀在短時可以獲得較精準的姿態信息,但由于陀螺零偏的存在,在長時間下會產生較大的誤差;加速度計在靜止時可獲得較高的測量的精度,一旦目標載體開始運動,就會導致測量的精度下降;磁力計在測量姿態信息時容易受到磁場變化的影響,任何單一的傳感器都不能獲得高精度的姿態信息[9]。綜合各傳感器的特點,對傳感器采集的信息進行數據融合,因此在進行融合前需對傳感器的誤差進行分析。

陀螺儀測量模型為

ω=ωm-ωb-ωn

(11)

式(11)中:ω為真實值;ωm為角速度的測量值;ωb為陀螺儀噪聲;ωn為角速度測量噪聲向量。

加速度計的測量模型為

a=am-ab-an

(12)

式(12)中:a為比力真值;am為加速度測量值;ab為漂移誤差;an為加速度測量噪聲向量。

磁力計的測量模型為

(13)

式(13)中:mb為漂移誤差;mn為測量噪聲;mn為磁力計測量噪聲向量。

2 基于擴展卡爾曼濾波的數據融合誤差補償算法

2.1 擴展卡爾曼濾波算法

姿態解算是建立在MEMS傳感器數據融合的基礎上,擴展卡爾曼濾波算法(EKF)是常用的數據融合方法。由于陀螺儀存在飄移,將四元數和陀螺零偏作為EKF狀態量,加速度計和磁力計作為EKF測量變量,以消除陀螺儀的誤差。

建立EKF模型為

(14)

式(14)中:x(k)為系統的狀態量。

x(k)=[q0q1q2q3bgxbgybgz]T

(15)

式(15)中:q=[q0q1q2q3]T為姿態信息的四元數;bg=[bgxbgybgz]T為陀螺儀角速度測量偏差。

采用姿態四元數作為測量變量z(k),即

z(k)=[q0(k)q1(k)q2(k)q3(k)]T

(16)

通過加速度計和磁力計輸出的姿態四元數作為測量變量,爬架在運行時速度較慢,故加速度計的零偏不計,磁力計在測量時易受架體材料的干擾。因此,在建立測量方程時參考文獻[10]給出的向量正交法,在獲得姿態四元數的同時減少地磁干擾。

設旋轉矩陣由三維向量i、j、k組成,分別代表機體的X、Y、Z軸向量。故有

(17)

(18)

加速度計的輸出用于構造機體坐標系的Z軸,將X軸強制與Z軸垂直,可以計算出i為

(19)

(20)

(21)

由此可得四元數的測量值為

z(k)=h[x(k)]=qk+1

(22)

擴展卡爾曼濾波算法步驟如圖2所示。

圖2 擴展卡爾曼算法流程圖Fig.2 Flow chart of extended Kalman algorithm

2.2 改進擴展卡爾曼濾波算法

EKF在融合MEMS傳感器數據時的噪聲通常被認為是均值為零的高斯白噪聲。而爬架在實際應用場景中完成一次爬升的時間長,工作環境較復雜。傳感器在測量時的噪聲來源廣泛多變,若不對噪聲進行調節將會導致濾波發散,影響架體姿態的測量精度。

Sage-Husa擴展卡爾曼濾波算法(SHEKF)是EKF的衍生,可以新息自適應更新Qk和Rk調節卡爾曼增益系數,使EKF根據噪聲的變化進行自適應的調節,保持高精度濾波效果[11]。其包含迭代估計和噪聲更新兩部分,迭代估計部分和EKF一致,噪聲更新部分如下。

(23)

(24)

(25)

(26)

(27)

式中:b為遺忘因子,通常為0.95～0.99。

(28)

SHEKF在進行噪聲統計時,對初值是否準確要求較高,當初值不準確時會導致濾波發散。因此,通過協方差匹配思想對濾波是否發散進行判定,提高濾波器的實時性和準確性。協方差匹配的思想是:判斷實際新息與理論是否一致,在不一致時進行調節,爬架在爬升過程中姿態獲取濾波異常的判定條件[15]如下

(29)

(30)

式中:γ為調節因子。

當式(29)、式(30)成立時,說明系統濾波異常,此時需對觀測噪聲進行調節。

爬架處于作業狀態時,由于卸荷頂撐連接架體與建筑結構之間形成穩定的結構,相關的承載力也傳遞給建筑結構,此狀態下姿態變化很小。當爬架進行爬升作業,需松開卸荷頂撐,此時整個架體相當于懸掛在空中,除了導軌限位以外,再無其他結構的支撐,架體姿態最容易發生變化且變化幅度較大。據統計爬升階段最易出現安全事故,需要加強在爬升階段的運動感知。

為提高爬架在不同運行狀態濾波的精準性和自適應性,通過爬架的運動狀態的判定實現對SHEKF的自適應調節,當爬架處于運動狀態時加強濾波發散嚴格判定依據。因此,引入爬架運動速度對調節因子γ進行優化。

γ=100Avx+C

(31)

(32)

式中:vx為爬架沿導軌運動的速度;A和C為常數,可以根據實際濾波精度進行調節。

改進Sage-Husa擴展卡爾曼濾波算法如圖3所示。

圖3 改進Sage-Husa擴展卡爾曼濾波算法流程圖Fig.3 Improved flow chart of Sage-Husa extended Kalman filtering algorithm

3 實驗與驗證

3.1 系統硬件設計

為了驗證該方法的可行性,進行實驗驗證,爬架姿態估計實驗如圖4所示。實驗開發環境為C/C++,開發軟件為keil5和origin,實驗設備主要由傳感器模塊、處理器模塊、通訊模塊、上位機組成。傳感器采用Mpu9250集成了三軸加速度計、三軸磁力計、以及三軸陀螺儀,處理器采用野火指南者STM32F103CT86開發板。

圖4 爬架姿態估計實驗Fig.4 Experiment on attitude estimation of a climbing frame

將實驗設備固定于爬架的樣架上進行實驗驗證,考慮綜合因素,從爬架一榀中任選3個機位進行試驗,在傳感器安裝時對傳感器在基面進行校零,如圖4所示。

3.2 靜態實驗

經過試湊法確定ISHEKF的調節因子γ中取A=0.8,C=0.4,取遺忘因子b=0.98。準備3組相同的傳感器,在keil5中進行相關算法代碼的編譯,在對應的單片機中燒錄不同的算法程序。將傳感器安裝在架體的同一位置,置于架上靜止不動。對mpu-9250初始值再次進行校準,設置mpu-9250采樣頻率為50 Hz,采樣時間為40 s,采集到的姿態角信息如圖5～圖7所示。

圖5 俯仰角Fig.5 Pitching angle

圖6 橫滾角Fig.6 Roll angle

圖7 偏航角Fig.7 Yaw angle

由圖5～圖7可知,在靜止工況下,姿態角的理想的輸出為0°,受環境噪聲的影響,姿態角都出現了不同程度的變化。以橫滾角為例,驗證算法的魯棒性,EKF的波動范圍為-0.03°～0.06°,其最大誤差為0.6%;SHEKF波動范圍為0°～0.022 4°,其最大誤差為0.224%;ISHEKF的波動范圍為0°～0.017 5°,其最大誤差為0.175%。ISHEKF的最大誤差較SHEKF減少了21.9%,較EKF最大誤差減少了70.8%。靜態實驗不同算法的誤差性能指標評價如表1所示。

表1 靜態實驗不同算法誤差性能比較Table 1 Comparison of error performance of different algorithms in static experiment

如表1所示的4種誤差性能指標綜合評價可以得出,ISHEKF在靜態工況下受環境噪聲的干擾最小,姿態角的輸出的魯棒性最高,更接近實際值。

3.3 動態實驗

根據《建筑施工附著式腳手架安全技術規范》(JGJ 202—2010)規定相鄰機位的升降不同步量應不大于30 mm[16]。整體不同步量不大于80 mm。實驗樣架兩機位的間距為3.5 m,即理想相鄰兩機位橫滾角的變化量為0.49°。為能更好地體現本方法的可行性,模擬運行可能出現的極端情況,在極端情況下可更直觀地體現監測效果。在無外界干擾下,由于實驗設備和相關條件的限制,扭轉、傾覆等情形較難人為復現。不同步主要導致橫滾角出現變化,短時間內俯仰角和偏航角在無外界干擾的條件下變化很小,故只采用橫滾角的變化反映架體運行時的不同步姿態信息。數據采集傳感器為3組,將傳感器安裝于架體上,對傳感器進行再次校準。設置傳感器采樣頻率為50 Hz,采樣時間為15 s,根據爬架實際運行情況出現的不同步情況,設置不同測試條件。

3.3.1 模擬相鄰不同步狀況

假定機位1、機位3所在的葫蘆出現故障不動,對機位2所在的葫蘆進行提升。架體姿態變化結果如圖8和圖9所示。

圖8 機位1和機位2之間架體橫滾角變化Fig.8 Change of frame roll angle between position 1 and position 2

圖9 機位2和機位3之間架體橫滾角變化Fig.9 Change of frame roll angle between position 2 and position 3

3.3.2 模擬整體不同步狀況

假定機位2所在的葫蘆出現故障不動,機位1號所在的葫蘆進行提升,機位3號所在的葫蘆進行下降。其姿態變化結果如圖10和圖11所示。

圖10 機位1與機位2之間架體橫滾角變化Fig.10 Change of frame roll angle between position 1 and position 2

圖11 機位2于機位3之間架體橫滾角變化Fig.11 Change of frame roll angle between position 2 and position 3

由圖8～圖11可知,在動態實驗時的初始2 s內,3種算法的姿態輸出差別不大,隨著架體繼續運行,EKF受環境噪聲的干擾出現了較大程度的濾波發散,SHEKF能調節噪聲的變化,減少受環境噪聲影響濾波發散的情況;ISHEKF能有效減少環境噪聲的影響,且在架體運動時提高濾波異常判定的嚴格程度,較SHEKF減少了濾波發散的程度,姿態角的輸出精度更號,較為真實地反映了架體姿態變化的情況。以機位2于機位3之間架體橫滾角變化為例,不同算法的誤差性能評價如表2所示。

表2 不同算法誤差性能比較Table 2 Comparison of error performance of different algorithms

從表2可知,動態實驗中ISHEKF的4種誤差評價指標均優于SHEKF和EKF,說明ISHEKF具有更好的動態響應精度及魯棒性。在經過15 s的運行后橫滾角的變化范圍為0.48°～0.52°,與理想值的最大誤差為6.1%,證明了該方法用于爬架安全監測的可行性。

4 結論

針對爬架運行中狀態感知問題,提出一種基于改進Sage-Husa擴展卡爾曼濾波算法的爬架姿態估計方法。在樣架上進行了實驗,對ISHEKF、SHEKF、EKF 3種算法進行了比較。在靜態試驗下,以橫滾角為例:ISHEKF的最大誤差為0.175%,SHEKF的最大誤差為0.224%;EKF的最大誤差為6%。說明ISHEKF有較高的魯棒性。在動態試驗下,ISHEKF能準確反映運行時的姿態角變化,傳感器在ISHEKF算法下的誤差性能評價指標最優。從以上結果可得出所提的ISHEKF算法能更有效地反映爬架運行姿態,減少噪聲造成的誤差影響,為提高爬架運行安全監測能力提供了參考價值。