基于STM32的四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)

曹延超

摘要：隨著無(wú)人飛行器的發(fā)展，四旋翼飛行器逐漸受到更多研究者的關(guān)注。本文主要提出了一種基于STM32的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)方案。該姿態(tài)測(cè)量平臺(tái)是采用STM32為主控制器的數(shù)據(jù)采集和計(jì)算姿態(tài)平臺(tái)。在本文中，整個(gè)系統(tǒng)在STM32上移植μC/OS-Ⅲ操作系統(tǒng)，實(shí)現(xiàn)對(duì)加速度、角速度、磁阻數(shù)據(jù)等的采集，采用四元數(shù)計(jì)算、卡爾曼濾波等實(shí)現(xiàn)姿態(tài)數(shù)據(jù)測(cè)量，通過(guò)無(wú)線模塊實(shí)現(xiàn)數(shù)據(jù)流的傳送。通過(guò)無(wú)線接收模塊，PC端上位機(jī)實(shí)現(xiàn)姿態(tài)獲取展示。最后，通過(guò)對(duì)建立的實(shí)驗(yàn)平臺(tái)的綜合實(shí)驗(yàn)，驗(yàn)證了本文系統(tǒng)的可行性和有效性，能夠?qū)崟r(shí)采集有效的姿態(tài)數(shù)據(jù)。

關(guān)鍵詞：四旋翼飛行器；姿態(tài)測(cè)量；STM32；μC/OS-Ⅲ；四元數(shù)

中圖分類號(hào)：V19

文獻(xiàn)標(biāo)識(shí)碼：A

0 引言

四旋翼飛行器近來(lái)已經(jīng)成為研究的大熱課題。四旋翼飛行器是一種四螺旋槳驅(qū)動(dòng)的、可垂直起降的飛行器。四旋翼飛行器在總體布局形式上屬于非共軸式碟形飛行器，比起普通飛行器結(jié)構(gòu)緊湊，升力更大，4只旋翼能夠相互抵消反扭力矩，設(shè)計(jì)更加簡(jiǎn)單。四旋翼飛行器以其新穎的外形、簡(jiǎn)單的結(jié)構(gòu)、低廉的成本、卓越的性能以及獨(dú)特的飛行控制方式等諸多優(yōu)勢(shì)吸引了越來(lái)越多的科研人員，成為了現(xiàn)在的研究熱點(diǎn)。目前國(guó)內(nèi)外都對(duì)四旋翼飛行器進(jìn)行了大量的研究，如美國(guó)Draganflyer公司研制的DraganflyerⅢ和賓夕法尼亞大學(xué)的HMX4等。

姿態(tài)測(cè)量是無(wú)人四旋翼飛行器重要功能之一，無(wú)人四旋翼飛行器需要實(shí)時(shí)準(zhǔn)確地采集來(lái)自多路傳感器的數(shù)據(jù)，并根據(jù)數(shù)據(jù)采集結(jié)果獲取姿態(tài)，通過(guò)控制器完成對(duì)無(wú)人飛行器的控制。本文將主要圍繞四旋翼飛行器的姿態(tài)測(cè)量系統(tǒng)展開(kāi)軟硬件的設(shè)計(jì)。本文采用ARM Cortex-M3架構(gòu)為基礎(chǔ)的STM32開(kāi)發(fā)板，移植最新的μC/OS-Ⅲ實(shí)時(shí)操作系統(tǒng)，通過(guò)全數(shù)字三軸角速率陀螺儀及三軸加速度傳感器和無(wú)線傳輸模塊對(duì)無(wú)人四旋翼飛行器的姿態(tài)進(jìn)行采集，通過(guò)無(wú)線傳送最后實(shí)現(xiàn)上位機(jī)的三維立體展示。本文將μC/OS-Ⅲ實(shí)時(shí)操作系統(tǒng)應(yīng)用到四旋翼飛行器數(shù)據(jù)采中，提高了四旋翼飛行器的數(shù)據(jù)采集能力。最終通過(guò)本文實(shí)現(xiàn)了一個(gè)穩(wěn)定的四旋翼飛行器姿態(tài)測(cè)量的實(shí)驗(yàn)平臺(tái)。

1 四旋翼飛行器原理與系統(tǒng)框架設(shè)計(jì)

1.1四旋翼飛行器簡(jiǎn)介

1.1.1四旋翼飛行器結(jié)構(gòu)

四旋翼直升機(jī)，國(guó)外又稱Quadrotor，F(xiàn)our-rotor，4rotors helicopter，X4-flyer等等，是一種具有四個(gè)螺旋槳的飛行器并且四個(gè)螺旋槳呈十字形交叉結(jié)構(gòu)。飛行器相對(duì)的四旋翼具有相同的旋轉(zhuǎn)方向，可以分為兩組，兩組的旋轉(zhuǎn)方向不同。如圖1，與傳統(tǒng)的直升機(jī)不同，四旋翼直升機(jī)只能通過(guò)改變螺旋槳的速度來(lái)實(shí)現(xiàn)各種動(dòng)作。

1.1.2四旋翼飛行器工作原理

簡(jiǎn)化的四旋翼飛行器可以得出，四旋翼飛行器具有呈十字交叉的四個(gè)螺旋槳，它通過(guò)改變四個(gè)螺旋槳的升力來(lái)獲取不同的運(yùn)動(dòng)，主要方法是改變螺旋槳的轉(zhuǎn)速。四旋翼有且僅有四個(gè)輸入力，卻需要產(chǎn)生6個(gè)自由度方向的運(yùn)動(dòng)，屬于典型的欠驅(qū)動(dòng)系統(tǒng)。四旋翼飛行器具有高度的耦合動(dòng)特性，一個(gè)螺旋槳速度發(fā)生變化，將會(huì)引起整個(gè)系統(tǒng)的不穩(wěn)定。四旋翼飛行器在空間共有6個(gè)自由度可以通過(guò)調(diào)節(jié)不同電機(jī)的轉(zhuǎn)速來(lái)實(shí)現(xiàn)?；具\(yùn)動(dòng)狀態(tài)分別是：1）垂直運(yùn)動(dòng)；2）俯仰運(yùn)動(dòng)；3）滾轉(zhuǎn)運(yùn)動(dòng)；4）偏航運(yùn)動(dòng)；5）前后運(yùn)動(dòng)；6）側(cè)向運(yùn)動(dòng)。

1.2四旋翼飛行器系統(tǒng)框架設(shè)計(jì)

1.2.1系統(tǒng)框架設(shè)計(jì)

本系統(tǒng)是設(shè)計(jì)基于STM32的四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)，本系統(tǒng)采用自頂向下的設(shè)計(jì)思路進(jìn)行。STM32系列基于專為要求高性能、低成本、低功耗的嵌入式應(yīng)用專門設(shè)計(jì)的ARM Cortex-M3內(nèi)核。按性能分成兩個(gè)不同的系列：STM32F103“增強(qiáng)型”系列和STM32F101“基本型”系列。增強(qiáng)型系列時(shí)鐘頻率達(dá)到72MHz，是同類產(chǎn)品中性能最高的產(chǎn)品；基本型時(shí)鐘頻率為36MHz，以16位產(chǎn)品的價(jià)格得到比16位產(chǎn)品大幅提升的性能，是16位產(chǎn)品用戶的最佳選擇。整個(gè)系統(tǒng)分為傳感器測(cè)量部分，無(wú)線傳輸部分和上位機(jī)顯示部分。傳感器測(cè)量部分主要負(fù)責(zé)將傳感器的數(shù)據(jù)獲取，根據(jù)獲取的傳感器數(shù)據(jù)轉(zhuǎn)換成所需的數(shù)據(jù)，整合幾個(gè)傳感器的數(shù)據(jù)，根據(jù)算法得到相應(yīng)的姿態(tài)狀況。無(wú)線模塊將獲取的姿態(tài)狀況以及測(cè)得的角速度、加速度和電子羅盤數(shù)據(jù)轉(zhuǎn)換成所需的特定格式，最后通過(guò)STM32端的無(wú)線模塊傳輸?shù)紸Tmega16端的無(wú)線接收模塊。上位機(jī)部分接收來(lái)自ATmega16的串口數(shù)據(jù)，將獲取的姿態(tài)狀況顯示出來(lái)。

1.2.2姿態(tài)測(cè)量平臺(tái)設(shè)計(jì)

四旋翼飛行器的姿態(tài)測(cè)量主要依靠處理傳感器數(shù)據(jù)獲取。本文的姿態(tài)測(cè)量系統(tǒng)將使用加速度傳感器、角速度傳感器和電子羅盤來(lái)對(duì)四旋翼飛行器的姿態(tài)做測(cè)量。在本課題中的四旋翼姿態(tài)測(cè)量系統(tǒng)的陀螺儀角速度傳感器采用的是L3G4200D，加速度計(jì)采用的是BMA180，電子羅盤采用的是HMC5883L。軟件部分，本文移植最新的μC/OS-Ⅲ實(shí)時(shí)操作系統(tǒng)，通過(guò)全數(shù)字三軸角速率陀螺儀、三軸加速度傳感器、磁阻傳感器和無(wú)線傳輸模塊對(duì)無(wú)人四旋翼飛行器的姿態(tài)進(jìn)行采集，實(shí)現(xiàn)測(cè)量捷聯(lián)慣性系統(tǒng)姿態(tài)角。在本文中采用了四元數(shù)法來(lái)更新姿態(tài)測(cè)量。

2 姿態(tài)測(cè)量系統(tǒng)

2.1數(shù)據(jù)采集平臺(tái)

在這個(gè)系統(tǒng)中主要是獲取相應(yīng)傳感器的數(shù)據(jù)，然后將得到的傳感器的數(shù)據(jù)進(jìn)行處理，得到姿態(tài)測(cè)量平臺(tái)的各項(xiàng)姿態(tài)參數(shù)，為旋翼電機(jī)的控制提供參數(shù)。如圖2，本姿態(tài)測(cè)量平臺(tái)主要是獲取當(dāng)前的三軸加速度、三軸角速度和磁通量，更新當(dāng)前的姿態(tài)，得到飛行器平臺(tái)的姿態(tài)，為四個(gè)旋翼電機(jī)的控制提供參數(shù)，引導(dǎo)飛行器的飛行。

2.2無(wú)線傳輸

在本文四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)中采用的無(wú)線模塊為nRF24L01模塊，無(wú)線模塊的發(fā)送端主機(jī)為四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)的主控制器STM32，無(wú)線模塊的接收端為ATmega16，發(fā)送的數(shù)據(jù)為姿態(tài)數(shù)據(jù)與三軸的加速度、角速度、磁通量。如圖3為無(wú)線傳輸?shù)氖疽鈭D。

2.3上位機(jī)展示

如圖4，上位機(jī)顯示部分顯示四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)的測(cè)量結(jié)果。在上位機(jī)的顯示部分主要顯示的是一個(gè)三維的姿態(tài)變換和三軸的角度、角速度、加速度、磁通量數(shù)值。該上位機(jī)比較簡(jiǎn)單明了，方便對(duì)姿態(tài)進(jìn)行觀測(cè)。上位機(jī)接收的數(shù)據(jù)格式為“$x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12$”，其中傳輸12個(gè)數(shù)據(jù)，分別為三軸姿態(tài)數(shù)據(jù)、三軸角速度、三軸加速度和三軸磁通量。這些數(shù)據(jù)將分別打印在對(duì)應(yīng)位置，其中姿態(tài)數(shù)據(jù)將用于三維圖像的姿態(tài)更新。

3 姿態(tài)測(cè)量軟件算法

3.1采集平臺(tái)軟件設(shè)計(jì)

嵌入式操作系統(tǒng)是一種支持嵌入式系統(tǒng)應(yīng)用的操作系統(tǒng)軟件。μC/OS-Ⅲ前身是μC/OS，最早出自于1992年美國(guó)嵌入式系統(tǒng)專家Jean J.Labrosse在《嵌入式系統(tǒng)編程》雜志的5月和6月刊上刊登的文章連載，并把μC/OS的源碼發(fā)布在該雜志的BBS上。在本文中，移植μC/OS-Ⅲ的主要步驟包括：1）從官網(wǎng)下載源代碼；2）建立IAR工程；3）BSP移植；4）編譯測(cè)試。

在完成基本操作系統(tǒng)之后，在工程中將創(chuàng)建任務(wù)，實(shí)現(xiàn)應(yīng)用開(kāi)發(fā)。如圖5為程序流程圖，主要包括以下幾個(gè)模塊：1）角速度測(cè)量任務(wù)；2）加速度測(cè)量任務(wù)；3）測(cè)通量測(cè)量任務(wù)；4）濾波姿態(tài)更新任務(wù)；5）定時(shí)中斷任務(wù)；6）無(wú)線發(fā)送任務(wù)。任務(wù)間通過(guò)發(fā)送消息和信號(hào)量來(lái)進(jìn)行通信，實(shí)現(xiàn)任務(wù)的工作。

3.2四元數(shù)姿態(tài)解析

在3D圖形學(xué)中，最常用的旋轉(zhuǎn)表示方法便是四元數(shù)和歐拉角，比起矩陣來(lái)具有節(jié)省存儲(chǔ)空間和方便插值的優(yōu)點(diǎn)。兩種表達(dá)方式可以互相轉(zhuǎn)換，計(jì)算公式采用3D笛卡爾坐標(biāo)系。

如圖6，歐拉角-XYZ系統(tǒng)（固定），XYZ系統(tǒng)（旋轉(zhuǎn)），線的節(jié)點(diǎn)，標(biāo)記為N。四元數(shù)可以用于描述一個(gè)坐標(biāo)系或矢量相對(duì)某一坐標(biāo)系的旋轉(zhuǎn)幅度。這個(gè)轉(zhuǎn)角的一半余弦值可以用四元數(shù)的標(biāo)量部分來(lái)表示，瞬時(shí)轉(zhuǎn)軸的方向、瞬時(shí)轉(zhuǎn)動(dòng)軸與參考坐標(biāo)系軸間的方向余弦值可以用矢量部分來(lái)表示。一個(gè)四元數(shù)可以包含轉(zhuǎn)軸的方向和轉(zhuǎn)角大小信息，往往稱其為轉(zhuǎn)動(dòng)四元數(shù)。如R′=qRq′表示矢量R相對(duì)參考坐標(biāo)系旋轉(zhuǎn)一個(gè)轉(zhuǎn)角，旋轉(zhuǎn)軸的方向由四元數(shù)的虛部確定，cosα、cosβ、cosγ表示旋轉(zhuǎn)軸n與參考坐標(biāo)系軸間的方向余弦值。式中：R為某矢量；

本文系統(tǒng)的實(shí)驗(yàn)將圍繞得到的傳感器數(shù)據(jù)分析，得到的傳感器數(shù)據(jù)進(jìn)行姿態(tài)數(shù)據(jù)更新。定時(shí)中斷發(fā)送信號(hào)量使得數(shù)據(jù)得以更新，更新數(shù)據(jù)后運(yùn)對(duì)這些數(shù)據(jù)進(jìn)行上述計(jì)算處理，得到當(dāng)前姿態(tài)，最后通過(guò)無(wú)線模塊和串口傳輸給PC。

4 平臺(tái)測(cè)量實(shí)驗(yàn)

在完成本四旋翼飛行器姿態(tài)測(cè)量系統(tǒng)后，對(duì)系統(tǒng)的各項(xiàng)參數(shù)進(jìn)行了實(shí)驗(yàn)，如圖7、圖8、圖9、圖10，得到三軸加速度、三軸角速度、三軸磁通量和計(jì)算的實(shí)時(shí)姿態(tài)數(shù)據(jù)。

通過(guò)實(shí)驗(yàn)可以知道，本文設(shè)計(jì)的姿態(tài)測(cè)量系統(tǒng)在實(shí)際中是可行的，能夠?qū)崟r(shí)的測(cè)量出飛行器平臺(tái)的姿態(tài)。通過(guò)跟蹤上位機(jī)的三維姿態(tài)展示可以很好的反饋出目前的空中姿態(tài)情況。

5 總結(jié)

本文給出了一個(gè)基于STM32的四旋翼飛行器的姿態(tài)測(cè)量系統(tǒng)設(shè)計(jì)；通過(guò)移植最新的實(shí)時(shí)操作系統(tǒng)μC/OS-Ⅲ創(chuàng)建任務(wù)，實(shí)現(xiàn)系統(tǒng)流程；通過(guò)陀螺儀、加速度傳感器等傳感器來(lái)實(shí)現(xiàn)捷聯(lián)慣性系統(tǒng)、四元數(shù)更新姿態(tài)；采用了卡爾曼濾波來(lái)提高系統(tǒng)的穩(wěn)定性和姿態(tài)精度；最后，本文在平臺(tái)實(shí)現(xiàn)中驗(yàn)證了姿態(tài)測(cè)量數(shù)據(jù)基本穩(wěn)定，基本達(dá)到了設(shè)計(jì)要求。