基于無(wú)人機(jī)慣性導(dǎo)航系統(tǒng)軌跡發(fā)生器的設(shè)計(jì)與仿真

陳 超

(火箭軍士官學(xué)校，山東 青州 262500 )

0 引言

20世紀(jì)末至今，科學(xué)技術(shù)高速發(fā)展，科學(xué)研究不斷深入，新一代高精尖技術(shù)裝備層出不窮，軍事裝備也呈現(xiàn)出明顯的智能化、無(wú)人化趨勢(shì)。近些年來(lái)，世界范圍內(nèi)將大量的資源投入到無(wú)人機(jī)的設(shè)計(jì)、研發(fā)和使用中。為了能夠?qū)崿F(xiàn)無(wú)人機(jī)完全自主精確著陸，通過(guò)搭建組合導(dǎo)航系統(tǒng)來(lái)實(shí)現(xiàn)著陸任務(wù)是目前研究的主要課題之一，研究無(wú)人機(jī)著陸的自主導(dǎo)航方案首先需搭建一套完整的捷聯(lián)慣性導(dǎo)航系統(tǒng)當(dāng)作研究基礎(chǔ)[1]。為了準(zhǔn)確研究系統(tǒng)導(dǎo)航算法的性能，以往在研究捷聯(lián)慣性系統(tǒng)時(shí)，都是通過(guò)真實(shí)測(cè)量的方法獲得慣性器件的輸出數(shù)據(jù)，并以此作為為捷聯(lián)慣性系統(tǒng)的輸入值和基準(zhǔn)[2]。但是慣性器件實(shí)時(shí)輸出的數(shù)據(jù)極易受到外界環(huán)境、人本身等諸多因素的影響，成本比較高[3]。為了解決這些問(wèn)題，更好地研究系統(tǒng)導(dǎo)航參數(shù)的綜合性能，設(shè)計(jì)一種針對(duì)無(wú)人機(jī)的運(yùn)動(dòng)軌跡發(fā)生器是非常有意義的。

1 坐標(biāo)的定義與轉(zhuǎn)換

在進(jìn)行UAV運(yùn)動(dòng)軌跡發(fā)生器模型搭建之前，先進(jìn)行運(yùn)動(dòng)軌跡發(fā)生器坐標(biāo)系定義[4]，在此基礎(chǔ)上進(jìn)行參數(shù)設(shè)定和模型搭建。

1.1 運(yùn)動(dòng)軌跡發(fā)生器坐標(biāo)系定義

無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器進(jìn)行設(shè)計(jì)，首先對(duì)所需的基本坐標(biāo)系給出定義，然后對(duì)無(wú)人機(jī)的三大基本飛行過(guò)程進(jìn)行數(shù)學(xué)建模，在進(jìn)行無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器模型搭建之前，先進(jìn)行運(yùn)動(dòng)軌跡發(fā)生器坐標(biāo)系定義，在此基礎(chǔ)上進(jìn)行參數(shù)設(shè)定和模型搭建[5]。

導(dǎo)航坐標(biāo)系(OXnYnZn)取為東北天坐標(biāo)系；機(jī)體坐標(biāo)系(OXbYbZb)是與機(jī)體相固連的，其原點(diǎn)與地理坐標(biāo)系的原點(diǎn)相同[6]，定義在機(jī)體重心上，Xb軸沿機(jī)體橫軸指向右，Yb軸沿機(jī)體縱軸指向前方，Zb軸垂直于OXbYb平面向上；軌跡坐標(biāo)系(OXtYtZt)的Xt軸方向?yàn)樗较蛴遥琘t軸與無(wú)人機(jī)飛行軌跡相切并指向其前進(jìn)方向，Xt，Yt軸遵循右手定則指向Zt軸方向；軌跡水平坐標(biāo)系(OXhYhZh)是軌跡坐標(biāo)系在水平面的投影，它們的X軸相同，指向同一方向，Yh軸是Yt軸在水平面的投影，Zh軸垂直于水平面向上，并與Xh，Yh軸組成右手定則。

1.2 坐標(biāo)轉(zhuǎn)換

在對(duì)各個(gè)坐標(biāo)系進(jìn)行定義之后，可以推導(dǎo)得出各個(gè)坐標(biāo)系之間的相互轉(zhuǎn)換關(guān)系：導(dǎo)航坐標(biāo)系繞Zn軸逆向轉(zhuǎn)過(guò)ψ角度得到軌跡水平坐標(biāo)系；軌跡水平坐標(biāo)系繞Xh軸正方向轉(zhuǎn)過(guò)θ角度得到軌跡坐標(biāo)系；軌跡坐標(biāo)系繞Yt軸正方向轉(zhuǎn)過(guò)γ角度得到機(jī)體坐標(biāo)系。

相應(yīng)的坐標(biāo)系轉(zhuǎn)換矩陣如下所示：

(1)

(2)

(3)

(4)

(5)

2 無(wú)人機(jī)機(jī)動(dòng)飛行過(guò)程設(shè)定

根據(jù)無(wú)人機(jī)飛行過(guò)程不同的機(jī)動(dòng)動(dòng)作，此處將無(wú)人機(jī)的機(jī)動(dòng)飛行過(guò)程分成爬升、轉(zhuǎn)彎和俯沖3個(gè)過(guò)程分別闡述其各自的模型[7]。

2.1 無(wú)人機(jī)的爬升過(guò)程

根據(jù)實(shí)際飛行過(guò)程，無(wú)人機(jī)的爬升過(guò)程可以劃分成3個(gè)子過(guò)程：拉起階段、恒定俯仰角的爬升階段以及結(jié)束爬升階段。

2.1.1 拉起階段

在拉起階段，無(wú)人機(jī)的俯仰角以等角加速度β0不斷增加，直到達(dá)到預(yù)設(shè)等角爬升階段的俯仰角度。假設(shè)該階段的初試時(shí)刻為t01，則有：

β=β0，θ=β(t-t01)。

(6)

2.1.2 等角爬升階段

在等角爬升階段，無(wú)人機(jī)以恒定不變的俯仰角θc持續(xù)飛行到預(yù)定的海拔高度，可得：

θ=0，θ=θc。

(7)

2.1.3 結(jié)束爬升階段

在結(jié)束爬升階段，無(wú)人機(jī)的俯仰角以恒定角加速度-β0不斷減小，直到俯仰角為0。假設(shè)該階段的初始時(shí)刻為t02，則有：

β=-β0，θ=θc+β(t-t02)。

(8)

2.2 無(wú)人機(jī)的轉(zhuǎn)彎過(guò)程

(9)

飛機(jī)的轉(zhuǎn)彎過(guò)程同樣分為3個(gè)子階段：首先，由水平飛行狀態(tài)改變橫滾角，開(kāi)始進(jìn)入轉(zhuǎn)彎階段；然后，維持橫滾角不變開(kāi)始等角速度轉(zhuǎn)彎階段；最后，進(jìn)入改平階段。

2.2.1 進(jìn)入轉(zhuǎn)彎階段

在這個(gè)階段，無(wú)人機(jī)的橫滾角以等角加速度χ0改變橫滾角直至預(yù)定數(shù)值，假設(shè)t03為這個(gè)階段的起始時(shí)刻，可得：

(10)

2.2.2 等角速度轉(zhuǎn)彎階段

在這個(gè)階段，無(wú)人機(jī)保持恒定的橫滾角γc同時(shí)以恒定的角速度ω0轉(zhuǎn)彎，則有：

(11)

2.2.3 改平階段

在這個(gè)階段，無(wú)人機(jī)的橫滾角以恒定角加速度-χ0不斷減小。假設(shè)該階段的初始時(shí)刻為t04，則有：

χ=-χ0，γ=γc+χ(t-t04)。

(12)

2.2.4 俯沖過(guò)程

無(wú)人機(jī)俯沖過(guò)程的俯仰角調(diào)整過(guò)程與前述爬升過(guò)程恰好相反，整個(gè)過(guò)程同樣分為3個(gè)子階段[8]：調(diào)整俯仰角進(jìn)入俯沖階段、維持恒定俯仰角的持續(xù)俯沖階段以及結(jié)束俯沖的改平階段[9]。

(1) 進(jìn)入俯沖階段

在這個(gè)階段，無(wú)人機(jī)的俯仰角開(kāi)始改變，以等角加速度-β1不斷減小直至預(yù)定的俯仰角。假設(shè)該階段的初始時(shí)刻為t05，則有：

β=-β1，θ=β(t-t05)。

(13)

(2) 持續(xù)俯沖階段

在這個(gè)階段，無(wú)人機(jī)以恒定不變的俯仰角θc1持續(xù)俯沖直至預(yù)定的海拔高度，可得：

θ=0，θ=θc1。

(14)

(3) 俯沖后的改平節(jié)段

在這個(gè)階段，無(wú)人機(jī)的俯仰角以恒定的角加速度β1不斷增加。假設(shè)該階段的初始時(shí)刻為t06，則有：

β=β1，θ=θc1+β(t-t06)。

(15)

3 無(wú)人機(jī)軌跡參數(shù)獲取

根據(jù)前述無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器各個(gè)飛行過(guò)程模型的建立，可得無(wú)人機(jī)各飛行參數(shù)如下。

3.1 無(wú)人機(jī)的加速度

無(wú)人機(jī)在空中飛行時(shí)的加速度用軌跡坐標(biāo)系表示。

無(wú)人機(jī)進(jìn)行直線加速飛行的加速度a表示為：

(16)

無(wú)人機(jī)以滾轉(zhuǎn)角γ進(jìn)行轉(zhuǎn)彎時(shí)的加速度a表示為：

(17)

無(wú)人機(jī)爬升或俯沖時(shí)的加速度表示為：

(18)

無(wú)人機(jī)爬升改平或者俯沖改平時(shí)的加速度表示為：

(19)

無(wú)人機(jī)進(jìn)行勻速等俯仰角爬升或勻速等角俯仰角俯沖時(shí)的加速度表示為：

(20)

無(wú)人機(jī)在導(dǎo)航坐標(biāo)系中的加速度：

(21)

無(wú)人機(jī)在機(jī)體坐標(biāo)系中的加速度：

(22)

3.2 無(wú)人機(jī)的速度

無(wú)人機(jī)在導(dǎo)航坐標(biāo)系中的速度：

(23)

無(wú)人機(jī)在軌跡坐標(biāo)系中的速度：

(24)

無(wú)人機(jī)在機(jī)體坐標(biāo)系中的速度：

(25)

3.3 無(wú)人機(jī)的位置

式(26)中，RM=Re(1-2e+3esin2L)，RN=Re(1+esin2L)，其中，e為地球橢圓度；Re為地球的長(zhǎng)半軸；L為無(wú)人機(jī)及時(shí)緯度值；λ為無(wú)人機(jī)及時(shí)經(jīng)度值；h為無(wú)人機(jī)的及時(shí)高度：

(26)

4 無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器仿真驗(yàn)證

在搭建完畢無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器模型后，通過(guò)設(shè)定各項(xiàng)參數(shù)和機(jī)體運(yùn)動(dòng)狀態(tài)，可以根據(jù)實(shí)際需要模擬出無(wú)人機(jī)的各種飛行軌跡，記錄無(wú)人機(jī)在此期間的各項(xiàng)實(shí)時(shí)參數(shù)[10]。經(jīng)過(guò)誤差補(bǔ)償之后即可作為捷聯(lián)慣性導(dǎo)航系統(tǒng)模型的實(shí)際輸入，供后續(xù)整個(gè)自主導(dǎo)航系統(tǒng)應(yīng)用[11]。

此處通過(guò)對(duì)無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程和無(wú)人機(jī)水平低空勻速飛行過(guò)程進(jìn)行仿真，驗(yàn)證無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器工作的可靠性和準(zhǔn)確性。

4.1 基于Matlab建立仿真模型

Matlab是Mathworks公司開(kāi)發(fā)的一種面向科學(xué)計(jì)算和工程應(yīng)用的高級(jí)語(yǔ)言[12]，是當(dāng)今國(guó)際科學(xué)界最具影響力、最具活力的軟件，也是集強(qiáng)大的數(shù)值計(jì)算、信號(hào)處理、自動(dòng)控制、圖像處理等功能于一體的，用于概念設(shè)計(jì)、建模仿真、算法開(kāi)發(fā)、實(shí)時(shí)實(shí)現(xiàn)的理想集成環(huán)境[13]；1990年，Mathworks公司推出了Simulink模塊庫(kù)，它是一個(gè)用于動(dòng)態(tài)系統(tǒng)建模、仿真和分析的軟件包，它提供了基于Matlab核心的數(shù)值、圖形和編程功能的一個(gè)塊狀圖界面，通過(guò)塊與塊的連線和屬性設(shè)置，用戶很容易構(gòu)建出符合特定要求的模型，可方便地改變模型的參數(shù)，并對(duì)模型進(jìn)行仿真和分析[14-15]；在Matlab7.13的Simulink環(huán)境下，利用SimPowerSystem提供的豐富模塊庫(kù)[16]，建立了軌跡發(fā)生器的仿真模型。

4.2 無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程驗(yàn)證

首先通過(guò)無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程來(lái)對(duì)運(yùn)動(dòng)軌跡發(fā)生器實(shí)施仿真驗(yàn)證，檢驗(yàn)運(yùn)動(dòng)軌跡發(fā)生器的可靠性以及準(zhǔn)確性。此飛行過(guò)程設(shè)計(jì)了無(wú)人機(jī)水平直線飛行、水平加速飛行、爬升過(guò)程、俯沖過(guò)程、轉(zhuǎn)彎過(guò)程以及水平減速飛行過(guò)程，全面涵蓋了無(wú)人機(jī)正常飛行機(jī)動(dòng)動(dòng)作，通過(guò)此復(fù)雜飛行過(guò)程對(duì)運(yùn)動(dòng)軌跡發(fā)生器進(jìn)行驗(yàn)證，具有說(shuō)服力。假設(shè)無(wú)人機(jī)起始點(diǎn)坐標(biāo)為120°E，30°N，起始飛行高度1 000 m，速度為沿正北方向500 m/s，仿真時(shí)間1 000 s。無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程的仿真驗(yàn)證結(jié)果如圖1～4所示。

圖1 無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程三維圖

圖2 無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程位置仿真

圖4 無(wú)人機(jī)多機(jī)動(dòng)動(dòng)作復(fù)雜飛行過(guò)程姿態(tài)角仿真

4.3 無(wú)人機(jī)低空水平飛行過(guò)程驗(yàn)證

由于本文研究目的是對(duì)無(wú)人機(jī)著陸進(jìn)近階段進(jìn)行自主導(dǎo)航方案設(shè)計(jì)，力求保證無(wú)人機(jī)慣導(dǎo)校準(zhǔn)階段的導(dǎo)航精度，故此處模擬無(wú)人機(jī)進(jìn)近階段的低空平飛過(guò)程，對(duì)運(yùn)動(dòng)軌跡發(fā)生器進(jìn)行次飛行過(guò)程的驗(yàn)證。假設(shè)無(wú)人機(jī)起始點(diǎn)坐標(biāo)為116°E，40°N，飛行高度保持在250 m，速度為40 m/s，正北方向，仿真時(shí)間200 s。圖5為無(wú)人機(jī)低空水平飛行軌跡三維圖。

圖5 無(wú)人機(jī)低空水平飛行軌跡三維圖

圖6為無(wú)人機(jī)低空水平飛行位置仿真圖，其中經(jīng)度保持116°E，高度保持250 m，維度變化符合無(wú)人機(jī)實(shí)際變化規(guī)律。

圖6 無(wú)人機(jī)低空水平飛行位置仿真圖

圖7為低空水平飛行三向速度仿真圖，其中東向和天向速度保持為0 m/s，北向速度為40 m/s符合實(shí)無(wú)人機(jī)際速度。

圖7 無(wú)人機(jī)低空水平飛行三向速度仿真圖

圖8為無(wú)人機(jī)低空水平飛行姿態(tài)角仿真圖，其中俯仰角、偏航角和橫滾角保持為零，能夠準(zhǔn)確反映無(wú)人機(jī)的姿態(tài)角。

圖8 無(wú)人機(jī)低空水平飛行姿態(tài)角仿真圖

通過(guò)上述2種方案的驗(yàn)證結(jié)果得出，無(wú)人機(jī)運(yùn)動(dòng)軌跡發(fā)生器能夠準(zhǔn)確地模擬出給定的無(wú)人機(jī)飛行過(guò)程，可以滿足后續(xù)工作的需求，經(jīng)過(guò)運(yùn)動(dòng)軌跡發(fā)生器得出的無(wú)人機(jī)飛行數(shù)據(jù)在進(jìn)行誤差補(bǔ)償之后即可以作為實(shí)際的無(wú)人機(jī)飛行數(shù)據(jù)投入到后續(xù)的組合導(dǎo)航系統(tǒng)信息融合方案應(yīng)用中。

5 結(jié)束語(yǔ)

慣性器件的輸出數(shù)據(jù)是研究無(wú)人機(jī)慣性導(dǎo)航系統(tǒng)性能的基礎(chǔ)，通過(guò)軌跡發(fā)生器可以方便快捷地模擬出無(wú)人機(jī)飛行過(guò)程中的各種姿態(tài)變化。通過(guò)軟件仿真，驗(yàn)證了軌跡發(fā)生器的正確性和準(zhǔn)確性。因此，該方法具有一定的優(yōu)越性和可行性，可以應(yīng)用于慣性導(dǎo)航系統(tǒng)性能或組合導(dǎo)航算法研究。