三維固態(tài)電子羅盤及其傳感器的零點(diǎn)和比例系數(shù)核正方法
【專利摘要】本發(fā)明公開了一種三維固態(tài)電子羅盤及其傳感器的零點(diǎn)和比例系數(shù)校正方法，電子羅盤包括三軸地磁傳感器電路、三軸加速度傳感器電路、通訊接口電路、電源轉(zhuǎn)換電路、MCU處理器電路以及三軸地磁傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊和三軸加速度傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，根據(jù)三軸地磁傳感器電路、三軸加速度傳感器電路的檢測(cè)結(jié)果分別估計(jì)得到三軸地磁傳感器和零點(diǎn)和比例系數(shù)比值以及三軸加速度傳感器和零點(diǎn)和比例系數(shù)比值。本發(fā)明，集成了不依賴于校正設(shè)備的加速度傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊以及地磁傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，方便多旋翼飛行器在野外進(jìn)行加速度傳感器與地磁傳感器的校正，提高了電子羅盤俯仰角、橫滾角以及航向角的測(cè)量精度。
【專利說(shuō)明】三維固態(tài)電子羅盤及其傳感器的零點(diǎn)和比例系數(shù)核正方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及多旋翼無(wú)人飛行器，具體涉及用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤及其傳感器的零點(diǎn)和比例系數(shù)校正方法。

【背景技術(shù)】
[0002]近年來(lái)，具備可垂直起降、懸停以及低空低速飛行能力的多旋翼無(wú)人飛行器已經(jīng)成為應(yīng)用與研究的一種主流無(wú)人飛行器，此類無(wú)人飛行器在偵察、搜索救援、交通監(jiān)控、災(zāi)情監(jiān)測(cè)、航拍、地理測(cè)繪、遙測(cè)遙感等領(lǐng)域有著非常廣泛的應(yīng)用前景。多旋翼無(wú)人飛行器的橫、縱向速度控制依賴于航向角測(cè)量信息對(duì)GPS速度的分解。目前，絕大部分無(wú)人飛行器都是通過(guò)三維電子羅盤對(duì)地磁的測(cè)量獲得航向角信息的。因此，三維電子羅盤在多旋翼無(wú)人飛行器控制系統(tǒng)中占有重要的地位。
[0003]由于地磁場(chǎng)是微弱磁場(chǎng)，傳感器制造工藝、硬磁干擾、信號(hào)調(diào)理電路、模數(shù)轉(zhuǎn)換器所引入的零點(diǎn)偏差和比例系數(shù)誤差都會(huì)使磁傳感器測(cè)量值偏離地磁真值，影響航向角測(cè)量精度，因此需要對(duì)每個(gè)地磁傳感器的零點(diǎn)和比例系數(shù)進(jìn)行校正?，F(xiàn)有技術(shù)對(duì)于三維電子羅盤的校正主要有幾下幾種:
[0004](I)中國(guó)發(fā)明專利CN102197277A(專利申請(qǐng)?zhí)?00980141990.8)提出了一種用于確定電子羅盤零點(diǎn)偏移的方法:首先借助于三軸磁傳感器求得所述電子羅盤的第一坐標(biāo)系中的第一磁場(chǎng)強(qiáng)度，然后由所述第一磁場(chǎng)強(qiáng)度計(jì)算平行于地球表面的第二坐標(biāo)系中的經(jīng)傾斜補(bǔ)償?shù)牡诙艌?chǎng)強(qiáng)度，最后使試探函數(shù)匹配于經(jīng)補(bǔ)償?shù)牡诙艌?chǎng)強(qiáng)度進(jìn)而確定出零點(diǎn)偏移；中國(guó)發(fā)明專利CN102288170B (專利申請(qǐng)?zhí)?01110198784.X)提出了一種水下航行器內(nèi)電子羅盤的校正方法，通過(guò)搜索的方法確定使校準(zhǔn)軌跡與參考軌跡之間誤差最小的硬磁干擾參數(shù)。這兩種方法對(duì)水平面上兩個(gè)地磁分量的零點(diǎn)進(jìn)行了校正，無(wú)法對(duì)3個(gè)地磁傳感器各自的零點(diǎn)偏差和比例系數(shù)誤差進(jìn)行完全校正。
[0005](2)中國(guó)發(fā)明專利CN101438131B(專利申請(qǐng)?zhí)?00780016572.7)提出了一種校準(zhǔn)方法及電子羅盤，在三維空間中利用地磁傳感器的至少4個(gè)輸出作出至少兩個(gè)三角形，求出上述三角形的各自外接圓，再求出通過(guò)各個(gè)外接圓中線法線矢量的交點(diǎn)作為基準(zhǔn)點(diǎn)(磁傳感器零點(diǎn))；中國(guó)發(fā)明專利CN102460069A(專利申請(qǐng)?zhí)?01080030411.5)提出了一種用于便攜式設(shè)備中的電子羅盤的校準(zhǔn)技術(shù)，用于計(jì)算和去除來(lái)自3軸磁傳感器輸出的干擾局部偏移磁場(chǎng)的影響以獲得地磁場(chǎng)精確讀數(shù)的技術(shù)，所述三維校準(zhǔn)器基于由傳感器獲得的限定大致球面的一組磁場(chǎng)測(cè)量計(jì)算器偏移場(chǎng)(零點(diǎn)偏差矢量)；中國(guó)發(fā)明專利CN102364354A(申請(qǐng)?zhí)?01110166034.4)提出了一種磁性傳感器裝置和電子羅盤設(shè)備，將數(shù)字磁數(shù)據(jù)輸出到數(shù)據(jù)處理裝置，該數(shù)據(jù)處理裝置根據(jù)由多個(gè)數(shù)字磁數(shù)據(jù)形成的統(tǒng)計(jì)學(xué)群體來(lái)導(dǎo)出數(shù)字磁數(shù)據(jù)的偏移量(零點(diǎn)偏差)。上述三種方法都只對(duì)三個(gè)磁傳感器的零點(diǎn)偏差進(jìn)行了校正，無(wú)法校正磁傳感器的比例系數(shù)誤差。
[0006](3)中國(guó)發(fā)明專利CN101241009B(申請(qǐng)?zhí)?00710304535.8)提出了一種磁電子羅盤誤差補(bǔ)償方法，以精確知道自身方位角的無(wú)磁轉(zhuǎn)臺(tái)為校正工具，基于變分圓分布和周期性假設(shè)的誤差補(bǔ)償算法，建立測(cè)量角誤差的周期函數(shù)，以補(bǔ)償周圍鐵磁物質(zhì)等因素所引起的測(cè)量誤差；中國(guó)發(fā)明專利CN102297687B(申請(qǐng)?zhí)?01110124791.5)提出了一種電子羅盤的標(biāo)定方法，以電子羅盤測(cè)量值與轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角度為訓(xùn)練樣本，采用改進(jìn)的自適應(yīng)差分進(jìn)化算法對(duì)神經(jīng)網(wǎng)絡(luò)的權(quán)值進(jìn)行優(yōu)化，從而得到較為精確的誤差模型來(lái)補(bǔ)償電子羅盤測(cè)量值；中國(guó)發(fā)明專利CN102818564A(申請(qǐng)?zhí)?01210273633.0)提出了一種三維電子羅盤的標(biāo)定方法，使用一個(gè)三維無(wú)磁旋轉(zhuǎn)平臺(tái)，利用三維電子羅盤內(nèi)置的磁傳感器和傾角傳感器在不同空間方向的輸出值變化對(duì)三維電子羅盤進(jìn)行標(biāo)定；中國(guó)發(fā)明專利CN103175520A(申請(qǐng)?zhí)?01310090322.5)提出了一種電子羅盤校正方法、裝置和設(shè)備，將多個(gè)傳感器安裝于幾何體(正多面體或球體)框架的面上并進(jìn)行三維球面旋轉(zhuǎn)，對(duì)采集得到的磁力數(shù)據(jù)通過(guò)橢球處理得到校正參數(shù)的方法，以校正三維磁傳感器的零點(diǎn)與比例系數(shù)。上述四種方法相比前述方法采用了更準(zhǔn)確的傳感器誤差模型，可以取得更好的校正效果，但需要額外的校正設(shè)備，如轉(zhuǎn)臺(tái)或幾何體框架。由于地磁傳感器的誤差參數(shù)易受溫度、外圍干擾磁場(chǎng)變化的影響而需要頻繁校正，這些依賴校正設(shè)備的方法顯然難以滿足實(shí)際應(yīng)用需要。
[0007](4)中國(guó)發(fā)明專利CN1624425B(申請(qǐng)?zhí)?00410001581.7)提出了一種用在電子羅盤內(nèi)的自動(dòng)校準(zhǔn)方法，使用接收到的地磁數(shù)據(jù)找到該地磁傳感器的單獨(dú)的軸的傳感器信號(hào)的最大和最小值以計(jì)算偏移和標(biāo)度值；中國(guó)發(fā)明專利CN102589537A(申請(qǐng)?zhí)?01210055586.2)提出了一種有磁環(huán)境下無(wú)人機(jī)的電子羅盤校正方法，磁力計(jì)獲得電子羅盤X、y、z三軸的相對(duì)磁通量，濾波處理后送入極值獲取單元獲得三軸磁通量的極大值和極小值進(jìn)行橢球擬合校正。這兩種方法在理論上都能較好地校正三軸磁傳感器的零點(diǎn)偏差和比例系數(shù)誤差，而在實(shí)際應(yīng)用中所述磁通量極大值與極小值難以精確得到，因此校正效果受限。
[0008](5)中國(guó)發(fā)明專利CN102252689A(申請(qǐng)?zhí)?01010176346.9)提出了一種電子羅盤全方位校準(zhǔn)方法，利用IGRF模型計(jì)算地磁場(chǎng)強(qiáng)度，結(jié)合軟硬磁修正的遞歸最小二乘估計(jì)器，計(jì)算軟磁干擾的上三角矩陣與硬磁干擾向量，這種方法需要IGRF模型的輔助估計(jì)9個(gè)參數(shù)，計(jì)算量和存儲(chǔ)量都較大，不適合單片機(jī)應(yīng)用。
[0009]此外，除了地磁傳感器，電子羅盤用于測(cè)量?jī)A角的加速度計(jì)的零點(diǎn)和比例系數(shù)也易受溫度變化影響而引入誤差，需要經(jīng)常校正，而上述各電子羅盤相關(guān)的專利都沒(méi)有涉及到。對(duì)于加速度計(jì)的校正，中國(guó)發(fā)明專利CN101629970B(申請(qǐng)?zhí)?00910160117.5)提出了一種用于加速度傳感器的誤差校正方法和誤差校正裝置，通過(guò)向振動(dòng)質(zhì)量塊施壓，測(cè)量振動(dòng)質(zhì)量塊偏移引起的第一電流和第二電流，求得校正值；中國(guó)發(fā)明專利CN101852817B(申請(qǐng)?zhí)?01010164389.5)提出了一種正交雙高精度加速度計(jì)的標(biāo)定方法，利用多面體棱鏡、光柵分度頭、光電自準(zhǔn)值儀獲得兩個(gè)加速度計(jì)在0、90、180、120度4個(gè)角度位置上的采樣輸出，并采用正交雙表法獲得誤差模型的各個(gè)參數(shù)，完成標(biāo)定；中國(guó)發(fā)明專利CN101968496A(申請(qǐng)?zhí)?01010221061.2)提出了一種加速度傳感器的全自動(dòng)校正系統(tǒng)，基于該系統(tǒng)測(cè)量并計(jì)算得到各標(biāo)準(zhǔn)g值下不同溫度時(shí)加速度傳感器對(duì)應(yīng)的待校正g值與標(biāo)準(zhǔn)g值的偏差值，求出偏差值與溫度的對(duì)應(yīng)曲線，對(duì)曲線進(jìn)行多項(xiàng)式擬合求出校正系數(shù)；中國(guó)發(fā)明專利CN103090882A(申請(qǐng)?zhí)?01310006526.6)提出了一種加速度計(jì)實(shí)現(xiàn)高精度傾角測(cè)量中的靈敏軸非正交補(bǔ)償校正方法，首先在垂直平面內(nèi)設(shè)置四個(gè)兩兩垂直的校正位置點(diǎn)，通過(guò)夾角函數(shù)，在四個(gè)象限內(nèi)，分別前后計(jì)算兩靈敏軸對(duì)應(yīng)于校正點(diǎn)的夾角作為非正交值；中國(guó)發(fā)明專利CN103399177A(申請(qǐng)?zhí)?01310356585.6)提出了一種加速度傳感器實(shí)現(xiàn)傾角測(cè)量應(yīng)用中的靈敏軸參數(shù)多點(diǎn)方程式校正標(biāo)定方法，采用多點(diǎn)標(biāo)定和方程組計(jì)算方法來(lái)精密校正標(biāo)定靈敏軸參數(shù)，從而得到精確的零點(diǎn)和靈敏度值。上述五種方法都依賴專用的校正設(shè)備，難以滿足野外應(yīng)用的需要。
[0010]由此可見(jiàn)，現(xiàn)有的三維電子羅盤及其校正方法存在依賴專用的校正設(shè)備，校正效果差，難以滿足野外應(yīng)用的需要。


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

[0011]本發(fā)明所要解決的問(wèn)題是三維電子羅盤的校正依賴專用的校正設(shè)備，校正效果差，難以滿足野外應(yīng)用的需要的問(wèn)題。
[0012]為了解決上述技術(shù)問(wèn)題，本發(fā)明所采用的技術(shù)方案是提供一種用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，包括三軸地磁傳感器電路、三軸加速度傳感器電路、通訊接口電路、電源轉(zhuǎn)換電路和MCU處理器電路，三軸地磁傳感器電路與MCU處理器電路通過(guò)I2C總線連接，用于檢測(cè)地磁場(chǎng)分量；三軸加速度傳感器電路與MCU處理器電路的AD采樣輸入管腳連接，用于檢測(cè)重力分量；通訊接口電路與MCU處理器電路通過(guò)第一、第二 UART通訊接口連接，實(shí)現(xiàn)TTL-RS232電平轉(zhuǎn)換與USB-UART協(xié)議轉(zhuǎn)換，為上位機(jī)提供RS232和USB通訊接口 ；電源轉(zhuǎn)換電路將3.5V-5.5V的輸入電壓轉(zhuǎn)換成+3.3V的穩(wěn)定電壓,用于為三軸地磁傳感器電路、三軸加速度傳感器電路、通訊接口電路和MCU處理器電路供電，其特征在于，還包括:
[0013]三軸地磁傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，根據(jù)三軸地磁傳感器電路的檢測(cè)結(jié)果估計(jì)三軸地磁傳感器的零點(diǎn)bx，h、by，h、bz；h和比例系數(shù)比值rx，h/ry，h、rxh/rzh ；
[0014]三軸加速度傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，根據(jù)三軸加速度傳感器電路的檢測(cè)結(jié)果估計(jì)三軸加速度傳感器的零點(diǎn)bx，g、by，g、bz，g和比例系數(shù)比值rx’ Jry，g、rx； Jrz g0
[0015]在上述三維固態(tài)電子羅盤中，三軸地磁傳感器電路包括三軸地磁傳感器芯片Ul，第一電容Cl和第二電容器C2并聯(lián)后，一端接三軸地磁傳感器芯片Ul的SI引腳和+3.3V電壓，另一端接地；第三電容C3 —端接三軸地磁傳感器芯片Ul的Cl引腳，另一端接地；第四電容C4的兩端分別連接三軸地磁傳感器芯片Ul的SETC引腳和SETP引腳。
[0016]在上述三維固態(tài)電子羅盤中，三軸地磁傳感器芯片Ul型號(hào)為HMC5883L。
[0017]在上述三維固態(tài)電子羅盤中，三軸加速度傳感器電路包括三軸加速度傳感器芯片U2，三軸加速度傳感器芯片U2采用意法半導(dǎo)體公司生產(chǎn)的帶模擬電壓信號(hào)輸出的MEMS加速度傳感芯片LIS3L06AL，三軸加速度傳感器芯片U2的VDD引腳接+3.3V電壓和第五電容C5和第六電容C6，第五電容C5和第六電容C6的另一端分別接地；三軸加速度傳感器芯片U2的GND引腳接地，并通過(guò)第七電容C7連接到三軸加速度傳感器芯片U2的AZ引腳；三軸加速度傳感器芯片U2的AY和AX引腳分別通過(guò)第八、第九電容接地，第七、第八和第九電容將三軸加速度計(jì)的輸出電壓低通濾波到1Hz ;三軸加速度傳感器芯片U2的FS引腳通過(guò)第一電阻Rl連接到+3.3V電壓，將三軸加速度傳感器芯片U2的量程選擇管腳FS上拉到+3.3V，使得三軸加速度傳感器芯片U2工作在±6g量程。
[0018]在上述三維固態(tài)電子羅盤中，通訊接口電路包括RS2329針COM接口 J1、RS_232收發(fā)器芯片U3、A型USB接口 J2和USB-UART橋接芯片U4 ；
[0019]RS-232收發(fā)器芯片U3的Cl+和Cl-引腳之間設(shè)有第十電容，C2+和C2-引腳之間設(shè)有第十二電容，VCC和GND引腳之間設(shè)有第十四電容，V+引腳通過(guò)第十一電容接地，V-引腳通過(guò)第十三電容接地，GND引腳接地，VCC引腳接+3.3V電壓，T20UT引腳和R2IN引腳分別連接RS2329針COM接口 Jl的第2和第3端子；
[0020]USB-UART橋接芯片U4通過(guò)USB-UART協(xié)議轉(zhuǎn)換電路與A型USB接口 J2連接。
[0021]本發(fā)明還提供了一種三維固態(tài)電子羅盤的傳感器零點(diǎn)和比例系數(shù)核正方法，包括以下步驟:
[0022]步驟10:初始化三軸地磁傳感器和三軸加速度傳感器的零點(diǎn)與比例系數(shù)，其中:
[0023]三軸地磁傳感器的初始化零點(diǎn)與比例系數(shù)最小二乘估計(jì)器初始化為Iitl = [hx0 hyQhzQ]T、Ph= 1000父15和4= [0101 0]T，hxQ、hy(l、hzQ為進(jìn)入校正狀態(tài)時(shí)三軸地磁傳感器的輸出；Ph為三軸地磁傳感器的估計(jì)誤差協(xié)方差陣；15為三軸地磁傳感器的5維單位陣；ch為要估計(jì)的三軸地磁傳感器的參數(shù)向量；
[0024]三軸加速度傳感器的零點(diǎn)與比例系數(shù)最小二乘估計(jì)器初始化為gQ = [gxQgyQgzQ]T，Pg = 100Xi5, Cg = [O I O I 0]T, gxQ、gyQ、gzQ為進(jìn)入校正狀態(tài)時(shí)三軸加速度傳感器的輸出；Pg為三軸加速度傳感器的估計(jì)誤差協(xié)方差陣；15為三軸加速度傳感器的5維單位陣；Cg為要估計(jì)的三軸加速度傳感器的參數(shù)向量；
[0025]步驟20:分別將三軸地磁傳感器和三軸加速度傳感器繞三軸旋轉(zhuǎn)，并采集三軸地磁傳感器的當(dāng)前數(shù)據(jù)h = [hx hy hz]T和三軸加速度傳感器當(dāng)前數(shù)據(jù)g = [gx gy gJT ；
[0026]步驟30:分別構(gòu)建yh、ah和yg、ag,其中:
[0027]yh = h1-% = 2OlX-hXO^ hy0 - hy 2(hy ~ hy^ hL.- 2(h: - hzQ).'，9
[0028]yg = -g^Q ag =~^zO ~~Szq).*? i
[0029]步驟40:計(jì)算三軸地磁傳感器的增益Kh = Pjafi I (I + )，估計(jì)參數(shù)^ =
ch+Kh (yh-ahch)，并更新其誤差協(xié)方差陣Ph = (I5_Khah) Ph ;計(jì)算二軸加速度傳感器的增益~ ^pgag /(l + agpgag'>，估計(jì)參數(shù)Cg = cg+Kg(yg-agcg),并更新其誤差協(xié)方差陣Pg =(I5-Kgag) Pg ；
[0030]步驟50:向上位機(jī)發(fā)送三軸地磁傳感器的估計(jì)參數(shù)Ch和三軸加速度傳感器的估計(jì)參數(shù)Cg，并通過(guò)上位機(jī)觀察Ch和Cg是否收斂；
[0031]如果ch和Cg未收斂,則返回步驟20繼續(xù)估計(jì)Ch和Cg ；
[0032]如果Ch收斂，則結(jié)束地磁傳感器校正，保存估計(jì)的Ch和三軸地磁傳感器的零點(diǎn)bx，h、by，h、bz，h 及比例系數(shù)比值 rx，h/ry，h、rx；h/rz；h, bx，h = ch(l), by，h = ch(3)/ch(2), bz，h =
ch (5) /ch ⑷，rxjt 丨 ry，h = Vc/?⑵，rx:h 7 rz.h = ⑷，ch ⑴、ch ⑵、ch (3)、ch ⑷、ch (5)為 Ch 的
5個(gè)分量；
[0033]如果Cg收斂，則結(jié)束加速度計(jì)校正，保存估計(jì)的Cg和三軸加速度傳感器的零點(diǎn)bx，g、by，g、bz，g 及比例系數(shù)比值 rx，g/ry，g、rx；g/rz；g, bx，g = cg(l), by，g = cg(3)/cg(2), bz，g =
Cg (5)/Cg (4)，rx,g I ry,g = ^Cg(2)，rx^g I r.z'g =」Cg (4)，cg(l)、cg(2)、cg(3)、cg(4)、cg(5)為 Cg 的5個(gè)分量。
[0034]在上述方法中，上位機(jī)通過(guò)RS232串口或USB串口發(fā)送三軸地磁傳感器和三軸加速度傳感器的校正開始及結(jié)束命令。
[0035]在上述方法中，利用重力場(chǎng)、地磁場(chǎng)與姿態(tài)角的關(guān)系計(jì)算三個(gè)姿態(tài)角，具體計(jì)算公式為:

【權(quán)利要求】
1.用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，包括三軸地磁傳感器電路、三軸加速度傳感器電路、通訊接口電路、電源轉(zhuǎn)換電路和MCU處理器電路，三軸地磁傳感器電路與MCU處理器電路通過(guò)I2C總線連接，用于檢測(cè)地磁場(chǎng)分量；三軸加速度傳感器電路與MCU處理器電路的AD采樣輸入管腳連接，用于檢測(cè)重力分量；通訊接口電路與MCU處理器電路通過(guò)第一、第二 UART通訊接口連接，實(shí)現(xiàn)TTL-RS232電平轉(zhuǎn)換與USB-UART協(xié)議轉(zhuǎn)換，為上位機(jī)提供RS232和USB通訊接口 ；電源轉(zhuǎn)換電路將3.5V-5.5V的輸入電壓轉(zhuǎn)換成+3.3V的穩(wěn)定電壓，用于為三軸地磁傳感器電路、三軸加速度傳感器電路、通訊接口電路和MCU處理器電路供電，其特征在于，還包括: 三軸地磁傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，根據(jù)三軸地磁傳感器電路的檢測(cè)結(jié)果估計(jì)三軸地磁傳感器的零點(diǎn)bx，h、by，h、bz；h和比例系數(shù)比值rx，h/ry，h、rxh/rzh ； 三軸加速度傳感器零點(diǎn)與比例系數(shù)估計(jì)模塊，根據(jù)三軸加速度傳感器電路的檢測(cè)結(jié)果估計(jì)三軸加速度傳感器的零點(diǎn)bx，g、by，g、bz，g和比例系數(shù)比值rx’ Jry，g、rx； Jrz g0
2.如權(quán)利要求書I所述的用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，其特征在于，三軸地磁傳感器電路包括三軸地磁傳感器芯片Ul,第一電容Cl和第二電容器C2并聯(lián)后,一端接三軸地磁傳感器芯片Ul的SI引腳和+3.3V電壓，另一端接地；第三電容C3 —端接三軸地磁傳感器芯片Ul的Cl引腳，另一端接地；第四電容C4的兩端分別連接三軸地磁傳感器芯片Ul的SETC引腳和SETP引腳。
3.如權(quán)利要求書2所述的用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，其特征在于，三軸地磁傳感器芯片Ul型號(hào)為HMC5883L。
4.如權(quán)利要求 書I所述的用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，其特征在于，三軸加速度傳感器電路包括三軸加速度傳感器芯片U2，三軸加速度傳感器芯片U2采用意法半導(dǎo)體公司生產(chǎn)的帶模擬電壓信號(hào)輸出的MEMS加速度傳感芯片LIS3L06AL，三軸加速度傳感器芯片U2的VDD引腳接+3.3V電壓和第五電容C5和第六電容C6，第五電容C5和第六電容C6的另一端分別接地；三軸加速度傳感器芯片U2的GND引腳接地，并通過(guò)第七電容C7連接到三軸加速度傳感器芯片U2的AZ引腳；三軸加速度傳感器芯片U2的AY和AX引腳分別通過(guò)第八、第九電容接地，第七、第八和第九電容將三軸加速度計(jì)的輸出電壓低通濾波到1Hz ;三軸加速度傳感器芯片U2的FS引腳通過(guò)第一電阻Rl連接到+3.3V電壓，將三軸加速度傳感器芯片U2的量程選擇管腳FS上拉到+3.3V，使得三軸加速度傳感器芯片U2工作在±6g量程。
5.如權(quán)利要求書I所述的用于多旋翼無(wú)人飛行器的三維固態(tài)電子羅盤，其特征在于，通訊接口電路包括RS2329針COM接口 Jl、RS-232收發(fā)器芯片U3、A型USB接口 J2和USB-UART橋接芯片U4 ； RS-232收發(fā)器芯片U3的Cl+和Cl-引腳之間設(shè)有第十電容，C2+和C2-引腳之間設(shè)有第十二電容，VCC和GND引腳之間設(shè)有第十四電容，V+引腳通過(guò)第十一電容接地，V-引腳通過(guò)第十三電容接地，GND引腳接地，VCC引腳接+3.3V電壓，T20UT引腳和R2IN引腳分別連接RS2329針COM接口 Jl的第2和第3端子； USB-UART橋接芯片U4通過(guò)USB-UART協(xié)議轉(zhuǎn)換電路與A型USB接口 J2連接。
6.三維固態(tài)電子羅盤的傳感器零點(diǎn)和比例系數(shù)核正方法，其特征在于，包括以下步驟:步驟10:初始化三軸地磁傳感器和三軸加速度傳感器的零點(diǎn)與比例系數(shù)，其中: 三軸地磁傳感器的初始化零點(diǎn)與比例系數(shù)最小二乘估計(jì)器初始化為1? = [hx0 hy0 hz0]T、Ph= 100XI5和Ch= [0101 0]T，hxQ、hy(l、hzQ為進(jìn)入校正狀態(tài)時(shí)三軸地磁傳感器的輸出；Ph為三軸地磁傳感器的估計(jì)誤差協(xié)方差陣；15為三軸地磁傳感器的5維單位陣；ch為要估計(jì)的三軸地磁傳感器的參數(shù)向量； 三軸加速度傳感器的零點(diǎn)與比例系數(shù)最小二乘估計(jì)器初始化為go = [gx0 gyO gZJT，Pg=1000XI5, Cg = [0101 0]T，gxQ、gy(l、gzQ為進(jìn)入校正狀態(tài)時(shí)三軸加速度傳感器的輸出；Pg為三軸加速度傳感器的估計(jì)誤差協(xié)方差陣；I5為三軸加速度傳感器的5維單位陣；eg為要估計(jì)的三軸加速度傳感器的參數(shù)向量； 步驟20:分別將三軸地磁傳感器和三軸加速度傳感器繞三軸旋轉(zhuǎn)，并采集三軸地磁傳感器的當(dāng)前數(shù)據(jù)h = [hx hy hz]T和三軸加速度傳感器當(dāng)前數(shù)據(jù)g = [gx gy gJT ； 步驟30:分別構(gòu)建yh、ab和yg、ag,其中:
=hX-^2-Q ah= 2(A「/7xO) hy0 ~ hy 2^hy ~hy0^ hzQ~ ^ 2^z ~hz0^.79
~ 2 2 2 2 ~~ ^=Sx-S1xQ ? ag =[2fe-gxo)2(gj -gvo) ~Sz 2(gz Q)J ^ 步驟40:計(jì)算三軸地磁傳感器的增益Kh=PhaJ1 /(i + ahPhal)f估計(jì)參數(shù)Ch =ch+Kh (yh-ahch)，并更新其誤差協(xié)方差陣Ph = (I5_Khah) Ph ;計(jì)算二軸加速度傳感器的增益~ = / (I + agPgJg )，估計(jì)參數(shù)Cg = Cg+Kg (yg-agCg)，并更新其誤差協(xié)方差陣Pg =(I5-Kgag) Pg ； 步驟50:向上位機(jī)發(fā)送三軸地磁傳感器的估計(jì)參數(shù)Ch和三軸加速度傳感器的估計(jì)參數(shù)cg，并通過(guò)上位機(jī)觀察Ch和Cg是否收斂； 如果Ch和Cg未收斂,則返回步驟20繼續(xù)估計(jì)Ch和Cg ； 如果Ch收斂，則地磁傳感器結(jié)束校正，保存估計(jì)的Ch和三軸地磁傳感器的零點(diǎn)bx，h、by，h、匕^及比例系數(shù)比值!^/!^、!^/!^，bx，h = Ch(I), by；h = ch(3)/ch(2)，bz，h = ch(5)/ch(4)，rx,h 1 r；,A =VcA(2)，rx,h ,rz,h =」ch(4)，ch(l)、ch(2)、ch(3)、ch(4)、ch(5)為 ch 的 5 個(gè)分量; 如果cg收斂，則結(jié)束加速度計(jì)校正，保存估計(jì)的Cg和三軸加速度傳感器的零點(diǎn)bx，g、\，8、\8及比例系數(shù)比值1^/1^、1^/1^，bx，g = cg(l)，by，g = cg(3)/cg(2)，bz，g = cg(5)/Cg(4)，rx,g Iry,g =」cg(2)，tXg 丨rz，g =」cg(4)，cg(l)、cg (2)、cg(3)、cg(4)、cg(5)為 cg 的 5 個(gè)分量。
7.如權(quán)利要求書6所述的方法，其特征在于，上位機(jī)通過(guò)RS232串口或USB串口發(fā)送三軸地磁傳感器和三軸加速度傳感器的校正開始及結(jié)束命令。
8.如權(quán)利要求書6所述的方法，其特征在于，用重力場(chǎng)、地磁場(chǎng)與姿態(tài)角的關(guān)系計(jì)算三個(gè)姿態(tài)角，具體計(jì)算公式為:
其中:θ、φ、Ψ分別為俯仰角、橫滾角、航向角。
【文檔編號(hào)】G01C17/38GK104075699SQ201410321025
【公開日】2014年10月1日 申請(qǐng)日期:2014年7月7日 優(yōu)先權(quán)日:2014年7月7日
【發(fā)明者】徐玉, 童長(zhǎng)飛, 劉軍 申請(qǐng)人:溫州大學(xué)