基于IQPSO-EKF的多傳感器融合姿態測量方法研究*

胡啟國,王 磊,馬鑒望,任渝榮

(重慶交通大學 機電與車輛工程學院,重慶 400074)

0 引 言

豎井掘進設備工作環境狹隘,地面的測量方法如衛星定位、圖像識別等無法使用。豎井掘進設備是否能按設計姿態運行,并且符合施工要求,首先要看姿態測量系統的精度能否滿足設計要求。

當前國內外的隧道、井下施工中主要使用的姿態測量方法是以自動化測量為主,人工測量法為輔。人工測量法包括支撐桿法、直角三角形測量法、前后標尺法等。其中,前后標尺法測量為常用方法,測量過程中需要保證標尺的水平度和垂直度,不然將會嚴重影響測量精度[1]。自動測量法主要有視覺測量導向系統、棱鏡導向系統、陀螺儀導向系統、激光標靶導向系統等方法。

其中,陀螺儀導向系統是利用陀螺儀與傾角儀相結合的方法,對掘進機進行姿態測量[2]。陀螺儀通過測量物體在不同方向上的旋轉速度,計算出物體的姿態信息,傾角儀輔助測量綜合解算以確定掘進機位置。陀螺儀導向系統具有高精度、高可靠性和高性能等特點,其可以提高設備的導向精度和機體穩定性,在豎井掘進方面有著廣泛的應用前景[3]。陀螺儀導向系統中通常采用MEMS傳感器,其具有體積占比小、質量輕、能耗低等特點,目前已經成為豎井主流姿態傳感器設備。但由于受到傳感器制造工藝、安裝精度、工作環境的影響,MEMS傳感器存在不可避免的漂移與測量誤差,僅靠實驗標定去除并不具有普適性[4]。

劉震等人[5]提出了一種改進的卡爾曼濾波算法,以減弱卡爾曼濾波過程中加速度對姿態角解算精度的影響。

張春宇等人[6]采用基函數神經網絡算法對訓練數據進行了訓練和分類,以提高水下航行器的測量姿態精度。楊小平等人[7]采用了一種基于一維卷積神經網絡與長期短期網絡相結合的加速度誤差補償方法,以實現誤差補償目的。高俊強等人[8]討論了前后標尺法的測量原理和盾構姿態計算方法,以分析姿態測量的最低精度。陳旭光等人[9]提出了一種動態的零值偏移誤差補償算法,以濾除陀螺儀的零值偏移誤差,提高了陀螺儀的測量精度。NEKRSOV Y A等人[10]研究了振動、沖擊和聲學噪聲對MEMS陀螺儀性能的影響,比較了慣性體懸架不同共振頻率的陀螺儀特性,揭示了具有更高共振頻率的傳感器的優勢。

以上研究者均對MEMS傳感器姿態測量做了分析或提出了新的測量方法,以減小測量誤差,但是這些方法平均誤差均較大,精度稍有不足。

因此,筆者采用基于擴展卡爾曼濾波(EFK)的多傳感器數據融合方法,以加速度計、磁力計傳感器等為觀測測量值,引入平均位置最優值來避免陷入局部最優的IQPSO-EFK算法,優化EKF的系統,測量噪聲的協方差參數。

1 姿態測量與解算理論

姿態解算的主要過程是姿態數據采集,筆者采用MEMS傳感器進行數據采集;姿態數據預處理,主要是對采集的數據進行去除噪、濾波、校準等;解算姿態角,即利用數學模型計算出物體的姿態角。姿態角解算法主要有歐拉角法、方向余弦法、四元數法等方法[11]。

豎井姿態測量系統由MEMS傳感器、激光指向儀、位置靈敏探測器(position sensitive detector,PSD)等組成。MEMS傳感器,安裝于豎井掘進設備結構中心位置,用于測量豎井掘進設備橫滾角、俯仰角與偏航角。激光指向儀垂直于豎井井筒軸線,安裝于豎井井口處,PSD位置傳感器安裝于豎井掘進設備頂部,用于接收激光指向儀的激光信號。

其中,PSD傳感器與激光指向儀利用光電效應來完成位置的測量任務。PSD傳感器由一系列光敏元件(光電二極管或光敏電阻)組成,這些元件被布置在傳感器的表面上形成一個二維陣列。當光線照射到PSD傳感器上時,光敏元件會產生電流或電壓信號,根據這些信號的強弱和相對位置的差異來測量Z軸的位移。

其硬件構架如圖1所示。

圖1 姿態測量系統硬件構架

2 姿態角解算法

定義坐標系是描述豎井掘進設備在豎井工作中姿態狀態的基礎。目前可參考現有飛行器的坐標系建立法,建立地面坐標系和機體坐標系。

地面坐標系原點可為地面任意一點,O-X指向正北方,O-Y指向正東方,O-Z垂直于地面,坐標系符合右手原則。機體坐標系以飛行器質心為原點,Oa-Xa指向飛行器地速方向,Oa-Za垂直機體軸線,Oa-Ya通過右手原則確定。飛行器位置可以通過機體自帶的衛星定位系統獲取[12]。但由于豎井特殊的工況環境,衛星定位系統不適用。

為此,筆者根據豎井掘進設備姿態特點,為了獲取角度、位移偏移等姿態數據,參考飛行器坐標系建立方法,建立地面坐標系O-XYZ,其中O-Z平行于井筒軸線,通過傳感器(MEMS、激光指向儀、PSD位置傳感器等)建立機體坐標系Oa-XaYaZa,如圖2所示。

圖2 豎井姿態雙坐標系

姿態角是用于描述機體在三維空間中的姿態狀態的角度參數。分析豎井掘進設備地面坐標系與剛體坐標系之間的聯系,可得到豎井掘進設備與地面坐標系的相對姿態[13]。

在圖1中,用來表示豎井掘進設備的姿態角有:α(roll)橫滾角;β(pitch)俯仰角;γ(yaw)偏航角。

歐拉角法的基本原理是三維空間坐標系,描述機體按一定次序進行三次旋轉動作。此法簡便、直觀,容易理解。但由于三角函數的特點,歐拉角法在解算機體全姿態時,微分方程在特殊角度會出現奇異點[14]。

歐拉角坐標變化分為兩個步驟:

1)初始時刻將地理坐標系O-XYZ和機體坐標系Oa-XaYaZa重合;2)機體坐標系嚴格依照轉動順序進行三次轉動,其三次旋轉角度順序依次是α、β、γ。

上述三次旋轉連起來,合并則有:

(1)

陀螺儀并非直接測量豎井掘進機的角度,而是測量設備角速度再轉化為角度。

陀螺儀姿態角關系式如下:

(2)

其中,當β=90°時,不能進行全姿態測量。

加速度計和磁力計的角度坐標系輸出值分別是as=[axayaz]T和ms=[mxmymz]T。相對于參考坐標系處于靜止狀態或者勻速運動狀態,參考坐標系下重力加速度的值為ar=[0 0g]T,單位向量是[0 0 1]T。

橫滾角αa、俯仰角βa由加速度計求解得到,偏航角γm通過磁力計求解得到,可以表示為:

(3)

3 姿態數據處理

在實際姿態測量過程中,由于不可避免且不可控制的外界擾動,導致整個測量系統充滿不確定性,易產生測量誤差。目前,雖然尚未存在完美的建模方法來建立觀測模型,但有較為接近其真實模型的估計方法。卡爾曼濾波器(Kalman filter)就是利用遞推方式獲得系統最小方差估計的方法[15-16]。

筆者利用MEMS陀螺儀的數據,建立卡爾曼濾波狀態方程。因為狀態方程呈現為非線性,所以必須采用非線性的擴展卡爾曼濾波算法(EKF)進行狀態估計[17],即把狀態方程中非線性系統線性化,即在真實點(線性化點)上展開為泰勒級數,省略二階或更高,然后利用線性卡爾曼濾波對狀態進行估計[18]。

在式(2)姿態角微分方程及運動學分析的基礎上,可以構建卡爾曼濾波狀態方程:

(4)

式中:Xk為時刻的理論值;k-1為上一時刻;μ為控制量;w為理論誤差矩陣。

通過加速度與磁力計積分解出速度、位移為觀測量,獲得卡爾曼濾波觀測方程:

(5)

式中:Z為測量值;σ為測量誤差矩陣。

令:

(6)

則Xk線性化為:

(7)

(8)

AJ、WkJ、HJ與VJ為雅克比矩陣,表達式如下:

(9)

擴展卡爾曼濾波算法預測矯正部分:

(10)

一般情況下,初始誤差協方差應該盡可能接近實際系統的誤差,而初始估計值應該盡可能接近實際系統的狀態。另外,也可以通過實驗來確定初始誤差協方差和初始估計值,以獲得更好的濾波效果。

4 粒子群算法分析和驗證

對于狀態方程與觀測方程的初始參數估計不準確的問題,可采用最小二乘法、最小均方差法等方法進行合理估計。對于狀態方程與觀測方程的噪聲誤差協方差矩陣,即Q、R值,它們分別代表系統噪聲和測量噪聲的協方差矩陣,用來衡量系統噪聲與測量噪聲的大小[19]。一般情況下,Q、R的取值應該盡可能接近實際噪聲的方差。而Q、R的取值也可以利用群智能算法尋參來確定,以獲得更好的濾波效果[21-22]。

(11)

通過上述公式可以看出,Q、R的比值大小決定了Kk的大小,而Q或R絕對值的大小對Kk影響并不明顯。因此,筆者應該權衡系統理論值與系統觀測值的權重對Q、R進行設計。

粒子群算法易于實現,計算效率高,但存在早熟收斂的缺陷。學者們利用調控參數、引入退火算法、融合其他擬物類算法思想等方法,增加種群多樣性,調高尋優速度,避免陷入局部最優的能力,提高其局部搜索能力,從而使其能夠更精確、更穩定地解決眾多優化問題。

孫俊等人[23-24]基于量子力學觀點,提出了一種量子行為粒子群優化算法(QPSO),將量子力學不確定原理引入QPSO算法,即粒子更新不受粒子上一時刻位置與速度影響,增加了粒子位置的隨機性。具有量子行為的粒子位置與速度是無法同時確定的。

粒子的位置迭代公式可以表示為:

(12)

式中:xid(t+1)為粒子t+1時刻的位置;u為[0,1]之間的隨機數;Lai為局部吸引因子,由個體最優位置與全局最優位置確定。

Lai值的大小決定了粒子的搜索空間,Lai的具體表達式如下:

(13)

式中:φ為[0,1]之間的隨機數;Pid為個體最優值;gd為全局最優值;c1,c2為權重值。

式(12)中β為壓縮-擴張系數,其具體表達式如下:

(14)

式中:w1,w2為權重;itermax為最大迭代次數;t為當前迭代次數。

4.1 改進量子粒子群算法優化過程分析

群智能算法多以隨機的方式初始化種群,但會造成種群分布不均,影響種群質量。因此,筆者借助混沌序列對QPSO種群進行初始化,利用混沌方程對初始種群進行優化,可實現種群的高隨機性、均勻性等特點,高質量的初始種群可以進一步提高算法的收斂速度與尋優性能[25]。分段映射是通過調整p的取值對種群密度進行分段。

分段映射圖如圖3所示。

圖3 分段映射迭代取值散點分布圖

混沌方程如下:

(15)

其中:p∈(0,0.5];xN∈(0,1)。

Logistic映射的混沌狀態,取決于u的取值。因此,筆者分別做了u=3.2,u=3.6,u=3.8時迭代1 000次的散點分布,其混沌方程為:

Xn+1=Xnu(1-Xn),u∈[0,4],Xn∈(0,1)

(16)

其散點分布圖如圖4所示。

圖4 Logistic映射迭代取值散點分布圖

由圖4可看出:在u=3.2時,種群并不處于混沌狀態,種群集中分布在兩個區域;在u=3.6時,種群較為混沌,但其不均狀態在多處區域并未分布;當u=3.8時,種群處于混沌均勻狀態。

由于u的取值變化導致Logistic映射混沌狀態的不穩定,而分段映射的參數設計簡單且均勻性、混沌性要優于Logistic映射。因此,筆者采用分段混沌映射優化初始種群。

量子粒子群優化算法具有公式簡潔、易于實現、參數調節少、所需種群規模較小、計算效率高、收斂速度快等優點。但在算法迭代末期,粒子位置受定權重下的個體最優解影響,導致粒子位置陷入局部最優解[26]。為解決這一問題,引入了平均最優位置mbest,其表達式如下:

(17)

式中:Pi為個體最優位置;n為粒子數。

由式(17)可知,平均最優位置mbest為當前所有個體最優位置的平均值。

引入平均最優位置mbest量子粒子群算法的粒子位置更新公式如下:

(18)

由式(18)可知,由于迭代末期種群惡化,局部吸引因子Lai中全局最優陷入個體最優,而引入平均最優位置mbest取代局部吸引因子Lai,通過平均值增加粒子間的消息交流,避免在歷代迭代粒子中陷入局部最優,從而優化下代種群質量。

4.2 改進算法驗證

為驗證改進量子粒子群優化算法的可行性與有效性,筆者對比改進算法與粒子群優化算法在求解三種典型目標函數時的收斂速度、尋優性能等方面的能力。

目標函數F1為:

(19)

其中,自變量x的取值范圍為[-100,100]。

目標函數F2為:

(20)

其中,自變量x的取值范圍為[-32,32]。

目標函數F3為:

(21)

其中,自變量x的取值范圍為[-600,600]。

函數F1為單峰函數,存在一個極值,其用于檢驗算法局部尋優能力;函數F2、F3為多峰函數,用于檢驗算法全局尋優能力。

F1、F2、F3進行仿真求解的最優適應度曲線如圖5所示。

圖5 適應度變化曲線

由圖5可知:通過適應度變化曲線圖可以看出:基于混沌序列與平均最優值的改進量子粒子群算法不管求解多峰還是單峰函數,改進算法在尋優能力與收斂精度上,均強于普通量子粒子群算法[27]。

5 姿態誤差估計與姿態解算實驗

姿態估計實驗源數據來源于公開數據庫。忽略機械結構振動和電機磁場對姿態測量的影響,數據采集于XC-M305型三軸MEMS陀螺儀。

擴展卡爾曼濾波中的Q、R值,采用基于混沌序列與平均最優值的改進量子粒子群算法進行參數尋優。其具體步驟如下:

1)參數初始化,并通過分段映射生成初始種群;

2)將陀螺儀數據輸入擴展卡爾曼濾波器,同時將歷代迭代中粒子位置作為Q、R,并計算測量數據的適應度值;

3)個體最優位置中適應度值最好的,作為全局最優值;

4)根據種群規模與個體最優位置,計算平均最優位置;

5)進入下次迭代,基于個體最優值、平均最優值與全局最優值,更新粒子位置;

6)計算新的粒子適應度值,更新個體最優值與全局最優值;

7)如果符合終止條件,終止尋優計算,輸出最優值,即為卡爾曼濾波中的最佳Q、R。否則,繼續執行步驟4)～步驟6),直到達到迭代的最大次數,輸出所述卡爾曼濾波中的最佳Q、R數值。

筆者采用MATLAB軟件實現上述參數尋優過程。算法參數設計如表1所示。

表1 IQPSO參數

姿態解算實驗的適應度函數是經擴展卡爾曼濾波后得到的MSE值,解算實驗初始轉速值為三個儀器標定值。筆者設置結束條件為MSE小于或等于10-3。

參數優化結構記錄如表2所示。

表2 IQPSO優化擴展卡爾曼參數

由表2可知:對于三組MEMS陀螺信號,經過基于混沌序列平均最優位置的改進量子粒子群算法參數尋優后,Q值偏小,R值較大,MSE小于設定值。因此,該優化算法能實現自動尋優,尋到全局最優參數。

5.1 IQPSO-EKF對比實驗

為了測試IQPSO-EFK算法的有效性和精確性,筆者分別在轉速為1.50°/s、-3.05°/s、6.04°/s下對陀螺儀測量值、擴展卡爾曼濾波、QPSO-EFK和IQPSO-EFK做了對比實驗,實驗結果如圖6所示。

圖6 +1.50°/s條件下對比圖

由圖6可知:在轉速+1.50°/s條件下,擴展卡爾曼濾波的姿態估計誤差為-0.30°～0.34°;QPSO-EKF的姿態角估計誤差為-0.11°～0.14°;IQPSO-EKF的姿態角估計誤差為-0.08°～0.08°。

-3.05°/s條件下對比圖如圖7所示。

圖7 -3.05°/s條件下對比圖

由圖7可知:在轉速-3.05°/s條件下,擴展卡爾曼濾波的姿態估計誤差為-0.32°～0.30°;QPSO-EKF的估計誤差為-0.12°～0.11°;IQPSO-EKF的估計誤差為-0.08°～0.08°。

+6.04°/s條件下對比圖如圖8所示。

圖8 +6.04°/s條件下對比圖

由圖8可知:在轉速+6.04°/s條件下,擴展卡爾曼濾波法的姿態角估計誤差為-0.38°～0.34°;QPSO優化擴展卡爾曼濾波的姿態角估計誤差為-0.11°～0.12°;改進QPSO優化擴展卡爾曼濾波的姿態角估計誤差為-0.12°～0.10°。

為了驗算基于IQPSO-EKF優化后的解算位置姿態誤差,筆者分別對真實姿態、測量姿態、擴展卡爾曼優化姿態作了對比實驗。

其姿態解算坐標對比如圖9所示(其中,粗直線為設備真實解算位置,雙點劃線為陀螺儀測量數據的解算位置,點線為擴展卡爾曼濾波優化后的解算位置,點劃線為基于IQPSO-EKF優化后的解算位置)。

圖9 姿態解算坐標對比

由圖9可以看出:對比真實姿態、MEMS傳感器測量姿態和基于EFK方法解算的姿態,基于IQPSO-EKF方法解算姿態更接近真實值,從而達到了優化目的[30-31]。

5.2 MAE、MSE評價指標表

為更直觀地對改進量子粒子群算法與其他優化算法姿態誤差估計效果進行對比評估,筆者計算各優化算法在MEMS陀螺儀不同角速率下的MAE、MSE指標。

其具體計算結果如表3所示。

表3 評價指標表

由表3中可以看出:筆者采用基于混沌序列與平均最優值的改進量子粒子群算法優化擴展卡爾曼濾波方法,對MEMS陀螺姿態數據進行了誤差估計,MAE、MSE值均下降了約86.7%、68.7%、28.2%。

由此可見,采用IQPSO-EFK方法能減小豎井掘進姿態測量誤差。

6 結束語

筆者首先分析了豎井傳感器設備現狀,采用基于多傳感器數據融合技術,配置多傳感器姿態測量硬件,進行了姿態測量方法研究;然后,分析了豎井掘進設備姿態特征,利用多傳感器數據信息,建立了描述豎井裝備姿態特性的坐標系,分析了基于擴展卡爾曼濾波器進行多傳感器數據融合方法的誤差來源,針對擴展卡爾曼濾波算法的特點,采用量子粒子群優化算法進行了擴展卡爾曼濾波算法參數優化;同時依據算法特點,引入混沌序列與平均最優值改進方法,提出了一種基于分段映射與平均最優值的改進量子粒子群優化算法,并進行了實驗驗證;最后,進行了豎井掘進姿態測量與估計對比實驗,將改進量子粒子群優化算法優化擴展卡爾曼濾波方法與其他方法對比,進行了姿態解算實驗。

研究結果表明:

1)IQPSO-EFK不管求解多峰還是單峰函數,該改進算法在尋優能力與收斂精度上,均強于QPSO-EFK;

2)在三種不同的轉速下,根據MAE、MSE評價指標,IQPSO-EFK對比原數據、EFK、QPSO-EFK,其值分別下降了約86.7%、68.7%、28.2%;

3)在姿態對比實驗中,IQPSO-EFK方法更接近真實姿態,因此,其能減小豎井掘進姿態測量誤差。

因為模型細節尚未完善,所以現階段的具體實驗很難進行實例驗證。考慮豎井掘進機模型較為龐大,因此,筆者后續的研究方向為完善豎井掘進機的模型細節和具體安裝細節。