本發(fā)明涉及一種基于魯棒自適應(yīng)濾波的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，屬于無人機(jī)系統(tǒng)辨識(shí)與自主導(dǎo)航。

背景技術(shù)：

1、在實(shí)際應(yīng)用中，無人機(jī)面臨參數(shù)、結(jié)構(gòu)和環(huán)境擾動(dòng)等因素的不確定性所帶來的影響，需要通過傳感器之間的態(tài)勢(shì)融合以完成對(duì)于自身態(tài)勢(shì)的獲取。承擔(dān)其自主導(dǎo)航功能的傳感器中，慣性導(dǎo)航系統(tǒng)(inertial?navigation?system,ins,以下簡(jiǎn)稱：慣導(dǎo))起到至關(guān)重要的作用，慣導(dǎo)應(yīng)用于無人機(jī)自主導(dǎo)航的測(cè)量過程不依賴于外部信息，依靠加速度計(jì)(以下簡(jiǎn)稱：加計(jì))和陀螺儀(以下簡(jiǎn)稱：陀螺)等慣性元件，分別對(duì)無人機(jī)本體在慣性參考系中的線加速度以及轉(zhuǎn)動(dòng)角加速度進(jìn)行測(cè)量，并通過時(shí)間積分和坐標(biāo)系變換得到無人機(jī)在導(dǎo)航坐標(biāo)系中速度、位置、姿態(tài)等信息。

2、無人機(jī)中的導(dǎo)航傳感器通常由慣導(dǎo)同全球衛(wèi)星定位系統(tǒng)(global?navigationsatellite?system,gnss,以下簡(jiǎn)稱：衛(wèi)星)形成組合導(dǎo)航系統(tǒng)。慣導(dǎo)具備體積小、成本低、功耗低和信息獲取全面等優(yōu)勢(shì)，但若單獨(dú)使用慣導(dǎo)，在不經(jīng)任何對(duì)誤差的預(yù)估、校正和補(bǔ)償?shù)那疤嵯?，慣導(dǎo)輸出的姿態(tài)角、速度和位置信息會(huì)隨時(shí)間積分產(chǎn)生累積誤差，為降低慣導(dǎo)中由時(shí)間積分所引起的誤差累積效應(yīng)，通常需要開展慣導(dǎo)的系統(tǒng)級(jí)標(biāo)定與補(bǔ)償，并通過參數(shù)辨識(shí)等手段獲取慣導(dǎo)等傳感器測(cè)量模型中的誤差特性參數(shù)，以建立精確的傳感器測(cè)量數(shù)學(xué)模型。

3、在研究中，為降低慣性傳感器裝置在使用中的導(dǎo)航測(cè)量偏差，常對(duì)慣導(dǎo)的轉(zhuǎn)位機(jī)構(gòu)進(jìn)行系統(tǒng)級(jí)旋轉(zhuǎn)以模擬imu在飛行機(jī)動(dòng)中的旋轉(zhuǎn)特性，并采用自對(duì)準(zhǔn)等方式對(duì)慣導(dǎo)進(jìn)行校準(zhǔn)，即獲取慣導(dǎo)系統(tǒng)的測(cè)量誤差特性和慣性測(cè)量單元(inertial?measurement?unit,imu)的測(cè)量精度等信息，其本質(zhì)上是對(duì)慣導(dǎo)傳感器的測(cè)量模型進(jìn)行辨識(shí)，以校正自主導(dǎo)航過程中所使用的帶有各類測(cè)量誤差的狀態(tài)信息。慣性傳感器的自對(duì)準(zhǔn)標(biāo)定可使用最小二乘法、卡爾曼濾波法或頻域辨識(shí)等方法完成，需設(shè)置標(biāo)定轉(zhuǎn)序編排并基于轉(zhuǎn)臺(tái)試驗(yàn)采集數(shù)據(jù)進(jìn)行解算和誤差參數(shù)辨識(shí)。

4、多數(shù)已有的研究中是利用卡爾曼濾波所衍生的一些非線性濾波算法通過估計(jì)系統(tǒng)誤差和量測(cè)誤差協(xié)方差以降低組合導(dǎo)航系統(tǒng)的導(dǎo)航誤差，而并未給出慣導(dǎo)等傳感器的測(cè)量誤差特性參數(shù)的估計(jì)結(jié)果，故這些方法雖然在實(shí)際的導(dǎo)航系統(tǒng)中可以用作慣導(dǎo)和衛(wèi)星等傳感器的組合導(dǎo)航算法，但無法給出用于無人機(jī)飛行仿真中的傳感器數(shù)學(xué)模型，不利于實(shí)際試驗(yàn)前的仿真模型搭建工作。而本文所討論的方法，是基于自適應(yīng)理論的卡爾曼濾波衍生算法，能夠在用于慣導(dǎo)和衛(wèi)星形成的組合導(dǎo)航系統(tǒng)的同時(shí)，辨識(shí)出imu和組合導(dǎo)航系統(tǒng)的多個(gè)誤差測(cè)量參數(shù)，以建立傳感器的數(shù)學(xué)模型，支撐高保真度的無人機(jī)飛行仿真。

5、因此，如何設(shè)計(jì)出具有自適應(yīng)估計(jì)性能的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，是亟待解決的問題。

6、若已知無人機(jī)導(dǎo)航傳感器誤差特性的先驗(yàn)信息，可借鑒轉(zhuǎn)臺(tái)的原理設(shè)計(jì)無人機(jī)的機(jī)動(dòng)飛行動(dòng)作并對(duì)飛行數(shù)據(jù)進(jìn)行采樣，從而獲取用來驅(qū)動(dòng)模型辨識(shí)所需的試驗(yàn)數(shù)據(jù)樣本，以激活傳感器在導(dǎo)航過程中各個(gè)自由度的測(cè)量特性，并對(duì)慣導(dǎo)的導(dǎo)航解算過程進(jìn)行阻尼反演，設(shè)計(jì)魯棒自適應(yīng)卡爾曼濾波算法(robust?adaptive?kalman?filter,rakf)，以完成對(duì)傳感器測(cè)量誤差特性的后驗(yàn)估計(jì)，實(shí)現(xiàn)對(duì)于慣導(dǎo)模型參數(shù)的在線辨識(shí)。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明提供了一種基于魯棒自適應(yīng)濾波的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，其目的在于設(shè)計(jì)具有自適應(yīng)估計(jì)性能的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，該方法在無人機(jī)機(jī)動(dòng)飛行軌跡的輸入下，基于魯棒自適應(yīng)濾波算法實(shí)現(xiàn)對(duì)無人機(jī)慣導(dǎo)模型共計(jì)28個(gè)狀態(tài)量的辨識(shí)，搭建支持無人機(jī)慣導(dǎo)模型建模仿真及參數(shù)辨識(shí)的軟件平臺(tái)，實(shí)現(xiàn)具有良好的辨識(shí)精度、實(shí)時(shí)性及可靠性的慣導(dǎo)模型參數(shù)辨識(shí)，以建立可支持無人機(jī)高保真飛行自主導(dǎo)航仿真的慣導(dǎo)模型。

2、本發(fā)明針對(duì)無人機(jī)慣導(dǎo)模型的參數(shù)辨識(shí)問題，發(fā)明了一種基于魯棒自適應(yīng)濾波的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，旨在實(shí)現(xiàn)對(duì)于28個(gè)慣導(dǎo)模型誤差參數(shù)的辨識(shí)。首先建立imu的誤差特性方程，進(jìn)而推導(dǎo)慣導(dǎo)姿態(tài)、速度和位置的誤差傳播微分方程，將慣導(dǎo)測(cè)量信息作為狀態(tài)信息輸入，將gnss導(dǎo)航信息作為量測(cè)信息輸入，基于卡爾曼濾波原理并結(jié)合變分貝葉斯等自適應(yīng)理論機(jī)制，應(yīng)用于慣導(dǎo)及衛(wèi)星組合導(dǎo)航系統(tǒng)中，建立基于rakf估計(jì)算法的慣導(dǎo)模型參數(shù)辨識(shí)框架，提高對(duì)于無人機(jī)慣導(dǎo)模型參數(shù)的辨識(shí)精度。

3、本發(fā)明所設(shè)計(jì)的基于魯棒自適應(yīng)濾波的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法如圖1所示，是一種針對(duì)無人機(jī)慣導(dǎo)所設(shè)計(jì)的辨識(shí)算法框架，將慣導(dǎo)模型的平臺(tái)失準(zhǔn)角、慣導(dǎo)傳感器測(cè)量值相對(duì)實(shí)際飛行狀態(tài)值的速度和位置導(dǎo)航誤差、imu的儀器安裝誤差(以下簡(jiǎn)稱：安裝誤差)、標(biāo)度因數(shù)誤差(以下簡(jiǎn)稱：標(biāo)度誤差)與零位漂移誤差(以下簡(jiǎn)稱：零偏誤差)等誤差特性作為系統(tǒng)狀態(tài)量，將慣導(dǎo)和衛(wèi)星兩種不同傳感器輸出的速度和位置導(dǎo)航信息偏差作為系統(tǒng)觀測(cè)量，使用變分貝葉斯(variational?bayesian,vb)濾波器估計(jì)量測(cè)噪聲協(xié)方差矩陣(measurement?noise?covariance?matrix,mncm)，利用多重漸消因子(multiple?fadingfactor,mff)構(gòu)建強(qiáng)跟蹤濾波器(strong?tracking?filter,stf)，并利用基于加權(quán)滑動(dòng)窗口的suga-husa濾波機(jī)制自適應(yīng)調(diào)整系統(tǒng)噪聲協(xié)方差矩陣(system?state?covariancematrix,pncm)，完成基于rakf的慣導(dǎo)模型參數(shù)辨識(shí)算法設(shè)計(jì)。

4、本發(fā)明在提出參數(shù)辨識(shí)方法的基礎(chǔ)上，還設(shè)計(jì)了支持無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)的仿真模擬軟件平臺(tái)的軟件功能架構(gòu)。該平臺(tái)基于matlab?gui工具開發(fā)，具有無人機(jī)運(yùn)動(dòng)軌跡生成、慣導(dǎo)模型參數(shù)辨識(shí)、結(jié)果存儲(chǔ)與數(shù)據(jù)回放及顯示等功能，其框架架構(gòu)、模塊組成與操作方法如圖2所示。

5、若按功能模塊劃分，該仿真平臺(tái)由模型參數(shù)及輸入設(shè)置模塊、無人機(jī)運(yùn)動(dòng)軌跡生成模塊、辨識(shí)算法設(shè)置模塊、仿真模擬及結(jié)果存儲(chǔ)模塊、數(shù)據(jù)回放及顯示模塊等若干模塊組成。各個(gè)部分子模塊功能和使用說明具體如下：

6、1)模型參數(shù)及輸入設(shè)置模塊，包括軌跡生成設(shè)置、慣導(dǎo)模型參數(shù)設(shè)置兩部分功能。前者用來由用戶從無人機(jī)機(jī)動(dòng)動(dòng)作庫(kù)中指定其機(jī)動(dòng)動(dòng)作，可自由編排機(jī)動(dòng)動(dòng)作順序及每個(gè)動(dòng)作的持續(xù)時(shí)間，用來組成軌跡腳本文件；后者用來設(shè)置imu誤差特性參數(shù)和導(dǎo)航系統(tǒng)誤差特性參數(shù)，具體包含的誤差類型可參考圖2。

7、2)無人機(jī)運(yùn)動(dòng)軌跡生成模塊，選取并加載編寫好的軌跡腳本文件，軟件平臺(tái)能夠?qū)⑽募性O(shè)置的機(jī)動(dòng)動(dòng)作和持續(xù)時(shí)間將機(jī)動(dòng)動(dòng)作庫(kù)中的動(dòng)作按順序設(shè)置和持續(xù)時(shí)間進(jìn)行組合，生成一段無人機(jī)運(yùn)動(dòng)軌跡加載于matlab工作空間。

8、3)辨識(shí)算法設(shè)置模塊，用戶可從經(jīng)典卡爾曼濾波(kalman?filter,kf)、經(jīng)典變分貝葉斯自適應(yīng)濾波(variational?bayesian?adaptive?kalman?filter,vbakf)和本發(fā)明所提出的魯棒自適應(yīng)濾波(rakf)三種方法中選取其中一種，并設(shè)置濾波算法參數(shù)和導(dǎo)航誤差先驗(yàn)知識(shí)參數(shù)，這些參數(shù)具體在下列步驟中進(jìn)行詳細(xì)說明。

9、4)仿真模擬及結(jié)果存儲(chǔ)模塊，完成辨識(shí)算法設(shè)置后可運(yùn)行算法對(duì)無人機(jī)慣導(dǎo)模型進(jìn)行辨識(shí)，待提示仿真運(yùn)行完成后，以圖像和文字的兩種形式展示辨識(shí)算法的運(yùn)行結(jié)果；若結(jié)果令用戶滿意則可以存儲(chǔ)結(jié)果為mat文件，自定義命名后存放于指定的路徑下，軟件平臺(tái)支持無人機(jī)運(yùn)動(dòng)軌跡以及辨識(shí)結(jié)果的存儲(chǔ)。

10、5)數(shù)據(jù)回放及顯示模塊，該模塊支持繪制慣導(dǎo)模型對(duì)無人機(jī)運(yùn)動(dòng)軌跡的測(cè)量輸出、慣導(dǎo)模型辨識(shí)結(jié)果等圖像，且能夠加載已存儲(chǔ)的結(jié)果完成數(shù)據(jù)回放；該模塊還設(shè)置有操作日志記錄模塊，在用戶使用仿真平臺(tái)的交互界面進(jìn)行包括參數(shù)設(shè)置、數(shù)據(jù)加載、參數(shù)辨識(shí)仿真和數(shù)據(jù)保存等操作時(shí)，記錄相應(yīng)的操作日志，用戶能夠從操作日志中回溯查看自己所實(shí)施過的全部操作，以及查看某次仿真具體設(shè)置的參數(shù)和保存結(jié)果后數(shù)據(jù)所在路徑位置。

11、利用算法仿真平臺(tái)的上述各個(gè)模塊實(shí)現(xiàn)慣導(dǎo)模型參數(shù)設(shè)置、無人機(jī)運(yùn)動(dòng)軌跡生成、慣導(dǎo)模型參數(shù)辨識(shí)、結(jié)果輸出繪圖及存儲(chǔ)、數(shù)據(jù)回放與顯示等功能。

12、本發(fā)明提出一種基于魯棒自適應(yīng)濾波的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)方法，具體實(shí)施步驟如下：

13、步驟一：建立無人機(jī)運(yùn)動(dòng)模型和機(jī)動(dòng)動(dòng)作庫(kù)

14、包括建立無人機(jī)機(jī)動(dòng)動(dòng)作庫(kù)、無人機(jī)運(yùn)動(dòng)模型，以及包含陀螺、加計(jì)測(cè)量模型的imu傳感器測(cè)量模型。

15、<子步驟1>：所述的建立無人機(jī)運(yùn)動(dòng)模型：

16、無人機(jī)運(yùn)動(dòng)模型，包含線運(yùn)動(dòng)方程、角運(yùn)動(dòng)方程。線運(yùn)動(dòng)方程通過非線性微分方程的形式，將機(jī)體坐標(biāo)系下的加速度矢量積分得到導(dǎo)航坐標(biāo)系下“東-北-天”速度vn和位置pn。角運(yùn)動(dòng)方程通過對(duì)姿態(tài)角速率積分計(jì)算歐拉角維度均為三維，具體可表示為：

17、

18、式中，w為姿態(tài)角速率，為機(jī)體坐標(biāo)系線加速度，為姿態(tài)角，vn和pn分別為導(dǎo)航系下無人機(jī)的速度和位置，為無人機(jī)機(jī)體坐標(biāo)系向?qū)Ш阶鴺?biāo)系旋轉(zhuǎn)的姿態(tài)旋轉(zhuǎn)矩陣，mpv為位置誤差系數(shù)矩陣，可表示為：

19、

20、其中l(wèi)為緯度，rn和rm為參與計(jì)算的中間變量，具體求解方法參考公式(8)。

21、用和fb分別代表位于時(shí)間區(qū)間[tk-1,tk]內(nèi)的機(jī)體姿態(tài)角速率和比力(線加速度)，二者作為imu理想輸出量的導(dǎo)數(shù)，可由公式(1)計(jì)算飛機(jī)運(yùn)動(dòng)狀態(tài)時(shí)由w以及通過坐標(biāo)轉(zhuǎn)換得到，對(duì)其積分可得時(shí)間區(qū)間[tm-1,tm]內(nèi)機(jī)體的角增量δθm和速度增量δvm：

22、

23、<子步驟2>：所述的建立無人機(jī)機(jī)動(dòng)動(dòng)作庫(kù)：

24、無人機(jī)機(jī)動(dòng)動(dòng)作庫(kù)中涉及到的底層機(jī)動(dòng)動(dòng)作包括六自由度飛行器運(yùn)動(dòng)中最基本的運(yùn)動(dòng)，包含13種不同的機(jī)動(dòng)動(dòng)作，包括等速平飛、加速平飛、減速平飛、抬頭、低頭、爬升、下滑、左偏航、右偏航、左滾轉(zhuǎn)、右滾轉(zhuǎn)、向左協(xié)調(diào)轉(zhuǎn)彎、向右協(xié)調(diào)轉(zhuǎn)彎等。

25、步驟二：基于機(jī)動(dòng)動(dòng)作庫(kù)生成無人機(jī)運(yùn)動(dòng)軌跡

26、具體而言：將無人機(jī)視為具有平動(dòng)和轉(zhuǎn)動(dòng)行為的剛體，忽略其非線性氣動(dòng)特性和縱向、橫側(cè)向通道的運(yùn)動(dòng)耦合特性，且忽略環(huán)境中由突風(fēng)、大氣紊流等引起的干擾，僅考慮較為理想的條件。根據(jù)步驟一中所建立的無人機(jī)機(jī)動(dòng)動(dòng)作庫(kù)，以合理的方式選取其中的運(yùn)動(dòng)動(dòng)作，設(shè)置合適的加速度角速率w以及動(dòng)作持續(xù)時(shí)間kts等參數(shù)，并按適當(dāng)?shù)捻樞驅(qū)C(jī)動(dòng)動(dòng)作進(jìn)行排列?；诓襟E一中給出的無人機(jī)線運(yùn)動(dòng)及角運(yùn)動(dòng)模型，使用四階龍格庫(kù)塔(4-order?runge-kutta,4-rk)法更新公式(1)中的微分方程，通過對(duì)積分并轉(zhuǎn)換至導(dǎo)航坐標(biāo)系得到“東-北-天”方向的線速度和位置，對(duì)w積分得到姿態(tài)角從而生成無人機(jī)飛行軌跡和對(duì)應(yīng)時(shí)間內(nèi)的速度等狀態(tài)。

27、步驟三：根據(jù)慣性測(cè)量單元誤差特性模擬陀螺加計(jì)輸出

28、建立imu傳感器測(cè)量模型，包含陀螺和加計(jì)的傳感器測(cè)量誤差模型，其中為陀螺建立的誤差模型中包含零偏誤差wg、隨機(jī)游走誤差εr、標(biāo)度誤差安裝誤差等，其中δkg對(duì)角線元素為imu各旋轉(zhuǎn)軸的標(biāo)度因數(shù)，其余元素均為0，而δg除對(duì)角線外其余位置非0，對(duì)角線元素為0。加計(jì)的誤差模型與陀螺保持一致，加計(jì)中涉及的隨機(jī)游走誤差、零偏誤差、標(biāo)度誤差、安裝誤差用符號(hào)分別表示為wa,δka,δa。設(shè)置陀螺和加計(jì)的誤差參數(shù)，代入所建立的imu傳感器測(cè)量模型后，基于步驟二中機(jī)動(dòng)動(dòng)作庫(kù)生成的無人機(jī)運(yùn)動(dòng)軌跡生成機(jī)體坐標(biāo)系下的加速度比力fb與轉(zhuǎn)動(dòng)角速率等飛行狀態(tài)，將上述測(cè)量誤差疊加至陀螺和加計(jì)中輸出的姿態(tài)角速率與比力項(xiàng)fb上，得到誤差模型如下：

29、

30、則imu傳感器測(cè)量模型的真實(shí)輸出可記為：

31、

32、公式(4)、(5)中的隨機(jī)游走誤差εr和建模為一階馬爾可夫過程：

33、

34、公式(6)、(7)中，tg和ta分別表示陀螺和加計(jì)的隨機(jī)游走時(shí)間常數(shù)，εb為陀螺的白噪聲誤差，εb為陀螺的零偏誤差、隨機(jī)游走誤差之和。

35、步驟四：設(shè)置慣導(dǎo)模型的初始導(dǎo)航誤差估計(jì)值

36、設(shè)置慣導(dǎo)傳感器測(cè)量輸出相對(duì)理想飛行狀態(tài)中姿態(tài)角φnb、速度vn、位置pn的初始估計(jì)誤差為平臺(tái)失準(zhǔn)角速度誤差及位置誤差導(dǎo)航狀態(tài)量的初始估計(jì)誤差將作為本發(fā)明所提出的rakf算法的可調(diào)初始參數(shù)，調(diào)整初始估計(jì)誤差可直接影響rakf算法的收斂速度和辨識(shí)精度，影響后面步驟中對(duì)pncm后驗(yàn)估計(jì)值及mncm后驗(yàn)估計(jì)值的求解，一定程度影響參數(shù)辨識(shí)結(jié)果。

37、步驟五：建立慣導(dǎo)及衛(wèi)星的導(dǎo)航誤差模型，建立慣導(dǎo)及衛(wèi)星組合導(dǎo)航時(shí)間同步誤差模型，建立慣導(dǎo)與衛(wèi)星導(dǎo)航接收機(jī)間的空間桿臂效應(yīng)誤差模型，獲取慣導(dǎo)和衛(wèi)星的傳感器導(dǎo)航信息輸出。

38、首先，根據(jù)一些地理參數(shù)以及無人機(jī)當(dāng)前真實(shí)飛行狀態(tài)計(jì)算慣導(dǎo)更新中涉及到的一些坐標(biāo)轉(zhuǎn)換變量；其次，利用慣導(dǎo)系統(tǒng)的傳感器數(shù)學(xué)特性，建立慣導(dǎo)導(dǎo)航誤差模型，根據(jù)步驟二中模擬生成的無人機(jī)運(yùn)動(dòng)軌跡以及步驟四中設(shè)置的慣導(dǎo)初始導(dǎo)航誤差估計(jì)值及分別按慣導(dǎo)導(dǎo)航誤差模型反演更新慣導(dǎo)的傳感器測(cè)量輸出vn+δvn及pn+δpn；最后，建立衛(wèi)星導(dǎo)航誤差模型，并對(duì)無人機(jī)運(yùn)動(dòng)軌跡疊加衛(wèi)星傳感器測(cè)量誤差，進(jìn)一步建立慣導(dǎo)及衛(wèi)星組合導(dǎo)航時(shí)間同步誤差模型，以及慣導(dǎo)與衛(wèi)星接收機(jī)天線間的空間桿臂效應(yīng)誤差模型，疊加此兩種傳感器測(cè)量誤差后得到衛(wèi)星傳感器輸出的速度及位置導(dǎo)航數(shù)據(jù)。具體過程如下：

39、<子步驟1>：根據(jù)地球的基本地理參數(shù)和無人機(jī)當(dāng)前真實(shí)飛行狀態(tài)信息，計(jì)算一些地球坐標(biāo)系中的變量和坐標(biāo)系間轉(zhuǎn)動(dòng)角速率為：

40、

41、

42、公式(8)給出了地球系中各個(gè)計(jì)算變量的表達(dá)式，f表示地球的扁率，e表示地球的等效橢圓焦距，re為地球的等效橢圓半長(zhǎng)軸，l為緯度，rn和rm為參與計(jì)算的中間變量；公式(9)給出了地球轉(zhuǎn)動(dòng)相關(guān)的各坐標(biāo)系間的旋轉(zhuǎn)角速率，參與計(jì)算的各變量為，ωie為地球自轉(zhuǎn)角速率，h為無人機(jī)高度，和為北向和東向?qū)Ш阶鴺?biāo)系速度；公式(9)所求解的各變量分別表示地球轉(zhuǎn)動(dòng)角速率、導(dǎo)航坐標(biāo)系轉(zhuǎn)動(dòng)角速率、導(dǎo)航坐標(biāo)系的公轉(zhuǎn)角速率以及機(jī)體坐標(biāo)系繞導(dǎo)航坐標(biāo)系旋轉(zhuǎn)角速率；為無人機(jī)本體坐標(biāo)系向?qū)Ш阶鴺?biāo)系轉(zhuǎn)換的姿態(tài)旋轉(zhuǎn)矩陣，其轉(zhuǎn)置為導(dǎo)航坐標(biāo)系向本體坐標(biāo)系的旋轉(zhuǎn)矩陣。

43、<子步驟2>：根據(jù)步驟四中設(shè)置的初始平臺(tái)失準(zhǔn)角初始速度誤差以及初始位置誤差根據(jù)慣導(dǎo)的導(dǎo)航誤差模型，采用4-rk法計(jì)算慣導(dǎo)導(dǎo)航誤差輸出，所建立的慣導(dǎo)導(dǎo)航誤差模型為：

44、

45、公式(10)中和在前面已經(jīng)給出定義，為由公式(4)所求得的陀螺姿態(tài)角速率測(cè)量誤差，為導(dǎo)航坐標(biāo)系轉(zhuǎn)動(dòng)角速率誤差；公式(11)中fn為加計(jì)測(cè)量的比力fb向?qū)Ш阶鴺?biāo)系轉(zhuǎn)換的結(jié)果；公式(12)中狀態(tài)量可合并為位置矢量δpn＝[δlδλδh]t來表示。由公式(8)至公式(12)更新得到慣導(dǎo)模型的導(dǎo)航誤差，與無人機(jī)的運(yùn)動(dòng)軌跡相疊加可得慣導(dǎo)的實(shí)際輸出。

46、<子步驟3>：建立衛(wèi)星導(dǎo)航誤差模型，首先建立慣導(dǎo)及衛(wèi)星組合導(dǎo)航系統(tǒng)的時(shí)間同步誤差模型，進(jìn)而考慮慣導(dǎo)與衛(wèi)星接收機(jī)天線間的桿臂效應(yīng)誤差，建立空間桿臂效應(yīng)誤差模型，并加入數(shù)據(jù)傳輸時(shí)間延遲和一階馬爾可夫過程，給出最終的衛(wèi)星模型如下所示：

47、

48、公式(13)中，和表示慣導(dǎo)的傳感器測(cè)量輸出vn+δvn以及pn+δpn，an表示導(dǎo)航坐標(biāo)系下的無人機(jī)線加速度，δtd為時(shí)間同步誤差；公式(14)中，表示空間桿臂坐標(biāo)系相對(duì)無人機(jī)本體坐標(biāo)系的旋轉(zhuǎn)角速率，δrb為空間桿臂效應(yīng)誤差；公式(15)中τv，τp和ts分別為速度、位置時(shí)間常數(shù)以及量測(cè)方程慢回路更新的采樣時(shí)間間隔，v1和v2分別表示衛(wèi)星接收機(jī)速度和位置測(cè)量白噪聲。

49、步驟六：設(shè)置魯棒自適應(yīng)濾波算法初始參數(shù)

50、本發(fā)明所設(shè)計(jì)的rakf算法是基于卡爾曼濾波框架的自適應(yīng)辨識(shí)算法。該算法初始參數(shù)包括：經(jīng)典卡爾曼濾波算法參數(shù)，包括pncm初值mncm初值狀態(tài)誤差協(xié)方差矩陣(predicted?error?covariance?matrix,pecm)初值系統(tǒng)狀態(tài)向量維度n和量測(cè)向量維度m等；基于滑動(dòng)窗口的sage-husa濾波器(sliding?window?sage-husa?filter,swf)參數(shù)，包括滑動(dòng)窗口長(zhǎng)度nw、自適應(yīng)控制算子ba；強(qiáng)跟蹤濾波器(strong?tracking?filter,stf)參數(shù)，包括遺忘因子ρa(bǔ)；變分貝葉斯自適應(yīng)濾波(variational?bayesian?adaptive?kalmanfilter,vbakf)自適應(yīng)參數(shù)，包括變分迭代次數(shù)nvb、變分調(diào)控因子ζvb、逆gamma分布形狀參數(shù)αk、逆gamma分布尺度參數(shù)βk。rakf算法引入vbakf估計(jì)mncm和pecm，利用多重漸消因子(multiple?fading?factor,mff)建立stf，并引入swf來估計(jì)pncm。

51、步驟七：迭代更新采樣時(shí)間較短的狀態(tài)方程快回路和采樣時(shí)間較長(zhǎng)的量測(cè)方程慢回路，以獲取系統(tǒng)狀態(tài)向量的后驗(yàn)估計(jì)作為慣導(dǎo)模型參數(shù)辨識(shí)結(jié)果

52、更新采樣時(shí)間較短的狀態(tài)方程快回路(以下簡(jiǎn)稱：快回路)和采樣時(shí)間較長(zhǎng)的量測(cè)方程慢回路(以下簡(jiǎn)稱：慢回路)；加載步驟五中計(jì)算的慣導(dǎo)和衛(wèi)星的傳感器測(cè)量輸出，計(jì)算偏差并建立新息序列，利用swf給出新息序列的自適應(yīng)估計(jì)，利用新息序列估計(jì)值構(gòu)造多重漸消因子矩陣，并采用stf獲取一步狀態(tài)轉(zhuǎn)移矩陣；利用vbakf來更新獲取pecm和mncm的后驗(yàn)估計(jì)，并最終輸出系統(tǒng)狀態(tài)向量的后驗(yàn)估計(jì)作為慣導(dǎo)模型參數(shù)辨識(shí)結(jié)果。具體過程如下：

53、<子步驟1>：初始化系統(tǒng)狀態(tài)變量的后驗(yàn)估計(jì)值為全零向量根據(jù)步驟四中給出的慣導(dǎo)和衛(wèi)星的估計(jì)誤差初值設(shè)置，設(shè)置pncm的初始估計(jì)值及mncm的初始估計(jì)值為：

54、

55、其中，σ(wg)和σ(wa)分別為初始給定的陀螺與加計(jì)白噪聲的三維協(xié)方差，028×1為28維全零列向量，和分別為初始給定的量測(cè)方程中慣導(dǎo)和衛(wèi)星所測(cè)量的導(dǎo)航坐標(biāo)系速度和位置的偏差初始值。

56、注意，本發(fā)明中角標(biāo)若為k|k-1形式則表示先驗(yàn)估計(jì)量，若角標(biāo)寫為k|k形式則表示后驗(yàn)估計(jì)量。

57、<子步驟2>：根據(jù)慣導(dǎo)測(cè)量輸出初值及由公式(8)及公式(9)計(jì)算的地理坐標(biāo)參數(shù)，并結(jié)合狀態(tài)方程及其離散化形式xk＝fk-1xk-1，取k＝1時(shí)間步的值計(jì)算離散狀態(tài)系數(shù)矩陣初值f0，其中xk為系統(tǒng)狀態(tài)向量，可寫為：

58、

59、其中分別表示陀螺和加計(jì)的標(biāo)度誤差及安裝誤差矩陣。

60、預(yù)設(shè)變分貝葉斯估計(jì)參數(shù)ρ,ψ0,α0|0,β0|0，其初始取值設(shè)置如下：α0|0＝[1，...,1]t，初始化pecm為初始化卡爾曼濾波增益設(shè)置狀態(tài)方程更新回路為快回路，其采樣更新頻率為10hz；設(shè)置量測(cè)方程更新回路為慢回路，其采樣更新頻率1hz。按步驟六中所設(shè)置的rakf參數(shù)，遍歷無人機(jī)運(yùn)動(dòng)軌跡所對(duì)應(yīng)的全部采樣時(shí)間周期進(jìn)行迭代更新。

61、狀態(tài)方程快回路更新：具體為以下子步驟3-5

62、<子步驟3>：利用公式(3)至公式(7)計(jì)算陀螺和加計(jì)的傳感器測(cè)量值，進(jìn)一步根據(jù)公式(8)至公式(12)計(jì)算慣導(dǎo)的傳感器測(cè)量輸出，進(jìn)而基于慣導(dǎo)測(cè)量值和由公式(8)、(9)所得的姿態(tài)角速率和旋轉(zhuǎn)矩陣來計(jì)算狀態(tài)系數(shù)陣fk，通過泰勒展開公式近似計(jì)算得到一步狀態(tài)轉(zhuǎn)移矩陣

63、<子步驟4>：根據(jù)上一輪迭代所得的系統(tǒng)狀態(tài)向量貝葉斯后驗(yàn)估計(jì)值mk-1k-1，計(jì)算系統(tǒng)狀態(tài)向量xk的先驗(yàn)估計(jì)：

64、mk|k-1＝φk|k-1mk-1|k-1????????????????????????(16)

65、由公式(24)在采樣時(shí)刻所得的量測(cè)向量zk，根據(jù)系統(tǒng)狀態(tài)向量計(jì)算量測(cè)方程系數(shù)矩陣hk，進(jìn)而得到計(jì)算新息序列：

66、

67、對(duì)新息序列計(jì)算l2范數(shù)，并引入遺忘因子作為權(quán)重來更新pncm：

68、

69、進(jìn)一步根據(jù)本輪迭代所計(jì)算的一步狀態(tài)轉(zhuǎn)移矩陣φk|k-1、pncm以及上一輪所得的pecm后驗(yàn)估計(jì)pk-1|k-1計(jì)算pecm的先驗(yàn)估計(jì)為：

70、

71、<子步驟5>：利用變分貝葉斯估計(jì)理論，將系統(tǒng)狀態(tài)與的聯(lián)合后驗(yàn)估計(jì)通過分?jǐn)?shù)階計(jì)算展開為相互獨(dú)立的高斯分布同逆gamma分布的乘積：

72、

73、其中系統(tǒng)狀態(tài)向量mk|k-1和pecmpk|k-1的先驗(yàn)估計(jì)由高斯分布給出，對(duì)mncm的先驗(yàn)估計(jì)由逆gamma分布的形狀因子αk|k-1,j和逆尺度因子βk|k-1,j給出。

74、求mncm的先驗(yàn)估計(jì)參數(shù)，即逆gamma分布的兩個(gè)尺度因子，使用一個(gè)貝葉斯估計(jì)系數(shù)ρj(j＝1,2,...,m)來計(jì)算其先驗(yàn)估計(jì)值：

75、αk|k-1,j＝ρjαk-1|k-1,j??j＝1,2,…,m??????????(22)

76、βk|k-1,j＝ρjβk-1|k-1,j??j＝1,2,…,m??????????(23)

77、量測(cè)方程慢回路更新：具體為以下子步驟6-10；

78、<子步驟6>：若到達(dá)慢回路采樣時(shí)間點(diǎn)，則根據(jù)公式(13)至(15)計(jì)算衛(wèi)星傳感器測(cè)量輸出，即導(dǎo)航坐標(biāo)系下衛(wèi)星測(cè)量的速度及位置結(jié)合公式(10)至(12)所計(jì)算的慣導(dǎo)傳感器測(cè)量輸出無人機(jī)慣導(dǎo)測(cè)得的導(dǎo)航速度vn+δvn及慣導(dǎo)測(cè)得的導(dǎo)航位置pn+δpn，計(jì)算其偏差以獲得慢回路中的量測(cè)向量zk，可表示為：

79、

80、<子步驟7>：pecm的先驗(yàn)估計(jì)pk|k-1在本步使用帶漸消因子矩陣的強(qiáng)跟蹤濾波器更新。利用式(18)所得的新息序列，使用加權(quán)滑動(dòng)窗口自適應(yīng)估計(jì)新息序列，其表達(dá)式如下：

81、

82、再新息項(xiàng)構(gòu)造多重漸消因子陣，使算法具備強(qiáng)跟蹤特性：

83、

84、其中χ表示遺忘因子，在區(qū)間[0.95,0.99]內(nèi)取值。

85、應(yīng)用stf進(jìn)行次優(yōu)估計(jì)的方法，計(jì)算具有強(qiáng)跟蹤特性的一步狀態(tài)轉(zhuǎn)移矩陣ψk，首先計(jì)算多重漸消因子矩陣，進(jìn)而通過對(duì)其加權(quán)得到ψk：

86、

87、ψk＝diag{ψk,11,ψk,22,...,ψk,nn}??????????????(31)

88、式中，diag{·}為矩陣對(duì)角化運(yùn)算符，tr[·]表示矩陣的跡運(yùn)算，μk表示所求多重漸消因子，為需要設(shè)置的權(quán)重系數(shù)，通常設(shè)置為不小于1即可。由公式(27)-(31)所得的具有強(qiáng)跟蹤特性的一步轉(zhuǎn)移矩陣，利用在式(19)中計(jì)算的pncm預(yù)測(cè)估計(jì)值以及上一輪所得的pncm后驗(yàn)估計(jì)pk-1k-1，在慢回路中可由下式替代快回路的公式(20)以獲取pecm的先驗(yàn)更新值：

89、

90、<子步驟8>：初始化本輪迭代中系統(tǒng)狀態(tài)向量的貝葉斯后驗(yàn)估計(jì)和pecm后驗(yàn)估計(jì)值為：

91、

92、設(shè)置迭代估計(jì)次數(shù)從n＝0增加到n＝nvb-1，在第n輪更新逆gamma分布的形狀因子并初始化逆尺度參數(shù)為：

93、

94、式中，表示第n輪迭代中g(shù)amma分布形狀因子的第j分量，表示逆尺度因子的第j分量的初始值。

95、<子步驟9>：迭代次數(shù)從n＝0取到n＝nvb-1，給出系統(tǒng)狀態(tài)向量與pecm的后驗(yàn)估計(jì)計(jì)算方法為：

96、

97、以上兩式中，mncm的后驗(yàn)估計(jì)值計(jì)算方式為：

98、

99、先使用公式(39)在第n輪迭代中更新逆尺度因子βk|k-1,j，再利用公式(35)、(39)及(40)的變分貝葉斯機(jī)制初步估計(jì)mncm。利用公式(41)、(42)的序貫濾波防止在自適應(yīng)更新過程中mncm變?yōu)榉钦?，最終得到mncm后驗(yàn)估計(jì)。注意，式中全部右下角的角標(biāo)j表示向量的第j元素，角標(biāo)jj表示矩陣的第j對(duì)角值，表示第n輪迭代中逆尺度因子的第j分量，表示mncm的后驗(yàn)估計(jì)值在未進(jìn)行序貫濾波校正前第i輪迭代的第j對(duì)角元素。在完成公式(37)-(42)的更新后，令變分貝葉斯估計(jì)迭代次數(shù)更新為n＝n+1，重新返回式(35)進(jìn)行新一輪迭代計(jì)算，直至迭代更新次數(shù)到達(dá)n＝nvb。

100、<子步驟10>：通過公式(19)、(32)與(40)-(42)分別基于所提出rakf完成了對(duì)pncm、pecm以及mncm的自適應(yīng)估計(jì)。當(dāng)n＝nvb完成更新后，可利用傳統(tǒng)卡爾曼濾波更新卡爾曼增益、狀態(tài)向量以及pecm的后驗(yàn)估計(jì)：

101、

102、pk|k＝pk|k-1-kkhkpk|k-1???????????(45)

103、其中，kk、和pk|k分別為卡爾曼增益、系統(tǒng)狀態(tài)向量后驗(yàn)估計(jì)以及系統(tǒng)pecm后驗(yàn)估計(jì)。完成后驗(yàn)估計(jì)更新后，則將時(shí)間步設(shè)置為k＝k+1，則返回<子步驟4>進(jìn)行下一時(shí)間步的預(yù)測(cè)和估計(jì)。若時(shí)間步到達(dá)無人機(jī)運(yùn)動(dòng)軌跡最大時(shí)間，則停止迭代，輸出系統(tǒng)狀態(tài)向量以及pecm的后驗(yàn)估計(jì)為：

104、

105、式中，分別表示系統(tǒng)狀態(tài)向量以及pecm的后驗(yàn)估計(jì)結(jié)果。

106、系統(tǒng)狀態(tài)向量后驗(yàn)估計(jì)各項(xiàng)分別表示，表示平臺(tái)失準(zhǔn)角，表示導(dǎo)航坐標(biāo)系速度測(cè)量誤差，表示導(dǎo)航坐標(biāo)系位置測(cè)量誤差，表示陀螺零偏誤差，表示加計(jì)零偏誤差，表示空間桿臂效應(yīng)誤差，表示慣導(dǎo)與衛(wèi)星組成的組合導(dǎo)航系統(tǒng)的時(shí)間同步誤差，陀螺安裝誤差與標(biāo)度誤差的增廣向量可寫為：

107、其中和分別為陀螺標(biāo)度誤差，其余元素為陀螺安裝誤差；加計(jì)安裝誤差與標(biāo)度誤差的增廣向量可寫為其中和分別為加計(jì)標(biāo)度誤差，其余元素為加計(jì)安裝誤差。

108、pecm的后驗(yàn)估計(jì)pk|k各項(xiàng)分別表示，為平臺(tái)失準(zhǔn)角估計(jì)協(xié)方差，為導(dǎo)航坐標(biāo)系速度估計(jì)誤差協(xié)方差，為導(dǎo)航坐標(biāo)系位置估計(jì)誤差協(xié)方差，為陀螺零偏誤差估計(jì)誤差協(xié)方差，為加計(jì)零偏誤差估計(jì)誤差協(xié)方差，為空間桿臂效應(yīng)誤差估計(jì)協(xié)方差，為慣導(dǎo)與衛(wèi)星組合導(dǎo)航系統(tǒng)時(shí)間同步誤差估計(jì)協(xié)方差，為陀螺安裝誤差與標(biāo)度誤差的增廣協(xié)方差矩陣，為加計(jì)安裝誤差與標(biāo)度誤差的增廣協(xié)方差矩陣。

109、步驟八：輸出、存儲(chǔ)及顯示參數(shù)辨識(shí)結(jié)果

110、通過提取步驟七完成后系統(tǒng)狀態(tài)向量的后驗(yàn)估計(jì)結(jié)果，可得辨識(shí)參數(shù)的輸出值，包括慣導(dǎo)平臺(tái)失準(zhǔn)角陀螺零漂εb、加計(jì)零漂空間桿臂效應(yīng)誤差δrb及時(shí)間同步誤差δtd，陀螺標(biāo)度誤差δkg和安裝誤差δg，加計(jì)標(biāo)度誤差δka和安裝誤差δa。

111、將上述基于rakf方法辨識(shí)慣導(dǎo)模型后輸出系統(tǒng)狀態(tài)向量后驗(yàn)估計(jì)值，可代入慣導(dǎo)與衛(wèi)星的組合導(dǎo)航模型，輸出在模型在參數(shù)辨識(shí)結(jié)果的驅(qū)動(dòng)下，對(duì)于無人機(jī)運(yùn)動(dòng)軌跡的在導(dǎo)航坐標(biāo)系下姿態(tài)、速度和位置等飛行狀態(tài)的傳感器測(cè)量值，并將測(cè)量值同無人機(jī)的實(shí)際飛行狀態(tài)原始數(shù)據(jù)做差可得速度濾波誤差δvn以及位置濾波誤差δpn，在遍歷整個(gè)運(yùn)動(dòng)軌跡的時(shí)間段過程中，記錄每個(gè)采樣時(shí)刻的慣導(dǎo)模型辨識(shí)參數(shù)以及導(dǎo)航誤差，并繪圖顯示。

112、本發(fā)明建立了基于魯棒自適應(yīng)濾波算法的無人機(jī)慣導(dǎo)模型參數(shù)辨識(shí)框架，該方法能夠以一段理想的無人機(jī)機(jī)動(dòng)飛行軌跡的輸入作為基準(zhǔn)，基于魯棒自適應(yīng)濾波算法，利用加權(quán)滑動(dòng)窗的sage-husa自適應(yīng)濾波器給出量測(cè)向量偏差組成的新息序列的觀測(cè)值，并據(jù)此構(gòu)造多重漸消因子矩陣，采用強(qiáng)跟蹤濾波器獲取一步狀態(tài)轉(zhuǎn)移矩陣，利用變分貝葉斯估計(jì)來更新獲取系統(tǒng)狀態(tài)預(yù)測(cè)誤差協(xié)方差矩陣和量測(cè)誤差協(xié)方差矩陣的后驗(yàn)估計(jì)，最終實(shí)現(xiàn)對(duì)無人機(jī)慣導(dǎo)模型共計(jì)28個(gè)誤差狀態(tài)量的辨識(shí)，本發(fā)明還搭建支持無人機(jī)慣導(dǎo)模型建模仿真及參數(shù)辨識(shí)的軟件平臺(tái)，實(shí)現(xiàn)具有良好的辨識(shí)精度、實(shí)時(shí)性及可靠性的慣導(dǎo)模型參數(shù)辨識(shí)，以建立可支持無人機(jī)高保真飛行自主導(dǎo)航仿真的慣導(dǎo)模型。