一種mems慣性導(dǎo)航系統(tǒng)及基于該系統(tǒng)的軌跡重構(gòu)方法
【專利摘要】本發(fā)明公開了一種MEMS慣性導(dǎo)航系統(tǒng)及基于該系統(tǒng)的軌跡重構(gòu)方法，應(yīng)用六位置法對(duì)陀螺儀和加速度計(jì)進(jìn)行誤差因數(shù)的辨識(shí)得到靜態(tài)誤差模型從而補(bǔ)償靜態(tài)誤差；經(jīng)過低通濾波器濾波顯著地降低陀螺儀和加速度計(jì)的隨機(jī)誤差，降低噪聲，提高輸出精度；采用互補(bǔ)濾波算法將陀螺儀和加速度計(jì)更新的姿態(tài)角度有效地結(jié)合起來得到姿態(tài)矩陣；利用姿態(tài)矩陣變換加速度和重力補(bǔ)償后得到導(dǎo)航模塊在慣性坐標(biāo)系下的三軸加速度；最后通過迭代式雙重積分得到軌跡和速度信息。本發(fā)明的互補(bǔ)濾波算法將加速度計(jì)和陀螺儀獨(dú)立解算的姿態(tài)角融合，可以抑制姿態(tài)角漂移的發(fā)散，使得姿態(tài)推算在動(dòng)態(tài)和靜態(tài)下都大為改善。
【專利說明】
一種MEMS慣性導(dǎo)航系統(tǒng)及基于該系統(tǒng)的軌跡重構(gòu)方法
技術(shù)領(lǐng)域
[0001]本發(fā)明屬于慣性導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種基于MEMS慣性導(dǎo)航系統(tǒng)的軌跡重構(gòu) 方法。 技術(shù)背景
[0002] 微機(jī)械電子系統(tǒng)(MEMS,Micro Electro Mechanical System)是微電子技術(shù)的延 伸和發(fā)展，在二十世紀(jì)末以及本世紀(jì)的十幾年中得到較快的發(fā)展。由于電子工業(yè)和計(jì)算機(jī) 技術(shù)的快速發(fā)展以及電子器件制造成本的不斷降低，一些軍事科技方面的高端技術(shù)開始應(yīng) 用于消費(fèi)電子領(lǐng)域，其中MEMS在人機(jī)交互中的應(yīng)用越來越引起關(guān)注。MEMS慣性傳感器是 MEMS在慣導(dǎo)領(lǐng)域應(yīng)用的產(chǎn)物，它是由MEMS加速度計(jì)和MEMS陀螺儀構(gòu)成。這一技術(shù)不但在航 空航天、國(guó)防科技等軍事領(lǐng)域有著較大的應(yīng)用，在一些民用領(lǐng)域也慢慢有了較大的發(fā)展，比 如體感游戲、人體運(yùn)動(dòng)感知、空中鼠標(biāo)系統(tǒng)，以及目前比較熱門的VR設(shè)備上。在基于MEMS的 慣性導(dǎo)航系統(tǒng)軌跡重構(gòu)技術(shù)上，對(duì)慣性傳感器的精度要求非常高，因?yàn)檫@將直接影響慣性 導(dǎo)航重構(gòu)軌跡的精度。從現(xiàn)在的技術(shù)發(fā)展趨勢(shì)上看，MEMS慣性傳感器也是沿著高精度、高集 成度趨勢(shì)發(fā)展，現(xiàn)有的MEMS加速度傳感器精度可以達(dá)1 · l(T4g，并且還有更進(jìn)一步的提升空 間?，F(xiàn)有的MEMS陀螺儀極限精度能夠達(dá)到0.01/h以下，隨機(jī)游走系數(shù)可以控制在Ι/h以下。 在消費(fèi)電子領(lǐng)域中由于成本限制，選用的MEMS陀螺儀精度一般在10-1 ΟΟ/h范圍內(nèi)。因此精 度的降低需要用特殊的算法來確保重構(gòu)軌跡的精度。
[0003] MEMS慣性傳感器在消費(fèi)電子領(lǐng)域軌跡重構(gòu)應(yīng)用已經(jīng)成為當(dāng)前科研的熱點(diǎn)。05年以 前MEMS傳感器的研究主要集中于MEMS傳感器的制造研究、MEMS傳感器的標(biāo)定誤差處理方法 研究等方面。近幾年對(duì)著人們生活質(zhì)量的不斷提高、智能手機(jī)的興起，MEMS傳感器的應(yīng)用逐 漸進(jìn)入普通人生活。如當(dāng)前IBM推出的VR游戲體驗(yàn)的裝備中就有慣性傳感器，荷蘭XSENS公 司、英國(guó)的x _i〇 Technologies Limited公司和背景WSENS公司推出的人體運(yùn)動(dòng)識(shí)別技術(shù) 等，這些技術(shù)無疑的完善了 MEMS技術(shù)應(yīng)用，同時(shí)也豐富了人們的生活。

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

[0004] 本發(fā)明旨在提供一種基于MEMS慣性導(dǎo)航系統(tǒng)的軌跡重構(gòu)方法，通過對(duì)傳感器的模 型校正，采用濾波算法，減小導(dǎo)航誤差，在一個(gè)范圍內(nèi)提供該系統(tǒng)的運(yùn)動(dòng)姿態(tài)和運(yùn)動(dòng)軌跡， 以滿足對(duì)小范圍內(nèi)的高精度運(yùn)動(dòng)軌跡的導(dǎo)航或跟蹤要求。
[0005] 為了實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案如下:一種MEMS慣性導(dǎo)航系統(tǒng)，包括 慣性傳感器、第一電平轉(zhuǎn)換模塊、電源模塊、第二電平轉(zhuǎn)換模塊、上位機(jī);所述慣性傳感器由 加速度計(jì)和陀螺儀組成;所述第一電平轉(zhuǎn)換模塊上搭載無線傳輸模塊;所述第二電平轉(zhuǎn)換 模塊上搭載無線接收模塊;所述加速度計(jì)用于測(cè)量傳感器的三軸加速度，陀螺儀用于測(cè)量 傳感器的三軸角速度;所述加速度計(jì)和陀螺儀均與第一電平轉(zhuǎn)換模塊相連;所述無線傳輸 模塊與無線接收模塊通過無線傳輸數(shù)據(jù);所述第二電平轉(zhuǎn)換模塊通過USB與上位機(jī)相連;上 位機(jī)對(duì)接收到的信號(hào)進(jìn)行靜態(tài)誤差補(bǔ)償、濾波，并通過分析處理算法解算和更新姿態(tài)，最后 對(duì)瞬時(shí)姿態(tài)進(jìn)行重力補(bǔ)償和雙積分，得到該時(shí)間段內(nèi)的慣導(dǎo)軌跡，實(shí)現(xiàn)軌跡重構(gòu)。
[0006] -種上述的系統(tǒng)的軌跡重構(gòu)方法，具體包括以下步驟：
[0007] (1)所述加速度計(jì)測(cè)量傳感器的三軸加速度，陀螺儀測(cè)量傳感器的三軸角速度；
[0008] (2)所述第一電平轉(zhuǎn)換模塊接收加速度計(jì)測(cè)量到的三軸加速度和陀螺儀測(cè)量到的 三軸角速度，并通過其上搭載無線傳輸模塊傳輸給搭載在第二電平轉(zhuǎn)換模塊上的無線接收 模塊，第二電平轉(zhuǎn)換模塊將該信號(hào)傳輸給上位機(jī)的誤差補(bǔ)償模塊，對(duì)由六位置法標(biāo)定的陀 螺儀和加速度計(jì)得到的數(shù)據(jù)進(jìn)行校正，得到優(yōu)化補(bǔ)償后的加速度和角速度信號(hào)；
[0009] (3)將步驟2得到優(yōu)化補(bǔ)償后的的加速度和角速度信號(hào)送往上位機(jī)的濾波模塊，濾 波模塊先對(duì)信號(hào)進(jìn)行數(shù)字低通濾波，以消除高頻的噪聲信號(hào)，減小信號(hào)的隨機(jī)誤差；
[0010] (4)將步驟3得到的數(shù)字低通濾波后的加速度信號(hào)通過三軸分量的關(guān)系求解加速 度計(jì)的姿態(tài)角；
[0011] (5)將步驟3得到的數(shù)字低通濾波后的角速度信號(hào)通過歐拉法求解陀螺儀的姿態(tài) 角；
[0012] (6)將步驟4和步驟5得到的加速度計(jì)的姿態(tài)角和陀螺儀的姿態(tài)角送往互補(bǔ)濾波模 塊，將加速度計(jì)和陀螺儀的姿態(tài)角進(jìn)行互補(bǔ)，得到慣性傳感器的最佳姿態(tài)角；
[0013] (7)由步驟6得到的慣性傳感器的最佳姿態(tài)角，可獲得慣性傳感器的最佳姿態(tài)矩 陣，將步驟3得到的加速度計(jì)的三軸加速度通過姿態(tài)矩陣進(jìn)行坐標(biāo)系的變換，得到慣性系下 的三軸加速度;再對(duì)三軸加速度進(jìn)行重力補(bǔ)償，對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，得到 物體的位置和速度信息，從而實(shí)現(xiàn)軌跡重構(gòu)。
[0014] 進(jìn)一步的，所述誤差補(bǔ)償模塊的處理步驟具體如下：
[0015] 建立加速度計(jì)誤差模型方程，模型方程如下：
[0016]
[0017] 其中，Ax、Ay、Az分別代表加速度計(jì)x、y、z三軸上的輸出，a XQ、ayQ、az()分別代表加速度 計(jì)x、y、z三軸的零偏，Sx、Sy、Sz分別代表加速度計(jì)x、y、z三軸上的刻度因數(shù)，K xy表示X與y軸間 的耦合誤差因數(shù)，Kyx表示y與X軸間的耦合誤差因數(shù);Kyz表示y與z軸間的耦合誤差因數(shù)，K zy 表示z與y軸間的耦合誤差因數(shù);Kzx表示z與X軸間的耦合誤差因數(shù)，Kxz表示X與z軸間的耦合 誤差因數(shù)，K x2、Ky2、Kz2均代表加速度計(jì)三軸關(guān)于二次方的誤差系數(shù)，Vx、 Vy、Vz分別代表加速 度計(jì)X、y、z三軸的隨機(jī)誤差；
[0018] 建立陀螺儀的誤差模型方程，模型方程如下：
[0019]
[0020] 具中，Gx、Gy、Gz分別代表陽(yáng)螺儀x、y、z三雅上的湔出，g XQ、gyQ、gz()分別代表陀螺儀X、 y、z三軸的零偏，Sx、Sy、Sz分別代表陀螺儀X、y、z三軸上的刻度因數(shù)，K xy表示X與y軸間的耦合 誤差因數(shù)，Kyx表示y與X軸間的耦合誤差因數(shù);Kyz表示y與z軸間的耦合誤差因數(shù)，K zy表示z與 y軸間的耦合誤差因數(shù);Kzx表示z與X軸間的耦合誤差因數(shù)，Kxz表示X與z軸間的耦合誤差因 數(shù)，Kx2、Ky2、Kz2分別代表陀螺儀三軸關(guān)于二次方的誤差系數(shù)， Vx、Vy、Vz分別代表陀螺儀x、y、z 二軸的隨機(jī)誤差；
[0021]在建立MEMS加速度計(jì)和陀螺儀的誤差模型方程后，通過六位置標(biāo)定法標(biāo)定加速度 計(jì)陀螺儀的模型誤差系數(shù)，它們分別為：
[0024] 在誤差系數(shù)的求解上加速度計(jì)與陀螺儀相同，以加速度計(jì)為例，按照加速度計(jì)標(biāo) 定過程，將加速度計(jì)檢測(cè)到的數(shù)據(jù)代入加速度計(jì)誤差模型方程中，得到加速度計(jì)誤差模型 里面的相關(guān)誤差系數(shù)，即
[0025] 加速度計(jì)X軸：
[0026]
[0027]
[0028]
[0029]
[0030]
[0031]
[0032]
[0033]
[0034]解這三組方程得到所有的模型誤差系數(shù)；同理，得到陀螺儀誤差模型里面的相關(guān) 誤差系數(shù)。
[0035]進(jìn)一步的，所述濾波模塊的處理步驟具體如下：
[0036]采用基于Kaiser窗的有限沖激響應(yīng)數(shù)字濾波器對(duì)陀螺儀、加速度計(jì)的隨機(jī)誤差進(jìn) 行抑制并且對(duì)滯后的相位進(jìn)行補(bǔ)償。
[0037]進(jìn)一步的，所述步驟4具體如下：
[0038]由公式(3)和公式(4)展開可得公式(5)和公式(6)，并將步驟3得到的數(shù)字低通濾 波后的加速度信號(hào)代入公式(5)和公式(6)，得到加速度計(jì)的姿態(tài)角；

ax、ay、aj別是X、Y、Z軸的方向余弦，g x、gy、gz分別為重力在三軸上的分 量;Θ為加速度計(jì)的橫滾角和γ為俯仰角。
[0044] 進(jìn)一步的，所述步驟5具體如下：
[0045] 將步驟3得到的數(shù)字低通濾波后的角速度信號(hào)代入公式（7)，得到陀螺儀的姿態(tài) 角；
[0046] (7).
[0047] 共中/、g、#分別表不萩體的航冋用、橫滾角、俯仰角的微分，ωχ、ωΥ、ω#別 表示陀螺儀的三軸角速度。
[0048] 進(jìn)一步的，所述互補(bǔ)濾波模塊的處理步驟如下：
[0049] 將步驟4和步驟5得到的加速度計(jì)的姿態(tài)角和陀螺儀的姿態(tài)角代入公式(8)，得到 慣性傳感器的最佳姿態(tài)角；
[0050] Jk = Kj (jA-jc)+jG (8)
[0051] qK = Kq(qA-qG)+qG (9)
[0052] gK = Kg(gA_gG) +gG (10)其中，jK、qK、gK為慣性傳感器的三個(gè)最佳姿態(tài)角；Kj、Kq、K g 均為三個(gè)姿態(tài)角融合的增益值；<^、0/1、丫/1均為表示加速度計(jì)解算得到的姿態(tài)角，供( ?、0(；、 γ c表示陀螺儀解算得到的姿態(tài)角。
[0053] 進(jìn)一步的，所述步驟7具體為：
[0054] 由步驟6得到的慣性傳感器的最佳姿態(tài)角，可獲得慣性傳感器的最佳姿態(tài)矩陣，將 步驟3得到的加速度計(jì)的三軸加速度通過姿態(tài)矩陣進(jìn)行坐標(biāo)系的變換，得到慣性系下的三 軸加速度;再對(duì)三軸加速度進(jìn)行重力補(bǔ)償，對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，得到物體 的位置和速度信息，從而實(shí)現(xiàn)軌跡重構(gòu)。
[0055] 將步驟6得到的慣性傳感器的最佳姿態(tài)角代入公式（11)，得到慣性傳感器的最佳 姿態(tài)矩F-
[0056] (U)
[0057] 通過上述最佳姿態(tài)矩陣將三軸加速度變換到慣性坐標(biāo)系下，并進(jìn)行重力加速度補(bǔ) 償，公式加下，
[0058] (12)
[0059] (13)
[0060]其中，xb、yb、zb表示載體坐標(biāo)系下三軸的加速度信號(hào); Xn、yn、zn表示慣性坐標(biāo)系下 三軸的加速度信號(hào)，g為重力矢量，Xa、ya、Za為經(jīng)過重力補(bǔ)償后三軸的加速度信號(hào)；
[0061 ]假定從to時(shí)刻開始采樣，t時(shí)刻載體的位移s (t)和速度V (t)的表達(dá)式為：
[0062] (.14)
[0063] (15)
[0064] 其中，SQ、VQ表示初始時(shí)刻載體的位移和速度;(1τ是一個(gè)小量，令
[0065] dt = ti_ti-i = Dt，to ￡ ti ￡ t (16)
[0066] 其中，i為正整數(shù)；
[0067] 將公式(14)和公式(15)改寫為累加式，得到公式(17)和公式(18):
[0068] (17)
[0069] (18)
[0070] 其中，a(i)、v(i)表示i時(shí)刻載體的加速度和速度;η為正數(shù);s(n)為載體最終的位 移;V (η)為載體最終的速度;Dt為時(shí)間間隔；
[0074] 由公式(19)和公式(20)得到載體最終的位移和速度計(jì)算公式：
[0071 ] 通過公式（17)，對(duì)s(n)與s(n-l)進(jìn)行相減，通過公式（18)對(duì)ν(η)與v(n-l)相減得 到遞推廣
[0072] ( 19)
[0073] (20)
[0077] 與現(xiàn)有技術(shù)相比，本發(fā)明的技術(shù)方案具有以下優(yōu)勢(shì)：[0078] 1、本發(fā)明對(duì)硬件的要求較低，僅需要一個(gè)多傳感器的慣導(dǎo)模塊和信號(hào)遠(yuǎn)傳模塊。
[0075] (21)
[0076] (22) 而信號(hào)處理的過程主要在上位機(jī)內(nèi)完成，因此慣導(dǎo)系統(tǒng)的體積較小，可以在微小的工作區(qū) 域內(nèi)實(shí)現(xiàn)慣導(dǎo)軌跡重構(gòu)，而不會(huì)因自身體積太大影響精度。
[0079] 2、本發(fā)明針對(duì)傳感器自身的特性進(jìn)行誤差模型的測(cè)試。通過六位置法收集系統(tǒng)中 搭載的傳感器的大量數(shù)據(jù)，通過對(duì)這些數(shù)據(jù)的誤差分析統(tǒng)計(jì)出傳感器的零偏等只針對(duì)該傳 感器的補(bǔ)償數(shù)據(jù)，建立誤差補(bǔ)償矩陣，具有較好的專一性，而不是采取普遍的校正模型。所 以能根據(jù)自身的結(jié)構(gòu)特點(diǎn)和精度等級(jí)對(duì)傳感器進(jìn)行合適的校正。
[0080] 3、本發(fā)明利用互補(bǔ)濾波的方法，確定當(dāng)前時(shí)刻姿態(tài)角的最佳估計(jì)值，概念清晰，比 較符合工程實(shí)際的應(yīng)用，并且具有較好的補(bǔ)償效果，具有較高的實(shí)用價(jià)值。陀螺儀和加速度 計(jì)都可以獨(dú)立的得到載體的姿態(tài)角，但是兩種信號(hào)都存在較大的誤差，如果將這兩種傳感 器的信號(hào)融合在一起，將可以互補(bǔ)彼此的缺點(diǎn)，使得算法的誤差收斂，姿態(tài)推算的精度大為 改善。
【附圖說明】
[0081 ]圖1為本發(fā)明基于MEMS慣性導(dǎo)航系統(tǒng)的軌跡重構(gòu)算法結(jié)構(gòu)流程圖；
[0082]圖2為加速度計(jì)、陀螺儀數(shù)據(jù)融合的互補(bǔ)濾波算法框圖；
[0083]圖3為捷聯(lián)式慣性導(dǎo)航系統(tǒng)原理框圖。
【具體實(shí)施方式】
[0084]下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步的說明。
[0085] 一種MEMS慣性導(dǎo)航系統(tǒng)，包括慣性傳感器、第一電平轉(zhuǎn)換模塊、電源模塊、第二電 平轉(zhuǎn)換模塊、上位機(jī);所述慣性傳感器由加速度計(jì)和陀螺儀組成;所述第一電平轉(zhuǎn)換模塊上 搭載無線傳輸模塊;所述第二電平轉(zhuǎn)換模塊上搭載無線接收模塊;所述加速度計(jì)用于測(cè)量 傳感器的三軸加速度，陀螺儀用于測(cè)量傳感器的三軸角速度;所述加速度計(jì)和陀螺儀均與 第一電平轉(zhuǎn)換模塊相連;所述無線傳輸模塊與無線接收模塊通過無線傳輸數(shù)據(jù);所述第二 電平轉(zhuǎn)換模塊通過USB與上位機(jī)相連;上位機(jī)對(duì)接收到的信號(hào)進(jìn)行靜態(tài)誤差補(bǔ)償、濾波，并 通過分析處理算法解算和更新姿態(tài)，最后對(duì)瞬時(shí)姿態(tài)進(jìn)行重力補(bǔ)償和雙積分，得到該時(shí)間 段內(nèi)的慣導(dǎo)軌跡，實(shí)現(xiàn)軌跡重構(gòu)。
[0086] -種上述的系統(tǒng)的軌跡重構(gòu)方法，具體包括以下步驟：
[0087] (1)所述加速度計(jì)測(cè)量傳感器的三軸加速度，陀螺儀測(cè)量傳感器的三軸角速度；
[0088] (2)所述第一電平轉(zhuǎn)換模塊接收加速度計(jì)測(cè)量到的三軸加速度和陀螺儀測(cè)量到的 三軸角速度，并通過其上搭載無線傳輸模塊傳輸給搭載在第二電平轉(zhuǎn)換模塊上的無線接收 模塊，第二電平轉(zhuǎn)換模塊將該信號(hào)傳輸給上位機(jī)的誤差補(bǔ)償模塊，對(duì)由六位置法標(biāo)定的陀 螺儀和加速度計(jì)得到的數(shù)據(jù)進(jìn)行校正，得到優(yōu)化補(bǔ)償后的加速度和角速度信號(hào)；
[0089] (3)將步驟2得到優(yōu)化補(bǔ)償后的的加速度和角速度信號(hào)送往上位機(jī)的濾波模塊，濾 波模塊先對(duì)信號(hào)進(jìn)行數(shù)字低通濾波，以消除高頻的噪聲信號(hào)，減小信號(hào)的隨機(jī)誤差；
[0090] (4)將步驟3得到的數(shù)字低通濾波后的加速度信號(hào)通過三軸分量的關(guān)系求解加速 度計(jì)的姿態(tài)角；
[0091] (5)將步驟3得到的數(shù)字低通濾波后的角速度信號(hào)通過歐拉法求解陀螺儀的姿態(tài) 角；
[0092] (6)將步驟4和步驟5得到的加速度計(jì)的姿態(tài)角和陀螺儀的姿態(tài)角送往互補(bǔ)濾波模 塊，將加速度計(jì)和陀螺儀的姿態(tài)角進(jìn)行互補(bǔ)，得到慣性傳感器的最佳姿態(tài)角；
[0093] (7)由步驟6得到的慣性傳感器的最佳姿態(tài)角，可獲得慣性傳感器的最佳姿態(tài)矩 陣，將步驟3得到的加速度計(jì)的三軸加速度通過姿態(tài)矩陣進(jìn)行坐標(biāo)系的變換，得到慣性系下 的三軸加速度;再對(duì)三軸加速度進(jìn)行重力補(bǔ)償，對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，得到 物體的位置和速度信息，從而實(shí)現(xiàn)軌跡重構(gòu)。
[0094] A:誤差模型分析：分析MEMS加速度計(jì)誤差形成的原因可以得到誤差模型方程如 下：
[0095]
[0096] 其中，Ax、Ay、Az分別代表加速度計(jì)x、y、z三軸上的輸出，a xQ、ayQ、az()分別代表加速度 計(jì)x、y、z三軸的零偏，Sx、Sy、Sz分別代表加速度計(jì)x、y、z三軸上的刻度因數(shù)，K xy表示X與y軸間 的耦合誤差因數(shù)，Kyx表示y與X軸間的耦合誤差因數(shù);Kyz表示y與z軸間的耦合誤差因數(shù)，K zy 表示z與y軸間的耦合誤差因數(shù);Kzx表示z與X軸間的耦合誤差因數(shù)，Kxz表示X與z軸間的耦合 誤差因數(shù)，K x2、Ky2、Kz2均代表加速度計(jì)三軸關(guān)于二次方的誤差系數(shù)，Vx、 Vy、Vz分別代表加速 度計(jì)X、y、z三軸的隨機(jī)誤差；
[0097] 與MEMS加速度計(jì)相似，也建立了陀螺儀的誤差模型方程如下：
[0098]
[0099]其中，Gx、Gy、Gz分別代表陀螺儀三軸上的輸出，g xQ、gyQ、gz()分別代表陀螺儀三軸的 零偏，Sx、Sy、Sz分別代表陀螺儀三軸上的刻度因數(shù)，Kyx、K xy表示X與Y軸間的耦合誤差因數(shù)； Kzy、Kyz表示Y與Z軸間的耦合誤差因數(shù);KXZ、KZX表示Z與X軸間的耦合誤差因數(shù)，K x2、Ky2、Kz2分 別代表陀螺儀關(guān)于二次方的誤差系數(shù)，Vx、v y、vz分別代表陀螺儀三軸的隨機(jī)誤差。
[0100]在建立MEMS加速度計(jì)和陀螺儀的誤差模型方程后，可以通過位置標(biāo)定法標(biāo)定兩種 傳感器的模型誤差系數(shù)。本發(fā)明根據(jù)誤差模型的未知系數(shù)的個(gè)數(shù)采用了六位置的標(biāo)定方 法，它們分別為：
[0102] 實(shí)驗(yàn)中對(duì)于加速度計(jì)和陀螺儀唯一的區(qū)別在于加速度計(jì)需要恒定的g輸入，模塊 需要保持靜止?fàn)顟B(tài)，可以在平臺(tái)上進(jìn)行靜態(tài)的測(cè)量;但是陀螺儀在靜態(tài)是沒有輸出的，需要 將模塊固定在轉(zhuǎn)臺(tái)上保持轉(zhuǎn)臺(tái)轉(zhuǎn)軸與模塊坐標(biāo)軸的重合并且依靠轉(zhuǎn)臺(tái)的勻速轉(zhuǎn)動(dòng)完成測(cè) 試。
[0103] 誤差系數(shù)的求解上加速度計(jì)與陀螺儀相同，以加速度計(jì)為例，按照規(guī)定的MEMS加 速度計(jì)標(biāo)定過程，將加速度計(jì)測(cè)得的數(shù)據(jù)代入加速度計(jì)的誤差模型，得到如下三組方程：
[0104] 加速度計(jì)的X軸：
[0105]
[0106]
[0107]
[0108]
[0109]
[0110]
[0111]
[0112]
[0113] 由上述三組方程可以分別得到加速度傳感器誤差模型里面的相關(guān)誤差系數(shù)：
[0114] 加諫庶i+X軸：
[0115]
[0116]
[0117]
[0118]
[0119]
[0120]
[0121]
[0122]
[0123] B:低通濾波模塊:建立加速度計(jì)和陀螺儀的誤差模型后就可以對(duì)其輸出進(jìn)行靜態(tài) 的補(bǔ)償，但是傳感器的隨機(jī)誤差是無法用實(shí)驗(yàn)的方法確定下來的。因?yàn)樵贏中標(biāo)定加速度計(jì) 和陀螺儀誤差模型時(shí)每個(gè)測(cè)試點(diǎn)值用的都是平均值，所以在求解參數(shù)的過程中忽略隨機(jī)誤 差~、^、^。一般處理隨機(jī)誤差的方法有低通濾波、卡爾曼濾波以及啟發(fā)式消減法濾波 (HDR)，本發(fā)明將用基于Kaiser窗的有限沖激響應(yīng)數(shù)字濾波器對(duì)陀螺儀、加速度計(jì)三軸的隨 機(jī)誤差進(jìn)行抑制并且對(duì)滯后的相位進(jìn)行補(bǔ)償。
[0124] C:姿態(tài)更新模塊:矯正三軸角速度和三軸加速度的零偏誤差和隨機(jī)噪聲后，三軸 角速度通過歐拉法計(jì)算此時(shí)模塊姿態(tài)角的微分;三軸加速度通過坐標(biāo)變換表達(dá)當(dāng)前的姿態(tài) 角。
[0125] 以重力為例，在三維空間中，重力在三個(gè)坐標(biāo)軸上的分量與重力方向和三個(gè)坐標(biāo) 軸的方向有如下關(guān)系：
[0126] q = 9〇-ay (3)
[0127] cosqcosg = cosaz (4)
[0128] 由公式(3)和公式(4)得到公式(5)和公式(6)即橫滾角和俯仰角的表達(dá)式：
[0129]
[0130]
[0131]
別是X、Y、Z軸的方向余弦，gx、gy、gz分別為重力在三軸上的分 M; ? 73 皮it的楨滾角和γ為俯仰角。
[0132] 歐拉角姿態(tài)更新算法利用微分方程關(guān)系，通過角速度得到三個(gè)姿態(tài)角的微分，方 程最終姑里加下.
[0133] (7)
[0134] 其中，/、. .g、女分別表不載體的航向角、橫滾角、俯仰角的微分，ωχ、c〇y、(〇2表 示陀螺儀的三軸角速度，并且經(jīng)過地轉(zhuǎn)角速度和相關(guān)誤差的補(bǔ)償。
[0135] D:多傳感器互補(bǔ)融合濾波模塊：加速度計(jì)和陀螺儀可以獨(dú)立解算出載體的姿態(tài) 角，利用互補(bǔ)濾波模塊將兩者通過一個(gè)隨運(yùn)動(dòng)狀態(tài)變化的增益值融合起來，得到當(dāng)前載體 姿態(tài)角的最佳估計(jì)值，互補(bǔ)濾波的方程如下：
[0136] jK = Kj( jA-jG)+jG (8)
[0137] qK = Kq(qA-qc)+qG (9)
[0138] gK = Kg(gA-gG)+gG (10)其中，攸、θκ、γκ為慣性傳感器的三個(gè)最佳姿態(tài)角；御、 Κθ、Κγ為三個(gè)姿態(tài)角融合的增益值，它與載體當(dāng)前的運(yùn)動(dòng)狀態(tài)有關(guān)。?y、0A、γ Α表示加速度計(jì) 解算得到的姿態(tài)角，抖,'、9C、y c表示陀螺儀解算得到的姿態(tài)角。
[0139] E:加速度變換和重力補(bǔ)償模塊:通過互補(bǔ)濾波解算得到的當(dāng)前時(shí)刻最優(yōu)姿態(tài)角， 得到當(dāng)前時(shí)刻模塊的姿態(tài)矩陣：
[0140]
(11)
[0141] 通過上述最佳姿態(tài)矩陣將三軸加速度變換到慣性坐標(biāo)系下，并進(jìn)行重力加速度補(bǔ) 償，公式如下：
[0142] (12)
[0143] (13)
[0144] 其中，Xb、yb、Zb表示載體坐標(biāo)系下的加速度信號(hào);&、5^、 211表示慣性坐標(biāo)系下加速 度信號(hào)，g為重力矢量，Xa、ya、z a為經(jīng)過重力補(bǔ)償后的加速度信號(hào)；
[0145] F:積分環(huán)節(jié)和軌跡重構(gòu)模塊:通過坐標(biāo)系轉(zhuǎn)換和重力濾波的加速度信號(hào)代表載體 當(dāng)前時(shí)刻在慣性系下的三軸加速度，對(duì)其進(jìn)行雙重積分后就可以得到載體的位置和速度信 息，從而實(shí)現(xiàn)載體的軌跡重構(gòu)。
[0146] 假定從to時(shí)刻開始采樣，t時(shí)刻載體的位移s (t)和速度V (t)的表達(dá)式為：
[0147] ^ 14)
[0148] (15.)
[0149] 其中，SQ、VQ表示初始時(shí)刻載體的位移和速度，(1τ是一個(gè)小量，令
[0150] dt = ti_ti-i = Dt，to ￡ ti ￡ t (16)
[0151] 其中i為正整數(shù)；
[0152] 將公式（14)和公式（15)改寫為累加式，得到公式(17)和公式(18):
[0153]
(17)
[0154]
(18)
[0155] 其中，a(i)、v(i)表示i時(shí)刻載體的加速度和速度;η為正數(shù);s(n)為載體最終的位 移;V (η)為載體最終的速度;Dt為時(shí)間間隔；
[0?5?] 通過公式（17)，對(duì)s(n)與s(n-l)進(jìn)行相減，通過公式（18)對(duì)ν(η)與v(n-l)相減得 到遞推式：
[0159] 由公式(19)和公式(20)得到載體最終的位移和速度計(jì)算公式：
[0157] (19)
[0158] P〇)
[0160] (21)
[0161] (22)
【主權(quán)項(xiàng)】
1. 一種MEMS慣性導(dǎo)航系統(tǒng)，其特征在于，包括慣性傳感器、第一電平轉(zhuǎn)換模塊、電源模 塊、第二電平轉(zhuǎn)換模塊、上位機(jī);所述慣性傳感器由加速度計(jì)和陀螺儀組成;所述第一電平 轉(zhuǎn)換模塊上搭載無線傳輸模塊;所述第二電平轉(zhuǎn)換模塊上搭載無線接收模塊;所述加速度 計(jì)用于測(cè)量傳感器的三軸加速度，陀螺儀用于測(cè)量傳感器的三軸角速度;所述加速度計(jì)和 陀螺儀均與第一電平轉(zhuǎn)換模塊相連;所述無線傳輸模塊與無線接收模塊通過無線傳輸數(shù) 據(jù);所述第二電平轉(zhuǎn)換模塊通過USB與上位機(jī)相連;上位機(jī)對(duì)接收到的信號(hào)進(jìn)行靜態(tài)誤差補(bǔ) 償、濾波，并通過分析處理算法解算和更新姿態(tài)，最后對(duì)瞬時(shí)姿態(tài)進(jìn)行重力補(bǔ)償和雙積分， 得到該時(shí)間段內(nèi)的慣導(dǎo)軌跡，實(shí)現(xiàn)軌跡重構(gòu)。2. -種利用權(quán)利要求1所述的系統(tǒng)的軌跡重構(gòu)方法，其特征在于，具體包括以下步驟： (1) 所述加速度計(jì)測(cè)量傳感器的三軸加速度，陀螺儀測(cè)量傳感器的三軸角速度； (2) 所述第一電平轉(zhuǎn)換模塊接收加速度計(jì)測(cè)量到的三軸加速度和陀螺儀測(cè)量到的三軸 角速度，并通過其上搭載無線傳輸模塊傳輸給搭載在第二電平轉(zhuǎn)換模塊上的無線接收模 塊，第二電平轉(zhuǎn)換模塊將該信號(hào)傳輸給上位機(jī)的誤差補(bǔ)償模塊，對(duì)由六位置法標(biāo)定的陀螺 儀和加速度計(jì)得到的數(shù)據(jù)進(jìn)行校正，得到優(yōu)化補(bǔ)償后的加速度和角速度信號(hào)； (3) 將步驟2得到優(yōu)化補(bǔ)償后的的加速度和角速度信號(hào)送往上位機(jī)的濾波模塊，濾波模 塊先對(duì)信號(hào)進(jìn)行數(shù)字低通濾波，以消除高頻的噪聲信號(hào)，減小信號(hào)的隨機(jī)誤差； (4) 將步驟3得到的數(shù)字低通濾波后的加速度信號(hào)通過三軸分量的關(guān)系求解加速度計(jì) 的姿態(tài)角； (5) 將步驟3得到的數(shù)字低通濾波后的角速度信號(hào)通過歐拉法求解陀螺儀的姿態(tài)角； (6) 將步驟4和步驟5得到的加速度計(jì)的姿態(tài)角和陀螺儀的姿態(tài)角送往互補(bǔ)濾波模塊， 將加速度計(jì)和陀螺儀的姿態(tài)角進(jìn)行互補(bǔ)，得到慣性傳感器的最佳姿態(tài)角； (7) 由步驟6得到的慣性傳感器的最佳姿態(tài)角，可獲得慣性傳感器的最佳姿態(tài)矩陣，將 步驟3得到的加速度計(jì)的三軸加速度通過姿態(tài)矩陣進(jìn)行坐標(biāo)系的變換，得到慣性系下的三 軸加速度;再對(duì)三軸加速度進(jìn)行重力補(bǔ)償，對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，得到物體 的位置和速度信息，從而實(shí)現(xiàn)軌跡重構(gòu)。3. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述誤差補(bǔ)償模塊的處理步驟具 體如下： 建立加速度計(jì)誤差模型方程，模型方程如下：其中，Ax、Ay、Az分別代表加速度計(jì)x、y、z三軸上的輸出，a xQ、ayQ、az()分別代表加速度計(jì)X、 y、z三軸的零偏，Sx、Sy、Sz分別代表加速度計(jì)x、y、z三軸上的刻度因數(shù)，K xy表示X與y軸間的耦 合誤差因數(shù)，Kyx表示y與X軸間的耦合誤差因數(shù);Kyz表示y與z軸間的耦合誤差因數(shù)，K zy表示z 與y軸間的耦合誤差因數(shù);Kzx表示z與X軸間的耦合誤差因數(shù)，Kxz表示X與z軸間的耦合誤差 因數(shù)，K x2、Ky2、Kz2均代表加速度計(jì)三軸關(guān)于二次方的誤差系數(shù)，Vx、 Vy、Vz分別代表加速度計(jì) X、y、z三軸的隨機(jī)誤差； 建立陀螺儀的誤差模型方程，模型方程如下：其中，Gx、Gy、Gz分別代表陀螺儀x、y、z三軸上的輸出，g xQ、gyQ、gzQ分別代表陀螺儀x、y、z 三軸的零偏，Sx、Sy、Sz分別代表陀螺儀X、y、z三軸上的刻度因數(shù)，K xy表示X與y軸間的耦合誤 差因數(shù)，Kyx表示y與X軸間的耦合誤差因數(shù);Kyz表示y與z軸間的耦合誤差因數(shù)，K zy表示z與y 軸間的耦合誤差因數(shù);Kzx表示z與X軸間的耦合誤差因數(shù)，Kxz表示X與z軸間的耦合誤差因 數(shù)，K x2、Ky2、Kz2分別代表陀螺儀三軸關(guān)于二次方的誤差系數(shù)，Vx、 Vy、Vz分別代表陀螺儀x、y、z 二軸的隨機(jī)誤差； 在建立MEMS加速度計(jì)和陀螺儀的誤差模型方程后，通過六位置標(biāo)定法標(biāo)定加速度計(jì)陀 螺儀的模型誤差系數(shù)，它們分別為：在誤差系數(shù)的求解上加速度計(jì)與陀螺儀相同，以加速度計(jì)為例，按照加速度計(jì)標(biāo)定過 程，將加速度計(jì)檢測(cè)到的數(shù)據(jù)代入加速度計(jì)誤差模型方程中，得到加速度計(jì)誤差模型里面 的相關(guān)誤差系數(shù)，即 加速度計(jì)X軸： Axl - ax〇+Sx+Kx2，Αχ2 - 3<x〇+Kyx，Αχ3 - 3·χ〇+Κζχ Αχ4 - 3<x〇_Sx+Kx2，Αχ5 - BxO-Kyx，Αχ6 - 3·χ〇_Κζχ 加速度計(jì)Y軸： Ayl - 3y〇+Kxy，Ay2 - 3<y〇+Sy+Ky2，Ay3 - 3<y〇+Kzy Ay4-3<y〇_Kxy，Ay5 - 3<y〇_Sy+Ky2，Ay6 - 3<y〇_Kzy 加速度計(jì)Z軸： Azl - ΒζΟ+Κχζ，Ay2 - BzO+Kyz，Az3 - 3<zO+Sz+Kz2 Az4 - BzO-Kxz，Az5 - BzO-Kzy，Az6 - 3<zO_Sz+Kz2 解這三組方程得到所有的模型誤差系數(shù)；同理，得到陀螺儀誤差模型里面的相關(guān)誤差 系數(shù)。4. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述濾波模塊的處理步驟具體如 下： 采用基于Kaiser窗的有限沖激響應(yīng)數(shù)字濾波器對(duì)陀螺儀、加速度計(jì)的隨機(jī)誤差進(jìn)行抑 制并且對(duì)滯后的相位進(jìn)行補(bǔ)償。5. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述步驟4具體如下： 由公式(3)和公式(4)展開可得公式(5)和公式(6)，并將步驟3得到的數(shù)字低通濾波后 的加速度信號(hào)代入公式(5)和公式(6)，得到加速度計(jì)的姿態(tài)角；量;θ為加速度計(jì)的橫滾角和γ為俯仰角。6. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述步驟5具體如下： 將步驟3得到的數(shù)字低通濾波后的角速度信號(hào)代入公式(7)，得到陀螺儀的姿態(tài)角；(7) 其中@、會(huì)分別表不載體的航向角、橫滾角、俯仰角的微分，《。0^、(02分別表不 陀螺儀的三軸角速度。7. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述互補(bǔ)濾波模塊的處理步驟如 下： 將步驟4和步驟5得到的加速度計(jì)的姿態(tài)角和陀螺儀的姿態(tài)角代入公式(8)，得到慣性 傳感器的最佳姿態(tài)角； jK = Kj( jA-jc)+jG (8) qK = Kq (qA_qc) +qc (9) gK = Kg(gA-gG)+gG (10) 其中，jK、qK、gK為慣性傳感器的三個(gè)最佳姿態(tài)角;Kj、K q、Kg均為三個(gè)姿態(tài)角融合的增益 值;<^、ΘΑ、γΑ均為表示加速度計(jì)解算得到的姿態(tài)角表示陀螺儀解算得到的姿態(tài) 角。8. 根據(jù)權(quán)利要求2所述的軌跡重構(gòu)方法，其特征在于，所述步驟7具體為： 由步驟6得到的慣性傳感器的最佳姿態(tài)角，可獲得慣性傳感器的最佳姿態(tài)矩陣，將步驟 3得到的加速度計(jì)的三軸加速度通過姿態(tài)矩陣進(jìn)行坐標(biāo)系的變換，得到慣性系下的三軸加 速度;再對(duì)三軸加速度進(jìn)行重力補(bǔ)償，對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，得到物體的位 置和速度信息，從而實(shí)現(xiàn)軌跡重構(gòu)。 將步驟6得到的慣性傳感器的最佳姿態(tài)角代入公式（11)，得到慣性傳感器的最佳姿態(tài) 矩陣：(11)通過上述最佳姿態(tài)矩陣將三軸加速度變換到慣性坐標(biāo)系下，并進(jìn)行重力加速度補(bǔ)償， 公式如下 (12) (13) 其中，Xb、yb、Zb表示載體坐標(biāo)系下三軸的加速度信號(hào);知+、^表示慣性坐標(biāo)系下三軸 的加速度信號(hào)，g為重力矢量，Xa、ya、Za為經(jīng)過重力補(bǔ)償后三軸的加速度信號(hào)； 假定從to時(shí)刻開始采樣，t時(shí)刻載體的位移S (t)和速度V (t)的表達(dá)式為：其中，so、VO表示初始時(shí)亥Ij載體的位移和速度；辦是一個(gè)小量，令 (14) (15)(16) 其中，i為正整數(shù)；將公式(14)和公式(15)改寫為累加式，得到公式(17)和公式(18):(Π ) (18) 其中，a(i)、v(i)表示i時(shí)刻載體的加速度和速度;η為正數(shù);s(n)為載體最終的位移;V (η)為載體最終的速度;Dt為時(shí)間間隔； 通過公式（17)，對(duì)s(n)與s(n-l)進(jìn)行相減，通過公式（18)對(duì)ν(η)與v(n-l)相減得到遞 推式： (19) 20)(21) (22)
【文檔編號(hào)】G01C21/16GK105890593SQ201610210992
【公開日】2016年8月24日
【申請(qǐng)日】2016年4月6日
【發(fā)明人】鄔博騁, 徐方凱, 鄭開瑜, 謝磊
【申請(qǐng)人】浙江大學(xué)