本發(fā)明涉及運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真領(lǐng)域，具體涉及一種運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法。

背景技術(shù)：

為了保證運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真試驗(yàn)的可靠與價(jià)值，慣組被普遍用于飛行及試驗(yàn)的高精度測(cè)量，通常還配置雙慣組冗余。同時(shí)，閉環(huán)試驗(yàn)時(shí)還需配置大型高精度轉(zhuǎn)臺(tái)驅(qū)動(dòng)慣組，這些配置大大增加了試驗(yàn)成本。

慣組、轉(zhuǎn)臺(tái)的動(dòng)態(tài)特性對(duì)實(shí)時(shí)性要求極高的半實(shí)物仿真試驗(yàn)有很大影響。目前動(dòng)態(tài)特性測(cè)試完全依賴專業(yè)的動(dòng)態(tài)分析儀器，大大增加了試驗(yàn)成本。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的在于克服現(xiàn)有技術(shù)的上述不足，提供一種運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法，該模擬方法可以測(cè)出轉(zhuǎn)臺(tái)+慣組組合體的動(dòng)態(tài)特性，并且可以代替真實(shí)設(shè)備進(jìn)行試驗(yàn)，大大降低成本，具有更廣闊的應(yīng)用范圍。

本發(fā)明的上述目的主要是通過如下技術(shù)方案予以實(shí)現(xiàn)的：

一種運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法，包括如下步驟：

步驟(1)、每個(gè)時(shí)間周期采集外部發(fā)送的姿態(tài)、速度和位置信息，并將所述姿態(tài)、速度和位置信息輸入給慣組傳遞函數(shù)，模擬慣組的動(dòng)態(tài)特性，獲得動(dòng)態(tài)姿態(tài)、速度和位置信息；

步驟(2)、對(duì)所述動(dòng)態(tài)姿態(tài)、速度和位置信息進(jìn)行脈沖反算，得到陀螺脈沖增量和加表脈沖增量；

步驟(3)、對(duì)所述陀螺脈沖增量和加表脈沖增量進(jìn)行故障注入，并將故障注入后的陀螺脈沖增量和加表脈沖增量周期性實(shí)時(shí)向外輸出。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，還包括進(jìn)行慣組模擬器參數(shù)配置，所述慣組模擬器參數(shù)配置為根據(jù)需要模擬或者被測(cè)慣組的實(shí)際參數(shù)進(jìn)行慣組模擬器參數(shù)配置，包括安裝矩陣eg、ea，標(biāo)度因數(shù)kg、ka，零偏dg、da。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(1)中慣組傳遞函數(shù)通過如下方法得到：

步驟(1.1)、每個(gè)時(shí)間周期將正弦激勵(lì)信號(hào)通過轉(zhuǎn)換矩陣轉(zhuǎn)化為轉(zhuǎn)臺(tái)框角；

步驟(1.2)、采用所述轉(zhuǎn)臺(tái)框角控制轉(zhuǎn)臺(tái)運(yùn)轉(zhuǎn)，并實(shí)時(shí)采集慣組陀螺脈沖信號(hào)；

步驟(1.3)、將所述慣組陀螺脈沖信號(hào)進(jìn)行導(dǎo)航計(jì)算得到姿態(tài)角信號(hào)，包括俯仰姿態(tài)角偏航姿態(tài)角ψ和滾動(dòng)姿態(tài)角γ；

步驟(1.4)、根據(jù)所述姿態(tài)角信號(hào)與正弦激勵(lì)信號(hào)得到慣組及轉(zhuǎn)臺(tái)組合體的慣組傳遞函數(shù)。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(1.1)、(1.4)中的正弦激勵(lì)信號(hào)包括俯仰、偏航和滾動(dòng)姿態(tài)角的三個(gè)正弦信號(hào)，所述三個(gè)正弦信號(hào)分別根據(jù)如下正弦函數(shù)生成：

f(t)＝a×sin(w×t)

其中a為幅值，w為圓頻率，t為模擬器本地時(shí)鐘。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述轉(zhuǎn)換矩陣為姿態(tài)角到轉(zhuǎn)臺(tái)框角的轉(zhuǎn)化矩陣czb，所述轉(zhuǎn)臺(tái)框角包括轉(zhuǎn)臺(tái)外框α、中框β和內(nèi)框λ，具體轉(zhuǎn)換公式如下：

其中：ψtrig、γtrig分別為俯仰、偏航和滾動(dòng)姿態(tài)角。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(1.3)中將所述慣組陀螺脈沖信號(hào)進(jìn)行導(dǎo)航計(jì)算得到姿態(tài)角信號(hào)的具體方法如下：

(1.3.1)、將采集到的相鄰兩拍脈沖求差，得到周期時(shí)間內(nèi)的陀螺脈沖增量δng，所述δng包括慣組陀螺三軸及斜軸的脈沖增量δngx、δngy、δngz、δngs；

(1.3.2)、根據(jù)如下公式得到慣組安裝位置的角增量，包括慣組陀螺三軸及斜軸的角增量δθx、δθy、δθz、δθs：

其中：kgx、kgy、kgz分別為慣組陀螺三個(gè)軸的標(biāo)度因數(shù)，kgs為慣組陀螺斜軸的標(biāo)度因數(shù)，dgx、dgy、dgz分別為慣組陀螺三個(gè)軸的零偏，dgs為慣組陀螺斜軸的零偏，δt為解算周期，即相鄰兩拍脈沖的時(shí)間差；eg為安裝矩陣。

(1.3.3)、根據(jù)慣組安裝位置的角增量計(jì)算當(dāng)前拍四元數(shù)q0(n)、q1(n)、q2(n)、q3(n)：

其中：q0(n-1)、q1(n-1)、q2(n-1)、q3(n-1)為上一拍的四元數(shù)；n為正整數(shù)，且n≥1；

(1.3.4)、根據(jù)四元數(shù)q0(n)、q1(n)、q2(n)、q3(n)計(jì)算四元數(shù)到姿態(tài)角的轉(zhuǎn)換矩陣c，再由四元數(shù)到姿態(tài)角的轉(zhuǎn)換矩陣c計(jì)算姿態(tài)角信息,包括俯仰姿態(tài)角偏航姿態(tài)角ψnav、滾動(dòng)姿態(tài)角γnav，具體公式如下：

當(dāng)|c31|<0.9999999時(shí)

否則：

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(1.4)中根據(jù)所述姿態(tài)角信號(hào)與正弦激勵(lì)信號(hào)采用最小二乘法得到慣組及轉(zhuǎn)臺(tái)組合體的慣組傳遞函數(shù)

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，步驟(2)中對(duì)所述動(dòng)態(tài)姿態(tài)、速度和位置信息進(jìn)行脈沖反算，得到陀螺脈沖增量和加表脈沖增量的具體方法如下：

(3.1)、根據(jù)上一拍的動(dòng)態(tài)姿態(tài)ψn-1、γn-1生成上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1，根據(jù)當(dāng)前拍的動(dòng)態(tài)姿態(tài)ψn、γn生成當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn；

(3.2)、根據(jù)上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1和當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn計(jì)算角增量δθx、δθy、δθz：

(3.3)、根據(jù)角增量δθx、δθy、δθz計(jì)算慣組陀螺三軸及斜軸的陀螺脈沖增量δngx、δngy、δngz、δngs：

(3.4)、根據(jù)上一拍的位置px(n-1)、py(n-1)、pz(n-1)計(jì)算上一拍的引力加速度gx(n-1)、gy(n-1)、gz(n-1)，根據(jù)當(dāng)前拍的位置pxn、pyn、pzn計(jì)算當(dāng)前拍的引力加速度gxn、gyn、gzn：

(3.5)、根據(jù)上一拍的速度vx(n-1)、vy(n-1)、vz(n-1)和當(dāng)前拍的速度vxn、vyn、vzn計(jì)算視速度增量δwx、δwy、δwz：

(3.6)根據(jù)視速度增量δwx、δwy、δwz計(jì)算慣組加表三軸及斜軸的加表脈沖增量δnax、δnay、δnaz、δnas。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(2)中對(duì)所述動(dòng)態(tài)姿態(tài)、速度和位置信息進(jìn)行脈沖反算，得到陀螺脈沖增量和加表脈沖增量的具體方法如下，其中所述動(dòng)態(tài)姿態(tài)、速度和位置信息包括上一拍的動(dòng)態(tài)姿態(tài)ψn-1、γn-1，速度vx(n-1)、vy(n-1)、vz(n-1)，位置px(n-1)、py(n-1)、pz(n-1)，當(dāng)前拍的動(dòng)態(tài)姿態(tài)ψn、γn，速度vxn、vyn、vzn，位置pxn、pyn、pzn：

(2.1)、根據(jù)上一拍的動(dòng)態(tài)姿態(tài)ψn-1、γn-1生成上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1：

根據(jù)當(dāng)前拍的動(dòng)態(tài)姿態(tài)ψn、γn生成當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn：

(2.2)、計(jì)算角增量δθx、δθy、δθz：

(2.2.1)根據(jù)上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1得到上一拍的四元數(shù)：

根據(jù)當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn得到當(dāng)前拍的四元數(shù)：

(2.2.2)、根據(jù)上一拍和當(dāng)前拍的四元數(shù)計(jì)算角增量δθx、δθy、δθz：

qn-1＝[q(n-1)0-q(n-1)1-q(n-1)2-q(n-1)3]

qn＝[qn0qn1qn2qn3]

若δq≥10^-5，則

否則：

(2.3)、根據(jù)角增量δθx、δθy、δθz計(jì)算慣組陀螺三軸及斜軸的陀螺脈沖增量δngx、δngy、δngz、δngs：

(2.4)、根據(jù)上一拍的位置px(n-1)、py(n-1)、pz(n-1)計(jì)算上一拍的引力加速度gx(n-1)、gy(n-1)、gz(n-1)，根據(jù)當(dāng)前拍的位置pxn、pyn、pzn計(jì)算當(dāng)前拍的引力加速度gxn、gyn、gzn：

(rn-1)²＝(px(n-1))²+(py(n-1))²+(pz(n-1))²

(rn)²＝(pxn)²+(pyn)²+(pzn)²

其中：gm、re、j2為地球相關(guān)常數(shù)；

(2.5)、根據(jù)上一拍的速度vx(n-1)、vy(n-1)、vz(n-1)和當(dāng)前拍的速度vxn、vyn、vzn計(jì)算視速度增量δwx、δwy、δwz：

(2.6)根據(jù)視速度增量δwx、δwy、δwz計(jì)算計(jì)算慣組加表三軸及斜軸的加表脈沖增量δnax、δnay、δnaz、δnas：

dax、day、daz分別為慣組加表三個(gè)軸的零偏，ea為安裝矩陣；das為慣組加表斜軸的零偏。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述步驟(3)中對(duì)所述陀螺脈沖增量和加表脈沖增量在t0時(shí)刻注入故障，包括常值故障、常零故障、線性故障和交變故障。

在上述運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法中，所述常值故障、常零故障、線性故障、交變故障注入方式如下：

常值故障：

常零故障：

線性故障：

交變故障：

其中：t為時(shí)間，k為線性故障斜率，a為設(shè)定常值，t0為故障注入時(shí)刻，a'為交變故障幅值，ω'為交變故障圓頻率。

本發(fā)明與現(xiàn)有技術(shù)相比具有如下有益效果：

(1)、本發(fā)明提出的運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真試驗(yàn)慣組模擬方法，可代替真實(shí)的轉(zhuǎn)臺(tái)和慣組，接收動(dòng)力學(xué)模型的姿態(tài)、速度、位置信號(hào)，解算得到慣組陀螺及加表測(cè)量信號(hào)，模擬出慣組及轉(zhuǎn)臺(tái)的主要功能，實(shí)現(xiàn)正常模式及故障模式下的開環(huán)、閉環(huán)半實(shí)物仿真試驗(yàn)，測(cè)出轉(zhuǎn)臺(tái)+慣組組合體的動(dòng)態(tài)特性，并且本發(fā)明方法可對(duì)現(xiàn)有轉(zhuǎn)臺(tái)與慣組的動(dòng)態(tài)特性進(jìn)行測(cè)試和模擬。

(2)、本發(fā)明在進(jìn)行轉(zhuǎn)臺(tái)與慣組動(dòng)態(tài)特性測(cè)試時(shí)，慣組模擬器生成正弦激勵(lì)信號(hào)，轉(zhuǎn)化為框角并發(fā)送給轉(zhuǎn)臺(tái)，模擬器采集慣組測(cè)量信號(hào)并進(jìn)行導(dǎo)航姿態(tài)信息解算得到姿態(tài)角信息，將姿態(tài)角信息與激勵(lì)信號(hào)對(duì)比分析，可得轉(zhuǎn)臺(tái)+慣組的動(dòng)態(tài)特性。

(3)、本發(fā)明在測(cè)試試驗(yàn)時(shí)，慣組模擬器可模擬轉(zhuǎn)臺(tái)和慣組的主要功能，由姿態(tài)、速度、位置生成加表和陀螺測(cè)量信息，在試驗(yàn)過程中可以代替真實(shí)設(shè)備進(jìn)行試驗(yàn)，顯著降低成本，并降低試驗(yàn)復(fù)雜度，簡(jiǎn)化流程。

(4)、本發(fā)明可實(shí)現(xiàn)多慣組的實(shí)時(shí)模擬，并能對(duì)生成的測(cè)量信息注入特定故障，完成真實(shí)轉(zhuǎn)臺(tái)和慣組無法實(shí)現(xiàn)的試驗(yàn)項(xiàng)目，并且在試驗(yàn)中可以修改配置參數(shù)，模擬多套慣組，具有較廣的應(yīng)用范圍，實(shí)用性強(qiáng)。

附圖說明

圖1為本發(fā)明慣組模擬器測(cè)量動(dòng)態(tài)特性原理圖。

圖2為本發(fā)明慣組模擬器功能原理圖。

具體實(shí)施方式

下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述：

如圖1所示為本發(fā)明慣組模擬器測(cè)量動(dòng)態(tài)特性原理圖，圖2為本發(fā)明慣組模擬器主要功能原理圖，本發(fā)明運(yùn)載火箭半實(shí)物仿真試驗(yàn)慣組模擬方法，具體包括如下步驟：

步驟(一)、根據(jù)需要模擬或者被測(cè)的慣組實(shí)際參數(shù)進(jìn)行慣組模擬器參數(shù)配置，參數(shù)配置包括安裝矩陣eg、ea，標(biāo)度因數(shù)kg、ka，零偏dg、da。

步驟(二)、每個(gè)時(shí)間周期采集外部發(fā)送的姿態(tài)、速度和位置信息，并將所述姿態(tài)、速度和位置信息輸入給慣組傳遞函數(shù)，模擬慣組的動(dòng)態(tài)特性，獲得動(dòng)態(tài)姿態(tài)、速度和位置信息。

其中慣組傳遞函數(shù)通過如下方法得到：

步驟(2.1)、使能慣組模擬器的內(nèi)部時(shí)鐘，時(shí)間t每毫秒累加，由正弦激勵(lì)模塊產(chǎn)生正弦信號(hào)f(t)＝a[0]*sin(ω[0]*t)，當(dāng)正弦信號(hào)激勵(lì)次數(shù)大于n[0]時(shí)，即運(yùn)行時(shí)間t>n[0]*2*π/ω[0]時(shí)，正弦激勵(lì)更新為f(t)＝a[1]*sin(ω[1]*t)，當(dāng)正弦信號(hào)激勵(lì)次數(shù)大于n[1]時(shí)，即運(yùn)行時(shí)間t>(n[0]*2*π/ω[0]+n[1]*2*π/ω[1])時(shí)，正弦激勵(lì)更新為f(t)＝a[2]*sin(ω[2]*t)，依此更新正弦激勵(lì)信號(hào)為f(t)＝a[i]*sin(ω[i]*t)，更新時(shí)間為

步驟(2.2)、每個(gè)時(shí)間周期將正弦激勵(lì)信號(hào)通過轉(zhuǎn)換矩陣轉(zhuǎn)化為轉(zhuǎn)臺(tái)框角。正弦激勵(lì)信號(hào)包括俯仰、偏航和滾動(dòng)姿態(tài)角的三個(gè)正弦信號(hào)，所述三個(gè)正弦信號(hào)分別根據(jù)如下正弦函數(shù)生成：

f(t)＝a×sin(w×t)

其中：a為慣組模擬器設(shè)置幅值，w為圓頻率，t為模擬器本地時(shí)鐘，周期性累加，慣組模擬器可同時(shí)生成三個(gè)正弦信號(hào)，作為俯仰、偏航、滾動(dòng)姿態(tài)角的模擬。

轉(zhuǎn)換矩陣為姿態(tài)角到轉(zhuǎn)臺(tái)框角的轉(zhuǎn)化矩陣czb，轉(zhuǎn)臺(tái)框角包括轉(zhuǎn)臺(tái)外框α、中框β和內(nèi)框λ，具體轉(zhuǎn)換公式如下：

其中：ψtrig、γtrig分別為俯仰、偏航和滾動(dòng)姿態(tài)角。

步驟(2.3)、采用所述轉(zhuǎn)臺(tái)框角控制轉(zhuǎn)臺(tái)運(yùn)轉(zhuǎn)，并實(shí)時(shí)采集慣組陀螺脈沖信號(hào)；

步驟(2.4)、將慣組陀螺脈沖信號(hào)進(jìn)行導(dǎo)航計(jì)算得到姿態(tài)角信號(hào)，包括俯仰姿態(tài)角偏航姿態(tài)角ψ和滾動(dòng)姿態(tài)角γ；具體方法如下：

(2.4.1)、將采集到的相鄰兩拍脈沖求差，得到周期時(shí)間內(nèi)的陀螺脈沖增量δng，所述δng包括慣組陀螺三軸及斜軸的脈沖增量δngx、δngy、δngz、δngs；

(2.4.2)、根據(jù)如下公式得到慣組安裝位置的角增量，包括慣組陀螺三軸及斜軸的角增量δθx、δθy、δθz、δθs：

其中：kgx、kgy、kgz分別為慣組陀螺三個(gè)軸的標(biāo)度因數(shù)，kgs為慣組陀螺斜軸的標(biāo)度因數(shù)，dgx、dgy、dgz分別為慣組陀螺三個(gè)軸的零偏，dgs為慣組陀螺斜軸的零偏，δt為解算周期，即相鄰兩拍脈沖的時(shí)間差；eg為安裝矩陣。

(2.4.3)、根據(jù)慣組安裝位置的角增量計(jì)算當(dāng)前拍四元數(shù)q0(n)、q1(n)、q2(n)、q3(n)：

其中：q0(n-1)、q1(n-1)、q2(n-1)、q3(n-1)為上一拍的四元數(shù)；n為正整數(shù)，且n≥1。其中初始值即q00、q01、q02、q03為根據(jù)試驗(yàn)需求給出，為已知量。且四元數(shù)q0(n)、q1(n)、q2(n)、q3(n)為脈沖計(jì)算姿態(tài)角的中間變量。

(2.4.4)、根據(jù)四元數(shù)q0(n)、q1(n)、q2(n)、q3(n)計(jì)算四元數(shù)到姿態(tài)角的轉(zhuǎn)換矩陣c，再由四元數(shù)到姿態(tài)角的轉(zhuǎn)換矩陣c計(jì)算姿態(tài)角信息,包括俯仰姿態(tài)角偏航姿態(tài)角ψnav、滾動(dòng)姿態(tài)角γnav，具體公式如下：

當(dāng)|c31|<0.9999999時(shí)

否則：

步驟(2.4)、根據(jù)所述姿態(tài)角信號(hào)與正弦激勵(lì)信號(hào)采用最小二乘法得到慣組及轉(zhuǎn)臺(tái)組合體的慣組傳遞函數(shù)。

步驟(三)、對(duì)動(dòng)態(tài)姿態(tài)、速度和位置信息進(jìn)行脈沖反算，得到陀螺脈沖增量和加表脈沖增量。其中動(dòng)態(tài)姿態(tài)、速度和位置信息包括上一拍的動(dòng)態(tài)姿態(tài)ψn-1、γn-1，速度vx(n-1)、vy(n-1)、vz(n-1)，位置px(n-1)、py(n-1)、pz(n-1)，當(dāng)前拍的動(dòng)態(tài)姿態(tài)ψn、γn，速度vxn、vyn、vzn，位置pxn、pyn、pzn。

具體方法如下：

(3.1)、根據(jù)上一拍的動(dòng)態(tài)姿態(tài)ψn-1、γn-1生成上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1：

根據(jù)當(dāng)前拍的動(dòng)態(tài)姿態(tài)ψn、γn生成當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn：

(3.2)、計(jì)算角增量δθx、δθy、δθz：

(3.2.1)首先根據(jù)上一拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn-1得到上一拍的四元數(shù)：

根據(jù)當(dāng)前拍的姿態(tài)角到四元數(shù)的轉(zhuǎn)換矩陣cn得到當(dāng)前拍的四元數(shù)：

上述四元數(shù)q(n-1)0～q(n-1)3，qn0～qn3為姿態(tài)角計(jì)算脈沖的中間變量。

(3.2.2)、根據(jù)上一拍和當(dāng)前拍的四元數(shù)計(jì)算角增量δθx、δθy、δθz：

qn-1＝[q(n-1)0-q(n-1)1-q(n-1)2-q(n-1)3]

qn＝[qn0qn1qn2qn3]

若δq≥10^-5，則

否則：

其中δq為中間變量。

(3.3)、根據(jù)角增量δθx、δθy、δθz計(jì)算慣組陀螺三軸及斜軸的陀螺脈沖增量δngx、δngy、δngz、δngs：

其中δt為解算周期，即相鄰兩拍脈沖的時(shí)間差。

為安裝矩陣。

(3.4)、根據(jù)上一拍的位置px(n-1)、py(n-1)、pz(n-1)計(jì)算上一拍的引力加速度gx(n-1)、gy(n-1)、gz(n-1)，根據(jù)當(dāng)前拍的位置pxn、pyn、pzn計(jì)算當(dāng)前拍的引力加速度gxn、gyn、gzn：

(rn-1)²＝(px(n-1))²+(py(n-1))²+(pz(n-1))²

(rn)²＝(pxn)²+(pyn)²+(pzn)²

其中：gm、re、j2為地球相關(guān)常數(shù)；

(3.5)、根據(jù)上一拍的速度vx(n-1)、vy(n-1)、vz(n-1)和當(dāng)前拍的速度vxn、vyn、vzn計(jì)算視速度增量δwx、δwy、δwz：

其中：cn為轉(zhuǎn)換矩陣，步驟(3.1)得到。

(3.6)根據(jù)視速度增量δwx、δwy、δwz計(jì)算計(jì)算慣組加表三軸及斜軸的加表脈沖增量δnax、δnay、δnaz、δnas：

其中：dax、day、daz分別為慣組加表三個(gè)軸的零偏，das為慣組加表斜軸的零偏。

ea為安裝矩陣；且

步驟(四)、對(duì)陀螺脈沖增量δngx、δngy、δngz、δngs和加表脈沖增量δnax、δnay、δnaz、δnas進(jìn)行故障注入。并將故障注入后的陀螺脈沖增量和加表脈沖增量周期性實(shí)時(shí)向外輸出給控制計(jì)算機(jī)。

即對(duì)慣組模擬器陀螺的x、y、z、s軸及加表的x、y、z、s軸脈沖增量在t0時(shí)刻注入故障，包括常值故障、常零故障、線性故障、交變故障。注入方式如下：

常值故障：

常零故障：

線性故障：

交變故障：

其中：t為時(shí)間，k為線性故障斜率，a為設(shè)定常值，t0為故障注入時(shí)刻，a'為交變故障幅值，ω'為交變故障圓頻率。

本發(fā)明提出的運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真試驗(yàn)慣組模擬方法是在半實(shí)物仿真時(shí)利用慣組模擬器替換轉(zhuǎn)臺(tái)和慣組組合體，采集模型解算出的姿態(tài)、速度、位置信息，將該信息反算成測(cè)量信息再發(fā)送給控制計(jì)算機(jī)，另外，該發(fā)明可實(shí)現(xiàn)對(duì)真實(shí)轉(zhuǎn)臺(tái)和慣組組合體的動(dòng)態(tài)特性測(cè)試與模擬。

為了保證運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真試驗(yàn)的可靠與價(jià)值，慣組被普遍用于飛行及試驗(yàn)的高精度測(cè)量，通常還配置雙慣組冗余。同時(shí)，閉環(huán)試驗(yàn)時(shí)還需配置大型高精度轉(zhuǎn)臺(tái)驅(qū)動(dòng)慣組，這些配置大大增加了試驗(yàn)成本。在未配全這些設(shè)備、產(chǎn)品時(shí)，采用慣組模擬器可顯著節(jié)約成本。

慣組、轉(zhuǎn)臺(tái)的動(dòng)態(tài)特性對(duì)實(shí)時(shí)性要求極高的半實(shí)物仿真試驗(yàn)有很大影響。目前動(dòng)態(tài)特性測(cè)試完全依賴專業(yè)的動(dòng)態(tài)分析儀器，增加了試驗(yàn)成本，利用慣組模擬器即能完成該項(xiàng)測(cè)試并能進(jìn)一步模擬該特性。

不同系列的運(yùn)載火箭通常配有不同慣組，同一型號(hào)的慣組參數(shù)也可能有較大差異，采用慣組模擬器可通過配置參數(shù)的方式模擬各種不同類型的慣組，可便捷的進(jìn)行算法分析、參數(shù)分析、閉環(huán)試驗(yàn)。

本發(fā)明在進(jìn)行半實(shí)物仿真調(diào)試、測(cè)試及正式試驗(yàn)階段，可用于替代真實(shí)慣組及轉(zhuǎn)臺(tái)進(jìn)行仿真試驗(yàn)，另外還具備測(cè)試真實(shí)轉(zhuǎn)臺(tái)和慣組動(dòng)態(tài)特性的功能。真實(shí)慣組一般包含陀螺及加表測(cè)量裝置，地面試驗(yàn)時(shí)需將慣組放置在轉(zhuǎn)臺(tái)上，隨著轉(zhuǎn)臺(tái)的運(yùn)動(dòng)給出測(cè)量信號(hào)。慣組模擬器主要包含以下功能：測(cè)量與模擬轉(zhuǎn)臺(tái)與慣組組合體的動(dòng)態(tài)特性；通過配置參數(shù)模擬不同慣組的主要特性；根據(jù)姿態(tài)、速度、位置信息模擬慣組的陀螺及加表輸出；根據(jù)需求模擬慣組的多種故障模式。在不具備轉(zhuǎn)臺(tái)或者慣組時(shí)，可用慣組模擬器進(jìn)行試驗(yàn)；在系統(tǒng)調(diào)試階段，可用慣組模擬器進(jìn)行測(cè)試、調(diào)試與算法驗(yàn)證。

本發(fā)明提出的運(yùn)載火箭控制系統(tǒng)半實(shí)物仿真試驗(yàn)慣組模擬方法，該方法的應(yīng)用可替換轉(zhuǎn)臺(tái)和慣組，接收動(dòng)力學(xué)模型的姿態(tài)、速度、位置信號(hào)，解算得到慣組陀螺及加表測(cè)量信號(hào)，模擬出慣組及轉(zhuǎn)臺(tái)的主要功能，實(shí)現(xiàn)正常模式及故障模式下的開環(huán)、閉環(huán)半實(shí)物仿真試驗(yàn)。另外，應(yīng)用此方法可對(duì)現(xiàn)有轉(zhuǎn)臺(tái)與慣組的動(dòng)態(tài)特性進(jìn)行測(cè)試和模擬。

以上所述，僅為本發(fā)明最佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。

本發(fā)明說明書中未作詳細(xì)描述的內(nèi)容屬于本領(lǐng)域?qū)I(yè)技術(shù)人員的公知技術(shù)。