基于MEMS傳感器的船載無人機(jī)姿態(tài)檢測系統(tǒng)設(shè)計(jì)

馬凱 賈志成 吳佳平 高麗華

摘 要：文章提出了基于一種基于MEMS姿態(tài)傳感器的船載無人機(jī)姿態(tài)檢測系統(tǒng)，按照無人機(jī)姿態(tài)檢測準(zhǔn)確性高、實(shí)時(shí)性快的要求，進(jìn)行了系統(tǒng)方案設(shè)計(jì)。硬件設(shè)計(jì)選用M4核的ARM芯片STM32F303為控制器，以9軸姿態(tài)檢測傳感器MPU9150為傳感器，STM32對MPU9150進(jìn)行監(jiān)控、數(shù)據(jù)處理。軟件設(shè)計(jì)上將FreeRTOS實(shí)時(shí)操作系統(tǒng)移植到STM32嵌入式處理器中，通過四元數(shù)卡爾曼算法對傳感器數(shù)據(jù)進(jìn)行解算、融合，得到無人機(jī)的俯仰角、橫滾角和航向角姿態(tài)數(shù)據(jù)。通過與Pixhawk飛控姿態(tài)數(shù)據(jù)進(jìn)行對比實(shí)驗(yàn)，在準(zhǔn)確性和穩(wěn)定性方面占有優(yōu)勢。

關(guān)鍵詞：船載無人機(jī)；姿態(tài)檢測；STM32；MPU9150；四元數(shù)卡爾曼；FreeRTOS

船載無人機(jī)具有機(jī)動(dòng)性強(qiáng)、使用便捷、飛行成本低特點(diǎn)，在海域監(jiān)管、海洋環(huán)境監(jiān)測、資源保護(hù)等領(lǐng)域作用越發(fā)明顯，船載無人機(jī)的應(yīng)用成為當(dāng)前研究熱點(diǎn)之一[1]。船載無人機(jī)姿態(tài)實(shí)時(shí)精確檢測，成為無人機(jī)安全飛行、可靠避障的必要條件和基本保障。

陀螺儀是維持方向和獲取角速度信號的常用裝置，在一定條件下可以完成無人機(jī)姿態(tài)3個(gè)核心參數(shù)，即俯仰角、滾轉(zhuǎn)角、航向角的檢測。在軍用無人機(jī)領(lǐng)域，利用機(jī)械陀螺儀及光纖陀螺儀等高精度慣性導(dǎo)航傳感器，能夠完成無人機(jī)姿態(tài)檢測，但其造價(jià)昂貴，體積大，在民用船載無人機(jī)應(yīng)用領(lǐng)域中有一定局限性[2]。隨著微機(jī)電系統(tǒng)（Micro-Electro-Mechanical System，MEMS）技術(shù)的發(fā)展，體積小、集成度高、價(jià)格低MEMS陀螺儀傳感器成為小型姿態(tài)檢測系統(tǒng)的最佳選擇，MEMS陀螺儀具有良好的動(dòng)態(tài)響應(yīng)特性，但存在溫度漂移問題，而且積分累積誤差存在，適合做短時(shí)間的檢測，長期使用必須對其進(jìn)行修正。MEMS加速度計(jì)和電子羅盤不存在積分誤差問題，靜態(tài)性能好，但受到載體振動(dòng)和外界磁場干擾，動(dòng)態(tài)性能會(huì)較差[3]。因此，可由陀螺儀檢測無人機(jī)運(yùn)動(dòng)姿態(tài)，同時(shí)利用加速度計(jì)和電子羅盤分別檢測加速度和地磁場矢量來校準(zhǔn)陀螺儀的測量值，通過數(shù)據(jù)融合算法，提高無人機(jī)姿態(tài)信息檢測準(zhǔn)確性[4]。

1 系統(tǒng)方案設(shè)計(jì)

根據(jù)引言中分析，筆者選擇了集成陀螺儀、加速度計(jì)和電子羅盤的9軸MEMS姿態(tài)檢測傳感器MPU9150，用來實(shí)時(shí)采集3個(gè)軸的角速度、加速度和磁感應(yīng)強(qiáng)度，以M4核的ARM芯片STM32F303為控制器，實(shí)時(shí)對MPU9150的輸出信號進(jìn)行姿態(tài)解算、數(shù)據(jù)融合計(jì)算，得到無人機(jī)的俯仰角、橫滾角和航向角的姿態(tài)信息，整體系統(tǒng)框如圖1所示。

2 硬件電路

2.1 微控制器電路

微控制器總體電路由STM32F303最小系統(tǒng)、MPU9150姿態(tài)電路、TF存儲卡電路、電源電路4部分組成，如圖2所示。STM32F303主控芯片除了利用I2C接口采集MPU9150姿態(tài)信息外，還利用AD腳，即圖2中Bat端口所示，對電源電路供電情況進(jìn)行監(jiān)測，此外，還利用SPI接口，即圖2中SPI2_MISO，SPI2_CLK，SPI2_MOSI，SPI2_NSSI 4個(gè)端口所示，擴(kuò)展了TF卡電路，用于存儲姿態(tài)信息。

2.2 姿態(tài)傳感器電路

姿態(tài)傳感器MPU9150內(nèi)部集成陀螺儀、加速度計(jì)、電子羅盤，它可根據(jù)運(yùn)動(dòng)速度的快慢，編程設(shè)置不同的測量范圍，姿態(tài)傳感器電路如圖3所示。

圖3中I2C1_SDA，I2C1_SCL分別是I2C總線的數(shù)據(jù)傳輸引腳和時(shí)鐘信號引腳，利用上拉電阻，能夠提高系統(tǒng)抗干擾性；LSM_INT1是MPU9150的中斷輸出引腳，可將相關(guān)狀態(tài)信號觸發(fā)給STM32F303微控制器。MPU9150芯片工作電壓為3.3 V，為確保姿態(tài)檢測信號實(shí)時(shí)、有效工作，對MPU9150的供電采用斷電復(fù)位方式，即利用P型MOS管進(jìn)行復(fù)位，當(dāng)STM32F303微控制器檢測到姿態(tài)數(shù)據(jù)校驗(yàn)有誤或數(shù)據(jù)不響應(yīng)等異常時(shí)，CPU則通過MEMS_CTR引腳發(fā)出一個(gè)高電平給MOS管柵級將其斷電，隨后再發(fā)一個(gè)低電平將MOS管導(dǎo)通，完成復(fù)位工作。

2.3 電源電路

本系統(tǒng)采用可充電鋰電池供電，為獲取穩(wěn)定可靠系統(tǒng)電壓，采用ME6211C系列LDO線性穩(wěn)壓芯片，考慮到STM32F303控制器及MPU9150芯片實(shí)際工作電壓為2.7～3.6 V，在滿足功能前提下，從降低功耗角度出發(fā)，選用輸出電壓為3.0 V的ME6211C30芯片，電源電路如圖4所示，D1，D2為TVS穩(wěn)壓管，可以避免外界電源、負(fù)載不穩(wěn)的時(shí)候?qū)ο到y(tǒng)沖擊；PT為自恢復(fù)保險(xiǎn)絲，當(dāng)有超過200 ma自動(dòng)斷電，溫度較低后自動(dòng)恢復(fù)工作；穩(wěn)壓管Bat接到CPU的AD引腳，用于檢測ME6211C30芯片輸入電壓。

3 軟件設(shè)計(jì)

3.1 姿態(tài)解算與數(shù)據(jù)融合

控制器從姿態(tài)傳感器采集的信息不能直接表達(dá)姿態(tài)的俯仰角、橫滾角和航向角，需要利用算法進(jìn)行解算，即姿態(tài)解算，另外，由于3個(gè)MEMS傳感器的動(dòng)態(tài)響應(yīng)速度、累計(jì)誤差的不同，所計(jì)算的姿態(tài)信息具有一定的互補(bǔ)性，所以，必要采用相關(guān)算法對每個(gè)傳感器的信號進(jìn)行融合，提供測量精度和系統(tǒng)的動(dòng)態(tài)性能，即數(shù)據(jù)融合[5]。

俯仰角θ，橫滾角φ，航向角φ 3個(gè)姿態(tài)角需要通過地理坐標(biāo)系E與機(jī)體坐標(biāo)系B的坐標(biāo)變換獲得[6]。地理坐標(biāo)系E采用北東地坐標(biāo)系，其中原點(diǎn)在機(jī)體的中心，xe軸指向地理北，ye軸在水平面內(nèi)并指向東，ze軸垂直與地面并指向地心，構(gòu)成右手坐標(biāo)系；機(jī)體坐標(biāo)系B原點(diǎn)取在飛機(jī)的質(zhì)心，坐標(biāo)系與飛機(jī)固連，xb與飛機(jī)的縱軸重合，yb與飛機(jī)的橫軸重合，zb與飛機(jī)的豎軸重合。由地理坐標(biāo)系向機(jī)體坐標(biāo)系的轉(zhuǎn)換可以通過3次旋轉(zhuǎn)來實(shí)現(xiàn)，其轉(zhuǎn)換關(guān)系式如式（1）所示。

3.2 基于FreeRTOS程序設(shè)計(jì)

為提高系統(tǒng)實(shí)時(shí)性與可靠性，將FreeRTOS實(shí)時(shí)操作系統(tǒng)移植到STM32嵌入式處理器中，F(xiàn)reeRTOS是一種免費(fèi)、開源實(shí)時(shí)性操作系統(tǒng)，能夠?qū)崿F(xiàn)基本的實(shí)時(shí)調(diào)度、信號量、隊(duì)列和存儲管理功能[9]。

本系統(tǒng)設(shè)計(jì)的主要任務(wù)包括：（1）I2C控制任務(wù)，實(shí)現(xiàn)對9軸傳感器的數(shù)據(jù)采集。（2）UART控制任務(wù)，實(shí)現(xiàn)模塊和外界通信進(jìn)行數(shù)據(jù)交換的任務(wù)。（3）SPI控制任務(wù)，實(shí)現(xiàn)對TF卡的數(shù)據(jù)存取任務(wù)。（4）Kalman任務(wù)，實(shí)現(xiàn)對傳感器數(shù)據(jù)進(jìn)行融合處理的任務(wù)。FreeRTOS移植初始化流程如圖5所示。

4 實(shí)驗(yàn)和分析

為驗(yàn)證本9軸姿態(tài)檢測系統(tǒng)的性能及指標(biāo)，將此姿態(tài)檢測模塊接入無人機(jī)測試機(jī)進(jìn)行飛行驗(yàn)證，測試機(jī)采用Pixhawk飛控系統(tǒng)，數(shù)據(jù)取樣時(shí)間為從起飛開始的1 200 s。9軸姿態(tài)傳感器解算融合后得到俯仰角、橫滾角和航向角數(shù)據(jù)參數(shù)，這3個(gè)參數(shù)再與GPS數(shù)據(jù)融合得到相對誤差。同時(shí)，將Pixhawk飛控系統(tǒng)自身姿態(tài)數(shù)據(jù)與GPS數(shù)據(jù)融合也得到相對誤差，最后，將3個(gè)姿態(tài)參數(shù)的兩種相對誤差同時(shí)輸出，進(jìn)行比較分析，具體數(shù)據(jù)曲線如圖6所示。

圖6的橫軸為時(shí)間坐標(biāo)，即采樣時(shí)間0～1 200 s，縱軸為和GPS數(shù)據(jù)融合后的相對誤差角度。其中，USL標(biāo)注的曲線為本9軸姿態(tài)數(shù)據(jù)，Pixhawk標(biāo)注的曲線為測試機(jī)姿態(tài)數(shù)據(jù)?？梢钥闯?，開始起飛時(shí)姿態(tài)數(shù)據(jù)誤差較大，隨后趨于平穩(wěn)。USL俯仰角的誤差范圍在﹣25～20，約為0.7°，且偏離誤差低于Pixhawk的數(shù)據(jù)；USL橫滾角的誤差范圍0～50，約為0.8°，與Pixhawk誤差數(shù)據(jù)相差不大；USL航向角誤差接近0，但Pixhawk航向角誤差較大。綜合來說，本9軸姿態(tài)檢測模塊效果優(yōu)于Pixhawk飛控姿態(tài)數(shù)據(jù)，能夠滿足使用要求，但是誤差范圍還值得進(jìn)一步縮小和優(yōu)化。

5 結(jié)語

硬件電路上，設(shè)計(jì)了姿態(tài)檢測傳感器斷電復(fù)位電路，能夠?qū)崟r(shí)根據(jù)控制器CPU采樣數(shù)據(jù)異常，通過P型MOS管對MPU9150姿態(tài)傳感器復(fù)位，確保了采樣數(shù)據(jù)實(shí)時(shí)和有效，為飛控系統(tǒng)安全可靠運(yùn)行提供了保障；軟件設(shè)計(jì)上，將FreeRTOS實(shí)時(shí)操作系統(tǒng)移植到STM32嵌入式處理器中，采用四元數(shù)卡爾曼算法對傳感器數(shù)據(jù)進(jìn)行解算、融合得到姿態(tài)數(shù)據(jù)。實(shí)驗(yàn)數(shù)據(jù)表明，本9軸姿態(tài)檢測模塊在俯仰角誤差和橫滾角誤差、航向角的穩(wěn)定性方面，優(yōu)于Pixhawk飛控姿態(tài)數(shù)據(jù)，能夠滿足船載無人機(jī)飛控要求，具有實(shí)用性。

[參考文獻(xiàn)]

[1]林一平.我國加強(qiáng)無人機(jī)對海洋環(huán)境監(jiān)測與管理[J].交通與運(yùn)輸，2013（1）：31-32.

[2]張曉玉，丁文武.基于MEMS的慣性導(dǎo)航系統(tǒng)研究與設(shè)計(jì)[J].科學(xué)技術(shù)與工程，2012（1）：56-59.

[3]王建林，徐青菁，姜子庠.多功能自平衡智能車控制平臺的設(shè)計(jì)[J].實(shí)驗(yàn)室研究與探索，2016（4）：71-73.

[4]李世光，王文文，申夢茜.基于STM32的姿態(tài)測量系統(tǒng)設(shè)計(jì)[J].現(xiàn)代電子技術(shù)，2016（9）：12-14.

[5]劉凱，梁曉庚.基于陀螺儀和磁強(qiáng)計(jì)的姿態(tài)解算方法研究[J].計(jì)算機(jī)仿真，2014（5）：39-42.

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

[7]陳建翔，萬子敬，王向軍.基于四元數(shù)的低成本姿態(tài)測量系統(tǒng)設(shè)計(jì)[J].傳感技術(shù)學(xué)報(bào)，2016（5）：706-710.

[8]陳鵬展，朱年華，李杰.兩輪自平衡車姿態(tài)檢測與平衡控制[J].控制工程，2017（8）：1574-1578.

[9]楊斐文，張達(dá)，楊小聰.基于Cortex-M4和FreeRTOS的礦用三維激光掃描測量系統(tǒng)設(shè)計(jì)[J].中國礦業(yè)，2014（2）：316-318.

Abstract：A ship borne UAV attitude detection system based on MEMS attitude sensor is proposed. According to the requirements of high accuracy and fast real-time， the system scheme is designed. The hardware design chooses the ARM chip STM32F303 with M4 core as the controller， 9-axis attitude detection sensor MPU9150 as the sensor， and STM32 monitors and processes the data of MPU9150. In software design， FreeRTOS real-time operating system is transplanted to STM32 embedded processor. Quaternion-Kalman algorithm is used to compute and fuse the sensor data， so the pitch angle， roll angle and heading angle of UAV are obtained. Compared with Pixhawk flight control attitude data， it has an advantage in accuracy and stability.

Key words：ship borne UAV； attitude detection； STM32； MPU9150； Quaternion-Kalman； FreeRTOS