基于運(yùn)動(dòng)捕捉系統(tǒng)的旋轉(zhuǎn)mems慣導(dǎo)磁航向角誤差補(bǔ)償方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種基于運(yùn)動(dòng)捕捉系統(tǒng)的旋轉(zhuǎn)MEMS(微機(jī)電系統(tǒng)）慣導(dǎo)磁航向角誤 差補(bǔ)償方法，屬于一種旋轉(zhuǎn)慣導(dǎo)系統(tǒng)的磁航向角誤差補(bǔ)償方法。
【背景技術(shù)】
[0002] 慣導(dǎo)系統(tǒng)中陀螺存在誤差漂移，長(zhǎng)期運(yùn)行存在累積誤差，航向值會(huì)出現(xiàn)發(fā)散。在導(dǎo) 航定位中一般采用磁傳感器提供航向（如期刊文章《一種電子磁羅盤(pán)航向誤差的自適應(yīng)補(bǔ) 償方法》，儀器儀表學(xué)報(bào)，2014年第35卷第11期，頁(yè)碼：2607-2614 )。
[0003] 旋轉(zhuǎn)調(diào)制技術(shù)是一種誤差抑制技術(shù)，通過(guò)旋轉(zhuǎn)機(jī)構(gòu)帶動(dòng)慣性器件按照既定方 案旋轉(zhuǎn)，使得慣性器件常值偏差沿導(dǎo)航系統(tǒng)投影呈周期振蕩形式，在一個(gè)旋轉(zhuǎn)周期內(nèi)積 分結(jié)果為零，進(jìn)而抵消慣性器件常值偏差對(duì)導(dǎo)航精度的影響來(lái)提高導(dǎo)航精度(如專(zhuān)利 CN201410143285. 4公開(kāi)的《一種單軸旋轉(zhuǎn)慣導(dǎo)系統(tǒng)慣性器件誤差補(bǔ)償方法》）。
[0004] 然而，由于轉(zhuǎn)動(dòng)機(jī)構(gòu)的導(dǎo)磁材料以及轉(zhuǎn)動(dòng)機(jī)構(gòu)中電機(jī)的強(qiáng)磁體的影響，轉(zhuǎn)動(dòng)機(jī)構(gòu) 上磁傳感器周?chē)拇艌?chǎng)強(qiáng)度相對(duì)于地磁提高了數(shù)十倍（如專(zhuān)利CN201310688044. 3公開(kāi)的 《一種雙軸旋轉(zhuǎn)光纖慣導(dǎo)系統(tǒng)磁屏蔽裝置》），以上情況影響磁傳感器的正常工作。

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

[0005] 本發(fā)明提出了一種基于運(yùn)動(dòng)捕捉系統(tǒng)的旋轉(zhuǎn)MEMS(微機(jī)電系統(tǒng)）慣導(dǎo)磁航向角誤 差補(bǔ)償方法，解決旋轉(zhuǎn)MEMS(微機(jī)電系統(tǒng)）慣導(dǎo)系統(tǒng)中轉(zhuǎn)動(dòng)機(jī)構(gòu)的磁干擾對(duì)磁傳感器航向 角解算帶來(lái)的誤差影響。
[0006] 本發(fā)明為解決其技術(shù)問(wèn)題采用如下技術(shù)方案： 一種基于運(yùn)動(dòng)捕捉系統(tǒng)的旋轉(zhuǎn)MEMS慣導(dǎo)磁航向角誤差補(bǔ)償方法，包括如下步驟： 步驟1:對(duì)磁傳感器進(jìn)行標(biāo)定； 步驟2:獲得運(yùn)動(dòng)捕捉系統(tǒng)的參考坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的轉(zhuǎn)移矩陣； 步驟3:采集多個(gè)位置上，系統(tǒng)輸出的轉(zhuǎn)動(dòng)機(jī)構(gòu)位置信息、對(duì)應(yīng)位置上磁傳感器輸出 值、運(yùn)動(dòng)捕捉系統(tǒng)獲取到的標(biāo)記點(diǎn)位置坐標(biāo)信息； 步驟4:計(jì)算得到各個(gè)位置上由于轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)強(qiáng)度； 步驟5:從原始磁傳感器輸出值中扣除轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)強(qiáng)度，計(jì)算磁航向。
[0007] 所述步驟（1)的具體實(shí)施步驟如下：使用橢球標(biāo)定法對(duì)磁傳感器進(jìn)行標(biāo)定，得到 誤差參數(shù)尤、丑〇，其中r為誤差系數(shù)矩陣，為三軸磁傳感器的零偏； 使用上述參數(shù)對(duì)磁傳感器輸出進(jìn)行校正：
其中為三軸磁傳感器的實(shí)際輸出，//為校正后的三軸磁傳感器輸出值。
[0008] 所述步驟（2)的具體實(shí)施步驟如下：在磁傳感器數(shù)據(jù)采集系統(tǒng)上安裝3個(gè)標(biāo)記點(diǎn)， 3個(gè)標(biāo)記點(diǎn)的兩兩連線(xiàn)構(gòu)成了一個(gè)非等腰直角三角形； 記錄&系下磁傳感器輸出值，根據(jù)該磁傳感器輸出值與地磁矢量在"系下各 軸的分量，建立6系相對(duì)于《系的轉(zhuǎn)移矩陣< : 其中，《系是指導(dǎo)航坐標(biāo)系，屬于右手坐標(biāo)系，其、z軸分別指向東向、北向、天 向； 辦系是磁傳感器數(shù)據(jù)采集系統(tǒng)的機(jī)體坐標(biāo)系，屬于右手笛卡爾坐標(biāo)系，a'r、z軸分 別指向磁傳感器器件敏感軸方向；
其中，、Ar、沒(méi)《分別為A在&系下JT、F三軸上的分量， 分別為丑在《系下if、J7、Z三軸上的分量； 運(yùn)動(dòng)捕捉系統(tǒng)獲取標(biāo)記點(diǎn)在《系下的位置坐標(biāo)信息，從而解算磁傳感器數(shù)據(jù)采集系統(tǒng) 的姿態(tài)信息，使用歐拉角法得到i系相對(duì)于,w系的轉(zhuǎn)移矩陣其中，w系是運(yùn)動(dòng)捕捉系 統(tǒng)的參考坐標(biāo)系，屬于右手笛卡爾坐標(biāo)系，X、2軸平行于安裝場(chǎng)地的平面，7軸垂直于安 裝場(chǎng)地的平面向上，且與Z軸和Z軸滿(mǎn)足右手準(zhǔn)則； 通過(guò)<和< 獲得"系相對(duì)于^系的轉(zhuǎn)移矩陣q:
[0009] 所述步驟（3)的具體實(shí)施步驟如下：將磁傳感器數(shù)據(jù)采集系統(tǒng)安裝在旋轉(zhuǎn)慣導(dǎo)系 統(tǒng)的轉(zhuǎn)動(dòng)機(jī)構(gòu)上，轉(zhuǎn)動(dòng)機(jī)構(gòu)在0°到360°范圍每10°停留一次，一共停留36個(gè)位置，每個(gè) 位置上停留3分鐘； 記錄上述過(guò)程中，系統(tǒng)輸出的轉(zhuǎn)動(dòng)機(jī)構(gòu)位置信息P，對(duì)應(yīng)位置上磁傳感器輸出值/￡、 運(yùn)動(dòng)捕捉系統(tǒng)獲取到的標(biāo)記點(diǎn)位置坐標(biāo)信息； 使用標(biāo)記點(diǎn)位置坐標(biāo)信息，解算磁傳感器數(shù)據(jù)采集系統(tǒng)的姿態(tài)信息，進(jìn)而使用歐拉角 法得到磁傳感器數(shù)據(jù)采集系統(tǒng)的&系相對(duì)于《系的轉(zhuǎn)移矩陣結(jié)合對(duì)應(yīng)的轉(zhuǎn)動(dòng)機(jī)構(gòu)位 置信息P，建立4(F)，其中乂為轉(zhuǎn)移矩陣與轉(zhuǎn)動(dòng)機(jī)構(gòu)位置信息p的關(guān)系； 使用系統(tǒng)輸出的轉(zhuǎn)動(dòng)機(jī)構(gòu)位置信息P以及對(duì)應(yīng)位置上磁傳感器的輸出值//，H= /2(乃，其中/2為磁傳感器的輸出值及與轉(zhuǎn)動(dòng)機(jī)構(gòu)的位置信息f的關(guān)系； //由兩個(gè)分量組成：
其中為地磁矢量在&系下各軸的分量，/ffi^為轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)強(qiáng)度。
[0010] 所述步驟（4)的具體實(shí)施步驟如下： 計(jì)算旋轉(zhuǎn)慣導(dǎo)系統(tǒng)的工作過(guò)程中，各個(gè)位置上由于轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)強(qiáng)度；
G通過(guò)步驟2獲取，沒(méi)￡0^是已知量； 將上式簡(jiǎn)化，得到： =/3(P)，其中/3為轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)強(qiáng)度與轉(zhuǎn)動(dòng)機(jī)構(gòu)的位置信息f的 關(guān)系； 依此，建立了在0°到360°范圍內(nèi)轉(zhuǎn)動(dòng)機(jī)構(gòu)對(duì)磁傳感器輸出的影響與位置信息的關(guān) 系。
[0011] 所述步驟（5)的具體實(shí)施步驟如下： 在旋轉(zhuǎn)MEMS慣導(dǎo)工作的過(guò)程中，導(dǎo)航系統(tǒng)實(shí)時(shí)獲得當(dāng)前的轉(zhuǎn)動(dòng)機(jī)構(gòu)信息P，以及磁傳 感器輸出值沒(méi)； 對(duì)轉(zhuǎn)動(dòng)機(jī)構(gòu)造成的磁場(chǎng)進(jìn)行補(bǔ)償，補(bǔ)償后的磁傳感器值：
使用與地磁矢量#_在《系下各軸的分量建立S系相對(duì)于》系的轉(zhuǎn)移矩陣< :
其中，足r、&、分別為沒(méi)在A系下JT、7、Z三軸上的分量，丑、民rfr、 分別為在《系下i：、F、2三軸上的分量，使用歐拉角法得到磁航向值。
[0012] 本發(fā)明的有益效果如下： 本發(fā)明提出了一種基于運(yùn)動(dòng)捕捉系統(tǒng)的旋轉(zhuǎn)MEMS慣導(dǎo)磁航向角誤差補(bǔ)償方法，流程 簡(jiǎn)潔，可以標(biāo)定出轉(zhuǎn)動(dòng)機(jī)構(gòu)的磁干擾對(duì)磁傳感器輸出的影響，對(duì)磁航向角誤差進(jìn)行補(bǔ)償，為 旋轉(zhuǎn)MEMS慣導(dǎo)系統(tǒng)提供磁航向，抑制航向的誤差發(fā)散。
【附圖說(shuō)明】
[0013] 圖1表示使用本發(fā)明提出的補(bǔ)償方法進(jìn)行補(bǔ)償前后磁航向值角誤差值的對(duì)比圖。
【具體實(shí)施方式】 下面結(jié)合附圖對(duì)本發(fā)明創(chuàng)造做進(jìn)一步詳細(xì)說(shuō)明。
[0014] 實(shí)驗(yàn)中使用亞德諾半導(dǎo)體技術(shù)公司的ADIS16405傳感器提供磁強(qiáng)度信息，使用美 國(guó)意法半導(dǎo)體公司的STM32F103芯片構(gòu)建磁傳感器數(shù)據(jù)采集系統(tǒng)及控制系統(tǒng)，負(fù)責(zé)采集 ADIS16405傳感器的數(shù)據(jù)、控制轉(zhuǎn)動(dòng)機(jī)構(gòu)轉(zhuǎn)動(dòng)并記錄轉(zhuǎn)動(dòng)機(jī)構(gòu)位置信息。所用轉(zhuǎn)動(dòng)機(jī)構(gòu)使用 天瑞中海公司的DDM150SP轉(zhuǎn)動(dòng)機(jī)構(gòu)。整個(gè)MEMS旋轉(zhuǎn)慣導(dǎo)系統(tǒng)包括以上各個(gè)部分。
[0015] 實(shí)驗(yàn)中使用的運(yùn)動(dòng)捕捉系統(tǒng)是北京歐雷新宇動(dòng)畫(huà)科技有限公司的EasyTrack運(yùn) 動(dòng)捕捉系統(tǒng)。
[0016] 步驟 1: 使用橢球標(biāo)定法對(duì)磁傳感器進(jìn)行標(biāo)定，得到誤差參數(shù)￡、，其中f為誤差系數(shù)矩 陣，為三軸磁傳感器的零偏。
[0017]
，其中為三軸磁傳感器的實(shí)際輸出，為校正后的 三軸磁傳感器輸出值。下文所提到的磁傳感器輸出均為校正后的磁傳感器輸出。
[0018] 步驟 2: 本文中使用的坐標(biāo)系定義如下： ?系：導(dǎo)航坐標(biāo)系，軸分別指向東向、北向、天向。
[0019] &系：磁傳感器數(shù)據(jù)采集系統(tǒng)的機(jī)體坐標(biāo)系，Z、y、Z軸分別指向磁傳感器器件 敏感軸方向，呈右手笛卡爾坐標(biāo)系。
[0020] ?系：運(yùn)動(dòng)捕捉系統(tǒng)的參考坐標(biāo)系，ZZ軸平行于安裝場(chǎng)地的平面，7軸垂直于 安裝場(chǎng)地的平面向上，且與if軸和Z軸滿(mǎn)足右手準(zhǔn)則。
[0021] 在磁傳感器數(shù)據(jù)采集系統(tǒng)上安裝3個(gè)標(biāo)記點(diǎn)，3個(gè)標(biāo)記點(diǎn)的兩兩連線(xiàn)構(gòu)成了一個(gè) 非等腰直角三角形。
[0022]記錄&系下ADIS16405的磁傳感器輸出值，根據(jù)該磁傳感器輸出值與地磁矢量 在《系下各軸的分莖，建立I?系相對(duì)于》系的轉(zhuǎn)移矩陣。
[0023] 其中，沒(méi)K、盡r、分別為A在&系下I、r、Z三軸上的分量，、 好分別為丑―在》系下JT、7、Z三軸上的分量。
[0024] 運(yùn)動(dòng)捕捉系統(tǒng)獲取標(biāo)記點(diǎn)在《系下的位置坐標(biāo)信息，從而解算磁傳感器數(shù)據(jù)采集 系統(tǒng)的姿態(tài)信息，使用歐拉角法得到6系相對(duì)于《系的轉(zhuǎn)移矩陣。
[0025] 通過(guò)G和G獲得"系相對(duì)于樹(shù)系的轉(zhuǎn)移矩陣C7=:
步驟3: 將磁傳感器數(shù)據(jù)采集系統(tǒng)安裝在旋轉(zhuǎn)慣導(dǎo)系統(tǒng)的轉(zhuǎn)動(dòng)機(jī)構(gòu)上，轉(zhuǎn)動(dòng)機(jī)構(gòu)在0°到 360°范圍每10°停留一次，一共停留36個(gè)位置，每個(gè)位置上停留3分鐘。
[