基于ARM的微型航姿參考系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)

求解上述微分方程較常用的方法有泰勒展開法、比卡逼近法、四階-龍格庫塔法，考慮到算法的精度、速度與復(fù)雜度，本文選用了四階-龍格庫塔法。

4 系統(tǒng)軟件設(shè)計(jì)

AHRS的軟件設(shè)計(jì)流程圖如圖3所示。其中傳感器初始化部分包括設(shè)置傳感器量程，以及陀螺儀的零飄檢測，氣壓高度計(jì)的初始標(biāo)定。每當(dāng)傳感器完成一次數(shù)據(jù)的采集，就會(huì)向ARM處理器發(fā)出中斷請(qǐng)求，然后ARM處理器通過I2C總線讀取數(shù)據(jù)并完成處理，以此來達(dá)到快速響應(yīng)的目的，另外氣壓高度計(jì)的轉(zhuǎn)換時(shí)間較慢，為了保證系統(tǒng)的實(shí)時(shí)性，本文采用定時(shí)的方式來讀取氣壓值。

串口發(fā)送的數(shù)據(jù)除了歐拉角外，還包括各個(gè)方向的角速度、加速度和地磁場強(qiáng)度以及陀螺常值漂移和磁航向誤差補(bǔ)償系數(shù)。由于串口數(shù)據(jù)發(fā)送量較大，因此將導(dǎo)航數(shù)據(jù)幀分段發(fā)送。導(dǎo)航數(shù)據(jù)幀的幀頭為0xAA、0x55，幀尾為0x55、0xAA，數(shù)據(jù)幀中每個(gè)物理量都是32位浮點(diǎn)型數(shù)據(jù)，數(shù)據(jù)幀格式如下圖4所示。

在實(shí)際環(huán)境中，地磁場不可避免會(huì)受到干擾磁場的影響，地磁場的測量會(huì)帶來一定的誤差，計(jì)算得到的磁航向角也會(huì)有偏差。為了保證航姿系統(tǒng)輸出較為精確的航向角，需對(duì)磁場干擾進(jìn)行補(bǔ)償。

本文采用最小二乘擬合法，基本方法如下：選擇一干凈磁場環(huán)境，將電子羅盤置于轉(zhuǎn)臺(tái)上，調(diào)節(jié)轉(zhuǎn)臺(tái)使俯仰角為0°，將電子羅盤指北軸對(duì)準(zhǔn)磁北，控制轉(zhuǎn)臺(tái)以一定度數(shù)間隔轉(zhuǎn)動(dòng)一圈，存儲(chǔ)下每個(gè)點(diǎn)電子羅盤實(shí)際輸出的磁航向角;調(diào)節(jié)轉(zhuǎn)臺(tái)使俯仰角不為0°，重復(fù)上一步的動(dòng)作，并根據(jù)具體情況控制標(biāo)定次數(shù)。上述過程可以稱之為定標(biāo)過程，電子羅盤在定完標(biāo)后使用時(shí)就可根據(jù)存儲(chǔ)器中的數(shù)據(jù)自動(dòng)進(jìn)行誤差補(bǔ)償。

這里討論的最小二乘擬合算法不同于以往的最小二乘誤差補(bǔ)償法，它不是僅僅考慮了水平面內(nèi)的誤差補(bǔ)償，還考慮了不同俯仰角下的情況。用電子羅盤作標(biāo)定試驗(yàn)時(shí)，由于有真實(shí)的航向角作參考，且標(biāo)定數(shù)據(jù)點(diǎn)較多，最終擬合后的值相當(dāng)于把安裝誤差、靈敏度等影響航向角精度的其他因素都考慮進(jìn)去了，又因?yàn)閿M合用到的數(shù)據(jù)預(yù)先已存放在存儲(chǔ)器中，所以做完最初的一輪標(biāo)定試驗(yàn)后，以后便可進(jìn)行自動(dòng)補(bǔ)償了，可見這樣的方法具有較強(qiáng)的實(shí)用價(jià)值。

5 算法驗(yàn)證與實(shí)現(xiàn)

為了驗(yàn)證AHRS的穩(wěn)定性及算法的有效性，本文用LabWindows/CVI設(shè)計(jì)了一個(gè)簡易的上位機(jī)程序，以便于AHRS通信，實(shí)時(shí)觀測姿態(tài)角的變化，如圖5圖6所示。實(shí)際測試中，系統(tǒng)歐拉角的更新速度達(dá)到30 kHz，已滿足微型飛行器的控制要求。

6 結(jié)論

航姿參考系統(tǒng)是微小型飛行器的自主飛行的基礎(chǔ)環(huán)節(jié)，本文采用常用的ARM與MEMS器件，結(jié)合四元數(shù)算法設(shè)計(jì)了一套針對(duì)微小型飛行器的AHRS系統(tǒng)，它具有體積小、價(jià)格低廉、精度高、實(shí)時(shí)性好的特點(diǎn)，從實(shí)際測試來看，設(shè)計(jì)方案是可行、可靠的。該系統(tǒng)具有模塊化的特點(diǎn)，也可方便應(yīng)用于其他需要對(duì)姿態(tài)測量的系統(tǒng)中。