基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法
【專利摘要】本發(fā)明公開了一種基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，步驟包括：對實際飛行數(shù)據進行坐標系轉換得到ECI坐標系下的位置、速度、姿態(tài)；對姿態(tài)、位置、重力分別進行三次樣條函數(shù)插值并計算角速率解析表達式、載體系加速度樣條多項式、載體系重力樣條多項式，計算擾動加速度積分增量理論值和陀螺角增量理論值、積分計算加速度增量理論值、積分計算重力積分增量理論值；計算飛行器的加速度計比力積分增量；根據加速度計比力積分增量、陀螺角增量理論值進行慣導解算得到導航飛行器的動態(tài)軌跡。本發(fā)明能夠完全滿足組合導航動態(tài)仿真精度要求、同樣也適用于其他高精度高動態(tài)導航系統(tǒng)和剛體運動控制仿真中的角運動、線運動傳感器信號模擬。
【專利說明】
基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法
技術領域
[0001] 本發(fā)明涉及INS/GNSS組合導航技術，具體涉及一種基于實際飛行數(shù)據插值的仿真 動態(tài)軌跡解析生成方法。
【背景技術】
[0002] 利用信號模擬器進行科學研究、系統(tǒng)測試和性能評估是目前廣泛使用的方法，具 有成本低、高效、快速的特點。而在對捷聯(lián)慣導系統(tǒng)、慣性/衛(wèi)星組合導航系統(tǒng)的研究和實驗 中，也離不開軌跡發(fā)生器的研究和應用。國內外對此做了大量細致的研究，可分為以下幾 類：
[0003] -、基于載體運動學方程的典型軌跡拼接，其中最經典的軌跡發(fā)生器當屬美國空 軍航空電子實驗室的PR〇FGEN(Musick S H.，PR0FGEN_A computer program for generating flight profiles[R]//Air Force Avionics Lab.ffright-Patterson AFB Ohio，ADA034993，1977 .) WR0FGEN能夠支持四種基本的航跡仿真生成：垂直轉彎、水平轉 彎、正弦航向變化和直線飛行。類似地，文獻[2](Li，G.X.，Mao Y.L.，Song，C.L.，Design and Simulation of Trajectory Generator[C]//IEEE Computer Society/The 4th International Conference on Intelligent Human-Machine Systems and Cybernetics ,2012,201-205.)、文獻[3](范欣，張福斌，李曉暉.一種水下航行器軌跡發(fā)生 器的設計與仿真[J].魚雷技術，2010,18(3) :214-217.)、文獻[4](李海靜、陳偉強.機載武 器軌跡發(fā)生器設計與仿真[J].科學技術與工程，2010,10(13) :3158-3162.)將幾種飛行軌 跡(如直航、轉彎、爬升和俯沖等)或水下航行器的運動軌跡(如直航、轉彎和爬潛等)進行拼 接，在此基礎上產生比力、角速度信號，計算相應的姿態(tài)、速度和位置。雖然能實現(xiàn)一定的慣 性導航和組合導航仿真測試功能，但所生成軌跡與載體線運動、角運動實際情況還存在較 大差別。
[0004] 二、基于載體動力學方程計算飛行仿真條件下的比力、角速度信號，文獻[5]([5] 李軍偉，程詠梅，陳克吉，等.基于飛行仿真的捷聯(lián)慣導算法測試平臺[J].中國慣性技術學 報，2012,20(5):530-535.)在建立六自由度飛機非線性動力學模型基礎上，實現(xiàn)了預定航 跡多模態(tài)的飛行仿真；文獻[6](陳凱、衛(wèi)鳳、張前程等.基于飛行力學的慣導軌跡發(fā)生器及 其在半實物仿真中的應用[J].中國慣性技術學報，2014,22(4) :486-491.)基于飛行器六自 由度模型進行軌跡生成，軌跡的運動由飛行控制系統(tǒng)實現(xiàn)。所生成軌跡與載體線運動、角運 動實際情況的接近程度取決于動力學模型的準確度。
[0005] 三、基于組合導航實際實驗數(shù)據和捷聯(lián)慣性導航數(shù)值解算方程，文獻[7] (G.M.Yan.,J.L.Wang and X.Y.~Zhou.,High-Precision Simulator for Strapdown Inertial Navigation Systems Based on Real Dynamics from GNSS and IMU Integration[C]//Springer Verlag Berlin Heidelberg/China Satellite Navigation Conference(CSNC)2015, Proceedings : Volume III,Lecture Notes in Electrical Engineering 342,（2015) ,789-799.)在當?shù)厮降乩碜鴺讼迪?，對GPS/SINS組合導航卡爾 曼濾波得到的100Hz歐拉角姿態(tài)、速度和位置輸出進行插值，得到200Hz頻率下離散時間點 的等效轉動矢量、速度和位置，通過捷聯(lián)慣性導航數(shù)值解算算法的逆運算，得到角增量和加 速度計比力積分增量。因此，該方法不能對捷聯(lián)慣性導航數(shù)值解算算法本身進行仿真驗證。 由于沒有角速率及其一階導數(shù)的仿真數(shù)據，也沒有對桿臂效應進行模擬。

【發(fā)明內容】

[0006] 本發(fā)明要解決的技術問題:針對現(xiàn)有技術的上述問題，提供一種完全滿足組合導 航動態(tài)仿真精度要求、適用于其他高精度高動態(tài)導航系統(tǒng)和剛體運動控制仿真中的角運 動、線運動傳感器信號模擬的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法。
[0007] 為了解決上述技術問題，本發(fā)明采用的技術方案為：
[0008] -種基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，步驟包括：
[0009] 1)對實際飛行數(shù)據進行坐標系轉換，得到ECI坐標系下的位置、速度、姿態(tài)；
[0010] 2)對姿態(tài)進行三次樣條函數(shù)插值并計算角速率解析表達式A，根據角速率解析表 達式A計算擾動加速度積分增量理論值A1和陀螺角增量理論值A2;
[0011] 3)對位置進行三次樣條函數(shù)插值并計算載體系加速度樣條多項式B，根據載體系 加速度樣條多項式B進行積分計算加速度增量理論值B1;
[0012] 4)對重力進行三次樣條函數(shù)插值并計算載體系重力樣條多項式C，根據載體系重 力樣條多項式C進行積分計算重力積分增量理論值C1;
[0013] 5)根據擾動加速度積分增量理論值A1、加速度增量理論值B1、重力積分增量理論 值C1計算飛行器的加速度計比力積分增量D1;
[0014] 6)根據加速度計比力積分增量D1、陀螺角增量理論值A2進行慣導解算得到導航飛 行器的動態(tài)軌跡。
[0015] 優(yōu)選地，所述步驟2)的詳細步驟包括：
[0016] 2.1)根據姿態(tài)歐拉角計算姿態(tài)四元數(shù)；
[0017] 2.2)對姿態(tài)四元數(shù)以指定時間間隔進行三次樣條函數(shù)插值，得到姿態(tài)四元數(shù)各個 分量的三次樣條多項式;對所述三次樣條多項式求導得到三次樣條多項式的一階導數(shù)，代 入時間得到姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值;根據預設的四元數(shù)范數(shù)約束條 件，對姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值進行修正；
[0018] 2.3)根據修正后的姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值，將[tk，t k+1]轉換 至|J[0，T]區(qū)間，分段計算得到[ο，τ]區(qū)間的姿態(tài)四元數(shù)各個分量的三次樣條多項式；
[0019] 2.4)將轉換到區(qū)間[0，Τ]姿態(tài)四元數(shù)各個分量的三次樣條多項式利用泰勒近似展 開得到角速率解析表達式Α;
[0020] 2.5)根據角速率解析表達式Α計算擾動加速度積分增量理論值Α1;
[0021 ] 2.6)根據角速率解析表達式A計算陀螺角增量理論值A2。
[0022]優(yōu)選地，所述步驟2.1)計算姿態(tài)四元數(shù)的函數(shù)表達式如式(1)所示；
[0023]
(i)
[0024] 式（1)中，9(^1，92,93分別為姿態(tài)四元數(shù)的四個元素，丫為姿態(tài)的滾轉角，?1為姿態(tài) 的俯仰角，f為姿態(tài)的偏航角；
[0025] 所述步驟2.2)中，姿態(tài)四元數(shù)各個分量的三次樣條多項式的函數(shù)表達式如式(2) 所示，預設的四元數(shù)范數(shù)約束條件如式(3)所示，對姿態(tài)四元數(shù)各個分量進行修正的函數(shù)表 達式如式(4)所示，對姿態(tài)四元數(shù)各個分量的一階導數(shù)的離散值進行修正的函數(shù)表達式如 式(5)所示；
[0026] = a/ + bf + f (2).
[0027] 式⑵中，糾)為姿態(tài)四元數(shù)各個分量的三次樣條多項式，&1，131，(：1，(1 1分別為樣條 插值后三次樣條多項式的系數(shù)，t為時間；
[0028]
(3)
[0029] 式(3)中，| |q(t)| |代表t時刻的姿態(tài)四元數(shù)q(t)的范數(shù)，q〇(t)，qi(t)，q2(t)，q3 (t)分別為姿態(tài)四元數(shù)q(t)的四個元素，名⑴，么⑴⑴分別為姿態(tài)四元數(shù)q(t)的四 個元素的一階導數(shù)；
[0030]
⑷
[0031] 式(4)中，qi(tk)代表插值點tk時刻的姿態(tài)四元數(shù)各個分量的修正結果，砂J代表 插值點tk時刻姿態(tài)四元數(shù)的各個分量，|疋(4.)|代表插值點tk時刻姿態(tài)四元數(shù)的范數(shù)；
[0032]
(5：)
[0033] 式(5)中，女(/J代表插值點tk時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)的修正結果， $ (^)代表插值點t k時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)，(&) |_代表插值點t k時刻姿態(tài) 四元數(shù)機)的范數(shù);|(|機)||)代表插值點tk時刻姿態(tài)四元數(shù)階,)的范數(shù)I砍)丨|的求導結果， 其函數(shù)表達式如式(6)所示；
[0034]
(6)
[0035] 式(6)中，&匕(々)居k ),?⑷分別為插值點tk時刻的姿態(tài)四元數(shù)的四個元素， 4⑷4⑷名⑷、矣⑷分別為插值點tk時刻的姿態(tài)四元數(shù)的四個元素一階導數(shù)；
[0036] 所述步驟2.3)分段計算得到[0，T]區(qū)間的姿態(tài)四元數(shù)各個分量的三次樣條多項式 如式(7)所示；
[0037]
(7)
[0038] 式(7)中，ΚΟ代表轉換到區(qū)間[0，Τ]的插值樣條多項式，ai,k，bi,k，Ci,k，di,k分別為 轉換到[0，T]區(qū)間后的三次樣條多項式系數(shù)，t代表時間;轉換到[0，T]區(qū)間后的三次樣條多 項式系數(shù)的計算函數(shù)表達式如式(8)所示；
[0039]
(8'
[0040] 式(8)中，Τ代表分段樣條的分段間隔，qi(tk)代表tk時刻的姿態(tài)四元數(shù)中第i個元 素的值，惠 fe)代表tk時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值，qi(tk+1)代表t k+1時刻的姿 態(tài)四元數(shù)中第i個元素的值代表tk+1時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值，符 號i取值為〇或1或2或3， &^九,1{，(^1{，(^分別為轉換到區(qū)間[0，1']后的插值多項式系數(shù)；
[0041] 所述步驟2.4)的詳細步驟包括:根據式(9)所示的限制條件代入式（10)所示角速 率的計算式，對姿態(tài)四元數(shù)范數(shù)的平方的倒數(shù)部分進行泰勒近似展開，得到角速率 解析表達式A如式(11)所述；
[0042]
[0043] 式(9)中，qk(t)代表姿態(tài)四元數(shù)，4#)代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[0，T] 區(qū)間后的姿態(tài)四元數(shù)，_(〇|代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[0，Τ]區(qū)間后的姿態(tài)四元 數(shù)的范數(shù)，代表姿態(tài)四元數(shù)的一階導數(shù)Λ⑴代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[0， T]區(qū)間后的姿態(tài)四元數(shù)的一階導數(shù)，MO, H)，么((0, 分別代表原始[tk，t k+1M 間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)4切各個元素，4汴)，4i(0, 分別代表 原始[tk，tk+1]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)各個元素的一階導數(shù)，t代表時間；
[0044]
(10)
[0045] 式（10)中，代表角速率解析表達式A，所述角速率解析表達式A為姿態(tài)四元數(shù) 的樣條多項式，下標ib代表載體坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[tk， tk+1]、上標b代表載體坐標系;qk(t)代表姿態(tài)四元數(shù)的樣條多項式，⑴代表姿態(tài)四元數(shù)的 樣條多項式的一階導數(shù)，t代表時間；
[0046]
Π1)
[0047] 式(11)中，W代表角速率的時間函數(shù)，下標ib代表載體坐標系相對于慣性坐標 系、k代表原始數(shù)據為區(qū)間[tk，tk+1 ]、上標b代表載體坐標系;:(〇|代表原始數(shù)據區(qū)間為[tk， tk+1]轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)的范數(shù)，⑴，iuW，UO, 分別代表原始 [t k，t k + i ]區(qū)間轉換到區(qū)間[0，T ]后的姿態(tài)四元數(shù)I⑴各個元素， 4WU), 4,(小4,4)分別代表原始[t k，tk+1謳間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù) 各個元素的一階導數(shù)，t代表時間；
[0048] 所述步驟2.5)包括首先根據式（12)所示的擾動加速度解析積分表達式計算擾動 加速度積分理論值，然后根據指定時間間隔ΛΤ的擾動加速度積分理論值S Uk(t)和SUk(t+A T)計算差值，得到指定的時間間隔ΛΤ內的擾動加速度積分增量理論值A1;
[00491
12；
[0050] 式(12)中，t)代表[Q，t]時間的擾動加速度積分理論值，<*(，)W，<氺） 分別為角速率分別在X、y、ζ方向的解析表達式;代表X方向加速度計偏離質心的桿臂矢量 4中的X方向偏移量，'代表y方向加速度計偏離質心的桿臂矢量略中的y方向偏移量，4 代表z方向加速度計偏離質心的桿臂矢量4中的z方向偏移量，4 、 分別為三個方向的加速度計偏離質心的桿臂矢量 且均為常值，t代表時間；
[0051] 所述步驟2.6)具體是指根據式(13)計算陀螺角增量理論值A2;
[0052]
[0053] 式（13)中，0k(t)代表由角速率解析表達式A積分得到的陀螺角增量，ω:;4)代表角 速率解析表達式Α，所述角速率解析表達式Α為姿態(tài)四元數(shù)的樣條多項式，下標ib代表載體 坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[tk，tk+i]、上標b代表載體坐標系，A9 k,j+1 代表ΛΤ時間間隔內的陀螺角增量理論值A2，0k(tJ代表[0山]時間的陀螺角積分增量，0k (t j+ΛΤ)代表[0，t j+ΛΤ ]時間的陀螺角積分增量，t代表時間。
[0054]優(yōu)選地，所述步驟3)的詳細步驟包括：
[0055] 3.1)對ECI坐標系下位置的各個分量進行樣條插值得到位置的樣條多項式，對所 得到的位置的樣條多項式求一次導得到速度的樣條多項式，分別代入時間得到位置和速度 的離散值；
[0056] 3.2)根據位置和速度的離散值，將[^，^+1]轉換到[0，1']區(qū)間，分段計算得到[0， T]區(qū)間的位置各個分量的三次樣條多項式，進行兩次求導得到加速度各個分量的樣條多項 式⑴，其中k代表原始數(shù)據區(qū)間為[t k，tk+1];
[0057] 3.3)結合坐標系轉換，得到載體系b中的加速度4#何的解析表達式如式（14)所 示；
[0058]
Π 4)
[0059] 式（14)中，代表載體系b中的加速度的樣條多項式，代表原始數(shù)據為 [tk，tk+1]區(qū)間轉換到[ο，τ]區(qū)間的加速度的樣條多項式，?^)代表無誤差慣性坐標系到載 體坐標系的方向余弦矩陣，t代表時間，其中方向余弦矩陣與姿態(tài)四元數(shù)的轉換關系 如式(15)所示；
[0060]
1·^)
[0061 ]式(15)中，C;:』)代表無誤差慣性坐標系到載體坐標系的方向余弦矩陣，6 (，)代表未 修正時的慣性坐標系到載體坐標系的方向余弦矩陣，*^4):、 心(，）、'W、4小)、心⑴分別代表未修正時的慣性坐標系到載體坐標系的方向余弦 矩陣中的九個元素，代表原始數(shù)據為[tk，t k+1]轉換到[0，T]區(qū)間的四元數(shù)的范 數(shù)的平方，t代表時間;根據精度要求選擇泰勒近似展開計算i/||iWf的解析多項式，得到載 體系加速度樣條多項式B;
[0062] 3.4)根據載體系加速度樣條多項式B的解析多項式進行積分計算加速度增量理論 值Bl〇
[0063] 優(yōu)選地，所述步驟3.2)中分段計算得到[0，T]區(qū)間的位置各個分量的三次樣條多 項式如式(16)所示，兩次求導得到加速度各個分量的樣條多項式如式(17)所示；
[0064]
(16) (17)
[0066] 式（16)和（17)中，么"X.jKUO分別代表位置各個分量，,_ (〇分 力0代表加速度各分里，如,1<，匕\,1{，〇\,1{，(^,1{、辦,1{，匕}^1{，〇}^1{，(1}^1{、?12,1{，132,1{，(32,1{，(12,1{分力[]代表轉 換到區(qū)間[0，τ]的樣條多項式系數(shù)，t代表時間，轉換到區(qū)間[ο，τ]的樣條多項式系數(shù)之間的 轉換矩陣如式(18)所示；
[0067] (18) 1_〇:主 一、1 ·*· V」1_ 2少」
匕-訪』十I:/」
[0068] 式（18)中，Τ代表分段樣條的分段間隔，ax,k，bx,k，cx,k，d x,k、ay,k，by,k，Cy,k，dy,k、az,k， bz,k，cz,k，dz,k分別代表轉換到區(qū)間[0，T]的樣條多項式系數(shù)，d)，4,. (G).，.喊±.(?)代表原始 數(shù)據tk時刻的位置的值，C⑷<,.(々)，么⑷代表原始數(shù)據tk時刻的位置的導數(shù)值， i 代表原始數(shù)據tk+i時刻的位置的值，4, (^+1)，4,.(4+1)，t ^ 始數(shù)據tk+1時刻的位置的導數(shù)值；
[0069] 所述步驟3.3)得到的載體系加速度樣條多項式B如式(19)所示；
[0070]
(19)
[0071] 式（19)中，代表選擇泰勒近似展開后計算得到的載體系加速度樣條多項式 8,4(/)代表原始數(shù)據為[^，^ + 1]區(qū)間轉換到[0，1']區(qū)間的加速度的樣條多項式， kW、'WAuW、心(0、心(0、'(0、心W、'W 時的慣性坐標系到載體坐標系的方向余弦矩陣# (0中的九個元素 ，|i (Of代表原始數(shù)據 為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方，t代表時間；其中，未修正時的慣性坐 標系到載體坐標系的方向余弦矩陣中的九個元素如式(20)所示；
[0072]
(20)
[0073] 式(20)中，知,(斗，氣:*?，分別代表原始[tk，tk+1M間轉換到區(qū)間[0， T]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間；
[0074] 所述步驟3.4)中根據載體系加速度樣條多項式B的解析多項式進行積分計算加速 度增量理論值B1的函數(shù)表達式如式(21)所示；
[007c1 (21)
[0076] 式(21)中，11；"(〇代表加速度增量理論值Bl，afw(/)表示載體系加速度樣條多項式 B，t代表時間。
[0077]優(yōu)選地，所述步驟4)的詳細步驟包括：
[0078] 4.1)對重力以指定時間間隔進行三次樣條函數(shù)插值得到重力各分量的三次樣條 多項式，對重力各分量的三次樣條多項式進行求導得到重力各分量的三次樣條多項式的一 階導數(shù)，然后代入時間得到重力各個分量及其一階導數(shù)的離散值；
[0079] 4.2)根據重力各個分量及其一階導數(shù)的離散值，將[以，"+1]轉換到[0，1']區(qū)間，分 段計算得到[0，T]區(qū)間的重力各個分量的三次樣條多項式；
[0080] 4.3)根據精度要求選擇泰勒近似展開計算的解析多項式，得到載體系重 力樣條多項式C，其中IliWf代表原始數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的 平方，t代表時間；
[0081] 4.4)根據載體系重力樣條多項式C的解析多項式進行積分計算重力積分增量理論 值C1。
[0082]優(yōu)選地，所述步驟4.1)中重力各分量的三次樣條多項式如式(22)所示； (22)
[0083]
[OGm」 χνν?; τW代表重力在分別在x、y、z方向的分量，ax，bx, cx，dx、 ay，by，cy，dy、az，bz，c z，dz分別代表樣條插值后三次樣條多項式的系數(shù)，t代表時間；
[0085] 所述步驟4.2)中分段計算得到[0，T]區(qū)間的重力各分量的三次樣條多項式如式 (23)所示；
[0086]
(23)
[0087] 式(23)中，(Hi W，分別代表原數(shù)據區(qū)間為[tk，tk+i]轉換到[0，Τ]區(qū)間 后重力各分里的二次樣條多項式，ax,k，bx,k，Cx,k，dx,k、iiy,k，by,k，Cy,k，dy,k、ii z,k，bz,k，Cz,k，dz,k 代表原數(shù)據區(qū)間為[tk，tk+1]轉換到[ο，τ]區(qū)間后的三次樣條多項式的系數(shù)，t代表時間；轉 換到區(qū)間[0，τ]的樣條多項式系數(shù)之間的轉換矩陣如式(24)所示；
[0088]
(24)
[0089] 式(24)中，Τ代表分段樣條的分段間隔，ax,k，bx,k，cx,k，d x,k、ay,k，by,k，Cy,k，dy,k、az,k， bz,k，cz,k，dz,k分別代表轉換到[0，T]區(qū)間后三次樣條多項式的系數(shù)，g:,, (〇.g,U^(u代 表原始數(shù)據tk時刻的重力的值，仏,(認匕，⑷⑷代表原始數(shù)據tk時刻的重力的一階導 數(shù)值，義(L)，心_ (U代表原始數(shù)據tk+1時刻的重力的值，iUkUUL MUk) 代表原始數(shù)據tk+1時刻的重力的一階導數(shù)值；
[0090] 所述步驟4.3)得到的載體系重力樣條多項式C的函數(shù)表達式如式(25)所示；
[0091] (25)
[0092] 式(25)中，4.((〇代表原始數(shù)據為[tk，tk+1]區(qū)間轉換到[0，T]區(qū)間的重力的樣條多 項式，匕4)、LW、'(，)、kW、'W分別代表 未修正時的慣性坐標系到載體坐標系的方向余弦矩陣6:,, W中的九個元素，^ Wf代表原始 數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方，'機，么.少)，敘逆分 另|J代表原始[tk，tk+1]g間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時 間；其中，未修正時的慣性坐標系到載體坐標系的方向余弦矩陣中的九個元素如式 (26)所示；
[0093]
：(26)
[0094] 式(26)中，IW，t⑴，匕小)，tW分別代表原始[tk，tk+1M間轉換到區(qū)間[0， T]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間；
[0095]所述步驟4.4)根據載體系重力樣條多項式C的解析多項式進行積分計算重力積分 增量理論值C1的函數(shù)表達式如式(27)所示；
[0096]
(27)
[0097] 式(27)中，W為載體系重力的積分增量理論值Cl，gU:〇代表載體系重力樣條 多項式C，t代表時間。
[0098] 優(yōu)選地，所述步驟5)具體是指根據式(28)所示函數(shù)表達式計算飛行器的加速度計 比力積分增量D1;
[0099]
(28)
[0100] 式(28)中，Uk(t)代表加速度計比力積分樣條多項式?(0代表加速度積分理論 值Bl，⑴代表重力積分理論值Cl，Au k,」+1和厶辦(tj+i)均代表ΛΤ時間間隔內不考慮桿臂 效應等誤差源的加速度計比力積分增量，u k(b)代表[0，h]時間的加速度計比力積分，uk(tj +ΛΤ)代表[0Α+ΛΤ]時間的加速度計比力積分，碼#代表飛行器的加速度計比力積分增 量D1，所述飛行器的加速度計比力積分增量D1為考慮桿臂效應誤差源的加速度計比力積 分，Su k(tj+AT)代表[0，tj+AT]時間的擾動加速度積分增量理論值Al>k(tj)代表[0，tj] 時間的擾動加速度積分增量理論值A1。
[0101] 本發(fā)明基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法具有下述優(yōu)點:本發(fā) 明針對INS/GNSS組合導航仿真中，捷聯(lián)慣導系統(tǒng)陀螺、加速度計信號高精度模擬問題，對實 際飛行數(shù)據進行坐標系轉換，得到ECI坐標系下的位置、速度、姿態(tài)，對姿態(tài)進行三次樣條函 數(shù)插值并計算角速率解析表達式、對位置進行三次樣條函數(shù)插值并計算載體系加速度樣條 多項式、對重力進行三次樣條函數(shù)插值并計算載體系重力樣條多項式;根據角速率解析表 達式計算擾動加速度積分增量理論值和陀螺角增量理論值，根據載體系加速度樣條多項式 進行積分計算加速度增量理論值，根據載體系重力樣條多項式進行積分計算重力積分增量 理論值;根據擾動加速度積分增量理論值進行修正加速度增量理論值，根據重力積分增量 理論值、加速度增量理論值計算飛行器的加速度計比力積分增量，根據加速度計比力積分 增量、陀螺角增量理論值進行慣導解算得到導航飛行器的動態(tài)軌跡符合載體運動學和動力 學特性，反映桿臂效應等誤差源的影響，同時與經事后處理的實測GNSS偽距、偽距率等數(shù)據 特征保持一致，且本發(fā)明基于某實際無人機飛行數(shù)據，驗證了所提出算法的有效性，能夠完 全滿足組合導航動態(tài)仿真精度要求、適用于其他高精度高動態(tài)導航系統(tǒng)和剛體運動控制仿 真中的角運動、線運動傳感器信號模擬。
【附圖說明】
[0102] 圖1為本發(fā)明實施例方法的基本流程示意圖。
[0103] 圖2為發(fā)明實施例中的載體飛行軌跡 [0104]圖3為發(fā)明實施例中的載體姿態(tài)角變化曲線。
[0105]圖4為發(fā)明實施例中姿態(tài)各分量的誤差變化曲線。
[0106]圖5為發(fā)明實施例中速度各分量的誤差變化曲線。
[0107]圖6為發(fā)明實施例中位置各分量的誤差變化曲線。
【具體實施方式】
[0108] 如圖1所示，本實施例基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法的步 驟包括：
[0109] 1)對實際飛行數(shù)據進行坐標系轉換，得到ECI坐標系下的位置、速度、姿態(tài)；
[0110] 2)對姿態(tài)進行三次樣條函數(shù)插值并計算角速率解析表達式A，根據角速率解析表 達式A計算擾動加速度積分增量理論值A1和陀螺角增量理論值A2;
[0111] 3)對位置進行三次樣條函數(shù)插值并計算載體系加速度樣條多項式B，根據載體系 加速度樣條多項式B進行積分計算加速度增量理論值B1;
[0112] 4)對重力進行三次樣條函數(shù)插值并計算載體系重力樣條多項式C，根據載體系重 力樣條多項式C進行積分計算重力積分增量理論值C1;
[0113] 5)根據擾動加速度積分增量理論值A1、加速度增量理論值B1、重力積分增量理論 值C1計算飛行器的加速度計比力積分增量D1;
[0114] 6)根據加速度計比力積分增量D1、陀螺角增量理論值A2進行慣導解算得到導航飛 行器的動態(tài)軌跡。
[0115]本實施例步驟1)中，具體是指將無人機的實際飛行數(shù)據經事后濾波處理，再進行 坐標系轉換，得到ECI坐標系下與實測飛行過程中衛(wèi)星偽距、偽距變化率數(shù)據特征一致的 200Hz頻率的位置信息和速度信息。實際飛行數(shù)據中1Hz頻率的飛機GPS/INS組合導航姿態(tài) 信息雖然不是載體真實姿態(tài)，但基本反映了飛機實際姿態(tài)變化的運動學特性。由于所生成 的陀螺、加速度計信號是時間的分段解析函數(shù)，因此不僅可以對桿臂效應進行模擬，而且可 以對不同解算頻率、不同機械編排的捷聯(lián)慣性導航數(shù)值解算算法進行一定的比較和驗證。
[0116]本實施例中，步驟2)的詳細步驟包括：
[0117] 2.1)根據姿態(tài)歐拉角計算姿態(tài)四元數(shù)；
[0118] 2.2)對姿態(tài)四元數(shù)以指定時間間隔進行三次樣條函數(shù)插值，得到姿態(tài)四元數(shù)各個 分量的三次樣條多項式;對三次樣條多項式求導得到三次樣條多項式的一階導數(shù)，代入時 間得到姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值;根據預設的四元數(shù)范數(shù)約束條件，對 姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值進行修正；
[0119] 2.3)根據修正后的姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值，將[tk，t k+1]轉換 至lJ[0，T]區(qū)間，分段計算得到[0，T]區(qū)間的姿態(tài)四元數(shù)各個分量的三次樣條多項式；
[0120] 2.4)將轉換到區(qū)間[0，Τ]姿態(tài)四元數(shù)各個分量的三次樣條多項式利用泰勒近似展 開得到角速率解析表達式Α;
[0121 ] 2.5)根據角速率解析表達式Α計算擾動加速度積分增量理論值Α1;
[0122] 2.6)根據角速率解析表達式A計算陀螺角增量理論值A2。
[0123] 本實施例中，步驟2.1)計算姿態(tài)四元數(shù)的函數(shù)表達式如式(1)所示；
[0124]
(1)
[0125] 式（1)中，9〇^1^2^3分別為姿態(tài)四元數(shù)的四個元素，丫為姿態(tài)的滾轉角，#為姿態(tài) 的俯仰角，為姿態(tài)的偏航角；
[0126] 步驟2.2)中，姿態(tài)四元數(shù)各個分量的三次樣條多項式的函數(shù)表達式如式(2)所示， 預設的四元數(shù)范數(shù)約束條件如式(3)所示，對姿態(tài)四元數(shù)各個分量進行修正的函數(shù)表達式 如式(4)所示。根據樣條插值誤差界的相關理論，得到的各個區(qū)間[0，T]的姿態(tài)四元數(shù)樣條 多項式在非插值點仍不滿足范數(shù)為1的約束條件，因此在計算陀螺角速率、角增量時也需要 對姿態(tài)四元數(shù)范數(shù)進行修正，將角速率視為標量為〇的四元數(shù)，對姿態(tài)四元數(shù)各個分量的一 階導數(shù)的離散值進行修正的函數(shù)表達式如式(5)所示；
[0127] q(t) = af3 +h/- +cii + di ⑵
[0128] 式(2)中，$(0為姿態(tài)四元數(shù)各個分量的三次樣條多項式，ai，bi，ci，di分別為樣條 插值后三次樣條多項式的系數(shù)，t為時間；
[0129] (3)
[0130]式⑶中，I I q(t) I I代表t時刻的姿態(tài)四元數(shù)q(t)的范數(shù)，qo(t)，qi(t)，q2(t)，q3 (t)分別為姿態(tài)四元數(shù)q(t)的四個元素，^，⑴名(/)，士 (/)，么(/)分別為姿態(tài)四元數(shù)(1(〇的四 個元素的一階導數(shù)；
[0131]
(4)
[0132] 式(4)中，qi(tk)代表插值點tk時刻的姿態(tài)四元數(shù)各個分量的修正結果，歹九)代表 插值點tk時刻姿態(tài)四元數(shù)的各個分量，||f h }|代表插值點tk時刻姿態(tài)四元數(shù)的范數(shù)；
[01331
C5)
[0134] 式(5)中4(4)代表插值點tk時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)的修正結果， )代表插值點tk時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)，||牙(_々)|代表插值點tk時刻姿態(tài) 四元數(shù)5(4)的范數(shù);f )|)代表插值點tk時刻姿態(tài)四元數(shù)f (4)的范數(shù)Ilf h )||的求導結果， 其函數(shù)表達式如式(6)所示；
[0135]
[0136] 式(6)中，^"(^)4(〇名(^)名(^)分別為插值點"時刻的姿態(tài)四元數(shù)的四個元素， 乞⑷⑷名⑷分別為插值點tk時刻的姿態(tài)四元數(shù)的四個元素一階導數(shù)；
[0137] 所述步驟2.3)分段計算得到[0，T]區(qū)間的姿態(tài)四元數(shù)各個分量的三次樣條多項式 如式(7)所示；
[0138]
(7)
[0139] 式(7)中，代表轉換到區(qū)間[0，Τ]的插值樣條多項式，ai,k，bi,k，Ci,k，di,k分別為 轉換到[0，T]區(qū)間后的三次樣條多項式系數(shù)，t代表時間;轉換到[0，T]區(qū)間后的三次樣條多 項式系數(shù)的計算函數(shù)表達式如式(8)所示；
[0140]
C8；
[0141] 式(8)中，T代表分段樣條的分段間隔，qi(tk)代表tk時刻的姿態(tài)四元數(shù)中第i個元 素的值，料?代表tk時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值， qi(tk+1)代表tk+1時刻的姿 態(tài)四元數(shù)中第i個元素的值代表t k+1時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值，符 號i取值為〇或1或2或3，&^九, 1{，(^1{，(^分別為轉換到區(qū)間[0，1']后的插值多項式系數(shù)；
[0142] 本實施例中，步驟2.4)的詳細步驟包括:根據式(9)所示的限制條件代入式（10)所 示角速率的計算式，對姿態(tài)四元數(shù)范數(shù)的平方的倒數(shù)部分進行泰勒近似展開，得到 角速率解析表達式A如式(11)所述；
[0143]
C9)
[0144] 式(9)中，qk⑴代表姿態(tài)四元數(shù)，代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[0，T] 區(qū)間后的姿態(tài)四元數(shù)，|1(，)丨|代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[ο，τ]區(qū)間后的姿態(tài)四元 數(shù)的范數(shù)，*U0代表姿態(tài)四元數(shù)的一階導數(shù)Λ⑴代表原始數(shù)據區(qū)間為[t k，tk+1]轉換到[0， τ]區(qū)間后的姿態(tài)四元數(shù)的一階導數(shù)，M?)，H)，Μ小分別代表原始[tk，tk+1M 間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)44)各個元素，4』)，L卞)，L(i)分別代表 原始[tk，tk+1]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)各個元素的一階導數(shù)，t代表時間；
[0145]
(10)
[0146] 式（10)中，代表角速率解析表達式A，角速率解析表達式A為姿態(tài)四元數(shù)的樣 條多項式，下標ib代表載體坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[t k，tk+1]、上 標b代表載體坐標系；qk(t)代表姿態(tài)四元數(shù)的樣條多項式，屯(0代表姿態(tài)四元數(shù)的樣條多 項式的一階導數(shù)，t代表時間；
[0147]
(11；
[0148] 式（11)中，<』(〇代表角速率的時間函數(shù)，下標ib代表載體坐標系相對于慣性坐標 系、k代表原始數(shù)據為區(qū)間[t k，tk+1]、上標b代表載體坐標系;.W|代表原始數(shù)據區(qū)間為[tk， tk+1]轉換到[0，T]區(qū)間后的姿態(tài)四元數(shù)的范數(shù)，MO, mo分別代表原始 [t k，t k + i ]區(qū)間轉換到區(qū)間[0，T ]后的姿態(tài)四元數(shù)屯(〇各個元素， 4w(〇, '⑴，H 44)分別代表原始[tk，tk+1]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù) 各個元素的一階導數(shù)，t代表時間；
[0149] 對姿態(tài)四元數(shù)范數(shù)的平方的倒數(shù)部分1/&(if進行泰勒近似展開時，可以根據需 要選擇其近似展開階數(shù)，例如進行泰勒一階和二階近似展開時展開結果分別如式（11-1)和 (11-2)所示。將展開結果代入式（11)，則可以得到角速率解析表達式A的函數(shù)表達式如式 (11-3)或（11-4)所示。
[0150]
014)
[0151] 式（11-1)中，IMM代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[ο，τ]區(qū)間后的姿態(tài)四元 數(shù)的范數(shù)，t代表時間； 「01521
(11-2)
[0153] 式（11-2)中，|?U〇||代表原始數(shù)據區(qū)間為[tk，tk+1]轉換到[ο，τ]區(qū)間后的姿態(tài)四元 數(shù)的范數(shù)，t代表時間；
[0154]
[0156] 式（11-3)和式（11-4)中，(〇代表角速率解析表達式A，角速率解析表達式A為姿 態(tài)四元數(shù)的樣條多項式，下標ib代表載體坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間 [^1]、上標13代表載體坐標系;|屯_(〇||代表原始數(shù)據區(qū)間為[^ 1]轉換到[0，1']區(qū)間后 的姿態(tài)四元數(shù)的范數(shù)的樣條多項式，IW, H), 分別代表原始[tk，tk+1]g 間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，4.W), 4.4)，4i⑴分 別代表原始[tk，tk+1]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)各個元素的時間函數(shù)的一階導 數(shù)，t代表時間；
[0157] 加速度計偏離IMU質心而造成的桿臂效應或尺寸效應引起的擾動加速度，飛行器 中三個方向的加速度計由于物理結構上的差異，其相對頂U質心的偏倚量各不相同，因此考 慮桿臂效應或尺寸效應引起的擾動加速度，使得仿真動態(tài)軌跡解析更加準確。本實施例中， 步驟2.5)包括首先根據式（12)所示的擾動加速度解析積分表達式計算擾動加速度積分理 論值，然后根據指定時間間隔ΛΤ的擾動加速度積分理論值SUk(t)和SUk(t+AT)計算差值， 得到指定的時間間隔ΛΤ內的擾動加速度積分增量理論值Α1;
[0158]
C12)
[0159] 式(12)中，δυ??⑴代表[0，t]時間的擾動加速度積分理論值，β4α(Κα(Κ4) 分別為角速率分別在X、y、ζ方向的解析表達式;.代表X方向加速度計偏離質心的桿臂矢量 <中的X方向偏移量，4代表y方向加速度計偏離質心的桿臂矢量4中的y方向偏移量，4 代表z方向加速度計偏離質心的桿臂矢量4中的z方向偏移量，r丨、 4 4 分別為三個方向的加速度計偏離質心的桿臂矢量 且均為常值，t代表時間；
[0160]步驟2.6)具體是指根據式(13)計算陀螺角增量理論值A2;
[0161]
(13)
[0162] 式（13)中，0k⑴代表由角速率解析表達式A積分得到的陀螺角增量，?UW代表角 速率解析表達式A，所述角速率解析表達式A為姿態(tài)四元數(shù)的樣條多項式，下標ib代表載體 坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[tk，t k+i]、上標b代表載體坐標系，A9k,j+1 代表ΛΤ時間間隔內的陀螺角增量理論值A2，0k(t〇代表[0山]時間的陀螺角積分增量，0k (t j+ΛΤ)代表[0，t j+ΛΤ ]時間的陀螺角積分增量，t代表時間。
[0163] 對1Hz ECI坐標系下位置信息進行插值，得到位置的三次樣條解析多項式，求導得 到速度的二次樣條解析多項式。同樣，為避免時間增大帶來的誤差影響，用類似式角速率的 方法，將[t k，tk+1]轉換到[0，T]區(qū)間，分段計算得到[0，T]區(qū)間的位置各個分量的三次樣條 多項式。本實施例中，步驟3)的詳細步驟包括：
[0164] 3.1)對ECI坐標系下位置的各個分量進行樣條插值得到位置的樣條多項式，對所 得到的位置的樣條多項式求一次導得到速度的樣條多項式，分別代入時間得到位置和速度 的離散值；
[0165] 3.2)根據位置和速度的離散值，將[^，^+1]轉換到[0，1']區(qū)間，分段計算得到[0， Τ]區(qū)間的位置各個分量的三次樣條多項式，進行兩次求導得到加速度各個分量的樣條多項 式<?(0，其中k代表原始數(shù)據區(qū)間為[t k，tk+1];
[0166] 3.3)結合坐標系轉換，得到載體系b中的加速度的解析表達式如式（14)所 示；
[0167]
Π 4)
[0168] 式（14)中，<』.(〇代表載體系b中的加速度的樣條多項式，a:w(〇代表原始數(shù)據為
[tk，tk+1]區(qū)間轉換到[0，T]區(qū)間的加速度的樣條多項式，代表無誤差慣性坐標系到載 體坐標系的方向余弦矩陣，t代表時間，其中方向余弦矩陣C,M〇與姿態(tài)四元數(shù)的轉換關系 如式(15)所示；
[01691 (15)
[ω /u」 auwt衣兀扶左?員丨王坐標系到載體坐標系的方向余弦矩陣，<(/)代表未 修正時的慣性坐標系到載體坐標系的方向余弦矩陣，AuD、Au⑴、 心W、4.,,(0、4，)、k⑴分別代表未修正時的慣性坐標系到載體坐標系的方向余弦 矩陣<(0中的九個元素，|i(0f代表原始數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范 數(shù)的平方，t代表時間;根據精度要求選擇泰勒近似展開計算1/||^0|2的解析多項式，得到載 體系加速度樣條多項式B;
[0171] 3.4)根據載體系加速度樣條多項式B的解析多項式進行積分計算加速度增量理論 值Bl〇
[0172] 本實施例中，步驟3.2)中分段計算得到[0，T]區(qū)間的位置各個分量的三次樣條多 項式如式(16)所示，兩次求導得到加速度各個分量的樣條多項式如式(17)所示；
[0173]
(16) (17)
[0175] 式（16)和（17)中，^的、，._4,.,*(0，<,』(0分別代表位置各個分量， ，Cx，k，dx，k、Ely，k，by，k，Cy，k，dy，k、，k， 1^，(^，(^分別代表轉換到區(qū)間[0，1']的樣條多項式系數(shù)4代表時間，轉換到區(qū)間[0，1'] 的樣條多項式系數(shù)之間的轉換矩陣如式(18)所示；
[0176]
(1:8)
[0177] 式（18)中，T代表分段樣條的分段間隔，ax,k，bx,k，cx,k，d x,k、ay,k，by,k，Cy,k，dy,k、az,k， bz,k，cz,k，dz,k分別代表轉換到區(qū)間[0，T]的樣條多項式系數(shù)，代表原始 數(shù)據t k時刻的位置的值，.，也Κ_ι(~)代表原始數(shù)據tk時刻的位置的導數(shù)值， 4(LU_ (L)代表原始數(shù)據tk+1時刻的位置的值，g, (L)，4,.(U)，4(U)代表原始 數(shù)據tk+1時刻的位置的導數(shù)值；
[0178] 步驟3.3)得到的載體系加速度樣條多項式B如式(19)所示；
[0179]
(19)
[0180] 式（19)中，⑴代表選擇泰勒近似展開后計算得到的載體系加速度樣條多項式 8,44)代表原始數(shù)據為[^，^ + 1]區(qū)間轉換到[0，1']區(qū)間的加速度的樣條多項式， Wk W、(，）、&-(，）、匕4)、匕 a (0、4X (?)、' W 分別代表未修正 時的慣性坐標系到載體坐標系的方向余弦矩陣<(〇中的九個元素，||?Α(〇|2代表原始數(shù)據為 [t k，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方，t代表時間；其中，未修正時的慣性坐標 系到載體坐標系的方向余弦矩陣#氺)中的九個元素如式(20)所示；
[0181] (20)
[0182] 式(20)中，bW，t⑴，LW，分別代表原始[tk，tk+1]g間轉換到區(qū)間[0， T]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間；
[0183] 針對步驟3.3)得到的載體系加速度樣條多項式B，根據對選擇的泰勒近似 開展方式不同而有所不同。例如采用泰勒一階近似展開時，具體得到的載體系加速度樣條 多項式B如式(19-1)所示;采用泰勒二階近似展開時，具體得到的載體系加速度樣條多項式 B如式(19-2)所示；
[0184]
Π 9 1 ' 19 2)
[0186] 式(19-1)為選擇泰勒一階近似展開計算1/化(〇f的解析多項式得到的載體系加速 度樣條多項式B，式(19-2)為選擇泰勒二階近似展開計算的解析多項式得到的載體系 加速度樣條多項式Β，式(19-1)和式(19-2)中，a。(〇代表載體系加速度樣條多項式Β，a; w⑴代 表原始數(shù)據為[tk，tk+1]區(qū)間轉換到[0，T]區(qū)間的加速度的樣條多項式，⑷代表無誤差慣性坐標 系到載體坐標系的方向余弦矩陣，LW、44)、kW、⑴、匕4)、kW、44)、 4』}、& W分別代表未修正時的慣性坐標系到載體坐標系的方向余弦矩陣(，)中的九 個元素，||4^)|2代表原始數(shù)據為[t k，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方， 4u(0U)H)s 分別代表原始[tk，tk+1謳間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各 個元素的時間函數(shù)，t代表時間。
[0187] 步驟3.4)中根據載體系加速度樣條多項式B的解析多項式進行積分計算加速度增 量理論值B1的函數(shù)表達式如式(21)所示；
[0188]
(21)
[0189] 式(21)中，《1(0代表加速度增量理論值Bl，afw.(/)表示載體系加速度樣條多項式 B，t代表時間。
[0190] 同理，加速度增量理論值B1根據對l/||iWf選擇的泰勒近似開展方式不同而有所 不同。例如采用泰勒一階近似展開時，具體得到的加速度增量理論值B1如式(21-1)所示;采 用泰勒二階近似展開時，具體得到的加速度增量理論值B1如式(21-2)所示，其中載體系加 速度樣條多項式B已代入式(19);
[0191] (21-15
(21-2)
[0193] 式(21-1)和（21-2)中，ιΚο代表加速度增量理論值B1，||M0|代表原始數(shù)據區(qū)間 為[t k，t k + i ]轉換到[0，T ]區(qū)間后的姿態(tài)四元數(shù)的范數(shù)的樣條多項式， riuW、A:i.(0、AuW、h.U ⑴、Au.W、AijiW、分別代表未修正時 的慣性坐標系到載體坐標系的方向余弦矩陣中的九個元素，代表原始數(shù)據區(qū)間為 [tk，tk+1]轉換到[ο，τ]區(qū)間的慣性坐標系下加速度的樣條多項式，t代表時間。
[0194] 本實施例中，步驟4)的詳細步驟包括：
[0195] 4.1)對重力以指定時間間隔進行三次樣條函數(shù)插值得到重力各分量的三次樣條 多項式，對重力各分量的三次樣條多項式進行求導得到重力各分量的三次樣條多項式的一 階導數(shù)，然后代入時間得到重力各個分量及其一階導數(shù)的離散值；
[0196] 4.2)根據重力各個分量及其一階導數(shù)的離散值，將[以，"+1]轉換到[0，1']區(qū)間，分 段計算得到[0，T]區(qū)間的重力各個分量的三次樣條多項式；
[0197] 4.3)根據精度要求選擇泰勒近似展開計算l/||i (〇f的解析多項式，得到載體系重 力樣條多項式c，其中|4,(/)f代表原始數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的 平方，t代表時間；
[0198] 4.4)根據載體系重力樣條多項式C的解析多項式進行積分計算重力積分增量理論 值C1。
[0199] 根據重力計算式，由ECI系下的位置值得到離散的重力值。根據重力數(shù)據求飛行航 跡中重力各分量的三次樣條多項式，同樣為減小時間增大帶來的誤差影響，將樣條多項式 由[t k，tk+1]轉換到[ο，τ]區(qū)間，分段計算得到[ο，τ]區(qū)間的重力各個分量的三次樣條多項 式。
[0200] 本實施例中，步驟4.1)中重力各分量的三次樣條多項式如式(22)所示；
[0201]
(22)：
[0202] 式(22)中，'*)，義.,.(小':(〇代表重力在分別在1、7、2方向的分量，3^1^，(^，(^、 ay，by，Cy，dy、az，bz，cz，d z分別代表樣條插值后三次樣條多項式的系數(shù)，t代表時間；
[0203]本實施例中，步驟4.2)中分段計算得到[0，T]區(qū)間的重力各分量的三次樣條多項 式如式(23)所示；
[0204]
(23)
[0205] 式(23)中，'分別代表原數(shù)據區(qū)間為[tk，tk+1]轉換到[0，T]區(qū)間 后重力各分里的二次樣條多項式，ax,k，bx,k，Cx,k，dx,k、ay,k，by,k，Cy,k，dy,k、az,k，bz,k，Cz,k，dz,k 代表原數(shù)據區(qū)間為[tk，tk+1]轉換到[ο，τ]區(qū)間后的三次樣條多項式的系數(shù)，t代表時間；轉 換到區(qū)間[0，τ]的樣條多項式系數(shù)之間的轉換矩陣如式(24)所示；
[0206]
[0207] 式(24)中，T代表分段樣條的分段間隔，ax,k，bx,k，cx,k，d x,k、ay,k，by,k，Cy,k，dy,k、az,k， bz,k，cz,k，dz,k分別代表轉換到[0，T]區(qū)間后三次樣條多項式的系數(shù)，(〇,^私.)_，磁,_ (?,)代 表原始數(shù)據tk時刻的重力的值，Λ 代表原始數(shù)據tk時刻的重力的一階導數(shù) 值，少匕:(L)代表原始數(shù)據tk+i時刻的重力的值， 表原始數(shù)據tk+1時刻的重力的一階導數(shù)值；
[0208] 本實施例中，步驟4.3)得到的載體系重力樣條多項式C的函數(shù)表達式如式(25)所 示；
[0209]
(25)
[0210] 式(25)中，表原始數(shù)據為[tk，tk+1謳間轉換到[0，T]區(qū)間的重力的樣條多 項式，4,4)、Ι.Α(0、kW、&(，)、4^)、心 未修正時的慣性坐標系到載體坐標系的方向余弦矩陣私(4中的九個元素，IM〇f代表原始 數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方，H Μ小MO' 分 另|J代表原始[tk，tk+1]g間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時 間；其中，未修正時的慣性坐標系到載體坐標系的方向余弦矩陣中的九個元素如式 (26) (26)所示；
[02111
[0212] 式（26)中，
丨：?，/分別代表原始[tk，tk+i]區(qū)間轉換到區(qū)間[0， T]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間；
[0213] 步驟4.3)得到的載體系重力樣條多項式C根據選擇泰勒展開方式不同而有所不 同，例如采用一階泰勒近似展開時得到的載體系重力樣條多項式C的函數(shù)表達式如式(25- 1) 所示，采用二階泰勒近似展開時得到的載體系重力樣條多項式C的函數(shù)表達式如式(25- 2) 所示；
[0214]
(25-1) (25-2)
[0216] 式(25-1)為采用選擇泰勒一階近似展開計算:l/||M〇|f的解析多項式得到的載體系 重力樣條多項式C，式(25-2)為采用選擇泰勒二階近似展開計算l/||iWf的解析多項式得到 的載體系重力樣條多項式C;式(25-1)和式(25-2)中，代表原始數(shù)據為[tk，tk+1]區(qū)間轉 換到[0，T]區(qū)間的重力的樣條多項式，代表無誤差慣性坐標系到載體坐標系的方向余 弦矩陣，'W、. (:4、'切、W、匕.* (0、 表未修正時的慣性坐標系到載體坐標系的方向余弦矩陣A (4中的九個元素，|M〇f代表原 始數(shù)據為[tk，tk+1]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方，Μ中H)' MO' 分別代表原始[tk，tk+1]g間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表 時間。
[0217] 本實施例中，步驟4.4)根據載體系重力樣條多項式C的解析多項式進行積分計算 重力積分增量理論值C1的函數(shù)表達式如式(27)所示；
[0218] (27)
[0219]式(27)中，⑴為載體系重力的積分增量理論值Cl，gH)代表載體系重力樣條 多項式C，t代表時間。
[0220]同理，步驟4.4)得到的重力積分增量理論值C1的函數(shù)表達式根據選擇泰勒展開方 式不同而有所不同，例如采用一階泰勒近似展開時得到的重力積分增量理論值C1的函數(shù)表 達式如式(27-1)所示，采用二階泰勒近似展開時得到的重力積分增量理論值C1的函數(shù)表達 式如式(27-2)所示，其中載體系重力樣條多項式C已代入式(27);
[0221]
(27-1) (27-2)
[0223] 式(27-1)和(27-2)中，為載體系重力的積分增量理論值Cl，g;w(0代表從原 始區(qū)間[tk，tk+1]轉換到[ο，τ]區(qū)間的慣性坐標系下重力的樣條多項式，It⑷II代表原始數(shù)據 區(qū)間為[t k，tk+1]轉換到[ο，τ]區(qū)間后的四元數(shù)時間函數(shù)對應的姿態(tài)四元數(shù)的范數(shù)，t代表時 間，4,* W 4,* W 正時的慣性坐標系到載體坐標系的方向余弦矩陣GJO中的九個元素。
[0224] 本實施例中，步驟5)具體是指根據式(28)所示函數(shù)表達式計算飛行器的加速度計 比力積分增量D1;
[0225]
128)
[0226] 式(28)中，uk(t)代表加速度計比力積分樣條多項式?(〇代表加速度積分理論 值代表重力積分理論值Cl，Au k,川和Auk(t川)均代表ΛΤ時間間隔內不考慮桿臂 效應等誤差源的加速度計比力積分增量，u k(b)代表[0，h]時間的加速度計比力積分，uk(tj +ΛΤ)代表[0山+ΛΤ]時間的加速度計比力積分，△<,+,代表飛行器的加速度計比力積分增 量D1，所述飛行器的加速度計比力積分增量D1為考慮桿臂效應誤差源的加速度計比力積 分，Su k(tj+AT)代表[0，tj+AT]時間的擾動加速度積分增量理論值Al>k(tj)代表[0，tj] 時間的擾動加速度積分增量理論值A1。
[0227] 本實施例中所采用的實際飛行數(shù)據長度1200s，無人機做復雜機動飛行，導航參數(shù) 記錄頻率為200Hz，包含煒度、經度、高程和姿態(tài)歐拉角數(shù)據，以及相應的衛(wèi)星偽距、偽距率 信息。圖2給出了載體的運動軌跡，圖3給出了載體整個過程中的姿態(tài)變化，其中橫坐標均為 時間（單位為秒），Heading Angle代表偏航角P，_Pitch Angle代表俯仰角設.，.Roll Angle代 表滾轉角γ，上述角度的單位均為°，從圖2和圖3中可以看出載體運動的動態(tài)特性和航跡的 復雜程度。本實施例中，算法插值解析積分區(qū)間為ls，分別計算400Η Ζ、200ΗΖ、100Ηζ的陀螺 角增量和比力積分增量，采用雙子樣算法進行慣導解算，由等效轉動矢量計算四元數(shù)時采 用四階算法，對采用不同導航解算周期的導航參數(shù)精度進行比較。
[0228] 此外，四元數(shù)范數(shù)平方倒數(shù)l/|i(〇|f的泰勒近似展開分別采用一階、二階近似時， 通過比較400Hz采樣的雙子樣姿態(tài)計算精度，比較所生成的陀螺角增量精度。不同泰勒近似 展開方法時姿態(tài)歐拉角誤差如表1所示。
[0229] 表1:不同泰勒近似時姿態(tài)歐拉角誤差表。
[0230]
[0231] 表1給出了分別采用式（11-1)泰勒一階近似展開和式（11-2)泰勒二階近似展開 時，1200s時間內計算得到的姿態(tài)歐拉角最大誤差值。由表格數(shù)據可以看出，采用式（11-2) 的近似其精度優(yōu)于采用式（11-1)的近似，采用公式(11-1)計算得到的精度也滿足基本的仿 真需要。
[0232] 采用式（11-2)作四元數(shù)范數(shù)平方倒數(shù)的泰勒級數(shù)展開二階近似，生成100Hz、 200Ηζ、400Ηζ理想陀螺角增量和加速度計比力積分增量信號，采用雙子樣算法分別進行 50Hz、100Hz和200Hz的無初始誤差的純慣導解算，比較相應的姿態(tài)、速度和位置誤差，得到 如表2所示導航參數(shù)誤差數(shù)據和圖4~6所示導航參數(shù)誤差隨時間變化曲線，其中圖4為姿態(tài) 各分量的誤差變化曲線，圖5為速度各分量的誤差變化曲線，圖6為位置各分量的誤差變化 曲線。
[0233] 表2:不同解算頻率時捷聯(lián)慣導計算誤差表。
[0234]
L0235」從圖4~6可以看出，導航解算頻率越高，精度越高，且符合捷聯(lián)慣導誤差隨時間枳 累的規(guī)律，這也就說明，本實施例所使用的方法得到的角增量和比力積分增量可以用來檢 驗算法的性能。另一方面，表2給出的基于理想陀螺、加速度計信號的導航參數(shù)計算誤差的 模值，遠遠小于當前工程實際中由于陀螺、加速度計本身誤差造成的捷聯(lián)慣性導航誤差，從 精度的角度來看，本實施例所使用的利用實際飛行數(shù)據插值來解析生成模擬陀螺、加速度 計信號，完全滿足INS/GNSS組合導航動態(tài)仿真精度要求。
[0236] 此外，從圖4所示姿態(tài)誤差隨時間變化曲線可以看出，姿態(tài)誤差的變化呈現(xiàn)不規(guī)則 波動，這是由于在進行姿態(tài)插值計算理想陀螺角增量時，四元數(shù)約束插值中，四元數(shù)范數(shù)平 方倒數(shù)的泰勒級數(shù)展開得到的樣條多項式不能完全逼近真值導致的，另外，計算機計算過 程中的截斷誤差對精度也有一定的影響。但相應的姿態(tài)誤差已經足夠小，對INS/GNSS組合 導航動態(tài)仿真沒有影響。
[0237] 綜上所述，本實施例針對INS/GNSS組合導航仿真中器件信號的高精度模擬問題， 提出了基于實際飛行數(shù)據插值的動態(tài)仿真軌跡解析生成算法，通過分段樣條插值，結合約 束條件和泰勒近似展開得到了載體坐標系下陀螺角速率、角增量以及加速度計比力積分增 量的高精度解析表達式，具有下述優(yōu)點：（1)本實施例提出了基于實際飛行數(shù)據插值的動態(tài) 仿真軌跡解析生成算法，生成的陀螺、加速度計信號符合載體運動學和動力學特性，在此基 礎上疊加陀螺、加速度計系統(tǒng)性誤差模型以及實際靜態(tài)測試隨機噪聲數(shù)據，可使仿真的陀 螺、加速度計信號更貼近工程實際。仿真結果驗證了所提出算法的可行性，陀螺、加速度計 信號模擬精度完全滿足組合導航動態(tài)仿真精度要求。（2)本實施例在加速度計信號仿真中， 可反映桿臂效應的影響，由于基于實測飛行航跡的位置、速度進行插值，所生成仿真軌跡可 同時與經事后處理的實測GNSS偽距、偽距率等數(shù)據特征保持一致，有利于組合導航系統(tǒng)的 仿真分析。（3)本實施例所提出仿真軌跡生成算法可以有效地模擬動態(tài)環(huán)境條件下復雜航 跡的陀螺、加速度計信號，以支撐INS/GNSS組合導航算法性能評估和系統(tǒng)仿真測試，同時也 適用于其他高精度高動態(tài)導航系統(tǒng)和剛體運動控制仿真中的角運動、線運動傳感器信號模 擬。
[0238]以上所述僅是本發(fā)明的優(yōu)選實施方式，本發(fā)明的保護范圍并不僅局限于上述實施 例，凡屬于本發(fā)明思路下的技術方案均屬于本發(fā)明的保護范圍。應當指出，對于本技術領域 的普通技術人員來說，在不脫離本發(fā)明原理前提下的若干改進和潤飾，這些改進和潤飾也 應視為本發(fā)明的保護范圍。
【主權項】
1. 一種基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特征在于步驟包括： 1) 對實際飛行數(shù)據進行坐標系轉換，得到ECI坐標系下的位置、速度、姿態(tài)； 2) 對姿態(tài)進行Ξ次樣條函數(shù)插值并計算角速率解析表達式A，根據角速率解析表達式A 計算擾動加速度積分增量理論值A1和巧螺角增量理論值A2; 3) 對位置進行Ξ次樣條函數(shù)插值并計算載體系加速度樣條多項式B，根據載體系加速 度樣條多項式B進行積分計算加速度增量理論值B1; 4) 對重力進行Ξ次樣條函數(shù)插值并計算載體系重力樣條多項式C，根據載體系重力樣 條多項式C進行積分計算重力積分增量理論值C1; 5) 根據擾動加速度積分增量理論值A1、加速度增量理論值B1、重力積分增量理論值C1 計算飛行器的加速度計比力積分增量D1; 6) 根據加速度計比力積分增量D1、巧螺角增量理論值A2進行慣導解算得到導航飛行器 的動態(tài)軌跡。2. 根據權利要求1所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟2)的詳細步驟包括： 2.1) 根據姿態(tài)歐拉角計算姿態(tài)四元數(shù)； 2.2) 對姿態(tài)四元數(shù)W指定時間間隔進行Ξ次樣條函數(shù)插值，得到姿態(tài)四元數(shù)各個分量 的Ξ次樣條多項式;對所述Ξ次樣條多項式求導得到Ξ次樣條多項式的一階導數(shù)，代入時 間得到姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值;根據預設的四元數(shù)范數(shù)約束條件，對 姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值進行修正； 2.3) 根據修正后的姿態(tài)四元數(shù)各個分量及其一階導數(shù)的離散值，將[tk，tk+i]轉換到[0， T]區(qū)間，分段計算得到[0，Τ]區(qū)間的姿態(tài)四元數(shù)各個分量的Ξ次樣條多項式； 2.4) 將轉換到區(qū)間[0，Τ]姿態(tài)四元數(shù)各個分量的Ξ次樣條多項式利用泰勒近似展開得 到角速率解析表達式A; 2.5) 根據角速率解析表達式A計算擾動加速度積分增量理論值A1; 2.6) 根據角速率解析表達式A計算巧螺角增量理論值A2。3. 根據權利要求2所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟2.1)計算姿態(tài)四元數(shù)的函數(shù)表達式如式(1)所示；(1) 式（1)中，9〇地，92,93分別為姿態(tài)四元數(shù)的四個元素，丫為姿態(tài)的滾轉角，?9為姿態(tài)的俯 仰角，於％姿態(tài)的偏航角； 所述步驟2.2)中，姿態(tài)四元數(shù)各個分量的Ξ次樣條多項式的函數(shù)表達式如式(2)所示， 預設的四元數(shù)范數(shù)約束條件如式(3)所示，對姿態(tài)四元數(shù)各個分量進行修正的函數(shù)表達式 如式(4)所示，對姿態(tài)四元數(shù)各個分量的一階導數(shù)的離散值進行修正的函數(shù)表達式如式巧） 所示； ^. (/) = + cj + d. (2) 式(2)中，牽W為姿態(tài)四元數(shù)各個分量的Ξ次樣條多項式，曰1，61，(：1，(11分別為樣條插值后 Ξ次樣條多項式的系數(shù)，t為時間；(3) 式(3)中，Mq(t)M代表t時刻的姿態(tài)四元數(shù)q(t)的范數(shù)，q〇(t)，qi(t)，q2(t)，q3(t)分別 為姿態(tài)四元數(shù)q (t)的四個元素，如(/ U (0,如(0.車(0分別為姿態(tài)四元數(shù)q (t)的四個元素的 一階導數(shù)；(4) 式(4)中，qi(tk)代表插值點tk時刻的姿態(tài)四元數(shù)各個分量的修正結果，則。代表插值 點tk時刻姿態(tài)四元數(shù)的各個分量，防。1代表插值點tk時刻姿態(tài)四元數(shù)的范數(shù)；(5) 式(5)中，h)代表插值點tk時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)的修正結果，妾^) 代表插值點tk時刻姿態(tài)四元數(shù)中各個分量的一階導數(shù)，代表插值點tk時刻姿態(tài)四元 數(shù)巧y的范數(shù);新|也)|)代表插值點tk時刻姿態(tài)四元數(shù)抓)的范數(shù)||5(0||的求導結果，其函 數(shù)表達式如式(6)所示；(6) 式（6)中，吊hUh)毒似，否(0分別為插值點tk時刻的姿態(tài)四元數(shù)的四個元素， 完(0，禹k)，禹(。，擊h)分別為插值點tk時刻的姿態(tài)四元數(shù)的四個元素一階導數(shù)； 所述步驟2.3)分段計算得到[0，Τ]區(qū)間的姿態(tài)四元數(shù)各個分量的Ξ次樣條多項式如式 (7)所示； 山'如）=。，/ +b,/ +CJ +戍蛛 口) 式(7)中，心(0代表轉換到區(qū)間[0，Τ]的插值樣條多項式，曰川，6川，(3川，(1川分別為轉換 至lJ[〇，T]區(qū)間后的Ξ次樣條多項式系數(shù)，t代表時間;轉換到[0，Τ]區(qū)間后的Ξ次樣條多項式 系數(shù)的計算函數(shù)表達式如式(8)所示；(致) 式(8)中，Τ代表分段樣條的分段間隔，qi(tk)代表tk時刻的姿態(tài)四元數(shù)中第i個元素的 值，系化)代表tk時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值，qi(tk+i)代表tk+1時刻的姿態(tài)四 元數(shù)中第i個元素的值，<),心)代表tk+i時刻姿態(tài)四元數(shù)中第i個元素的一階導數(shù)值，符號i取 值為0或域2或3，曰1,川1,1^，化1^，(11,汾別為轉換到區(qū)間[0，門后的插值多項式系數(shù)； 所述步驟2.4)的詳細步驟包括:根據式(9)所示的限制條件代入式（10)所示角速率的 計算式，對姿態(tài)四元數(shù)范數(shù)的平方的倒數(shù)部分1/||心Wf進行泰勒近似展開，得到角速率解析 表達式A如式(11)所述；(9) 式(9)中，qk(t)代表姿態(tài)四元數(shù)，若約代表原始數(shù)據區(qū)間為[tk，tk+i]轉換到[〇，Τ]區(qū)間 后的姿態(tài)四元數(shù)，|也(0||代表原始數(shù)據區(qū)間為[tk，tk+i]轉換到[〇，Τ]區(qū)間后的姿態(tài)四元數(shù)的 范數(shù)Λ(0代表姿態(tài)四元數(shù)的一階導數(shù)，也(0代表原始數(shù)據區(qū)間為[tk，tk+i]轉換到[〇，Τ]區(qū) 間后的姿態(tài)四元數(shù)的一階導數(shù)，如*(0，如心(0分別代表原始[tk，tk+i]區(qū)間轉 換到區(qū)間[0，τ]后的姿態(tài)四元數(shù)％(/)各個元素，知,(〇, 的，4,:(/)分別代表原始 [tk，tk+i]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)各個元素的一階導數(shù)，t代表時間； <,-(') = 2q;-(〇噸.(/): (10) 式（10)中，代表角速率解析表達式A，所述角速率解析表達式A為姿態(tài)四元數(shù)的樣 條多項式，下標化代表載體坐標系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[tk，tk+i]、上 標b代表載體坐標系；qk(t)代表姿態(tài)四元數(shù)的樣條多項式，代表姿態(tài)四元數(shù)的樣條多 項式的一階導數(shù)，t代表時間；(11 ； 式（11)中，的i(0代表角速率的時間函數(shù)，下標化代表載體坐標系相對于慣性坐標系、k 代表原始數(shù)據為區(qū)間[tk，tk+i ]、上標b代表載體坐標系;II山樹I代表原始數(shù)據區(qū)間為[tk，tk+i ] 轉換到[〇，τ]區(qū)間后的姿態(tài)四元數(shù)的范數(shù)，如,(〇,如w，分別代表原始[tk， tk+1]區(qū)間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)也w各個元素，4.;(0, iy灼，4.;(0,奏，說分 別代表原始[tk，tk+i]區(qū)間轉換到[ο，τ]區(qū)間后的姿態(tài)四元數(shù)各個元素的一階導數(shù)，t代表時 間； 所述步驟2.5)包括首先根據式（12)所示的擾動加速度解析積分表達式計算擾動加速 度積分理論值，然后根據指定時間間隔ΔΤ的擾動加速度積分理論值δ化(t)和δ化(t+ΔΤ)計 算差值，得到指定的時間間隔ΔΤ內的擾動加速度積分增量理論值Al;(12) 式（12)中，Suk(t)代表[0，t]時間的擾動加速度積分理論值，娩成,,(〇，<,舶分別 為角速率分別在x、y、z方向的解析表達式;代表X方向加速度計偏離質屯、的桿臂矢量峻中 的X方向偏移量，4代表y方向加速度計偏離質屯、的桿臂矢量嗦中的y方向偏移量，代表Z 方向加速度計偏離質屯、的桿臂矢量嗦中的Z方向偏移量，分別為Ξ個方向的加速度計偏離質屯、的桿臂矢量 且均為常值，t代表時間； 所述步驟2.6)具體是指根據式(13)計算巧螺角增量理論值A2;(13) 式（13)中，0k(t)代表由角速率解析表達式A積分得到的巧螺角增量，似代表角速率 解析表達式A，所述角速率解析表達式A為姿態(tài)四元數(shù)的樣條多項式，下標ib代表載體坐標 系相對于慣性坐標系、k代表原始數(shù)據為區(qū)間[tk，tk+i]、上標b代表載體坐標系，ΔΘμ+1代表 ΔΤ時間間隔內的巧螺角增量理論值A2，0k(tj)代表[0，tj]時間的巧螺角積分增量，0k(tj+A T)代表[〇，t押ΔΤ]時間的巧螺角積分增量，t代表時間。4.根據權利要求1所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟3)的詳細步驟包括： 3.1) 對ECI坐標系下位置的各個分量進行樣條插值得到位置的樣條多項式，對所得到 的位置的樣條多項式求一次導得到速度的樣條多項式，分別代入時間得到位置和速度的離 散值； 3.2) 根據位置和速度的離散值，將[tk，tk+i ]轉換到[0，T ]區(qū)間，分段計算得到[0，T ]區(qū)間 的位置各個分量的Ξ次樣條多項式，進行兩次求導得到加速度各個分量的樣條多項式 aiw-(0，其中k代表原始數(shù)據區(qū)間為[tk，tk+i]; 3.3) 結合坐標系轉換，得到載體系帥的加速度at的的解析表達式如式(14)所示； aU')二貨(〇a;w(0 (14) 式（14)中，a：；; W代表載體系b中的加速度的樣條多項式，a;w.W代表原始數(shù)據為[tk， tk+i]區(qū)間轉換到[0，Τ]區(qū)間的加速度的樣條多項式，冷(0代表無誤差慣性坐標系到載體坐 標系的方向余弦矩陣，t代表時間，其中方向余弦矩陣巧tW與姿態(tài)四元數(shù)的轉換關系如式 (15) 所示；(15) 式（15)中，斬(/)代表無誤差慣性坐標系到載體坐標系的方向余弦矩陣，也(0代表未修 正時的慣性坐標系到載體坐標系的方向余弦矩陣，Au(0、4,?(0、fiw(0、與I.,(0、4.1(0、 ^.1(0、4,(0、^,(0、分別代表未修正時的慣性坐標系到載體坐標系的方向余弦 矩陣錢W中的九個元素，IliWif代表原始數(shù)據為[tk，tk+i]轉換到[ο，τ]區(qū)間的四元數(shù)的范 數(shù)的平方，t代表時間;根據精度要求選擇泰勒近似展開計算1/|松.(/f的解析多項式，得到載 體系加速度樣條多項式B; 3.4)根據載體系加速度樣條多項式B的解析多項式進行積分計算加速度增量理論值 Bio5.根據權利要求4所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟3.2)中分段計算得到[0，Τ]區(qū)間的位置各個分量的Ξ次樣條多項式如式 (16) 所示，兩次求導得到加速度各個分量的樣條多項式如式(17)所示；式（16)和（17)中，社、側，吃心的分別代表位置各個分量，成.,(〇，《;.,.(〇，心（0分別 代表加速度各分里，日x,k,bx,k,Cx,k,dx,k、日y,k,by,k,Cy,k,dy,k、日z,k,bz,k,Cz,k,dz,k分力[J代表轉換 到區(qū)間[0，τ]的樣條多項式系數(shù)，t代表時間，轉換到區(qū)間[ο，τ]的樣條多項式系數(shù)之間的轉 換矩陣如式(18)所示；(18； 式（18)中，Τ代表分段樣條的分段間隔，ax,k , bx,k , Cx,k , dx,k、ay,k , by,k , Cy,k , dy,k、az,k , bz,k , 〇3,1<也汾別代表轉換到區(qū)間[0，門的樣條多項式系數(shù)，吃.(〇<,(〇，心(〇代表原始數(shù)據*1^ 時刻的位置的值，《,(o 乂., h )乂. (o代表原始數(shù)據t k時刻的位置的導數(shù)值， 4,估1)，吃,.心1)，吃；心1)代表原始數(shù)據tk+1時刻的位置的值，吃，佔1)，吃,.h 1)，吃；h 1)代表原始 數(shù)據tk+1時刻的位置的導數(shù)值； 所述步驟3.3)得到的載體系加速度樣條多項式B如式(19)所示；(19) 式（19)中，代表選擇泰勒近似展開后計算得到的載體系加速度樣條多項式B， 代表原始數(shù)據為[tk，tk + i]區(qū)間轉換到[〇，Τ]區(qū)間的加速度的樣條多項式， Au(0、. 6。，*的、fl3j(0、4.A (小、如*.灼.、.與3.*(0、為:aW、(0 分別代表未修正 時的慣性坐標系到載體坐標系的方向余弦矩陣錢(0中的九個元素，I私wf代表原始數(shù)據 為[tk，tk+i]轉換到[0，Τ]區(qū)間的四元數(shù)的范數(shù)的平方，t代表時間；其中，未修正時的慣性坐 標系到載體坐標系的方向余弦矩陣蠟W中的九個元素如式(20)所示；(20) 式(20)中，如*此知*私;(〇,屯*(〇分別代表原始[tk，tk+i]區(qū)間轉換到區(qū)間[ο，τ]后 的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間； 所述步驟3.4)中根據載體系加速度樣條多項式B的解析多項式進行積分計算加速度增 量理論值B1的函數(shù)表達式如式(21)所示；(21) 式(21)中，u;w.的代表加速度增量理論值B1，a：；,:(〇表示載體系加速度樣條多項式B，t代 表時間。6.根據權利要求1所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟4)的詳細步驟包括： 4.1) 對重力W指定時間間隔進行Ξ次樣條函數(shù)插值得到重力各分量的Ξ次樣條多項 式，對重力各分量的Ξ次樣條多項式進行求導得到重力各分量的Ξ次樣條多項式的一階導 數(shù)，然后代入時間得到重力各個分量及其一階導數(shù)的離散值； 4.2) 根據重力各個分量及其一階導數(shù)的離散值，將[tk，tk+i]轉換到[0，Τ]區(qū)間，分段計 算得到[ο，τ]區(qū)間的重力各個分量的Ξ次樣條多項式； 4.3) 根據精度要求選擇泰勒近似展開計算HlM/f的解析多項式，得到載體系重力樣 條多項式C，其中IliWf代表原始數(shù)據為[tk，tk+i]轉換到[ο，τ]區(qū)間的四元數(shù)的范數(shù)的平方， t代表時間； 4.4) 根據載體系重力樣條多項式C的解析多項式進行積分計算重力積分增量理論值 Clo7.根據權利要求6所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟4.1)中重力各分量的Ξ次樣條多項式如式(22)所示；(22) 式(22)中，故,(0.紀,,,W，托:(0代表重力在分別在X、y、Z方向的分量，ax, bx, Cx, dx、ay, by， Cy，dy、az，bz，Cz，dz分別代表樣條插值后；次樣條多項式的系數(shù)，t代表時間； 所述步驟4.2)中分段計算得到[0，Τ]區(qū)間的重力各分量的Ξ次樣條多項式如式(23)所 示；(23) 式(23)中，瑞的，故，典)，沁。*")分別代表原數(shù)據區(qū)間為[tk，tk+i]轉換到[〇，Τ]區(qū)間后重 力各分里的二次樣條多項式，ax,k,bx,k,Cx,k,dx,k、ay,k,by,k,Cy,k,dy,k、az,k,bz,k,Cz,k, dz,k代表 原數(shù)據區(qū)間為[tk，tk+i]轉換到[0，Τ]區(qū)間后的立次樣條多項式的系數(shù)，t代表時間；轉換到 區(qū)間[0，τ]的樣條多項式系數(shù)之間的轉換矩陣如式(24)所示；(24) 式（24)中，Τ代表分段樣條的分段間隔，ax,k , bx,k , Cx,k , dx,k、ay,k , by,k , Cy,k , dy,k、az,k , bz,k , 〇,,1<山,1<分別代表轉換到[0，門區(qū)間后^次樣條多項式的系數(shù)，故,(〇，托，.(。，知,_-(。代表原 始數(shù)據tk時刻的重力的值，故J/;.)，茲,.(0.獻_-h)代表原始數(shù)據tk時刻的重力的一階導數(shù) 值，技尤+1)，轅^句+1)，誠戈表原始數(shù)據tk+1時刻的重力的值，茲Jiw)，無y心)，姑 代表原始數(shù)據tk+1時刻的重力的一階導數(shù)值； 所述步驟4.3)得到的載體系重力樣條多項式C的函數(shù)表達式如式(25)所示；(巧） 式(25)中，g;wW代表原始數(shù)據為[tk，tk+i]區(qū)間轉換到[〇，Τ]區(qū)間的重力的樣條多項式， Au(0、Α2,α-(')、4';^)、與 <1(0、 時的慣性坐標系到載體坐標系的方向余弦矩陣錢W中的九個元素，I屯.Wf代表原始數(shù)據為[tk，tk+i]轉換到[0，Τ]區(qū)間的四元數(shù)的范數(shù)的平方，4u(0'如.(〇'扛,(小知*(〇分別代表 原始[tk，tk+i]區(qū)間轉換到區(qū)間[ο，τ]后的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間；其 中，未修正時的慣性坐標系到載體坐標系的方向余弦矩陣蠟的中的九個元素如式(26)所 示· 餅） 式(26)中，4uW，扛*(0,知*(0分別代表原始[tk，tk+i]區(qū)間轉換到區(qū)間[ο，τ]后 的姿態(tài)四元數(shù)各個元素的時間函數(shù)，t代表時間； 所述步驟4.4)根據載體系重力樣條多項式C的解析多項式進行積分計算重力積分增量 理論值C1的函數(shù)表達式如式(27)所示；。7) 式(27)中，峻倘為載體系重力的積分增量理論值C1，私的代表載體系重力樣條多項 式C，t代表時間。8.根據權利要求1所述的基于實際飛行數(shù)據插值的仿真動態(tài)軌跡解析生成方法，其特 征在于，所述步驟5)具體是指根據式(28)所示函數(shù)表達式計算飛行器的加速度計比力積分 增量D1;C28； 式(28)中，Uk( t)代表加速度計比力積分樣條多項式，的J (0代表加速度積分理論值B1， "y句代表重力積分理論值C1，A化,W和Δ化(tw)均代表ΔΤ時間間隔內不考慮桿臂效應 等誤差源的加速度計比力積分增量，uk (t j)代表[0，t j ]時間的加速度計比力積分，化(t j+Δ τ)代表[〇，t^AT]時間的加速度計比力積分，代表飛行器的加速度計比力積分增量 D1，所述飛行器的加速度計比力積分增量D1為考慮桿臂效應誤差源的加速度計比力積分，δ Uk(tj+AT)代表[0，tj+AT]時間的擾動加速度積分增量理論值Al，Suk(tj)代表[0，tj]時間 的擾動加速度積分增量理論值A1。
【文檔編號】G01C21/16GK105973237SQ201610290487
【公開日】2016年9月28日
【申請日】2016年5月4日
【發(fā)明人】吳文啟, 劉科, 唐康華, 李濤, 文坤
【申請人】中國人民解放軍國防科學技術大學