基于地磁傳感器解算旋轉(zhuǎn)體姿態(tài)的方法

邵偉平，孫 林，張嘉易，郝永平，楊 超

(沈陽理工大學(xué) 機(jī)械工程學(xué)院，沈陽 110159)

隨著磁場測量技術(shù)的發(fā)展，利用地磁場對運(yùn)動載體進(jìn)行測姿的精確度不斷提高，同時各個國家也非常重視對地磁導(dǎo)航測姿技術(shù)的研究。美國首先提出了基于地磁異常場等值匹配的MAGCOM系統(tǒng)，在2006年F.Goldenberg針對飛機(jī)的地磁導(dǎo)航系統(tǒng)研究出了基于地磁場圖的測速定位方法。近些年國內(nèi)對地磁導(dǎo)航測姿技術(shù)的研究同樣發(fā)展迅速，例如：運(yùn)用平均絕對差法對地面測量的地磁強(qiáng)度數(shù)據(jù)進(jìn)行了匹配運(yùn)算，分辨率達(dá)到50 m；采用捷聯(lián)式地磁-太陽方位傳感器解算彈體滾轉(zhuǎn)角；將地磁感應(yīng)圈傳感器應(yīng)用于火炮彈藥上，測量轉(zhuǎn)速并控制炸點(diǎn)等。

目前，對于智能彈藥射擊精度不斷提高的要求，迫切需要能夠精確測量旋轉(zhuǎn)彈體的滾轉(zhuǎn)姿態(tài)位置信息的方法來提高彈體彈道修正精度，進(jìn)而提高彈體打擊精度。

本文將地磁傳感器角度解算方法應(yīng)用到旋轉(zhuǎn)彈體上，通過測量彈體在地磁場中的特性參數(shù)從而解算出彈體的滾轉(zhuǎn)姿態(tài)信息，該方法原理簡單，成本低，精度高，同時解算具有實(shí)時性，能夠為彈體精確制導(dǎo)提供準(zhǔn)確有效的數(shù)據(jù)信息。

1 原理分析

利用地磁傳感器可以測量旋轉(zhuǎn)體對地的滾轉(zhuǎn)姿態(tài)，結(jié)合光電傳感器可以測量相對旋轉(zhuǎn)體的滾轉(zhuǎn)姿態(tài)，其實(shí)驗裝置如圖1，兩側(cè)利用電機(jī)提供反向動力使前后旋轉(zhuǎn)體相對轉(zhuǎn)動，將地磁傳感器和光電傳感器放置在同一水平面的后部旋轉(zhuǎn)體上，同時將發(fā)光器件安裝在光電傳感器正上方的前部旋轉(zhuǎn)體上，當(dāng)發(fā)光器件與光電傳感器相遇時便會產(chǎn)生感應(yīng)信號，此時后部旋轉(zhuǎn)體的當(dāng)前角度值就是前部旋轉(zhuǎn)體的當(dāng)前角度值。

圖1 實(shí)驗裝置示意圖

2 地磁傳感器角度測量方法

2.1 基于單軸地磁解算方法

2.1.1判斷地磁信號有效值

將地磁傳感器調(diào)節(jié)成單軸輸出，通過DSP28335處理器對磁傳感器信號進(jìn)行實(shí)時處理，由于磁力計測試原理，當(dāng)旋轉(zhuǎn)體旋轉(zhuǎn)一周時，地磁信號呈周期性近似正弦波形式輸出電壓值，即地磁每個輸出值都唯一對應(yīng)一個地磁的位置，可表示為f(t)=Asin(t)+g(t)，其中A為地磁電壓幅值，t為采樣時間，g(t)為干擾信號。當(dāng)無干擾信號時，g(t)=0，此時f(t)=Asin(t)，當(dāng)f(t)=Asin(t)=0，可求得t=kπ時相鄰采樣點(diǎn)間電壓最大差值ΔVmax，離散型數(shù)字信號有：

ΔVmax=Asin(kπ+Δt)-sinkπ=AsinΔt

根據(jù)實(shí)驗臺試驗采樣特性可以得知，當(dāng)最高轉(zhuǎn)速為20 000 r/min時，采樣數(shù)m大于400。已知地磁信號電壓幅值為3.3 V，即A=3.3。由此可以得到：

ΔVmax=AsinΔt

本文采用均值濾波算法對地磁信號電壓幅值進(jìn)行濾波，設(shè)每10個采樣點(diǎn)為一組。

S=∑X1[i]，其中1≤i≤10

U=S/N，其中N=10

運(yùn)算中每加一個點(diǎn)i=i+1，求相鄰兩點(diǎn)間的差值：

D1[i]=|X1[i]-X1[i-1]|

將開始連續(xù)的10個點(diǎn)中除去第一個點(diǎn)外，滿足D1[i]>ΔVmax且D1[i-1]>ΔVmax的點(diǎn)X1[i-1]值作為臨近點(diǎn)后計算均值，令逐點(diǎn)推進(jìn)計算。逐點(diǎn)濾波有效區(qū)域值確定為

ΔV=6×ΔVmax=0.035

判斷地磁信號是否為有效信號，當(dāng)?shù)卮判盘栔祙X1[i]-U|≤ΔV為有效信號，當(dāng)?shù)卮判盘栔祙X1[i]-U|>ΔV為無效信號。

2.1.2轉(zhuǎn)體滾轉(zhuǎn)角度及轉(zhuǎn)速的計算

假設(shè)圖2為采樣的地磁信號電壓幅值，橫坐標(biāo)為電壓采樣點(diǎn)數(shù)量，縱坐標(biāo)為電壓值，采樣周期為T，通過對零點(diǎn)C的標(biāo)定以及采樣周期點(diǎn)數(shù)對轉(zhuǎn)體的轉(zhuǎn)速和滾轉(zhuǎn)角進(jìn)行結(jié)算。當(dāng)轉(zhuǎn)體旋轉(zhuǎn)一周的時候，地磁信號電壓幅值為一個周期，轉(zhuǎn)體滾轉(zhuǎn)角為360°，此時這個周期的地磁信號電壓采樣點(diǎn)都唯一對應(yīng)著轉(zhuǎn)體滾轉(zhuǎn)角的度數(shù)。因此通過周期采樣點(diǎn)數(shù)就能求得轉(zhuǎn)體的滾轉(zhuǎn)角。

圖2 地磁信號電壓幅值

首先是在線調(diào)節(jié)，算法的所有數(shù)據(jù)都基于在線采集數(shù)據(jù)。在嵌入式算法程序中，使用定時器定時采集地磁信號，設(shè)采集地磁電壓速率為V1，采集地磁電壓點(diǎn)數(shù)為P1，采樣時間為t1，則有關(guān)系式：P1=V1×t1。通過在實(shí)驗臺上對轉(zhuǎn)體不同轉(zhuǎn)速下采集地磁信號電壓采樣周期的調(diào)節(jié)，使得轉(zhuǎn)體轉(zhuǎn)速在1 000～15 000 r/min時，地磁信號電壓采樣點(diǎn)數(shù)P1控制在400～1 500之間。

其次，針對地磁信號電壓不斷變化的幅值，需要預(yù)先采集初始的2 000個電壓采樣點(diǎn)，對其進(jìn)行最高電壓Va和最低電壓Ve的挑選。在挑選過程中，如何判斷此Va最高電壓，Ve為最低電壓？將采集電壓值Va與Va前實(shí)時解算逐點(diǎn)累加的電壓均值U進(jìn)行比對，若Va-4、Va-3、Va-2、Va-1均大于U，Va+1、Va+2、Va+3、Va+4均小于U，則證明Va為最高電壓值。相反，若Ve-4、Ve-3、Ve-2、Ve-1均小于U，Ve+1、Ve+2、Ve+3、Ve+4均大于U，則證明Ve為最低電壓值。通過大量Va和Ve，可以求得均值電壓線Vc=(Va+Ve)/2，最大幅值電壓差A(yù)=Va-Ve。通過已知Vc后，可以求出上中邊界電壓線Vb與下中邊界電壓線Vd。因為采樣周期不變，設(shè)k為滿足Vb

設(shè)波谷后的均值電壓線Vc的采樣點(diǎn)橫坐標(biāo)為h1，波峰后的均值電壓線Vc的采樣點(diǎn)橫坐標(biāo)為h2，即可求出峰線位置采樣點(diǎn)的橫坐標(biāo)h：

h=floor[(h1+h2)/2]

最后，在解算旋轉(zhuǎn)體滾轉(zhuǎn)角時，標(biāo)定求出的地磁信號波峰最高點(diǎn)為對應(yīng)旋轉(zhuǎn)體滾轉(zhuǎn)角的0°，即為零點(diǎn)(標(biāo)定旋轉(zhuǎn)體零度位置點(diǎn))。兩個波峰線之間的采樣點(diǎn)橫坐標(biāo)之差就是地磁信號電壓幅值一個周期的采樣點(diǎn)數(shù)量t1(也就是旋轉(zhuǎn)體每轉(zhuǎn)的采樣點(diǎn)數(shù)量)。因為采樣周期不變，所以當(dāng)旋轉(zhuǎn)體在勻速旋轉(zhuǎn)時，可以通過電壓周期采樣點(diǎn)數(shù)t1和波峰零點(diǎn)坐標(biāo)實(shí)時解算旋轉(zhuǎn)體的滾轉(zhuǎn)角，得出當(dāng)前地磁信號電壓采樣點(diǎn)k對應(yīng)的角度α：

值得注意的是，此處采樣點(diǎn)數(shù)量是不斷更新的，更新周期為T，所以起到變幅值預(yù)測的作用，抵制外界干擾，通過單軸地磁信號解算角度值，避免了波形出現(xiàn)上下端截止時造成的誤差干擾。

已知地磁信號電壓采樣周期T和解算出的周期采樣點(diǎn)數(shù)t1，可以求得轉(zhuǎn)體的轉(zhuǎn)速n和角速度ω：

2.2 基于三軸地磁解算方法

2.2.1滾轉(zhuǎn)角解算

三軸地磁傳感器，相對于單軸地磁傳感器多了兩個兩兩相互垂直的地磁傳感器，由于在實(shí)驗中沒有俯仰角度，所以利用DSP28335處理器對雙軸地磁采集的信號進(jìn)行處理，通過示波器測量出的波形如圖3所示。X軸和Z軸的電壓幅值為0～3.3 V，首先對采集的信號進(jìn)行均值濾波處理，再利用轉(zhuǎn)換公式解算這些電壓幅值得出旋轉(zhuǎn)體滾轉(zhuǎn)角度。

圖3 X軸和Z軸的電壓幅值

解算的旋轉(zhuǎn)體滾轉(zhuǎn)角就是旋轉(zhuǎn)體在沿地磁傳感器Z軸旋轉(zhuǎn)時的X軸和Z軸的角度，也即是在垂直于轉(zhuǎn)軸的平面內(nèi)轉(zhuǎn)軸坐標(biāo)系和旋轉(zhuǎn)體坐標(biāo)系的夾角，如圖4所示。

圖4 旋轉(zhuǎn)體滾轉(zhuǎn)角示意圖

可以看出旋轉(zhuǎn)體的滾轉(zhuǎn)角γ：

2.2.2對于旋轉(zhuǎn)體滾轉(zhuǎn)角度及轉(zhuǎn)速的解算

通過已知采樣的雙軸電壓幅值如圖5所示，設(shè)X軸曲線為y1=A1sin(ω1t+φ)，Y軸曲線為y2=A2cos(ω2t+φ)，作垂直輔助線相交x軸于點(diǎn)E，可得出E時刻角度值θ=atan2(y1,y2)，θ∈[-π,π]，θ即為當(dāng)前E時刻時旋轉(zhuǎn)體的滾轉(zhuǎn)角。當(dāng)θ<0時，需要將角度歸一化，θ=θ+2π。

圖5 地磁雙軸電壓幅值

基于雙軸地磁解算旋轉(zhuǎn)體滾轉(zhuǎn)角的準(zhǔn)確性，根據(jù)在一個周期內(nèi)兩個時刻之差Δt，旋轉(zhuǎn)體旋轉(zhuǎn)了Δα的角度值，可以得出此時的角速度為ω=Δα/Δt，旋轉(zhuǎn)體的轉(zhuǎn)速為v=ω/2π。

通過使用單軸地磁傳感器和三軸地磁傳感器進(jìn)行角度測量實(shí)驗后，對兩種傳感器的性能進(jìn)行了比較，如表1所示。

從表1可以看出：單軸地磁傳感器成本低，適應(yīng)于高動態(tài)，運(yùn)算量為6 μs，可以適應(yīng)各種環(huán)境，但是精度一般且采樣率要求較高；三軸地磁成本高，運(yùn)算量為3～4 μs比單軸低，對環(huán)境要求較高，高低動態(tài)都適用，由于通過雙軸解姿所以精度非常高，對于采樣率要求相對低。因為三軸地磁傳感器對于以后的俯仰姿態(tài)解算有著重要作用，本文采用三軸地磁傳感器。

表1 單軸和三軸地磁傳感器的比較

3 對于前部旋轉(zhuǎn)體的姿態(tài)解算

已知實(shí)驗裝置結(jié)構(gòu)特點(diǎn)，前后部旋轉(zhuǎn)體相對旋轉(zhuǎn)，通過地磁傳感器測得后部旋轉(zhuǎn)體的滾轉(zhuǎn)角，不考慮初始對準(zhǔn)時，假設(shè)地磁位置指向即為后部旋轉(zhuǎn)體的角度數(shù)據(jù)，記為α；同理，將前部旋轉(zhuǎn)體的指向位置與初始角度的差值設(shè)為前部旋轉(zhuǎn)體的角度，記為β；當(dāng)光電傳感器與光源信號觸發(fā)時，產(chǎn)生感應(yīng)信號，此時后部旋轉(zhuǎn)體的當(dāng)前角度值就是前部旋轉(zhuǎn)體當(dāng)前的角度值，即α=β。另外，須知兩者本質(zhì)上都是對地的絕對轉(zhuǎn)角，只不過需要初始對準(zhǔn)，確定0角度位置。

已知后部旋轉(zhuǎn)體轉(zhuǎn)速n1=1/T，T為后部旋轉(zhuǎn)體的周期，單位為r/s，所以1/T也是頻率。假設(shè)前部旋轉(zhuǎn)體轉(zhuǎn)速為n2，前面已經(jīng)提及的光電傳感器本質(zhì)上給出了前部與后部旋轉(zhuǎn)體相遇點(diǎn)的標(biāo)識，已知n1解算n2?？梢詫⑶安颗c后部旋轉(zhuǎn)體比作甲乙兩人在圓形操場上跑步，先設(shè)定兩人反向跑步，則有公式n1Δt+n2Δt=1，即n1+n2=1/Δt(其中1表示單位圈，Δt表示兩人兩次相遇的時間間隔)，得出結(jié)論1/Δt為兩人的相對轉(zhuǎn)速，同理可得兩人同向跑步的情形，n1Δt-n2Δt=1。

所以現(xiàn)在的解算方法，n2=1/Δt-n1，其中Δt為兩次光電傳感器觸發(fā)的時間間隔，Δt=τ·Dtm(τ為時間常數(shù)，Dtm為兩次光電傳感器觸發(fā)間隔內(nèi)的地磁采集點(diǎn)數(shù))，1/Δt即為前部與后部旋轉(zhuǎn)體的相對轉(zhuǎn)速(頻率)。

4 前部旋轉(zhuǎn)體位置標(biāo)定實(shí)驗

由于在對前部旋轉(zhuǎn)體進(jìn)行滾轉(zhuǎn)角度測量時，當(dāng)前部旋轉(zhuǎn)體在某一角度停止時，并不知道此刻角度的具體數(shù)值是多少，因此需要對其進(jìn)行位置標(biāo)定實(shí)驗，并制作適合前部旋轉(zhuǎn)體標(biāo)定角度的0～360°角度圓盤，用于以后判斷停止時的角度位置。

首先將前部旋轉(zhuǎn)體固定在轉(zhuǎn)臺上靜止不動，取其中一個點(diǎn)做上標(biāo)記作為標(biāo)定角度指針，設(shè)置后部旋轉(zhuǎn)體轉(zhuǎn)速為3 000 r/min，測試1 min后提取數(shù)據(jù)。然后再將前部旋轉(zhuǎn)體隨機(jī)固定在另外3個位置上，每次固定時都需要在空白圓盤上做上標(biāo)記，最后通過4組角度數(shù)據(jù)繪制一個刻度圓盤后，再將前部旋轉(zhuǎn)體固定在刻度盤90°的位置上，提取數(shù)據(jù)并進(jìn)行對比，判斷舵機(jī)是否停止在90°位置上。表2為舵機(jī)隨機(jī)4次固定位置時的角度數(shù)據(jù)。如表3為舵機(jī)固定在90°時測試的角度值。

表2 舵機(jī)固定滾轉(zhuǎn)角度數(shù)據(jù)截取片段

表3 舵機(jī)滾轉(zhuǎn)角度為90°截取片段

實(shí)驗結(jié)果表明：通過對前部旋轉(zhuǎn)體停在4個角度位置的測試數(shù)據(jù)可以得出，即使把前部旋轉(zhuǎn)體固定不動時，也會存在轉(zhuǎn)體稍微抖動，造成測得的數(shù)據(jù)存在微小誤差。從表2可以看出極差在4°以內(nèi)，正常誤差在1°～2°之間，對其進(jìn)行一維線性標(biāo)定后均值誤差分別為：1.700 7°、0.904 1°、1.051 5°、1.765 8°，可以看出4個角度的標(biāo)定標(biāo)準(zhǔn)差維持在2°以內(nèi)，穩(wěn)定性較好。通過表3測得的數(shù)據(jù)與刻度盤標(biāo)定的90°對比可以看出，測量位置角度雖然存在微小誤差但是還是非常準(zhǔn)確。

5 結(jié)論

利用地磁傳感器與光電傳感器組合的方法可以實(shí)現(xiàn)對相對旋轉(zhuǎn)體姿態(tài)的實(shí)時解算。經(jīng)過對旋轉(zhuǎn)體滾轉(zhuǎn)角度解算的實(shí)驗數(shù)據(jù)分析，旋轉(zhuǎn)體停止不動時的最大角度誤差小于4°，在合理實(shí)驗誤差范圍內(nèi)。解算滾轉(zhuǎn)角位置的準(zhǔn)確度非常高，能夠利用低成本地磁傳感器高精度的實(shí)時解算旋轉(zhuǎn)體的滾轉(zhuǎn)姿態(tài)提供實(shí)際意義。