馬凱 賈志成 吳佳平 高麗華

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

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

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

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

1 系統(tǒng)方案設計

根據(jù)引言中分析，筆者選擇了集成陀螺儀、加速度計和電子羅盤的9軸MEMS姿態(tài)檢測傳感器MPU9150，用來實時采集3個軸的角速度、加速度和磁感應強度，以M4核的ARM芯片STM32F303為控制器，實時對MPU9150的輸出信號進行姿態(tài)解算、數(shù)據(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端口所示，對電源電路供電情況進行監(jiān)測，此外，還利用SPI接口，即圖2中SPI2_MISO，SPI2_CLK，SPI2_MOSI，SPI2_NSSI 4個端口所示，擴展了TF卡電路，用于存儲姿態(tài)信息。

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

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

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

2.3 電源電路

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

3 軟件設計

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

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

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

3.2 基于FreeRTOS程序設計

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

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

4 實驗和分析

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

圖6的橫軸為時間坐標，即采樣時間0～1 200 s，縱軸為和GPS數(shù)據(jù)融合后的相對誤差角度。其中，USL標注的曲線為本9軸姿態(tài)數(shù)據(jù)，Pixhawk標注的曲線為測試機姿態(tài)數(shù)據(jù)?？梢钥闯觯_始起飛時姿態(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ù)，能夠滿足使用要求，但是誤差范圍還值得進一步縮小和優(yōu)化。

5 結語

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

[參考文獻]

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

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

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

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

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

[6]秦永元.慣性導航[M].2版.北京：科學出版社，2014.

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

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

[9]楊斐文，張達，楊小聰.基于Cortex-M4和FreeRTOS的礦用三維激光掃描測量系統(tǒng)設計[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