基于MEMS傳感器的船載無人機姿態檢測系統設計

馬凱 賈志成 吳佳平 高麗華

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

關鍵詞：船載無人機；姿態檢測；STM32；MPU9150；四元數卡爾曼；FreeRTOS

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

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

1 系統方案設計

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

2 硬件電路

2.1 微控制器電路

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

2.2 姿態傳感器電路

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

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

2.3 電源電路

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

3 軟件設計

3.1 姿態解算與數據融合

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

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

3.2 基于FreeRTOS程序設計

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

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

4 實驗和分析

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

圖6的橫軸為時間坐標，即采樣時間0～1 200 s，縱軸為和GPS數據融合后的相對誤差角度。其中，USL標注的曲線為本9軸姿態數據，Pixhawk標注的曲線為測試機姿態數據。可以看出，開始起飛時姿態數據誤差較大，隨后趨于平穩。USL俯仰角的誤差范圍在﹣25～20，約為0.7°，且偏離誤差低于Pixhawk的數據；USL橫滾角的誤差范圍0～50，約為0.8°，與Pixhawk誤差數據相差不大；USL航向角誤差接近0，但Pixhawk航向角誤差較大。綜合來說，本9軸姿態檢測模塊效果優于Pixhawk飛控姿態數據，能夠滿足使用要求，但是誤差范圍還值得進一步縮小和優化。

5 結語

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

[參考文獻]

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

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

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

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

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

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

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

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

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