• 
    

    
    

      99热精品在线国产_美女午夜性视频免费_国产精品国产高清国产av_av欧美777_自拍偷自拍亚洲精品老妇_亚洲熟女精品中文字幕_www日本黄色视频网_国产精品野战在线观看 ?

      基于MSP430單片機(jī)的姿態(tài)測量系統(tǒng)設(shè)計

      2018-03-01 02:46:51,,
      船海工程 2018年1期
      關(guān)鍵詞:陀螺儀加速度計航向

      ,,

      (1.海軍工程大學(xué) a.兵器工程系;b.導(dǎo)航工程系,武漢 430033;2.海軍海洋測繪研究所,天津 300061)

      隨著MEMS技術(shù)的成熟,以及對姿態(tài)測量中低功耗、低成本的需求,微機(jī)械陀螺、加速度計和磁傳感器在無人機(jī)、智能機(jī)器人等領(lǐng)域得到廣泛的應(yīng)用[1]。針對單一傳感器測量姿態(tài)存在零漂,受環(huán)境因素影響大等問題,多傳感器融合測姿已成為一種發(fā)展趨勢,并且隨著MEMS傳感器的迅速發(fā)展以及向各個學(xué)科領(lǐng)域的滲透,其各個方面性能如精度、魯棒性、動態(tài)響應(yīng)等都得到了很大的提高,因此,如何在多MEMS傳感器系統(tǒng)中實(shí)現(xiàn)數(shù)據(jù)融合進(jìn)而進(jìn)行姿態(tài)測量成為主要研究方向。Marins基于四元數(shù)擴(kuò)展的卡爾曼濾波,研究使用MARG傳感器進(jìn)行姿態(tài)檢測[2],然而該算法計算量較大,難以在硬件上實(shí)現(xiàn)。宋東等人[3]提出了一種基于MEMS加速度計、磁阻傳感器的方法,該方法簡化了計算量,但無法抑制動態(tài)環(huán)境下數(shù)據(jù)中的噪聲。Mahony等人[4]利用MEMS器件及互補(bǔ)濾波器建立姿態(tài)估計方法,得出互補(bǔ)濾波器截止頻率設(shè)計方法,但該算法PI參數(shù)只能通過試湊,無法實(shí)時調(diào)整PI參數(shù)。針對以上姿態(tài)測量方法的缺陷,基于GY-85九軸傳感器模塊,設(shè)計一種以單片機(jī)MSP430F5438為核心的姿態(tài)檢測系統(tǒng),利用梯度下降法對傳感器進(jìn)行數(shù)據(jù)融合,彌補(bǔ)單一傳感器測姿的不足。

      1 硬件組成

      設(shè)計的系統(tǒng)以MSP430F5438為核心,通過與集成了三軸加速度計ADXL345、三軸地磁傳感器HMC5883L以及陀螺儀ITG3205的九軸傳感器GY-85進(jìn)行I2C通信以采集各個傳感器的輸出數(shù)據(jù),并將解算出的各個姿態(tài)角通過無線串口發(fā)送給上位機(jī)。系統(tǒng)設(shè)計的原理框圖如圖1所示。

      圖1 系統(tǒng)設(shè)計原理框圖

      MSP430F5438是德州儀器近兩年推出的MSP43054XX系列單片機(jī)中的最高型號,其資源充足,功能強(qiáng)大,非常適用于多傳感器的姿態(tài)檢測。ADXL345是一款超低功耗的三軸加速度計,分辨率高(13位),測量范圍達(dá)±16 g,能測量到小于1°的傾斜角度變化[5]。HMC5883L磁場傳感器分辨率達(dá)0.002 G,最大輸出頻率達(dá)160 Hz。陀螺儀ITG3205內(nèi)部具有3個整合16位模數(shù)轉(zhuǎn)換器,提供陀螺儀同步取樣,并帶有可選擇的數(shù)字低通濾波器以及快速模式(400 kHz)的I2C接口。

      2 軟件流程

      系統(tǒng)的主流程圖如圖2所示。為了便于移植,對所設(shè)計程序進(jìn)行模塊化處理,通過調(diào)用各個子函數(shù)完成所需的功能。程序首先完成MSP430F5438初始化,并對各個傳感器的工作模式進(jìn)行初始化設(shè)定,接著對四元數(shù)初始化及各個傳感器的初始偏移進(jìn)行校正。隨后在中斷函數(shù)程序中完成數(shù)據(jù)采集與濾波,并解算出姿態(tài)角發(fā)送給上位機(jī)。

      圖2 系統(tǒng)流程

      3 系統(tǒng)校正

      加速度計校正的方法為將傳感器水平放置,使z軸在1 g的重力場,x、y軸在0 g的重力場,取一系列樣本的平均值,記為x0、y0、z1。這些值即為相對于各軸的偏移量,通過減去這些值以獲得實(shí)際加速度的值。

      (1)

      式中:Xacc、Yacc、Zacc為加速度計實(shí)際值,Xmea、Ymea、Zmea為加速度計測量值,SZ為加速度計z軸在1 g的重力場下的理想靈敏度。

      陀螺儀校正時需采集初始時的一組數(shù)據(jù)求平均值后作為零位偏移,此后將測量值減去偏移即可。

      ωacc=ωmea-ωbias

      (2)

      式中:ωacc為陀螺儀實(shí)際值;ωmea為陀螺儀測量值;ωbias為零位偏移值。

      在理想狀態(tài)下,地磁傳感器三軸的輸出磁場矢量應(yīng)構(gòu)成一個圓心在原點(diǎn)的正球體,而在實(shí)驗的過程中,地磁傳感器受到周圍硬磁干擾和軟磁干擾,導(dǎo)致該球體變成了一個偏離圓心的橢球體,所以需對地磁傳感器進(jìn)行校正。圖3和圖4為采用AntMag軟件對地磁傳感器校正前后的對比示意圖。校正后的地磁傳感器各軸的零位偏移和均方誤差有了明顯的改善,如表1所示。

      圖3 校正前地磁傳感器輸出

      圖4 校正后地磁傳感器輸出

      校正X零位偏移Y零位偏移Z零位偏移均方誤差校正前0.08660.09380.11930.059校正后0.00080.00180.00330.029

      4 姿態(tài)解算

      4.1 坐標(biāo)系建立、四元數(shù)與歐拉角之間的轉(zhuǎn)換

      分別建立導(dǎo)航坐標(biāo)系N和載體坐標(biāo)系B,N系采用東北天坐標(biāo)系,B系固連在載體上,傳感器的三軸分別安裝在B系的x、y、z軸上。設(shè)α、β、γ分別為俯仰角,滾轉(zhuǎn)角、航向角。則由N系到B系的旋轉(zhuǎn)矩陣可表示為

      (3)

      用四元數(shù)Q=q0+q1i+q2j+q3k(|Q|=1)表示該旋轉(zhuǎn)矩陣為

      (4)

      通過上述2種表示形式,將歐拉角用四元數(shù)表示為

      (5)

      4.2 四元數(shù)微分方程求解

      根據(jù)四元數(shù)微分方程定義,對應(yīng)的矩陣[6]為

      采用一階龍格庫塔求解四元數(shù)微分方程。

      Q(t+h)=Q(t)+hk1

      (7)

      得到四元數(shù)更新方程為

      式中:ωx、ωy、ωz為△軸輸出角速度;h為采樣周期。

      4.3 單傳感器解算姿態(tài)角

      ADXL345加速度計在靜態(tài)或者低動態(tài)的情況下利用重力在各軸上的投影分量求解姿態(tài)角[7]。

      (9)

      式中:α、β、γ分別為俯仰角、翻滾角、z軸與重力加速度的夾角。

      ITG3205陀螺儀測得繞各個軸的角速度,通過積分便可以得到繞各個軸旋轉(zhuǎn)的角度[8]:

      θi=θi-1+ωdt

      (10)

      式中:θi為i時刻的角度值;θi-1為i-1時刻的角度值;ω為修正后的角速度;dt為積分時間。

      HMC5883L地磁傳感器通過測得地磁場在其各軸上的投影分量解算出航向角。當(dāng)?shù)卮艂鞲衅魈幱谒綘顟B(tài)時,可通過式(11)對航向角進(jìn)行檢測,而當(dāng)其處于傾斜狀態(tài)時,則需通過加速度計所測俯仰角α和翻滾角β代入式(12),對其進(jìn)行傾斜補(bǔ)償后解算出航向角。

      γ=arctan(Hy/Hx)

      (11)

      (12)

      式中:Hx、Hy、Hz為地磁傳感器各軸所測數(shù)據(jù)。

      4.4 梯度下降法融合解算姿態(tài)

      通過陀螺儀積分解算姿態(tài),由于受到溫度和零漂的影響,長時間使用誤差較大[9]。而加速度計不僅對重力,同時對外力產(chǎn)生的加速度十分敏感,動態(tài)下噪聲明顯,且其不能對陀螺儀所測的航向角進(jìn)行修正,故無法使用單傳感器對姿態(tài)進(jìn)行測量。本文基于Madgwick所提出的梯度下降法,將九軸傳感器中的加速度計、陀螺儀、地磁傳感器數(shù)據(jù)進(jìn)行融合修正旋轉(zhuǎn)矩陣,從而得到姿態(tài)的近似最優(yōu)解。

      (13)

      構(gòu)造目標(biāo)函數(shù)為

      F(Q)=(egx)2+(egy)2+(egz)2+

      (emx)2+(emy)2+(emz)2

      將四元數(shù)旋轉(zhuǎn)矩陣的優(yōu)化轉(zhuǎn)化為求F(Q)的最小值。

      由梯度下降法得到其迭代公式為[10]

      (14)

      式中:m為迭代步長。

      將其與由四元數(shù)微分方程得到的四元數(shù)Qω(t)相結(jié)合,得到最終的梯度下降法的姿態(tài)融合公式:

      (15)

      式中:n為迭代步長;Δt為采樣時間;Q(t)為所求姿態(tài)四元數(shù)。

      通過加速度計對地磁傳感器進(jìn)行傾斜補(bǔ)償后求得的歐拉角,帶入式(16)轉(zhuǎn)化為初始四元數(shù),以解決初始對準(zhǔn)時梯度下降法收斂過慢的問題。當(dāng)四元數(shù)初始化后,便可利用上述方法解算姿態(tài)的近似值。

      (16)

      5 實(shí)驗與分析

      5.1 靜態(tài)實(shí)驗

      利用角度分度裝置將九軸傳感器靜止放置水平,同時采集通過加速度計、陀螺儀、梯度下降法解算出的俯仰角,實(shí)驗結(jié)果如圖5所示。通過對比可以看出:加速度計存在明顯的噪聲,陀螺儀存在較明顯的偏移,隨著時間的增加,累積誤差逐漸增大。梯度下降法能有效的抑制加速度計的噪聲,俯仰角測量誤差大約在±0.3°以內(nèi)。

      圖5 俯仰角靜止實(shí)驗

      5.2 動態(tài)實(shí)驗

      將傳感器初始水平,此后繞x軸旋轉(zhuǎn)30°得到的圖像如圖6所示。由圖像看出,梯度下降法能及時跟蹤姿態(tài)的變化,同時一定程度上抑制了加速度計的噪聲和陀螺儀的漂移,所測的俯仰角誤差在±1°以內(nèi)。

      圖6 俯仰角動態(tài)實(shí)驗

      5.3 四元數(shù)初始化對比試驗

      初始時將傳感器x軸指向東方,然后旋轉(zhuǎn)90°使其指向北方,分別將前文所提出的對四元數(shù)初始化的方法與一般采用的將初始四元數(shù)設(shè)為(1,0,0,0)所測的航向角進(jìn)行比較,實(shí)驗效果如圖7、8所示。由實(shí)驗結(jié)果看出,進(jìn)行初始對準(zhǔn)后,梯度下降法初始收斂時間由原來的5 s降到了1 s以內(nèi),改善了其姿態(tài)角初始時刻不穩(wěn)的問題,采用該系統(tǒng)測的航向角誤差在±2°以內(nèi)。

      圖7 未初始對準(zhǔn)航向角實(shí)驗結(jié)果

      圖8 初始對準(zhǔn)航向角實(shí)驗結(jié)果

      6 結(jié)論

      基于MSP430F5438與九軸傳感器GY- 85搭建姿態(tài)測量系統(tǒng),實(shí)現(xiàn)了姿態(tài)角的檢測。針對地磁傳感器的所測數(shù)據(jù)受周圍磁場干擾比較嚴(yán)重的問題,使用AntMag軟件對其進(jìn)行了校正,實(shí)驗結(jié)果表明該軟件簡單可行,正確有效。另外為解決加速度計測量噪聲、陀螺儀漂移誤差及加速度計無法對航向角進(jìn)行修正等問題,引入梯度下降法,在角度分度裝置上進(jìn)行了動靜態(tài)實(shí)驗。結(jié)果表明,該系統(tǒng)克服了航向角初始難以對準(zhǔn)的問題,具有響應(yīng)快、實(shí)時性好的特點(diǎn),并可應(yīng)用于平衡小車、無人機(jī)姿態(tài)檢測、慣性導(dǎo)航等領(lǐng)域。

      [1] 夏圣,許勇.基于MEMS組合模塊的姿態(tài)檢測系統(tǒng)設(shè)計[J].單片機(jī)與嵌入式系統(tǒng)應(yīng)用,2011(4):52- 55.

      [2] MARINS J L,YUN X,BACHAMARM E,et al. An extended Kalman filter for quaternion based orientation rst- imation using MARG sensors[C]∥Proceedings of the 2001 IEEE/RSJ I- nternational Conference on Intel- ligent Robots and systems.Maui,Hawaii:IEEE,2001(8):2003- 2011.

      [3] 宋東,章侃,王彥文,等.基于MEMS傳感器的飛行姿態(tài)指示系統(tǒng)[J].測控技術(shù),2009,28(3):1- 4.

      [4] MAHONY R,HAMEL T,PFILMLIN J M,et al.Nonlinear complementary filte- rs on the special orthogonal gro- up[J].IEEE trans.on autom atic control,2008,53(5):1203- 1217.

      [5] 陳新雋,劉曉平,戴若犁,等.多MEMS傳感器的嵌入式姿態(tài)測量系統(tǒng)設(shè)計[A].單片機(jī)與嵌入式系統(tǒng):應(yīng)用天地,2011(10):58- 61.

      [6] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2014.

      [7] KIMBERLY T.Tilt sensing using linear Accelerometers[J].Freescale Semiconductor Application Note,2007(6):1- 7.

      [8] 李偉,何鵬舉,高社生.多傳感器加權(quán)信息融合算法研究[J].西北工業(yè)大學(xué)學(xué)報,2010,28(5):674 - 678.

      [9] 畢盛.閔華清.李淳,等.姿態(tài)傳感器采集測試系統(tǒng)的設(shè)計與實(shí)現(xiàn)[J].計算機(jī)測量與控制,2011,19(7):1562- 1564.

      [10] MADGWICK S.An efficient orientation filter for intertial and inertial/magnetic sensor arrays [R].Report xio and University of Bristol(UK),2010.

      猜你喜歡
      陀螺儀加速度計航向
      基于加速度計的起重機(jī)制動下滑量測量
      基于EMD的MEMS陀螺儀隨機(jī)漂移分析方法
      知坐標(biāo),明航向
      考慮幾何限制的航向道模式設(shè)計
      我國著名陀螺儀專家——林士諤
      基于遺傳算法的加速度計免轉(zhuǎn)臺標(biāo)定方法
      基于干擾觀測器的船舶系統(tǒng)航向Backstepping 控制
      電子制作(2017年24期)2017-02-02 07:14:16
      微機(jī)械陀螺儀概述和發(fā)展
      常溫下硅微諧振加速度計零偏穩(wěn)定性的提高
      MEMS三軸陀螺儀中不匹配干擾抑制方法
      拜城县| 禄劝| 册亨县| 海丰县| 榕江县| 旬邑县| 井冈山市| 通城县| 洛扎县| 衡阳县| 临汾市| 玉田县| 肇源县| 胶州市| 通河县| 乌鲁木齐市| 伊川县| 黔西| 遂宁市| 阿坝县| 哈巴河县| 石林| 通许县| 柳州市| 乐至县| 工布江达县| 金寨县| 启东市| 苗栗市| 武清区| 沭阳县| 会泽县| 措勤县| 永康市| 商都县| 民县| 凤凰县| 辽阳市| 临沂市| 辉县市| 都江堰市|