李耀軍 李喜民 潘 泉 程曉冉

(1.西安電子工程研究所 西安 710100；2.西北工業(yè)大學(xué) 西安 710129)

0 引言

載體姿態(tài)信息是導(dǎo)航與制導(dǎo)領(lǐng)域不可缺少的信息。炮彈、榴彈、火箭彈等常規(guī)旋轉(zhuǎn)彈藥，在獲得旋轉(zhuǎn)穩(wěn)定性的同時(shí)給彈姿測(cè)量帶來了困難[1]，主要體現(xiàn)在量程、動(dòng)態(tài)范圍及測(cè)量精度，轉(zhuǎn)速超過10Hz的情況，已將慣性導(dǎo)航排除在外，目前主流的測(cè)量組件是地磁類傳感器，量程大，動(dòng)態(tài)范圍寬，然而不足之處在于精度低且受地磁場(chǎng)方向約束。常規(guī)彈藥制導(dǎo)化是智能彈藥的發(fā)展趨勢(shì)[2]，無控彈藥如能實(shí)時(shí)獲取彈體的飛行姿態(tài)，以及電動(dòng)舵機(jī)、氣動(dòng)舵機(jī)或者脈沖發(fā)動(dòng)機(jī)的舵片的實(shí)時(shí)位置，基于事先設(shè)置的控制策略驅(qū)動(dòng)執(zhí)行機(jī)構(gòu)舵偏角，及時(shí)改變彈體氣動(dòng)力，持續(xù)調(diào)整彈體位置逼近預(yù)置彈道，實(shí)現(xiàn)彈道修正與精確控制，進(jìn)而實(shí)現(xiàn)精確打擊[3-5]。由此可見，如何獲取常規(guī)彈藥的飛行姿態(tài)直接關(guān)系到常規(guī)彈藥的精確制導(dǎo)能力[6-7]。

長基線衛(wèi)星姿態(tài)測(cè)量系統(tǒng)[6]具有高精度位置和姿態(tài)測(cè)量優(yōu)勢(shì)。多天線衛(wèi)星姿態(tài)測(cè)量技術(shù)目前已經(jīng)比較成熟[7-8]，已進(jìn)入工程應(yīng)用階段；同時(shí)，基于多天線衛(wèi)星測(cè)姿系統(tǒng)天線陣列結(jié)構(gòu)復(fù)雜、安裝要求較高、重量、體積、功耗及成本都難以滿足低成本制導(dǎo)彈藥的要求，極大地限制了衛(wèi)星導(dǎo)航與測(cè)姿技術(shù)在智能彈藥方面的應(yīng)用[8-9]。

探索單天線衛(wèi)星導(dǎo)航與測(cè)姿新技術(shù)，將基于運(yùn)動(dòng)學(xué)原理開展彈體姿態(tài)測(cè)量技術(shù)研究。作為基于載波衛(wèi)星導(dǎo)航與測(cè)姿技術(shù)的補(bǔ)充和備份[9-14]，面向制導(dǎo)彈藥二維彈道修正執(zhí)行機(jī)構(gòu)對(duì)彈體姿態(tài)的需求，運(yùn)用彈體運(yùn)動(dòng)學(xué)原理，在不引入慣性測(cè)量單元、地磁等其他傳感器的情況下，基于傳統(tǒng)衛(wèi)星導(dǎo)航接收機(jī)輸出的速度信息，在彈體飛行攻角和側(cè)滑角較小的情況下，實(shí)現(xiàn)了彈體三維姿態(tài)運(yùn)動(dòng)學(xué)建模，由于通常情況下攻角和側(cè)滑角難以給出，計(jì)算的姿態(tài)即稱之為偽姿態(tài)，其測(cè)量精度能夠滿足常規(guī)彈藥制導(dǎo)化需求。

引入偽姿態(tài)[9-14]的概念，分析了由偽姿態(tài)測(cè)量輔助求取載體姿態(tài)的具體算法，建立了單天線衛(wèi)星姿態(tài)測(cè)量的數(shù)學(xué)模型，通過設(shè)計(jì)相應(yīng)的濾波器對(duì)狀態(tài)參數(shù)進(jìn)行估計(jì)，得到相應(yīng)偽姿態(tài)信息，根據(jù)常規(guī)姿態(tài)與偽姿態(tài)的關(guān)系，得到常規(guī)姿態(tài)信息。

1 單天線偽姿態(tài)測(cè)量

1.1 偽姿態(tài)測(cè)量

根據(jù)彈體應(yīng)用實(shí)際條件，相關(guān)坐標(biāo)系，如載體坐標(biāo)系(Obxbybzb)、地理坐標(biāo)系(Otxtytzt)、風(fēng)軸系(Owxwywzw)、穩(wěn)定坐標(biāo)系(Osxsyszs)及常規(guī)姿態(tài)和偽姿態(tài)的定義參見文獻(xiàn)[18]?；谏鲜鲎鴺?biāo)系定義及載體姿態(tài)約定，給出載體偽姿態(tài)的解算模型[19]。

假設(shè)已知速度矢量為

v=vei+vnj+vuk

(1)

則，加速度矢量為

a=aei+anj+auk

(2)

如圖1所示偽姿態(tài)角之間的幾何關(guān)系，可得偽偏航角ψs計(jì)算公式為

ψs=arctan(ve/vn)

(3)

圖1 偽姿態(tài)角之間的幾何關(guān)系

同時(shí)，易得偽俯仰角?s計(jì)算公式為

(4)

(5)

圖2 加速度與各矢量之間的關(guān)系

滾轉(zhuǎn)角與各矢量之間的關(guān)系如圖3所示，則偽滾轉(zhuǎn)角計(jì)算公式為

(6)

圖3 滾轉(zhuǎn)角與各矢量之間的關(guān)系

1.2 計(jì)算流程

衛(wèi)星導(dǎo)航接收機(jī)在接收到衛(wèi)星信號(hào)后，通過一系列導(dǎo)航解算處理后，得到載體的位置和速度信息；地理坐標(biāo)系下的速度分量信息通過卡爾曼濾波處理得到穩(wěn)定的東、北、天方向的加速度分量；然后，基于速度分量信息和加速度分量信息進(jìn)行偽姿態(tài)信息的解算，求取載體的偽偏航角、偽俯仰角和偽滾轉(zhuǎn)角。最后，對(duì)計(jì)算得到的載體姿態(tài)信息，進(jìn)行優(yōu)化處理，以提高姿態(tài)信息的精度。整體方案如圖4所示。

圖4 單天線GPS姿態(tài)測(cè)量系統(tǒng)結(jié)構(gòu)圖

1.3 狀態(tài)估計(jì)

在地理坐標(biāo)系下速度v={ve,vu,vn}已知的情況下，其對(duì)應(yīng)的加速度a={ae,au,an}可以通過構(gòu)建三個(gè)獨(dú)立的卡爾曼濾波器估計(jì)出來，下面將以東向?yàn)槔龢?gòu)造卡爾曼濾波器。

假設(shè)k時(shí)刻xe(k)的東向分量為

(7)

設(shè)k+1時(shí)刻狀態(tài)[10]

xe(k+1)=Φxe(k)+z(k)

(8)

其中，Φ為轉(zhuǎn)移矩陣，z(k)為系統(tǒng)噪聲。

構(gòu)建狀態(tài)觀測(cè)方程

ye(k)=Hxe(k)+v(k)

(9)

其中H為觀測(cè)矩陣，只計(jì)量速度，寫為

H=[1 0 0]T

(10)

至此，完成東向速度分量與加速度分量的濾波器設(shè)計(jì)，相關(guān)計(jì)算公式如下

其中，Q為系統(tǒng)噪聲，R為觀測(cè)噪聲。同理，計(jì)算出北向和天向的相關(guān)狀態(tài)分量。

1.4 矢量計(jì)算

衛(wèi)星接收機(jī)輸出速度v和高度h，可差分出偏航角λ，由此可得東向及北向速度

(12)

速度分量vu可通過差分方式獲得

vu=[h(k+1)-h(k)]/Δt

(13)

假設(shè)地理坐標(biāo)系下加速度為a={an,ae,au}，可通過對(duì)速度的北向分量進(jìn)行差分，計(jì)算出加速度的北向分量

an=[vn(k+1)-vn(k)]/Δt

(14)

同理，計(jì)算出其他加速度分量，則

a=ani+aej+auk

(15)

1.5 姿態(tài)變換

若已知坐標(biāo)系旋轉(zhuǎn)角α,β，可根據(jù)變換矩陣[20]由偽姿態(tài)計(jì)算出常規(guī)姿態(tài)，計(jì)算公式如下所示。

地理坐標(biāo)系變換到穩(wěn)定坐標(biāo)系的方向余弦矩陣，記為R(ψs,?s,γs)

R(ψs,?s,γs)=

(16)

穩(wěn)定坐標(biāo)系變換到載體坐標(biāo)系的方向余弦矩陣，記為L(α,β)

(17)

地理坐標(biāo)系變換到載體坐標(biāo)系的方向余弦矩陣，記為R(ψ,?,γ)

R(ψ,?,γ)=L(α,β)R(ψs,?s,γs)

(18)

(ψ,?,γ)即為常規(guī)姿態(tài)。

1.6 優(yōu)化計(jì)算

對(duì)求得的姿態(tài)信息進(jìn)行優(yōu)化處理。嘗試了采取α-β濾波及高斯牛頓迭代法進(jìn)行姿態(tài)信息的優(yōu)化計(jì)算。

1)α-β濾波

建立估計(jì)方程

(19)

式(19)中，xp(k+1)為位置估計(jì)值，vp(k+1)為速度估計(jì)值，T為量測(cè)周期。

則，xs(k)與vs(k)可計(jì)算如下

(20)

式(20)中，xm(k)為實(shí)時(shí)量測(cè)。

2)高斯牛頓迭代法

高斯牛頓迭代法是非線性最小二乘的一種優(yōu)化算法，采用逐步迭代的方法，把非線性問題變成一步一步迭代的線性問題，從而達(dá)到極值收斂。算法步驟如下：

①確定目標(biāo)函數(shù)；

②選取初值，將解算的初始時(shí)刻的姿態(tài)信息選為迭代初值；

③對(duì)第k次迭代計(jì)算雅克比矩陣及增量Δxk=(JTJ)-1JTε(xk)；其中，J為雅可比矩陣，ε(xk)為第k步時(shí)的殘差，Δxk是第k步時(shí)的增量；

④若增量Δxk小于規(guī)定的精度值，則停止迭代。否則，更新xk+1=xk+Δxk；

⑤循環(huán)執(zhí)行步驟②及步驟③直到滿足終止條件或達(dá)到最大循環(huán)次數(shù)。

2 實(shí)驗(yàn)測(cè)試與分析

2.1 實(shí)驗(yàn)流程

圖5 穩(wěn)定坐標(biāo)系與載體坐標(biāo)系之間的關(guān)系

為驗(yàn)證算法的可行性和有效性，對(duì)某火箭彈單天線衛(wèi)星測(cè)姿系統(tǒng)進(jìn)行實(shí)驗(yàn)分析。整個(gè)仿真流程如圖6所示。

圖6 單天線測(cè)姿實(shí)驗(yàn)流程圖

1)將衛(wèi)星輸出的速度數(shù)據(jù)進(jìn)行矢量化處理，并分離出速度信息，該速度信息作為輸入量為卡爾曼濾波器所用，通過濾波估計(jì)，得到載體的實(shí)時(shí)加速度；

2)速度與加速度信息作為偽姿態(tài)模型的數(shù)據(jù)輸入，通過箭體計(jì)算機(jī)實(shí)時(shí)運(yùn)算可求取載體的偽姿態(tài)；

3)根據(jù)式(20)可求取常規(guī)姿態(tài)角(ψ,?,γ)。

2.2 實(shí)驗(yàn)結(jié)果及分析

對(duì)實(shí)驗(yàn)數(shù)據(jù)進(jìn)行算法驗(yàn)證，由從GPS解算的信息中，得到三個(gè)方向的速度，即東向速度，北向速度，和天向速度，通過偽姿態(tài)算法對(duì)姿態(tài)信息進(jìn)行解算與處理，將處理后的姿態(tài)信息，與真實(shí)姿態(tài)信息做比較，得到姿態(tài)的誤差角度，以驗(yàn)證該算法的有效性。

1)無人機(jī)掛飛實(shí)驗(yàn)一組

①偏航角實(shí)驗(yàn)測(cè)試

從掛飛數(shù)據(jù)的可以看出測(cè)姿誤差范圍都在±10°。在處理姿態(tài)信息時(shí)存在初值選取的問題，這是其弊端之一，如果初值的選取不合適，那么結(jié)果將會(huì)收斂到另外一個(gè)值上，甚至?xí)鸾Y(jié)果的發(fā)散。實(shí)驗(yàn)結(jié)果如圖7所示。

②俯仰角實(shí)驗(yàn)測(cè)試

從掛飛數(shù)據(jù)的俯仰角實(shí)驗(yàn)結(jié)果來看，高斯牛頓方法與α-β濾波方法的效果基本沒有差別，兩種方法的誤差都在±10°之間。同樣，高斯牛頓法存在初始值的選取問題，結(jié)果如圖8所示。

③滾轉(zhuǎn)角實(shí)驗(yàn)測(cè)試

使用高斯牛頓方法處理數(shù)據(jù)，滾轉(zhuǎn)角在一個(gè)較大的范圍內(nèi)波動(dòng)，若初值選取不合適，則無法收斂到合適的值上。從實(shí)驗(yàn)結(jié)果里看，使用α-β濾波方法處理后的姿態(tài)信息更加平穩(wěn)。從兩種方法的誤差來看，高斯牛頓方法的誤差范圍在±10°，α-β濾波方法處理后的數(shù)據(jù)誤差范圍在±8°，兩者有一定差別，但并不明顯。兩種方法的處理時(shí)間仍是高斯牛頓方法所需時(shí)間大于α-β濾波所需時(shí)間，多次實(shí)驗(yàn)所得結(jié)論一致。實(shí)驗(yàn)結(jié)果如圖9所示。

圖8 無人機(jī)掛飛實(shí)驗(yàn)俯仰角測(cè)試結(jié)果

圖9 無人機(jī)掛飛實(shí)驗(yàn)滾轉(zhuǎn)角測(cè)試結(jié)果

2)無人機(jī)掛飛實(shí)驗(yàn)二組

無人機(jī)掛飛實(shí)驗(yàn)二組實(shí)驗(yàn)結(jié)果如圖(9)所示。從無人機(jī)掛飛實(shí)驗(yàn)二組實(shí)驗(yàn)結(jié)果來看，在偏航角的信息處理中，兩種方法得到的結(jié)果近乎近似。在俯仰角的信息處理中，高斯牛頓方法相比α-β濾波方法更好一點(diǎn)，但不明顯，兩者的誤差范圍都在-10～10°。在滾轉(zhuǎn)角結(jié)果中，高斯牛頓方法相比α-β濾波方法較差，高斯牛頓方法的誤差在-10～10°，α-β濾波方法誤差在-6～6°?？傮w來看，本組實(shí)驗(yàn)與無人機(jī)掛飛實(shí)驗(yàn)一組所得結(jié)論一致，實(shí)驗(yàn)結(jié)果如圖10所示。

圖10 無人機(jī)掛飛實(shí)驗(yàn)偏航、俯仰、滾轉(zhuǎn)角測(cè)試結(jié)果

3 結(jié)束語

利用單天線衛(wèi)星測(cè)姿技術(shù)破解衛(wèi)星導(dǎo)航在旋轉(zhuǎn)彈上無法測(cè)姿的難題，可以用較低成本同時(shí)提供位置和姿態(tài)信息，具備彈姿/彈道一體化測(cè)量功能，替代慣性導(dǎo)航及地磁傳感器，在增強(qiáng)衛(wèi)星制導(dǎo)組件的功能和性能的同時(shí)，大幅提高制導(dǎo)組件耐高過載能力及可靠性，也將大幅降低二維制導(dǎo)組件成本，具有較強(qiáng)的實(shí)際應(yīng)用價(jià)值。