[發(fā)明專利]飛行器慣性/氣動(dòng)模型組合導(dǎo)航方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210289087.X
• 申請(qǐng)日: 2012-08-15
• 公開（公告）號(hào): CN102809377A
• 公開（公告）日: 2012-12-05
• 發(fā)明（設(shè)計(jì)）人: 賴際舟;呂品;劉建業(yè);李榮冰;宋亦凡
• 申請(qǐng)（專利權(quán)）人: 南京航空航天大學(xué)
• 主分類號(hào): G01C21/20
• 分類號(hào): G01C21/20
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司 32200
• 代理人: 許方

1.一種飛行器慣性/氣動(dòng)模型組合導(dǎo)航方法，其特征在于：利用飛行器已知的氣動(dòng)參數(shù)、外形參數(shù)、控制量以及運(yùn)動(dòng)參數(shù)信息對(duì)飛行器的速度、姿態(tài)進(jìn)行求解，利用慣導(dǎo)系統(tǒng)誤差模型與飛行器氣動(dòng)模型，構(gòu)建卡爾曼濾波器，將慣性導(dǎo)航系統(tǒng)得到的位置、速度、姿態(tài)信息與氣動(dòng)模型得到的速度、姿態(tài)信息進(jìn)行數(shù)據(jù)融合，同時(shí)對(duì)慣性器件誤差進(jìn)行實(shí)時(shí)估計(jì)與補(bǔ)償，具體步驟如下：

(1)以周期????????????????????????????????????????????????讀取飛行器的阻力系數(shù)，側(cè)力系數(shù)，升力系數(shù)，并將上述3個(gè)系數(shù)、、分別定義于機(jī)體系軸、軸和軸；以周期讀取飛行器的滾轉(zhuǎn)力矩系數(shù)，俯仰力矩系數(shù)，偏航力矩系數(shù)，并將上述3個(gè)系數(shù)、、分別定義于機(jī)體系軸、軸和軸；以周期讀取飛行器的發(fā)動(dòng)機(jī)推力，其在機(jī)體坐標(biāo)系下軸、軸和軸方向的分量分別為、、；以周期讀取飛行器的空速和飛行器的總質(zhì)量，飛行器的總質(zhì)量包括飛行器機(jī)體質(zhì)量、機(jī)載設(shè)備質(zhì)量、乘員質(zhì)量、武器質(zhì)量以及剩余燃油質(zhì)量；

(2)以周期讀取慣導(dǎo)系統(tǒng)輸出的飛行器位置、速度、姿態(tài)信息；

(3)根據(jù)飛行器動(dòng)力學(xué)方程計(jì)算飛行器所受的力與力矩；

根據(jù)步驟(1)得到的飛行器的總質(zhì)量，得飛行器重力為，為重力加速度，其在機(jī)體坐標(biāo)系下軸、軸和軸方向的分量分別為、、；

根據(jù)飛行器的動(dòng)力學(xué)方程，以及步驟(1)中得到的飛行器氣動(dòng)參數(shù)，即飛行器的阻力系數(shù)、側(cè)力系數(shù)、升力系數(shù)；滾轉(zhuǎn)力矩系數(shù)、俯仰力矩系數(shù)、偏航力矩系數(shù)；飛行器的發(fā)動(dòng)機(jī)推力在機(jī)體坐標(biāo)系下軸、軸和軸方向的分量、、；飛行器重力在機(jī)體坐標(biāo)系下軸、軸和軸方向的分量、、，得飛行器所受到合外力為：，

其中、、為飛行器所受合外力在機(jī)體系軸、軸和軸方向的分量，為當(dāng)?shù)卮髿饷芏龋瑸榭账伲瑸闄C(jī)翼面積，所受力矩為：，

其中、、為飛行器所受合外力在機(jī)體系軸、軸和軸方向的分量，被稱為滾轉(zhuǎn)力矩、俯仰力矩以及偏航力矩，為機(jī)翼展長(zhǎng)，為機(jī)翼平均氣動(dòng)弦長(zhǎng)，為機(jī)翼面積；為滾轉(zhuǎn)力矩系數(shù)，為俯仰力矩系數(shù)，為偏航力矩系數(shù)；

(4)根據(jù)飛行器運(yùn)動(dòng)學(xué)方程計(jì)算導(dǎo)航參數(shù)；

以、、分別表示飛行器機(jī)體系下軸、軸和軸方向的速度，以、、分別表示飛行器機(jī)體系下軸、軸和軸方向的角速度，以分別表示飛機(jī)繞機(jī)體軸的轉(zhuǎn)動(dòng)慣量，為飛行器對(duì)軸、軸的慣性積，由于飛機(jī)具有機(jī)體坐標(biāo)系的對(duì)稱面，所以飛行器對(duì)軸、軸的慣性和對(duì)軸、軸的慣性為零，由飛行器的運(yùn)動(dòng)學(xué)方程組，為飛行器機(jī)體系下軸的角加速度，為飛行器機(jī)體系下軸的角加速度，為飛行器機(jī)體系下軸的角加速度以及步驟(3)中求得的飛行器在機(jī)體系軸、軸和軸方向的滾轉(zhuǎn)力矩、俯仰力矩、偏航力矩，對(duì)機(jī)體系下的角速度、、進(jìn)行求解，其中，，，，，，，，，；

以、、分別表示飛行器的橫滾角、俯仰角、航向角三個(gè)姿態(tài)角，根據(jù)姿態(tài)角的變化率與角速率的關(guān)系，為橫滾角速率，?為俯仰角速率，為航向角速率，以及步驟(4)中求得的機(jī)體系下的角速度、、，求解三個(gè)姿態(tài)角的數(shù)值；

根據(jù)飛行器的運(yùn)動(dòng)學(xué)方程組，為飛行器機(jī)體系下軸的加速度，為飛行器機(jī)體系下軸的加速度，為飛行器機(jī)體系下軸的加速度，以及步驟(3)中求得的飛行器所受到的合外力、、，步驟(1)中獲得的飛行器的總質(zhì)量，步驟(4)中求得的機(jī)體系下的角速度、、，對(duì)飛行器機(jī)體系下軸、軸和軸方向的速度、、進(jìn)行求解；

(5)根據(jù)飛行器的氣動(dòng)模型與慣導(dǎo)系統(tǒng)誤差模型，選取氣動(dòng)模型解算出的速度誤差、姿態(tài)誤差、角速度誤差與慣導(dǎo)系統(tǒng)的位置誤差、姿態(tài)誤差、速度誤差、陀螺一階馬爾科夫誤差、陀螺零偏誤差、加速度計(jì)一階馬爾科夫誤差為狀態(tài)量，建立狀態(tài)方程；選取飛行器速度、姿態(tài)為量測(cè)量，建立觀測(cè)方程；根據(jù)步驟(2)得到慣導(dǎo)系統(tǒng)輸出導(dǎo)航參數(shù)與步驟(4)氣動(dòng)模型解算的導(dǎo)航參數(shù)，根據(jù)卡爾曼濾波方程得到時(shí)刻狀態(tài)量的最優(yōu)估計(jì)值，其具體步驟為：

（a）卡爾曼濾波器狀態(tài)方程的建立

根據(jù)飛行器的氣動(dòng)模型與慣導(dǎo)系統(tǒng)誤差模型，選取氣動(dòng)模型解算出的速度誤差、姿態(tài)誤差、角速度誤差與慣導(dǎo)系統(tǒng)的位置誤差、姿態(tài)誤差、速度誤差、陀螺一階馬爾科夫誤差、陀螺零偏誤差、加速度計(jì)一階馬爾科夫誤差為狀態(tài)量，狀態(tài)量共27維，表達(dá)式為，其中與慣導(dǎo)系統(tǒng)相關(guān)的狀態(tài)量

，其中下標(biāo)代表與慣導(dǎo)系統(tǒng)相關(guān)的參數(shù)，下同，分別為慣導(dǎo)系統(tǒng)解算的東北天坐標(biāo)系下軸、軸和軸三個(gè)方向的平臺(tái)誤差角，該坐標(biāo)系中軸、軸和軸分別于當(dāng)?shù)貣|向、北向、天向重合，、、分別為慣導(dǎo)系統(tǒng)解算的東北天坐標(biāo)系下軸、軸和軸三個(gè)方向的速度誤差，分別為慣導(dǎo)系統(tǒng)解算的經(jīng)度誤差、緯度誤差以及高度誤差，為機(jī)體系下三個(gè)陀螺的零偏誤差，為機(jī)體系下三個(gè)陀螺的一階馬爾科夫過程誤差，為機(jī)體系下三個(gè)加速度計(jì)的一階馬爾科夫過程誤差，其中與氣動(dòng)模型相關(guān)的狀態(tài)量，其中下標(biāo)代表與氣動(dòng)模型相關(guān)的參數(shù)，下同，、、分別表示氣動(dòng)模型解算的飛行器機(jī)體系下軸、軸和軸方向的速度誤差，、、分別表示氣動(dòng)模型解算的飛行器機(jī)體系下軸、軸和軸方向的角速度誤差，、、分別表示氣動(dòng)模型解算的飛行器的橫滾角誤差、俯仰角誤差、航向角誤差；

卡爾曼濾波的狀態(tài)方程為，其中為狀態(tài)向量，為狀態(tài)向量一階導(dǎo)數(shù)，為狀態(tài)轉(zhuǎn)換矩陣，為系統(tǒng)噪聲系數(shù)矩陣；

為系統(tǒng)噪聲，其值為，與慣導(dǎo)系統(tǒng)相關(guān)的系統(tǒng)噪聲，其中、、代表慣導(dǎo)系統(tǒng)機(jī)體系下三個(gè)陀螺的白噪聲，、、代表慣導(dǎo)系統(tǒng)機(jī)體系下三個(gè)陀螺一階馬爾科夫誤差的驅(qū)動(dòng)白噪聲，、、代表慣導(dǎo)系統(tǒng)機(jī)體系下三個(gè)加速度計(jì)一階馬爾科夫誤差的驅(qū)動(dòng)白噪聲，與氣動(dòng)模型相關(guān)的系統(tǒng)噪聲，其中、、代表飛行器受到的力的誤差，、、代表飛行器所受到的力矩的誤差；

所述狀態(tài)轉(zhuǎn)換矩陣為，，

與慣性導(dǎo)航相關(guān)的部分，其中，表達(dá)式如下：

，

和為地球子午圈和卯酉圈主曲率半徑，，，為地球橢圓度，為地球赤道半徑，為地球自轉(zhuǎn)角速度，、、分別代表慣性導(dǎo)航系統(tǒng)解算出的飛行器在地理系下軸、軸和軸方向的速度，為慣性導(dǎo)航系統(tǒng)解算出的飛行器緯度，為慣性導(dǎo)航系統(tǒng)解算出的飛行器高度，

，，

，

，，

，；

，其中為慣性導(dǎo)航解算出的機(jī)體系到地理系的轉(zhuǎn)換矩陣，，其中、、為慣導(dǎo)系統(tǒng)機(jī)體系下三個(gè)陀螺一階馬爾科夫誤差的相關(guān)時(shí)間，、、為慣導(dǎo)系統(tǒng)機(jī)體系下三個(gè)加速度計(jì)一階馬爾科夫誤差的相關(guān)時(shí)間，

與氣動(dòng)模型相關(guān)的部分，表達(dá)式如下：，，，，，，，，，

其中、、分別表示氣動(dòng)模型解算的飛行器機(jī)體系下軸、軸和軸方向的速度，、、分別表示氣動(dòng)模型解算的飛行器機(jī)體系下軸、軸和軸方向的角速度，、分別表示氣動(dòng)模型解算的飛行器的橫滾角、俯仰角兩個(gè)姿態(tài)角；

所述系統(tǒng)噪聲系數(shù)矩陣為，，其中

，為慣性導(dǎo)航解算出的機(jī)體系到地理系的轉(zhuǎn)換矩陣：；

（b）卡爾曼濾波器量測(cè)方程的建立

根據(jù)飛行器的氣動(dòng)模型特點(diǎn)，選取姿態(tài)與速度為觀測(cè)量，卡爾曼濾波器的量測(cè)方程為，量測(cè)量為

，其中為氣動(dòng)模型求解的飛行器在地理系下的速度，為慣導(dǎo)系統(tǒng)求解的飛行器在地理系下的速度，為氣動(dòng)模型求解的飛行器姿態(tài)角，為慣導(dǎo)系統(tǒng)求解的飛行器姿態(tài)角；

在濾波方程中，狀態(tài)量為慣導(dǎo)系統(tǒng)的平臺(tái)誤差角，為了與氣動(dòng)模型的狀態(tài)量相統(tǒng)一，需將其轉(zhuǎn)換為慣導(dǎo)系統(tǒng)的姿態(tài)誤差角，其轉(zhuǎn)換關(guān)系式為

，其中、、為慣導(dǎo)系統(tǒng)解算的姿態(tài)誤差角，而狀態(tài)量為慣導(dǎo)系統(tǒng)求解的飛行器在地理系下的速度誤差，狀態(tài)量為氣動(dòng)模型求解的行器在機(jī)體系下的速度誤差，因此為使二者一致，將轉(zhuǎn)換到地理系下，綜上，得到，其中為氣動(dòng)模型解算的機(jī)體系到地理系的轉(zhuǎn)換矩陣，

，其中為量測(cè)噪聲；

（c）連續(xù)型線性方程離散化

取采樣周期對(duì)步驟(a)與步驟(b)中得到的連續(xù)型線性方程進(jìn)行離散化得到離散型線性方程：

式中，為時(shí)刻的狀態(tài)最優(yōu)估計(jì)值，為時(shí)刻的狀態(tài)最優(yōu)估計(jì)值，為狀態(tài)變量從時(shí)刻到時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，為時(shí)刻的系統(tǒng)噪聲，為時(shí)刻的系統(tǒng)噪聲對(duì)時(shí)刻狀態(tài)影響的噪聲系數(shù)矩陣，為時(shí)刻的觀測(cè)值，為時(shí)刻的觀測(cè)噪聲，，為步驟(a)中的狀態(tài)轉(zhuǎn)換矩陣在時(shí)刻的值，，為步驟(a)中的系統(tǒng)噪聲系數(shù)矩陣在時(shí)刻的值，，為步驟(b)中的量測(cè)矩陣在時(shí)刻的值；

（d）卡爾曼濾波方程

利用時(shí)刻的狀態(tài)最優(yōu)估計(jì)值，根據(jù)求取的一步預(yù)測(cè)值，其中根據(jù)步驟(c)得到，中的系數(shù)由步驟(2)、步驟(3)、步驟(4)得到；

通過式求解時(shí)刻狀態(tài)量的一步預(yù)測(cè)值的方差陣；通過式求解時(shí)刻濾波增益矩陣；

根據(jù)步驟(2)獲取慣導(dǎo)系統(tǒng)輸出的飛行器速度與姿態(tài)信息，以及步驟(4)獲取氣動(dòng)模型輸出的飛行器速度與姿態(tài)信息，組成時(shí)刻總的量測(cè)量，結(jié)合求解得到的時(shí)刻狀態(tài)量的一步預(yù)測(cè)值和濾波增益矩陣，利用公式對(duì)時(shí)刻狀態(tài)最優(yōu)估計(jì)值進(jìn)行求解；

通過式對(duì)時(shí)刻狀態(tài)最優(yōu)估計(jì)值的誤差方差陣進(jìn)行求解；

(6)慣導(dǎo)系統(tǒng)誤差修正，

利用步驟(5)得到的時(shí)刻狀態(tài)最優(yōu)估計(jì)值對(duì)慣導(dǎo)系統(tǒng)輸出的姿態(tài)、位置、速度進(jìn)行修正；

慣導(dǎo)系統(tǒng)修正速度值為，其中為修正后的慣導(dǎo)系統(tǒng)速度，為步驟(2)中得到的慣性導(dǎo)航系統(tǒng)解算出的飛行器在地理系下速度，為步驟(5)中解算出的的部分狀態(tài)量；

慣導(dǎo)系統(tǒng)修正位置為，其中為修正后的慣導(dǎo)系統(tǒng)速度，為步驟(2)中得到的慣性導(dǎo)航系統(tǒng)解算出的飛行器的經(jīng)度、緯度、高度信息，為步驟(5)中解算出的的部分狀態(tài)量；

以、、表示步驟(2)中得到的慣性導(dǎo)航系統(tǒng)解算出的飛行器的橫滾角、俯仰角、航向角信息，則慣導(dǎo)系統(tǒng)解算的機(jī)體系到地理系的轉(zhuǎn)換矩陣為

，為步驟(5)中解算出的的部分狀態(tài)量，則補(bǔ)償矩陣為，則修正后的機(jī)體系到地理系的轉(zhuǎn)換矩陣為，則補(bǔ)償后的橫滾角，俯仰角，航向角。