基于S698和MPU9150的無人機姿態(tài)識別

季宏祥 耿相銘

?

基于S698和MPU9150的無人機姿態(tài)識別

季宏祥 耿相銘

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

S698是一款由珠海歐比特控制工程股份有限公司研制出的32位嵌入式處理器。采用S698的主控芯片平臺，調(diào)用Motion- Fusion，其輸出的四元數(shù)轉(zhuǎn)換后的姿態(tài)角與無人機的姿態(tài)相符。介紹如何結(jié)合S698平臺重構(gòu)關(guān)鍵函數(shù)及底層驅(qū)動來實現(xiàn)調(diào)用MotionFusion來識別無人機的姿態(tài)。

S698；MPU9150；姿態(tài)；MotionFusion；四元素

引言

珠海歐比特工程研發(fā)的S698處理器，是國內(nèi)首款基于SPARK V8架構(gòu)的32為嵌入式處理器。由于國防、航空等領(lǐng)域的特殊性，國產(chǎn)的S698已被廣泛的應(yīng)用于這些領(lǐng)域[1]。

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

1 Invensense的Motion Driver概述

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

2 S698中關(guān)鍵函數(shù)及驅(qū)動的重構(gòu)

2.1 I2C總線驅(qū)動及接口函數(shù)重構(gòu)

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

圖1 I2C總線寫操作為例說明實現(xiàn)過程

3.2 中斷函數(shù)重構(gòu)

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

2.3 時鐘函數(shù)重構(gòu)

根據(jù)Motion Driver的要求，對于部分時鐘函數(shù)也需要重構(gòu)。采用S698通用定時器1來執(zhí)行延時[5]。

圖3 采用S698通用定時器1來執(zhí)行延時

4 S698平臺上配置并調(diào)用Motion Driver

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

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

5 總結(jié)

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

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

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

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

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

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

V249.12

A

1009-6434（2016）02-0018-02