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

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

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

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

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

1 原理分析

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

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

2 地磁傳感器角度測(cè)量方法

2.1 基于單軸地磁解算方法

2.1.1判斷地磁信號(hào)有效值

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

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

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

ΔVmax=AsinΔt

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

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

U=S/N，其中N=10

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

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

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

ΔV=6×ΔVmax=0.035

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

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

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

圖2 地磁信號(hào)電壓幅值

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

其次，針對(duì)地磁信號(hào)電壓不斷變化的幅值，需要預(yù)先采集初始的2 000個(gè)電壓采樣點(diǎn)，對(duì)其進(jìn)行最高電壓Va和最低電壓Ve的挑選。在挑選過程中，如何判斷此Va最高電壓，Ve為最低電壓？將采集電壓值Va與Va前實(shí)時(shí)解算逐點(diǎn)累加的電壓均值U進(jìn)行比對(duì)，若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。因?yàn)椴蓸又芷诓蛔?，設(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)角時(shí)，標(biāo)定求出的地磁信號(hào)波峰最高點(diǎn)為對(duì)應(yīng)旋轉(zhuǎn)體滾轉(zhuǎn)角的0°，即為零點(diǎn)(標(biāo)定旋轉(zhuǎn)體零度位置點(diǎn))。兩個(gè)波峰線之間的采樣點(diǎn)橫坐標(biāo)之差就是地磁信號(hào)電壓幅值一個(gè)周期的采樣點(diǎn)數(shù)量t1(也就是旋轉(zhuǎn)體每轉(zhuǎn)的采樣點(diǎn)數(shù)量)。因?yàn)椴蓸又芷诓蛔?，所以?dāng)旋轉(zhuǎn)體在勻速旋轉(zhuǎn)時(shí)，可以通過電壓周期采樣點(diǎn)數(shù)t1和波峰零點(diǎn)坐標(biāo)實(shí)時(shí)解算旋轉(zhuǎn)體的滾轉(zhuǎn)角，得出當(dāng)前地磁信號(hào)電壓采樣點(diǎn)k對(duì)應(yīng)的角度α：

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

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

2.2 基于三軸地磁解算方法

2.2.1滾轉(zhuǎn)角解算

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

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

解算的旋轉(zhuǎn)體滾轉(zhuǎn)角就是旋轉(zhuǎn)體在沿地磁傳感器Z軸旋轉(zhuǎn)時(shí)的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對(duì)于旋轉(zhuǎn)體滾轉(zhuǎn)角度及轉(zhuǎn)速的解算

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

圖5 地磁雙軸電壓幅值

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

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

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

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

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

已知實(shí)驗(yàn)裝置結(jié)構(gòu)特點(diǎn)，前后部旋轉(zhuǎn)體相對(duì)旋轉(zhuǎn)，通過地磁傳感器測(cè)得后部旋轉(zhuǎn)體的滾轉(zhuǎn)角，不考慮初始對(duì)準(zhǔn)時(shí)，假設(shè)地磁位置指向即為后部旋轉(zhuǎn)體的角度數(shù)據(jù)，記為α；同理，將前部旋轉(zhuǎn)體的指向位置與初始角度的差值設(shè)為前部旋轉(zhuǎn)體的角度，記為β；當(dāng)光電傳感器與光源信號(hào)觸發(fā)時(shí)，產(chǎn)生感應(yīng)信號(hào)，此時(shí)后部旋轉(zhuǎn)體的當(dāng)前角度值就是前部旋轉(zhuǎn)體當(dāng)前的角度值，即α=β。另外，須知兩者本質(zhì)上都是對(duì)地的絕對(duì)轉(zhuǎn)角，只不過需要初始對(duì)準(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)識(shí)，已知n1解算n2。可以將前部與后部旋轉(zhuǎn)體比作甲乙兩人在圓形操場(chǎng)上跑步，先設(shè)定兩人反向跑步，則有公式n1Δt+n2Δt=1，即n1+n2=1/Δt(其中1表示單位圈，Δt表示兩人兩次相遇的時(shí)間間隔)，得出結(jié)論1/Δt為兩人的相對(duì)轉(zhuǎn)速，同理可得兩人同向跑步的情形，n1Δt-n2Δt=1。

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

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

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

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

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

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

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

5 結(jié)論

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