一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，屬于慣導(dǎo)技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 里程計(jì)是測(cè)量車輛行駛速度和路程的一種傳感器，具有完全自主、動(dòng)態(tài)性能好、測(cè) 量誤差不隨時(shí)間發(fā)散的優(yōu)點(diǎn)。里程計(jì)與慣導(dǎo)組合能優(yōu)勢(shì)互補(bǔ)，實(shí)現(xiàn)全自主、高精度導(dǎo)航定 位。
[0003] 在進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真研究時(shí)，一般是設(shè)定仿真軌跡（包含載體的運(yùn) 動(dòng)速度、位置、姿態(tài)），利用軌跡發(fā)生器生成加速度計(jì)的標(biāo)準(zhǔn)值、巧螺的標(biāo)準(zhǔn)值，并由設(shè)定仿 真軌跡中載體的運(yùn)動(dòng)速度計(jì)算得到里程計(jì)的速率標(biāo)準(zhǔn)值，再給定里程計(jì)標(biāo)定誤差、偏航安 裝角及偏航安裝角誤差、俯仰安裝角及俯仰安裝角誤差，加速度計(jì)和巧螺的標(biāo)度誤差、零 偏、失準(zhǔn)角誤差，加速度計(jì)噪聲、巧螺噪聲、里程計(jì)噪聲，將W上數(shù)據(jù)融合得到用于組合導(dǎo)航 仿真的器件模擬數(shù)據(jù)，然后采用該些模擬數(shù)據(jù)進(jìn)行組合導(dǎo)航的仿真計(jì)算與分析。其中，給定 的仿真噪聲不能全面反映里程計(jì)、加速度計(jì)、巧螺的噪聲特性，導(dǎo)致仿真結(jié)果不能全面地反 映實(shí)際情況。為了更好地進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航研究，本專利申請(qǐng)?zhí)岢隽艘环N慣導(dǎo)/ 里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的是提供了一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，它能夠更 好地模擬慣導(dǎo)/里程計(jì)組合導(dǎo)航過程。
[0005] 本發(fā)明實(shí)現(xiàn)方案：一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，該方法具體步 驟如下：
[0006] 步驟1、將慣導(dǎo)/里程計(jì)組合系統(tǒng)安裝到載體上，并上電啟動(dòng)。
[0007] 步驟2、裝訂初始參數(shù)（包括初始的經(jīng)度、紳度、高度、里程計(jì)標(biāo)度值）至導(dǎo)航計(jì)算 機(jī)。
[000引步驟3、載體保持靜止，采集保存1小時(shí)巧螺和加速度計(jì)的輸出數(shù)據(jù)。求取巧螺和 加速度計(jì)的輸出數(shù)據(jù)的平均值，將采集的巧螺和加速度計(jì)數(shù)據(jù)分別與其平均值作差，得到 巧螺和加速度計(jì)的實(shí)際噪聲。
[0009] 步驟4、載體保持靜止，慣導(dǎo)進(jìn)行5分鐘靜態(tài)初始對(duì)準(zhǔn)。
[0010] 步驟5、完成對(duì)準(zhǔn)后載體開始運(yùn)動(dòng)，運(yùn)動(dòng)過程中慣導(dǎo)進(jìn)行慣性導(dǎo)航計(jì)算，采集并保 存開始運(yùn)動(dòng)后50秒內(nèi)慣導(dǎo)輸出的速度值和里程計(jì)輸出的速率值。
[0011] 步驟6、由采集的慣導(dǎo)速度值計(jì)算得到載體速率參考值，再將里程計(jì)輸出的速率值 與載體速率參考值作差，得到里程計(jì)輸出速率的噪聲值。
[0012] 步驟7、設(shè)定仿真軌跡（包含載體的運(yùn)動(dòng)速度、位置、姿態(tài)），利用軌跡發(fā)生器生成 加速度計(jì)的標(biāo)準(zhǔn)值、巧螺的標(biāo)準(zhǔn)值，并由設(shè)定仿真軌跡中載體的運(yùn)動(dòng)速度計(jì)算得到里程計(jì) 的速率標(biāo)準(zhǔn)值，再給定里程計(jì)標(biāo)度誤差、偏航安裝角及偏航安裝角誤差、俯仰安裝角及俯仰 安裝角誤差，加速度計(jì)和巧螺的標(biāo)度誤差、零偏、失準(zhǔn)角誤差，將該些數(shù)據(jù)與步驟3、步驟6 中得到的噪聲數(shù)據(jù)融合，得到用于慣導(dǎo)/里程計(jì)組合導(dǎo)航半實(shí)物仿真的器件模擬數(shù)據(jù)，然 后采用該些模擬數(shù)據(jù)進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真。
[0013] 其中，步驟3中所述的"得到巧螺和加速度計(jì)的實(shí)際噪聲"，其實(shí)現(xiàn)過程說明如下：
[0014] 定義Wg為巧螺噪聲，W。為加速度計(jì)噪聲，采集巧螺X、y、Z S個(gè)軸輸出的數(shù)據(jù)分別 為《 X (1)、W X 似…W X (n)，W y (1)、W y 似…W y (n)，W Z (1)、W Z 似…W Z (n)，平均值分別為 毎、w,.、毎。采集加速度計(jì)X、y、z ;個(gè)軸輸出的數(shù)據(jù)分別為3,(1)、a,似…ay(n)，ay(l)、 a, (2)…a, (n)，a, (1)、a, (2)…a, (n)，平均值分別為馬、S、是。
[0015] 巧螺噪聲計(jì)算公式如下：
【主權(quán)項(xiàng)】
1. 一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，其特征在于：該方法具體步驟如 下： 步驟1、將慣導(dǎo)/里程計(jì)組合系統(tǒng)安裝到載體上，并上電啟動(dòng)； 步驟2、裝訂初始參數(shù)，包括初始的經(jīng)度、紳度、高度、里程計(jì)標(biāo)度值至導(dǎo)航計(jì)算機(jī)； 步驟3、載體保持靜止，采集保存1小時(shí)巧螺和加速度計(jì)的輸出數(shù)據(jù)；求取巧螺和加速 度計(jì)的輸出數(shù)據(jù)的平均值，將采集的巧螺和加速度計(jì)數(shù)據(jù)分別與其平均值作差，得到巧螺 和加速度計(jì)的實(shí)際噪聲； 步驟4、載體保持靜止，慣導(dǎo)進(jìn)行5分鐘靜態(tài)初始對(duì)準(zhǔn)； 步驟5、完成對(duì)準(zhǔn)后載體開始運(yùn)動(dòng)，運(yùn)動(dòng)過程中慣導(dǎo)進(jìn)行慣性導(dǎo)航計(jì)算，采集并保存開 始運(yùn)動(dòng)后50秒內(nèi)慣導(dǎo)輸出的速度值和里程計(jì)輸出的速率值； 步驟6、由采集的慣導(dǎo)速度值計(jì)算得到載體速率參考值，再將里程計(jì)輸出的速率值與載 體速率參考值作差，得到里程計(jì)輸出速率的噪聲值； 步驟7、設(shè)定仿真軌跡，包含載體的運(yùn)動(dòng)速度、位置、姿態(tài)，利用軌跡發(fā)生器生成加速度 計(jì)的標(biāo)準(zhǔn)值、巧螺的標(biāo)準(zhǔn)值，并由設(shè)定仿真軌跡中載體的運(yùn)動(dòng)速度計(jì)算得到里程計(jì)的速率 標(biāo)準(zhǔn)值，再給定里程計(jì)標(biāo)度誤差、偏航安裝角及偏航安裝角誤差、俯仰安裝角及俯仰安裝角 誤差，加速度計(jì)和巧螺的標(biāo)度誤差、零偏、失準(zhǔn)角誤差，將該些數(shù)據(jù)與步驟3、步驟6中得到 的噪聲數(shù)據(jù)融合，得到用于慣導(dǎo)/里程計(jì)組合導(dǎo)航半實(shí)物仿真的器件模擬數(shù)據(jù)，然后采用 該些模擬數(shù)據(jù)進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真。
2. 根據(jù)權(quán)利要求1所述的一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，其特征在于： 步驟3中所述的"得到巧螺和加速度計(jì)的實(shí)際噪聲"，其實(shí)現(xiàn)過程說明如下： 定義Wg為巧螺噪聲，W。為加速度計(jì)噪聲，采集巧螺X、y、Z S個(gè)軸輸出的數(shù)據(jù)分別為 ?x(l)、似…《?，《y(l)、似…《>)，《z(l)、似…《z(n)，平均值分別為 瑪、卸、兩；采集加速度計(jì)X、y、z立個(gè)軸輸出的數(shù)據(jù)分別為a, (1)、a, (2)…a?，a, (1)、 3,(2)...3,(11)，3,(1)、3,(2)...3,(11)，平均值分別為立、、早.、疋； 巧螺噪聲計(jì)算公式如下：
3. 根據(jù)權(quán)利要求1所述的一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，其特征在于： 步驟6中所述的"得到里程計(jì)輸出速率的噪聲值"，其實(shí)現(xiàn)過程說明如下： 載體開始運(yùn)動(dòng)，采集得到了 50秒內(nèi)里程計(jì)輸出的速率 為戶。"'(1)、G"(2)…G"(n)及慣導(dǎo)輸出的東北天方向的速度
F；：'(l)、G'(2)...F；X,;)，r；；(l)、r;(2)...G:(n)，rja)、FJ(2)...F[;(,?);定義；載體速率參 考值為r (1)、r (2)…r (n)，里程計(jì)的速率噪聲為Wd (1)、Wd (2)…Wd (n)，載體速率參考值計(jì) 算公式如下：
里程計(jì)的速率噪聲計(jì)算公式如下；
4.根據(jù)權(quán)利要求1所述的一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，其特征在于： 步驟7中所述的"得到用于慣導(dǎo)/里程計(jì)組合導(dǎo)航半實(shí)物仿真的器件模擬數(shù)據(jù)，然后采用該 些模擬數(shù)據(jù)進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真"，其實(shí)現(xiàn)過程說明如下： 設(shè)定仿真軌跡，包含載體的運(yùn)動(dòng)速度、位置、姿態(tài)，利用軌跡發(fā)生器生成加速度計(jì)的 標(biāo)準(zhǔn)值、巧螺的標(biāo)準(zhǔn)值，由設(shè)定仿真軌跡中載體的運(yùn)動(dòng)速度計(jì)算得到里程計(jì)的速率標(biāo)準(zhǔn) 值，并給定里程計(jì)標(biāo)度誤差、偏航安裝角及偏航安裝角誤差、俯仰安裝角及俯仰安裝角 誤差，加速度計(jì)和巧螺的標(biāo)度誤差、零偏、失準(zhǔn)角誤差；將該些數(shù)據(jù)與步驟3、步驟6中 得到的噪聲數(shù)據(jù)融合，得到用于慣導(dǎo)/里程計(jì)組合導(dǎo)航半實(shí)物仿真的器件模擬數(shù)據(jù)，然 后采用該些模擬數(shù)據(jù)進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真；定義；仿真軌跡中載體在導(dǎo)航坐 標(biāo)系下運(yùn)動(dòng)速度向量為yn(l)、yn(2)、…yn(n)，在導(dǎo)航坐標(biāo)系S個(gè)方向的分量分別為 V';'0)、V:(2)... V:(?)，V:':(1)、I，:(2)...叫("），V';'(1)、的(2)...盧〇7);里程計(jì)的速率標(biāo)準(zhǔn)值 為ir。"押)、r之(2)...G:(n);里程計(jì)的標(biāo)度誤差值為5Kd;里程計(jì)偏航安裝角為a 4;偏航安 裝角誤差為5 a 4;俯仰安裝角為a e;俯仰安裝角誤差為5 a e;加速度計(jì)x、y、zS個(gè)軸 的標(biāo)準(zhǔn)值分別為atx(l)、3tx似…atx(n)，3巧（1)、a"似…a"(n)，atz(l)、3tz似…atz(n);加速 度計(jì)x、y、z S個(gè)軸的標(biāo)度誤差分別為5 K"、5 Kgy、5 K。,;加速度計(jì)的失準(zhǔn)角誤差為5 M w、 5 M"z、5 Mw、S Mwz、5 Mazx、5 Mazy;加速度計(jì) x、y、z ;個(gè)軸的零偏分別為 5 B "、S Bay、5 B。^ 巧螺 X、y、z S個(gè)軸的標(biāo)準(zhǔn)值分別為、"tx(2)…《tx(n)，"ty(l)、"ty(2)…《ty(n)， ?tz(l)、《tz(2)...?tz(n);蛇螺 x、y、zS個(gè)軸的標(biāo)度誤差分別為 5K口、SKgy、SKgz;蛇螺 的失準(zhǔn)角誤差為5Mgq、SMgu、SMgyy、SMgy,、SMgu、SMgq;巧螺x、y、zS個(gè)軸的零偏分別 為SBgx、SBgy、5Bg,;里程計(jì)的速率噪聲為Wd(1)、Wd似…WD(n);蛇螺x、y、z^個(gè)軸的噪聲 分別為 Wgx(l)、w口似…Wgx(n)，Wgy(l)、Wgy 似…Wgy(n)，Wgz(l)、Wgz 似…Wgz(n);加速度計(jì) X、 y、z S個(gè)軸的噪聲分別為 w"(l)、似…w"(n)，Ww(l)、Way 似…Ww(n)，Waz(l)、w。,似… Waz(n); 由設(shè)定仿真軌跡中載體的運(yùn)動(dòng)速度計(jì)算得到里程計(jì)的速率標(biāo)準(zhǔn)值，計(jì)算公式如 下：
由計(jì)算得到的里程計(jì)速率標(biāo)準(zhǔn)值G：。）、給定的里程計(jì)標(biāo)度誤差值5 Kd、提取得到的里 程計(jì)速率噪聲WdQ)，綜合得到用于組合導(dǎo)航半實(shí)物仿真的里程計(jì)輸出速率，計(jì)算公 式如下：

用于組合導(dǎo)航半實(shí)物仿真的里程計(jì)輸出速率在載體坐標(biāo)系下的速度分量計(jì)算公式如 下：
采用上述得到的里程計(jì)數(shù)據(jù)巧f、加速度計(jì)數(shù)據(jù)3f、巧螺數(shù)據(jù)Of,結(jié)合由仿真軌跡中給 定的載體初始位置、初始姿態(tài)、初始速度，進(jìn)行組合導(dǎo)航半實(shí)物仿真。

【專利摘要】一種慣導(dǎo)/里程計(jì)組合導(dǎo)航的半實(shí)物仿真方法，有七大步驟：一、將慣導(dǎo)/里程計(jì)系統(tǒng)安裝載體上并上電啟動(dòng)；二、裝訂初始參數(shù)至導(dǎo)航計(jì)算機(jī)；三、載體靜止，采集保存1小時(shí)陀螺和加速度計(jì)輸出數(shù)據(jù)；求取輸出數(shù)據(jù)的平均值并分別與其平均值作差，得到陀螺和加速度計(jì)的實(shí)際噪聲；四、載體保持靜止，慣導(dǎo)進(jìn)行5分鐘靜態(tài)初始對(duì)準(zhǔn)；五、慣導(dǎo)計(jì)算，采集并保存運(yùn)動(dòng)后50秒內(nèi)慣導(dǎo)輸出的速度值和里程計(jì)輸出的速率值；六、將里程計(jì)輸出速率值與載體速率參考值作差，得到里程計(jì)輸出速率的噪聲值；七、設(shè)定仿真軌跡，生成加速度計(jì)和陀螺的標(biāo)準(zhǔn)值，再給定相關(guān)數(shù)據(jù)并將這些數(shù)據(jù)與步驟3、6中得到的噪聲數(shù)據(jù)融合，得到器件的模擬數(shù)據(jù)來進(jìn)行慣導(dǎo)/里程計(jì)組合導(dǎo)航仿真。
【IPC分類】G01C25-00
【公開號(hào)】CN104596540
【申請(qǐng)?zhí)枴緾N201410539231
【發(fā)明人】張小躍, 宋凝芳, 時(shí)海濤, 易曉靜, 潘建業(yè), 劉鵬博
【申請(qǐng)人】北京航空航天大學(xué)
【公開日】2015年5月6日
【申請(qǐng)日】2014年10月13日