基于S698和MPU9150的無人機姿態識別

季宏祥 耿相銘

?

基于S698和MPU9150的無人機姿態識別

季宏祥 耿相銘

上海交通大學電子信息與電氣工程學院，上海200240

S698是一款由珠海歐比特控制工程股份有限公司研制出的32位嵌入式處理器。采用S698的主控芯片平臺，調用Motion- Fusion，其輸出的四元數轉換后的姿態角與無人機的姿態相符。介紹如何結合S698平臺重構關鍵函數及底層驅動來實現調用MotionFusion來識別無人機的姿態。

S698；MPU9150；姿態；MotionFusion；四元素

引言

珠海歐比特工程研發的S698處理器，是國內首款基于SPARK V8架構的32為嵌入式處理器。由于國防、航空等領域的特殊性，國產的S698已被廣泛的應用于這些領域[1]。

MPU9150的MotionFusion專利能夠幫助客戶減少在運動數據融合方面的工作，其目前主要支持TI-MSP430和ARM平臺，對于其他平臺，使用者需要結合Invensense官網提供的Motion Driver 5.1.2版本，重構部分函數進而實現運動數據的融合及四元數姿態輸出。Motion Driver 5.1.2的限制在于它不能融合磁力計的三軸數據，但由于Motion Driver平臺本身要求，若需要融合磁力計的數據，必須使用TI-MSP430或者ARM平臺，本文中所討論的是基于Motion Driver 5.1.2版本，而融合的數據也是六軸數據[2]。

1 Invensense的Motion Driver概述

Motion Driver是Invensense公司提供的傳感器驅動層，以便用戶能夠配置并啟動Invensense公司運動監測芯片的數字融合處理功能。Motion Driver的源代碼是以C語言的方式編寫的，方便進行移植。其融合的運動數據是可以根據用戶的需求進行配置的，關于Motion Driver中各個函數具體的接口定義及含義參見Embeded Motion Driver v5.1.1 APIs Specification。Motion Driver的移植需要根據Invensense提供的Motion Driver 5.1.1 Tutorial進行。本指導書中關于具體的哪些函數需要重構，并不明確，需要使用者去閱讀Motion Driver的源代碼，并且需要結合所需要移植的控制平臺對接口及部分底層驅動函數根據需要進行重構[3]。

2 S698中關鍵函數及驅動的重構

2.1 I2C總線驅動及接口函數重構

S698內部沒有I2C總線接口，而使用S698的多功能口模擬I2C總線的關鍵在于模擬時鐘SCL和根據SCL時鐘。模擬SCL時鐘有兩種方式，一種是使用空指令，等待延時。另外一種非阻塞式的實現方式是通過定時器來實現。結合Motion Driver中的源代碼，所有的涉及I2C的驅動調用均是阻塞式的方式，所以在S698平臺上采用阻塞是方式的I2C。下面以I2C總線寫操作為例說明實現過程[4]。

圖1 I2C總線寫操作為例說明實現過程

3.2 中斷函數重構

MPU9150的中斷輸出功能在Motion Driver中被多次調用，其可被用于提醒主芯片其所需的數據或者運動數據的融合已經完成等。以上的功能的實現需要結合硬件平臺上MPU9150中斷引腳與S698的連接，以及軟件對MPU9150的配置。由于在Motion Driver中具有專門的中斷注冊函數接口，用于關聯具體的中斷源于中斷處理函數，在移植的過程中，需要根據S698平臺重構此中斷注冊函數。主要的中斷相關函數重構如下。

2.3 時鐘函數重構

根據Motion Driver的要求，對于部分時鐘函數也需要重構。采用S698通用定時器1來執行延時[5]。

圖3 采用S698通用定時器1來執行延時

4 S698平臺上配置并調用Motion Driver

配置和調用的主要步驟包括以下幾步。（1）S698及MPU9150的初始化及自測。（2）配置加速度計及陀螺儀的量程。（3）加載數字運動處理程序（DMP）的鏡像文件到MPU9150中。（4）將陀螺儀和加速度計的初始方向矩陣輸入到DMP中。（5）配置具體需要DMP反饋的數據。（6）啟動DMP。（7）配置DMP的數據輸出的速率。

測試使用的是S698自帶的開發平臺Orion 4.0，使用的硬件測試平臺為基于S698的旋翼無人機的飛行控制板。在測試過程中，根據以上七步，成功調用MPU9150內部的數字運算處理模塊，輸出六軸融合數據及傳感器原始數據，同時將四元數轉換成歐拉角，結果顯示歐拉角的輸出與無人機機體的姿態相符。

5 總結

本文給出了基于S698平臺如何調用MPU9150內部的運動處理單元，并詳述了其關鍵函數重構的方法。通過將MPU9150其輸出的四元數轉換后的姿態角與無人機的姿態相比較驗證了該移植的有效性。需要指出的是，由于Invensense公司自身運動庫的限制，若需要在S698平臺上實現九軸數據的融合，則需要完全拋開Motion Driver，而這就不可以避免的涉及到各種融合算法。關于此項工作的研究，有待后續開展。

[1]冀亮，錢正洪，白茹.基于四元數的四軸無人機姿態的估計和控制[J].現代電子技術，2015（11）：112-116.

[2]任志明，李永樹，何敬，等.基于姿態數據的無人機影像自動刺點功能研究[J].測繪通報，2011（4）：44-46.

[3]宋宇，翁新武，郭昕剛.基于四元數EKF算法的小型無人機姿態估計[J].吉林大學學報：理學版，2015（3）：511-518.

[4]何永勃，賈輝，姜坤，等.基于Android終端陀螺儀傳感器的無人機飛行姿態控制[J].傳感技術學報，2015（4）：474-478.

[5]趙亞鵬，曹彬才，吳帥.基于飛行姿態校正的無人機影像快速拼接[J].測繪科學與工程，2015（1）：47-54.

V249.12

A

1009-6434（2016）02-0018-02