專利名稱：微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定方法
技術(shù)領(lǐng)域：
本發(fā)明屬于微小型飛行器的運(yùn)動(dòng)參數(shù)測(cè)量系統(tǒng)技術(shù)領(lǐng)域。
背景技術(shù)：
微小型飛行器在近距離軍事偵察、目標(biāo)搜索，災(zāi)害監(jiān)測(cè)等領(lǐng)域有特殊的使用價(jià)值和廣闊的前景。微小型飛行器的導(dǎo)航、制導(dǎo)和控制系統(tǒng)組成的閉環(huán)回路的核心功能是對(duì)微型飛行器的狀態(tài)進(jìn)行測(cè)量與控制，使其能夠達(dá)到自主飛行的目的。微小型飛行器姿態(tài)確定系統(tǒng)是微型導(dǎo)航系統(tǒng)的子系統(tǒng)，用于實(shí)時(shí)的測(cè)量微型飛行器的姿態(tài)，并將測(cè)定的姿態(tài)信息反饋到微型飛行器的控制器。微小型飛行器姿態(tài)確定系統(tǒng)為姿態(tài)穩(wěn)定和飛行控制算法提供微小型飛行器的姿態(tài)信息，是微小型飛行器中的關(guān)鍵技術(shù)之一，是微小型飛行器自主飛行器的重要條件。
微小型飛行器慣性組合姿態(tài)確定系統(tǒng)是采用MEMS(Micro Electro-MechanicalSystem)慣性傳感器(微陀螺、微加速度計(jì))的微型捷聯(lián)慣性導(dǎo)航系統(tǒng)。采用MEMS技術(shù)的硅微陀螺和加速度計(jì)體積小，重量輕，是目前唯一可滿足微型飛行器重量與體積要求的慣性傳感器。捷聯(lián)慣性導(dǎo)航系統(tǒng)(Strapdown Inertial Navigation System)是一種基于牛頓運(yùn)動(dòng)定律以推算的方式實(shí)現(xiàn)導(dǎo)航功能的系統(tǒng)，其核心傳感器是由陀螺儀(角運(yùn)動(dòng)傳感器)和加速度計(jì)(線運(yùn)動(dòng)傳感器)等兩類慣性傳感器構(gòu)成的測(cè)量單元，由上述兩類傳感器構(gòu)成的測(cè)量單元直接固聯(lián)在運(yùn)動(dòng)載體上。捷聯(lián)慣性導(dǎo)航系統(tǒng)中的處理器通過模擬/數(shù)字轉(zhuǎn)換電路或者直接采用數(shù)字接口，讀取上述測(cè)量單元內(nèi)角運(yùn)動(dòng)和線運(yùn)動(dòng)傳感器的數(shù)據(jù)，按慣性導(dǎo)航算法的原理流程，對(duì)原始的測(cè)量數(shù)據(jù)進(jìn)行加工，解算出運(yùn)動(dòng)載體的姿態(tài)、速度和位置等參數(shù)。
MEMS慣性傳感器測(cè)量誤差大，基于MEMS慣性傳感器的微慣性導(dǎo)航系統(tǒng)的姿態(tài)誤差會(huì)快速發(fā)散，難以滿足微小型飛行器自主飛行的需求。因此針對(duì)微小型飛行器的微型慣性導(dǎo)航系統(tǒng)中，需要引入誤差不隨時(shí)間增長的姿態(tài)信息，來抑制微型慣導(dǎo)系統(tǒng)姿態(tài)誤差發(fā)散。對(duì)微型慣導(dǎo)系統(tǒng)的誤差發(fā)散的抑制是微型慣性組合導(dǎo)航系統(tǒng)在微小型飛行器中應(yīng)用的關(guān)鍵，國內(nèi)外在此方面的提出的方法主要有利用加速度計(jì)作為傾角傳感器采用加權(quán)的方式，與微慣導(dǎo)系統(tǒng)的姿態(tài)融合如2004年8月，美國伯明翰揚(yáng)大學(xué)的碩士學(xué)位論文Design Of An Autopilot For Small Unmanned Aerial Vehicles，75-78；本專利發(fā)明人于2006年第6期應(yīng)用科學(xué)學(xué)報(bào)論文“MEMS-INS微型飛行器姿態(tài)確定系統(tǒng)的實(shí)現(xiàn)研究”提出利用現(xiàn)代最優(yōu)狀態(tài)估計(jì)方法——卡爾曼濾波實(shí)現(xiàn)微慣性導(dǎo)航系統(tǒng)與加速度計(jì)獲取水平姿態(tài)進(jìn)行融合的方法等。
加速度計(jì)傾角傳感器受載體動(dòng)態(tài)的影響，已有文獻(xiàn)的常規(guī)思路是對(duì)加速度計(jì)傾角傳感器的使用進(jìn)行約束，根據(jù)慣性測(cè)量單元IMU和航姿系統(tǒng)的信息對(duì)載體的動(dòng)態(tài)過程進(jìn)行判斷，當(dāng)載體動(dòng)態(tài)過程超過一定范圍后，不再利用加速度計(jì)傾角傳感器進(jìn)行姿態(tài)測(cè)量，由于微小型飛行器轉(zhuǎn)動(dòng)慣量小，機(jī)動(dòng)靈活，受氣流的影響嚴(yán)重，姿態(tài)變化范圍和變化速率遠(yuǎn)大于一般無人機(jī)，這一方式不適用于微小型飛行器。

發(fā)明內(nèi)容
本發(fā)明的主要目的是，改進(jìn)已有文獻(xiàn)中對(duì)動(dòng)態(tài)條件下加速度計(jì)傾角傳感器誤差處理方法的不足，在導(dǎo)航、制導(dǎo)與控制(GNC)閉環(huán)條件下，從GNC回路中提取關(guān)鍵特征信息，探索適應(yīng)于微小型飛行器特點(diǎn)的新途徑，抑制微小型飛行器的動(dòng)態(tài)飛行中由加速度計(jì)傾角傳感器導(dǎo)致的姿態(tài)誤差。
本發(fā)明的內(nèi)容是微小型飛行器的MEMS微慣性組合導(dǎo)航系統(tǒng)利用其所在的微小型飛行器GNC閉環(huán)回路中的制導(dǎo)信息實(shí)現(xiàn)姿態(tài)確定方法，其特點(diǎn)在研通過以下步驟實(shí)現(xiàn)(1)微小型飛行器GNC閉環(huán)回路的制導(dǎo)算法解算步驟根據(jù)微小型飛行器當(dāng)前的位置和期望航路點(diǎn)的位置，計(jì)算出期望的飛行方向，將期望的飛行方向與當(dāng)前飛行方向相減，所得的差乘以一系數(shù)k，得到期望的微小型飛行器橫滾角，其中系數(shù)k可取范圍是0.2到0.5；(2)MEMS慣性測(cè)量單元信號(hào)采集步驟采集MEMS慣性測(cè)量單元的輸出信號(hào)，得到微小型飛行器的角速度和比力；(3)捷聯(lián)慣導(dǎo)算法解算步驟將步驟(2)采集到的角速度和比力信號(hào)，按捷聯(lián)慣性導(dǎo)航算法的流程，解算出微小型飛行器的姿態(tài)、速度和位置的導(dǎo)航信息；(4)獲取位置和姿態(tài)的測(cè)量信息步驟讀取GPS的位置、速度和航向信息，同時(shí)利用最近時(shí)刻采集到的比力信號(hào)，按加速度計(jì)傾角傳感器的測(cè)量原理計(jì)算微小型飛行器的橫滾角與俯仰角；(5)卡爾曼濾波器觀測(cè)矩陣的自適應(yīng)調(diào)整步驟以步驟(1)得到的期望的橫滾角為自變量，按一分段函數(shù)，實(shí)時(shí)計(jì)算卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差，實(shí)時(shí)計(jì)算的卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差與其初值之間保持一倍數(shù)關(guān)系，這一倍數(shù)是分段函數(shù)的因變量，該分段函數(shù)的具體函數(shù)關(guān)系通過實(shí)際的飛行試驗(yàn)來確定；(6)組合濾波及修正步驟利用步驟(5)實(shí)時(shí)調(diào)整觀測(cè)矩陣參數(shù)的卡爾曼濾波器，對(duì)步驟(4)獲取的測(cè)量信息進(jìn)行處理，估計(jì)步驟(2)捷聯(lián)慣導(dǎo)解算出的姿態(tài)、速度和位置的誤差，在步驟(2)獲得的姿態(tài)、速度和位置中，扣除本步驟估計(jì)出的誤差，并替換步驟(2)的姿態(tài)、速度和位置信息；
(7)姿態(tài)、位置信息的反饋步驟步驟(6)獲得的姿態(tài)信息輸入到微小型飛行器的控制模塊，控制模塊通過微小型飛行器的舵面控制微小型飛行器的姿態(tài)變化；步驟(6)獲得的位置和航向信息輸入到制導(dǎo)模塊，用于完成步驟(1)。
本發(fā)明從導(dǎo)航、制導(dǎo)于控制閉環(huán)系統(tǒng)的角度入手，基于卡爾曼濾波最有加強(qiáng)平均的本質(zhì)，結(jié)合對(duì)微小型飛行器的飛行狀態(tài)的判別，通過動(dòng)態(tài)調(diào)整微慣性組合導(dǎo)航系統(tǒng)卡爾曼濾波器的觀測(cè)噪聲矩陣的方法，提高微慣性組合導(dǎo)航系統(tǒng)姿態(tài)的精度，減弱微小型飛行器動(dòng)態(tài)飛行中傾角傳感器對(duì)系統(tǒng)姿態(tài)的修正量，減弱動(dòng)態(tài)飛行中導(dǎo)航、制導(dǎo)與控制回路的姿態(tài)修正等效干擾，提高微小型飛行器的飛行品質(zhì)。
四

圖1是微型飛行器導(dǎo)航、制導(dǎo)與控制系統(tǒng)原理框圖。
圖2是本發(fā)明的微型導(dǎo)航、制導(dǎo)與控制系統(tǒng)閉環(huán)回路框圖。
圖3是圖2中MEMS微慣性組合導(dǎo)航系統(tǒng)組成框圖。
圖4現(xiàn)有技術(shù)中微小型飛行器自主飛行的橫滾角曲線示意圖。
圖5是本發(fā)明微小型飛行器自主飛行的橫滾角曲線示意圖。
五具體實(shí)施例方式
本發(fā)明的原理是傳統(tǒng)的GNC閉環(huán)回路中，導(dǎo)航系統(tǒng)提供導(dǎo)航參數(shù)反饋給制導(dǎo)與飛行控制算法，如圖1中各模塊和實(shí)線構(gòu)成的原理框圖所示。本發(fā)明則在傳統(tǒng)GNC回路的基礎(chǔ)上，增加從制導(dǎo)模塊到導(dǎo)航子系統(tǒng)的前饋控制，如圖1的虛線所示，利用制導(dǎo)信息，控制組合卡爾曼濾波器噪聲矩陣，實(shí)現(xiàn)卡爾曼濾波器觀測(cè)噪聲陣隨飛行狀態(tài)的自適應(yīng)調(diào)整，通過參數(shù)調(diào)整，改進(jìn)微慣性組合導(dǎo)航系統(tǒng)對(duì)飛行狀態(tài)的自適應(yīng)性，實(shí)現(xiàn)已有的微小型飛行器微慣性組合導(dǎo)航系統(tǒng)在動(dòng)態(tài)飛行條件下的姿態(tài)確定，提高動(dòng)態(tài)飛行條件下的姿態(tài)精度和飛行的穩(wěn)定性。
為了實(shí)現(xiàn)微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定，適應(yīng)動(dòng)態(tài)的飛行條件，需要完成工作(1)微小型飛行器GNC閉環(huán)回路的制導(dǎo)算法解算微小型飛行器根據(jù)當(dāng)前的位置和期望航路點(diǎn)的位置，計(jì)算出期望的航向角ψe，將期望的航向角與微導(dǎo)航系統(tǒng)當(dāng)前實(shí)際測(cè)量得到的航向角ψ相減，所得的差乘以一系數(shù)k，得到期望的微小型飛行器橫滾角，k為比列系數(shù)，可取值范圍是0.2到0.5。
以微小型飛行器起飛點(diǎn)為原點(diǎn)，以北向?yàn)閤軸正方向，以東向?yàn)閥軸正方向，建立當(dāng)?shù)卮蟮厮阶鴺?biāo)系，在該坐標(biāo)系下，航向角以沿x軸正方向?yàn)?度，轉(zhuǎn)向右側(cè)則航向角增大，航向角的范圍為(0，360)度，微小型飛行器的實(shí)時(shí)位置坐標(biāo)為(X，Y)，當(dāng)前期望飛到的航路點(diǎn)坐標(biāo)表示為(Px，Py)，則期望的飛行方向按下式計(jì)算當(dāng)Px＞X，Py≥Y，ψe=tg-1Py-YPx-X;]]>當(dāng)Px＝X，Py＞Y，ψe＝π/2；當(dāng)Px＜X，ψe=tg-1Py-YPx-X+π;]]>當(dāng)Px＝X，Py＜Y，ψe＝3π/2；當(dāng)Px＞X，Py＜Yψe=tg-1Py-YPx-X+2π;]]>制導(dǎo)算法按下式生成期望的橫滾角γeγe＝k(ψe-ψ)，其中ψ為微慣性組合導(dǎo)航系統(tǒng)實(shí)際測(cè)量得到的航向角。
(2)MEMS慣性測(cè)量單元信號(hào)采集步驟采集MEMS慣性測(cè)量單元的輸出信號(hào)，得到微小型飛行器的角速度和比力；(3)捷聯(lián)慣導(dǎo)算法解算利用采集到的角速度和比力信號(hào)，按捷聯(lián)慣性導(dǎo)航算法的流程，解算出微小型飛行器的姿態(tài)、速度和位置等導(dǎo)航信息。捷聯(lián)慣導(dǎo)解算的初始姿態(tài)、速度和位置是由外部輸入的。
機(jī)體坐標(biāo)系相對(duì)于當(dāng)?shù)氐乩碜鴺?biāo)系的角速度的在機(jī)體系的三個(gè)軸的分量依次為繞橫滾軸的角速度ωx，繞俯仰軸的角速度ωy，繞方位軸的角速度ωz，捷聯(lián)慣導(dǎo)的計(jì)算周期為Δt，Δt時(shí)間內(nèi)機(jī)體轉(zhuǎn)過的角度Δθ的三個(gè)分量依次依次為橫滾軸角增量Δθx，俯仰軸角增量Δθy，方位軸角增量Δθz，則轉(zhuǎn)過的角度以矩陣方式表示為
Δθ=0-ΔθzΔθyΔθz0-Δθx-ΔθyΔθx0=0-ωzωyωz0-ωx-ωyωx0Δt]]>t時(shí)刻的載體的姿態(tài)矩陣為Cnb(t)=cosγcosψ+sinγsinθsinψ-cosγsinψ+sinγsinθcosψ-sinγcosθcosθsinψcosθcosψsinθsinγcosψ-cosγsinθsinψ-sinγsinψ-cosγsinθcosψcosγcosθ]]>那么t+Δt時(shí)刻姿態(tài)矩陣由Cnb(t+Δt)=-Δθ·Cnb(t)]]>從t時(shí)刻的姿態(tài)陣遞推求得，將Cnb(t+Δt)簡寫為T11T12T13T21T22T23T31T32T33,]]>則t+Δt時(shí)刻，捷聯(lián)慣導(dǎo)的俯仰角θ、橫滾角γ和航向角ψ等姿態(tài)信息由下式獲得。
θ=sin-1(T23)γ=tg-1(T13T33)ψ=tg-1(T21T32)]]>在姿態(tài)解算的基礎(chǔ)上，根據(jù)當(dāng)前的姿態(tài)和比力，獲得當(dāng)?shù)氐乩碜鴺?biāo)系中的運(yùn)動(dòng)加速度aN，aE，aD，其中，aN是北向的運(yùn)動(dòng)加速度，aE東向的運(yùn)動(dòng)加速度，aD地向的運(yùn)動(dòng)加速度。微小型飛行器的速度和位置由下式遞推得到，VN(t)表示t時(shí)刻微小型飛行器北向的速度，VE(t)表示t時(shí)刻微小型飛行器東向的速度，VD(t)表示t時(shí)刻微小型飛行器地向的速度，VN(t+Δt)，VE(t+Δt)，VD(t+Δt)分別表示t+Δt時(shí)刻北向、東向和地向的速度，R表示地球半徑，用符號(hào)緯度LI，經(jīng)度λI和高度hI分別表示由捷聯(lián)慣導(dǎo)算法得到的微小型飛行器的位置，則捷聯(lián)慣導(dǎo)的速度和位置可計(jì)算如下。
VN(t+Δt)=VN(t)+aNΔtVE(t+Δt)=VE(t)+aEΔtVD(t+Δt)=VD(t)+aDΔt]]>LI(t+Δt)=LI(t)+VN(t)ΔtRλI(t+Δt)=λI(t)+VE(t)ΔtRcos(LI(t))hI(t+Δt)=hI(t)-VD(t)Δt]]>(4)獲取位置和姿態(tài)的測(cè)量信息讀取GPS的位置(緯度LG、經(jīng)度λG和高度hG)和航向角ψG，同時(shí)利用最近時(shí)刻采集到的比力信號(hào)，按加速度計(jì)傾角傳感器的測(cè)量原理計(jì)算微小型飛行器的橫滾角γA與俯仰角θA。
(5)卡爾曼濾波器觀測(cè)矩陣的自適應(yīng)調(diào)整步驟以步驟(1)得到的期望的橫滾角γe為自變量，按一分段函數(shù)，t+1時(shí)刻的實(shí)時(shí)計(jì)算卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差Rt+1，實(shí)時(shí)計(jì)算的卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差與其初值之間保持一倍數(shù)關(guān)系，該倍數(shù)關(guān)系具有拋物線形式，這一倍數(shù)是分段函數(shù)的因變量，分段函數(shù)具有以下形式 分段函數(shù)的具體函數(shù)關(guān)系通過實(shí)際的飛行試驗(yàn)來確定，R0是水平姿態(tài)方差的初值，根據(jù)加速度計(jì)的測(cè)量噪聲大小確定，kR是不同狀態(tài)下拋物線形式的倍數(shù)關(guān)系的公共成分，通過試驗(yàn)獲得。
(6)組合濾波及修正步驟利用步驟(5)實(shí)時(shí)調(diào)整觀測(cè)矩陣參數(shù)的卡爾曼濾波器，對(duì)步驟(4)獲取的測(cè)量信息進(jìn)行處理，估計(jì)步驟(2)捷聯(lián)慣導(dǎo)解算出的姿態(tài)、速度和位置的誤差，在步驟(2)獲得的姿態(tài)、速度和位置中，扣除本步驟估計(jì)出的誤差，并替換步驟(2)的姿態(tài)、速度和位置信息。
該卡爾曼濾波器的特征在于濾波器的觀測(cè)噪聲矩陣是由步驟(5)實(shí)時(shí)確定的，狀態(tài)變量X包括捷聯(lián)慣導(dǎo)系統(tǒng)的北向平臺(tái)誤差角φN、東向平臺(tái)誤差角φE和地向平臺(tái)誤差角φD，北向速度誤差δvN、東向速度誤差δvE和地向速度誤差δvD，緯度誤差δL，經(jīng)度誤差δλ、和高度誤差δh，共9個(gè)，即X＝[φNφEφDδvNδvEδvDδL δλ δh]T，濾波器以平臺(tái)誤差角和位置誤差為觀測(cè)量，觀測(cè)變量為6個(gè)。
對(duì)步驟(4)獲取的測(cè)量信息進(jìn)行處理如下設(shè)t+1為濾波時(shí)刻，此時(shí)由GPS航向角ψG，傾角傳感器測(cè)得的橫滾角γA和俯仰角θA，可得到姿態(tài)測(cè)量信息確定的姿態(tài)矩陣Cn″bCn′′b=cosγAcosψG+sinγAsinθAsinψG-cosγAsinψG+sinγAsinθAcosψG-sinγAcosθAcosθAsinψGcosθAcosψGsinθAsinγAcosψG-cosγAsinθAsinψG-sinγAsinψG-cosγsinθAcosψGcosγAcosθA]]>由姿態(tài)矩陣Cn″b確定的數(shù)學(xué)平臺(tái)的三個(gè)平臺(tái)誤差角記為北向平臺(tái)誤差角φNA，東向平臺(tái)誤差角φEA，地向平臺(tái)誤差角φDG。
利用此時(shí)刻捷聯(lián)慣導(dǎo)的姿態(tài)矩陣Cnb(t+1)的轉(zhuǎn)置與Cn″b相乘，得到捷聯(lián)慣導(dǎo)系統(tǒng)的姿態(tài)陣對(duì)應(yīng)的數(shù)學(xué)平臺(tái)誤差角[φNφEφD]T與步驟(4)測(cè)量得到的姿態(tài)及姿態(tài)陣對(duì)應(yīng)的數(shù)學(xué)平臺(tái)誤差角[φNAφEAφDG]T之間的差，即Cnb(t+1)TCn′′b=1(φD-φDG)-(φE-φEA)-(φD-φDG)1(φN-φNA)(φE-φEA)-(φN-φNA)1]]>
卡爾曼濾波的平臺(tái)誤差角觀測(cè)量為φN-φNAφE-φEAφD-φDG;]]>捷聯(lián)慣導(dǎo)解算得到的微小型飛行器的位置與GPS得到的微小型飛行器的位置相減，得到位置的觀測(cè)量(LI-LG)R(λI-λG)RcosLhI-hG,]]>將平臺(tái)誤差角觀測(cè)量和位置觀測(cè)量合并為t+1時(shí)刻卡爾曼濾波器的總的觀測(cè)量Zt+1，則Zt+1=φN-φNAφE-φEAφD-φDG(LI-LG)R(λI-λG)RhI-hG.]]>定義測(cè)量矩陣Ht+1=100000000010000000001000000000000R000000000RcosL0000000001,]]>卡爾曼濾波器中，狀態(tài)向量為9維，狀態(tài)預(yù)測(cè)的方差陣P是9×9的矩陣，初始估值誤差方差陣初值P0，系統(tǒng)噪聲方差陣初值Q0觀測(cè)噪聲方差陣初值R0等矩陣初值都由系統(tǒng)外部直接輸入，狀態(tài)轉(zhuǎn)移φt+1，t是已有文獻(xiàn)中已知矩陣，系統(tǒng)噪聲矩陣Qt由統(tǒng)噪聲方差陣初值Q0確定，觀測(cè)噪聲矩陣Rt+1由步驟(5)計(jì)算得到。
則各濾波時(shí)刻的狀態(tài)估值的方差陣及狀態(tài)預(yù)測(cè)的方差陣都可以遞推得到。即在濾波時(shí)刻t的Pt基礎(chǔ)上，按如下遞推濾波方程，得到濾波時(shí)刻t+1的狀態(tài)預(yù)測(cè)的方差陣Pt+1|t，濾波器增益矩陣Kt+1，狀態(tài)估計(jì)值 及狀態(tài)估值的方差陣Pt+1可由下式計(jì)算得到。
Qt＝Q0Pt+1|t=φt+1,tPtφt+1,tT+Qt]]>Kt+1＝Pt+1|tHt+1T(Ht+1Pt+1|tHt+1T+Rt+1)-1X^t+1=Kt+1Zt+1]]>Pt+1＝(I-Kt+1Ht+1)Pt+1|t上述計(jì)算的狀態(tài)估計(jì)值 是步驟(4)捷聯(lián)慣導(dǎo)所得到得到的姿態(tài)、速度和位置的誤差，在步驟(4)所得到的結(jié)果中，減去估計(jì)值 中的對(duì)應(yīng)的量，即可提高微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)的精度，達(dá)到確定微小型飛行器姿態(tài)的目的。
(7)姿態(tài)、位置信息的反饋步驟步驟(6)獲得的姿態(tài)信息輸入到微小型飛行器的控制模塊，控制模塊通過微小型飛行器的舵面控制微小型飛行器的姿態(tài)變化；步驟(6)獲得的位置和航向信息輸入到制導(dǎo)模塊，用于完成步驟(1)。
這樣，通過動(dòng)態(tài)調(diào)整微慣性組合導(dǎo)航系統(tǒng)卡爾曼濾波器的觀測(cè)噪聲矩陣的方法，提高微慣性組合導(dǎo)航系統(tǒng)姿態(tài)的精度，減弱微小型飛行器動(dòng)態(tài)飛行中傾角傳感器對(duì)系統(tǒng)姿態(tài)的修正量，減弱動(dòng)態(tài)飛行中導(dǎo)航、制導(dǎo)與控制回路的姿態(tài)修正等效干擾，提高微小型飛行器的飛行品質(zhì)。
圖1中，各模塊的及連接的實(shí)線箭頭代表了導(dǎo)航、制導(dǎo)與控制閉環(huán)回路的基本的邏輯連接關(guān)系，從微小型飛行器制導(dǎo)算法模塊和微小型飛行器飛行控制算法之間引出的指向微慣性組合導(dǎo)航系統(tǒng)的虛線箭頭代表的是本發(fā)明的期望的橫滾角的傳遞和利用關(guān)系。
圖2是對(duì)圖1的進(jìn)一步細(xì)化，位置比較模塊和航向控制模塊構(gòu)成圖1中的微小型飛行器制導(dǎo)算法模塊，期望俯仰模塊、俯仰控制模塊和橫滾控制模塊共同組成圖1中的微小型飛行器飛行控制算法模塊。虛線箭頭與圖1中相同，更清晰的表明了信息的來源和傳遞。
圖3是圖2中MEMS微慣性組合導(dǎo)航系統(tǒng)的進(jìn)一步細(xì)化，表明了圖2中虛線代表的期望的橫滾角γe對(duì)微慣性組合導(dǎo)航系統(tǒng)卡爾曼濾波器觀測(cè)噪聲矩陣Rt+1的控制調(diào)整。
圖4是采用現(xiàn)有技術(shù)情況下微型飛行器左盤旋飛行時(shí)的橫滾角數(shù)據(jù)曲線，橫軸代表時(shí)間，縱軸代表橫滾角，315秒到330秒一直處于向左盤旋，橫滾角的波動(dòng)范圍-2度到-32度，飛行中橫向波動(dòng)范圍大，達(dá)到了30度。
圖5是應(yīng)用本發(fā)明后，微型飛行器重復(fù)左、右盤旋的數(shù)據(jù)曲線，曲線反映的飛行過程80秒后，5次左盤旋，4次右盤旋，左、右盤旋相互交替，應(yīng)用本發(fā)明后，左盤旋飛行中，橫滾角的波動(dòng)范圍約在-18度到-30度，右盤旋飛行時(shí)橫滾角的波動(dòng)范圍18度到30度。應(yīng)用本發(fā)明后，波動(dòng)范圍明顯減小，約12度，誤差縮小了60％。
六、發(fā)明的效果本發(fā)明從導(dǎo)航、制導(dǎo)與控制閉環(huán)系統(tǒng)的角度入手，基于卡爾曼濾波最優(yōu)加權(quán)平均的本質(zhì)，結(jié)合對(duì)微小型飛行器的飛行狀態(tài)判別，通過動(dòng)態(tài)調(diào)整微型組合導(dǎo)航系統(tǒng)卡爾曼濾波器的觀測(cè)噪聲R的方法，提高微慣性組合導(dǎo)航系統(tǒng)姿態(tài)的精度，來減弱微小型飛行器動(dòng)態(tài)飛行中傾角傳感器對(duì)系統(tǒng)姿態(tài)的觀測(cè)修正量，減弱GNC回路的姿態(tài)修正等效干擾，提高飛行品質(zhì)。
盤旋飛行試驗(yàn)效果對(duì)于微小型飛行器自主飛行中的微慣性組合導(dǎo)航系統(tǒng)，微小型飛行器盤旋轉(zhuǎn)彎飛行是要經(jīng)常經(jīng)歷的最惡劣的飛行狀態(tài)。采用本發(fā)明前后的盤旋飛行試驗(yàn)中，兩組橫滾角對(duì)比曲線如圖4和圖5，從曲線上可以看出，采用本發(fā)明前，盤旋飛行中，微小型飛行器的姿態(tài)圍繞著期望值的變化范圍30度，采用本發(fā)明后，微小型飛行器自主飛行中的姿態(tài)圍繞著期望值的波動(dòng)范圍小于12度，波動(dòng)明顯減小，飛行的曲線與期望值之間的偏差明顯減小，誤差縮小了60％。
本發(fā)明針對(duì)動(dòng)態(tài)飛行中傾角傳感器引入的測(cè)量誤差，利用動(dòng)態(tài)調(diào)整卡爾曼濾波器觀測(cè)噪聲的方法，改進(jìn)微慣性組合導(dǎo)航系統(tǒng)的性能，減小測(cè)量誤差，提高微小型飛行器的飛行平穩(wěn)度。
大量試飛試驗(yàn)結(jié)果表明本發(fā)明應(yīng)用于微型飛行器，構(gòu)成導(dǎo)航、制導(dǎo)與控制閉環(huán)系統(tǒng)，微型飛行器能夠?qū)崿F(xiàn)5-6米/秒的陣風(fēng)環(huán)境下的自主姿態(tài)穩(wěn)定和航路點(diǎn)的導(dǎo)航飛行，微小型飛行器的自主飛行性能達(dá)到了期望的水平。本發(fā)明具有很強(qiáng)的工程應(yīng)用價(jià)值。
權(quán)利要求
1.一種微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定方法，其特點(diǎn)在于通過以下步驟實(shí)現(xiàn)(1)微小型飛行器GNC閉環(huán)回路的制導(dǎo)算法解算步驟根據(jù)微小型飛行器當(dāng)前的位置和期望航路點(diǎn)的位置，計(jì)算出期望的飛行方向，將期望的飛行方向與當(dāng)前飛行方向相減，所得的差乘以一系數(shù)k，得到期望的微小型飛行器橫滾角，其中系數(shù)k可取范圍是0.2到0.5；(2)MEMS慣性測(cè)量單元信號(hào)采集步驟采集MEMS慣性測(cè)量單元的輸出信號(hào)，得到微小型飛行器的角速度和比力；(3)捷聯(lián)慣導(dǎo)算法解算步驟將步驟(2)采集到的角速度和比力信號(hào)，按捷聯(lián)慣性導(dǎo)航算法的流程，解算出微小型飛行器的姿態(tài)、速度和位置的導(dǎo)航信息；(4)獲取位置和姿態(tài)的測(cè)量信息步驟讀取GPS的位置、速度和航向信息，同時(shí)利用最近時(shí)刻采集到的比力信號(hào)，按加速度計(jì)傾角傳感器的測(cè)量原理計(jì)算微小型飛行器的橫滾角與俯仰角；(5)卡爾曼濾波器觀測(cè)矩陣的自適應(yīng)調(diào)整步驟以步驟(1)得到的期望的橫滾角為自變量，按一分段函數(shù)，實(shí)時(shí)計(jì)算卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差，實(shí)時(shí)計(jì)算的卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差與其初值之間保持一倍數(shù)關(guān)系，這一倍數(shù)是分段函數(shù)的因變量，該分段函數(shù)的具體函數(shù)關(guān)系通過實(shí)際的飛行試驗(yàn)來確定；(6)組合濾波及修正步驟利用步驟(5)實(shí)時(shí)調(diào)整觀測(cè)矩陣參數(shù)的卡爾曼濾波器，對(duì)步驟(4)獲取的測(cè)量信息進(jìn)行處理，估計(jì)步驟(2)捷聯(lián)慣導(dǎo)解算出的姿態(tài)、速度和位置的誤差，在步驟(2)獲得的姿態(tài)、速度和位置中，扣除本步驟估計(jì)出的誤差，并替換步驟(2)的姿態(tài)、速度和位置信息；(7)姿態(tài)、位置信息的反饋步驟步驟(6)獲得的姿態(tài)信息輸入到微小型飛行器的控制模塊，控制模塊通過微小型飛行器的舵面控制微小型飛行器的姿態(tài)變化；步驟(6)獲得的位置和航向信息輸入到制導(dǎo)模塊，用于完成步驟(1)。
2.如權(quán)利要求1所述的微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定方法，其特征在于期望的微小型飛行器橫滾角γe的計(jì)算法是以微小型飛行器起飛點(diǎn)為原點(diǎn)，以北向?yàn)閤軸正方向，以東向?yàn)閥軸正方向，建立當(dāng)?shù)卮蟮厮阶鴺?biāo)系，在該坐標(biāo)系下，航向角以沿x軸正方向?yàn)?度，轉(zhuǎn)向右側(cè)則航向角增大，航向角的范圍為(0，360)度，微小型飛行器的實(shí)時(shí)位置坐標(biāo)為(X，Y)，當(dāng)前期望飛到的航路點(diǎn)坐標(biāo)表示為(Px，Py)，則期望的飛行方向按下式計(jì)算當(dāng)Px＞X，Py≥Y，ψe=tg-1Py-YPx-X;]]>當(dāng)Px＝X，Py＞Y，Ψe＝π/2；當(dāng)Px＜X，ψe=tg-1Py-YPx-X+π;]]>當(dāng)Px＝X，Py＜Y，ψe＝3π/2；當(dāng)Px＞X，Py＜Yψe=tg-1Py-YPx-X+2π;]]>制導(dǎo)算法按下式生成期望的橫滾角γeγe＝k(ψe-ψ)，其中ψ為微慣性組合導(dǎo)航系統(tǒng)實(shí)際測(cè)量得到的航向角，k為比列系數(shù)，可取值范圍是0.2到0.5。
3.如權(quán)利要求1所述的微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定方法，其特征在于卡爾曼濾波觀測(cè)矩陣自適應(yīng)調(diào)整方法是以步驟(1)得到的期望的橫滾角γe為自變量，按一分段函數(shù)，t+1時(shí)刻的實(shí)時(shí)計(jì)算卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差Rt+1，實(shí)時(shí)計(jì)算的卡爾曼濾波器的觀測(cè)噪聲矩陣中水平姿態(tài)的方差與其初值之間保持一倍數(shù)關(guān)系，該倍數(shù)關(guān)系具有拋物線形式，這一倍數(shù)是分段函數(shù)的因變量，分段函數(shù)具有以下形式 分段函數(shù)的具體函數(shù)關(guān)系通過實(shí)際的飛行試驗(yàn)來確定，R0是水平姿態(tài)方差的初值，根據(jù)加速度計(jì)的測(cè)量噪聲大小確定，kR是不同狀態(tài)下拋物線形式的倍數(shù)關(guān)系的公共成分，通過試驗(yàn)獲得。。
全文摘要
一種微小型飛行器微慣性組合導(dǎo)航系統(tǒng)的姿態(tài)確定方法，屬微小型飛行器姿態(tài)確定方法。本方法利用制導(dǎo)信息，控制卡爾曼濾波器噪聲矩陣，實(shí)現(xiàn)隨飛行狀態(tài)的自適應(yīng)調(diào)整，通過參數(shù)調(diào)整，實(shí)現(xiàn)微慣性組合導(dǎo)航系統(tǒng)在動(dòng)態(tài)飛行條件下的姿態(tài)確定，具體步驟通過GNC閉環(huán)回路制導(dǎo)算法，得到飛行器橫滾角；通過傳感器采集，獲得飛行器角速度和比力；按捷聯(lián)慣性導(dǎo)航算法解算出飛行器姿態(tài)、速度和位置信息；由加速度計(jì)和GPS數(shù)據(jù)計(jì)算飛行器的姿態(tài)和位置測(cè)量信息；實(shí)時(shí)計(jì)算卡爾曼濾波器的觀測(cè)噪聲矩陣中的水平姿態(tài)方差，通過卡爾曼濾波器估計(jì)捷聯(lián)慣導(dǎo)系統(tǒng)的誤差；對(duì)上述獲得的姿態(tài)、速度和位置信息進(jìn)行修正，并作為反饋信息，輸入到控制系統(tǒng)。
文檔編號(hào)G01C21/10GK101033973SQ20071002140
公開日2007年9月12日 申請(qǐng)日期2007年4月10日 優(yōu)先權(quán)日2007年4月10日
發(fā)明者李榮冰, 劉建業(yè), 賴際舟, 熊智, 孫永榮, 趙偉, 曾慶化, 溫佰仟 申請(qǐng)人:南京航空航天大學(xué)