基于MPU6050的電子機(jī)械穩(wěn)定平臺(tái)設(shè)計(jì)

黃全振，張 寧，林偉鵬，孫清原，陳素霞

(河南工程學(xué)院 電氣信息工程學(xué)院，河南 鄭州 451191)

基于MPU6050的電子機(jī)械穩(wěn)定平臺(tái)設(shè)計(jì)

黃全振，張 寧，林偉鵬，孫清原，陳素霞

(河南工程學(xué)院 電氣信息工程學(xué)院，河南 鄭州 451191)

穩(wěn)定平臺(tái)具有隔離載體擾動(dòng)、實(shí)時(shí)保持動(dòng)態(tài)姿態(tài)基準(zhǔn)的特點(diǎn)，已在許多領(lǐng)域得到了廣泛的應(yīng)用.采用MPU6050姿態(tài)傳感器，設(shè)計(jì)了一個(gè)三自由度電子機(jī)械穩(wěn)定跟蹤平臺(tái).采用MPU6050監(jiān)測(cè)平臺(tái)的姿態(tài)變化，姿態(tài)控制器根據(jù)控制算法輸出控制信號(hào)，用來(lái)控制3個(gè)電機(jī)轉(zhuǎn)向及轉(zhuǎn)速，電機(jī)帶動(dòng)機(jī)械傳動(dòng)機(jī)構(gòu)實(shí)現(xiàn)穩(wěn)定平臺(tái)在X軸、Y軸、Z軸3個(gè)自由度方向的移動(dòng)，從而實(shí)現(xiàn)穩(wěn)定平臺(tái)姿態(tài)基準(zhǔn)的穩(wěn)定.測(cè)試與仿真結(jié)果表明，穩(wěn)定平臺(tái)的控制效果良好，為航空專(zhuān)用精密穩(wěn)定跟蹤平臺(tái)的開(kāi)發(fā)提供了重要參考.

穩(wěn)定跟蹤平臺(tái)；姿態(tài)傳感器；PID控制算法

穩(wěn)定跟蹤平臺(tái)以實(shí)時(shí)檢測(cè)平臺(tái)姿態(tài)和位置變化為手段，通過(guò)一系列電子機(jī)械傳動(dòng)機(jī)構(gòu)精確保持動(dòng)態(tài)姿態(tài)基準(zhǔn)，利用圖像探測(cè)設(shè)備實(shí)現(xiàn)對(duì)移動(dòng)目標(biāo)的自動(dòng)跟蹤.由于它有效地隔離了載體本身的擾動(dòng)，在航空偵查、海底探測(cè)、精密設(shè)備的檢測(cè)等領(lǐng)域得到了廣泛的應(yīng)用[1-3].

目前，穩(wěn)定平臺(tái)的姿態(tài)測(cè)量以陀螺儀為主，如文獻(xiàn)[4]采用兩個(gè)單軸陀螺和一個(gè)水平陀螺儀實(shí)現(xiàn)了航空相機(jī)穩(wěn)定平臺(tái)系統(tǒng)的設(shè)計(jì)；文獻(xiàn)[5]以電子機(jī)械三軸角速率陀螺、線(xiàn)加速度計(jì)和磁阻傳感器為基礎(chǔ),設(shè)計(jì)了一套低成本的低空遙感小型三軸陀螺穩(wěn)定平臺(tái)；文獻(xiàn)[6]設(shè)計(jì)了一個(gè)兩軸兩框架結(jié)構(gòu)的紅外相機(jī)穩(wěn)定平臺(tái)并進(jìn)行了模態(tài)分析.穩(wěn)定平臺(tái)在實(shí)際應(yīng)用場(chǎng)合中經(jīng)常受到外界環(huán)境的干擾，系統(tǒng)性能下降，如何提高抗干擾能力成為穩(wěn)定平臺(tái)精度的關(guān)鍵.許多學(xué)者將現(xiàn)代控制算法應(yīng)用到穩(wěn)定平臺(tái)的控制中，如文獻(xiàn)[7]為了解決非線(xiàn)性因素對(duì)穩(wěn)定平臺(tái)控制系統(tǒng)性能的影響，提出了一種自適應(yīng)模糊-PID復(fù)合控制方法；文獻(xiàn)[8]為了進(jìn)一步提高光電穩(wěn)定偵查平臺(tái)的抗干擾能力,提出了一種基于擾動(dòng)頻率自適應(yīng)的自抗擾控制新方法.上述控制方法雖然有效提高了穩(wěn)定平臺(tái)的抗干擾性能，但控制算法復(fù)雜、計(jì)算量大，很難在成本較低的單片機(jī)控制系統(tǒng)中實(shí)現(xiàn).

穩(wěn)定跟蹤平臺(tái)在航空拍攝、機(jī)載目標(biāo)跟蹤和海底聲吶探測(cè)等領(lǐng)域的應(yīng)用相當(dāng)廣泛，故采用MPU6050為姿態(tài)傳感元件、STM32單片機(jī)為主控芯片，設(shè)計(jì)一個(gè)三自由度電子穩(wěn)定平臺(tái)，該平臺(tái)具有體積小、精度高、成本低、自主穩(wěn)定等特點(diǎn).試驗(yàn)測(cè)試表明，穩(wěn)定平臺(tái)的轉(zhuǎn)向角測(cè)量精度可達(dá)1°，俯仰角和橫滾角可達(dá)0.1°，機(jī)械控制精度優(yōu)于0.1°，快速響應(yīng)時(shí)間小于20 ms.

圖1 相機(jī)三維立體圖Fig.1 Camera 3D stereogram

1 設(shè)計(jì)原理

由于圖像采集技術(shù)的飛速發(fā)展，相機(jī)的防抖功能在不斷完善，從機(jī)械傳動(dòng)機(jī)構(gòu)到圖像的合成過(guò)程、從硬件的防抖到軟件的防抖，均取得了較大的進(jìn)步.若將相機(jī)抖動(dòng)機(jī)制分解成三維立體圖(見(jiàn)圖1)，相機(jī)拍攝質(zhì)量干擾主要是由X軸、Y軸、Z軸方向位移偏差引起的.

穩(wěn)定跟蹤平臺(tái)的構(gòu)建思路類(lèi)似于相機(jī)三維消抖結(jié)構(gòu)，試圖在俯仰(X軸)、橫滾(Y軸)和轉(zhuǎn)向(Z軸)方向上運(yùn)動(dòng)，使平臺(tái)上攜帶的設(shè)備在地理坐標(biāo)系上保持穩(wěn)定，以消除由平臺(tái)載機(jī)引起的相移.精確跟蹤平臺(tái)的穩(wěn)定性取決于機(jī)械精準(zhǔn)順滑度、機(jī)構(gòu)轉(zhuǎn)向電機(jī)精度、運(yùn)動(dòng)姿態(tài)傳感器靈敏度和較好的反饋控制算法等.

圖2 穩(wěn)定平臺(tái)的結(jié)構(gòu)Fig.2 The structure diagram of stable platform

2 穩(wěn)定跟蹤平臺(tái)系統(tǒng)的設(shè)計(jì)

2.1 穩(wěn)定平臺(tái)的結(jié)構(gòu)

穩(wěn)定平臺(tái)大致由載機(jī)底座、3個(gè)機(jī)構(gòu)轉(zhuǎn)動(dòng)無(wú)刷電機(jī)、重力加速度陀螺儀傳感器MPU6050、攝像設(shè)備和相關(guān)驅(qū)動(dòng)機(jī)構(gòu)組成，結(jié)構(gòu)如圖2所示.

安裝在平臺(tái)上的MPU6050姿態(tài)傳感器感測(cè)平臺(tái)的運(yùn)動(dòng)，將載體姿態(tài)的變化實(shí)時(shí)地傳送給控制器模塊，控制器依據(jù)控制算法輸出控制信號(hào)，控制3個(gè)電機(jī)的轉(zhuǎn)向及轉(zhuǎn)速，電機(jī)帶動(dòng)傳動(dòng)機(jī)構(gòu)實(shí)現(xiàn)跟蹤平臺(tái)在X軸、Y軸、Z軸3個(gè)自由度方向的移動(dòng)，從而實(shí)現(xiàn)精確穩(wěn)定平臺(tái)的跟蹤功能.

圖3 穩(wěn)定平臺(tái)控制系統(tǒng)總體框圖Fig.3 The overall block diagram of stabilized platform control system

2.2 穩(wěn)定跟蹤平臺(tái)控制系統(tǒng)

穩(wěn)定跟蹤平臺(tái)控制系統(tǒng)包含控制器模塊、姿態(tài)采集模塊、電機(jī)驅(qū)動(dòng)模塊、電機(jī)測(cè)速模塊和電源轉(zhuǎn)換模塊，如圖3所示.

2.2.1 STM32控制器

本系統(tǒng)采用STM32F103RCT6單片機(jī)為主控芯片，它的內(nèi)核是ARM 32位的CortexTM-M3，最高工作頻率為72 MHz，工作電壓為2.0～3.6 V，具有256 K字節(jié)的閃存程序存儲(chǔ)器，可利用外置的紐扣電池實(shí)現(xiàn)掉電數(shù)據(jù)的保存，支持JTAG和SWD調(diào)試，含有6個(gè)可產(chǎn)生PWM波的定時(shí)器，其中的定時(shí)器1和定時(shí)器8為高級(jí)定時(shí)器.

圖4 STM32控制器接口電路Fig.4 Interface circuit of STM32 controller

控制器接口電路如圖4所示，在PD0和PD1連接8 MHz的外部晶振，由于電機(jī)為三相直流無(wú)刷電機(jī)，控制器針對(duì)每一個(gè)電機(jī)需要連接6個(gè)端口，如滾動(dòng)電機(jī)接口為YawA，YawB，YawC，YawAN，YawBN，YawCN，3個(gè)電機(jī)測(cè)速接口分別為VPitch，VYaw，VRoll.

具體控制過(guò)程如下：STM32控制器實(shí)時(shí)接收MPU6050測(cè)得的姿態(tài)信息，同時(shí)進(jìn)行解析運(yùn)算，通過(guò)控制算法輸出PWM控制信號(hào)，通過(guò)電機(jī)驅(qū)動(dòng)調(diào)整平臺(tái)姿態(tài)，同時(shí)把電機(jī)轉(zhuǎn)速反饋給控制器，進(jìn)一步修正輸出信號(hào)，最終實(shí)現(xiàn)跟蹤平臺(tái)的穩(wěn)定.

2.2.2 姿態(tài)采集模塊

姿態(tài)采集模塊以MPU6050為核心元件，它集成了3軸MEMS陀螺儀和3軸MEMS加速度計(jì)，自帶數(shù)字運(yùn)動(dòng)處理功能，減少了MCU復(fù)雜的融合演算數(shù)據(jù)、感測(cè)器同步化、姿勢(shì)感應(yīng)等負(fù)荷，可以直接從DMP寄存器中讀出處理結(jié)果，具有400 kHz快速模式的I2C接口，角速度全格感測(cè)為-250°～250°/s，-500°～500°/s，-1 000°～1 000°/s，-2 000°～2 000°/s，其三維檢測(cè)軸及旋轉(zhuǎn)方向如圖5所示.

MPU6050姿態(tài)采集電路如圖6所示，芯片檢測(cè)到的3個(gè)軸向的姿態(tài)信息通過(guò)I2C串口傳遞給控制器.

圖5 MPU6050三維檢測(cè)軸及旋轉(zhuǎn)方向Fig.5 Three-dimensional detection of axis and the direction of rotation

圖6 MPU6050電路Fig.6 Schematic circuit diagram of MPU6050

2.2.3 電機(jī)驅(qū)動(dòng)模塊

圖7 X軸俯視電機(jī)的A相驅(qū)動(dòng)電路Fig.7 A phase driver circuit of X-axis overlook motor

由于系統(tǒng)機(jī)構(gòu)的特殊性，3個(gè)軸向旋轉(zhuǎn)電機(jī)均選用空心軸無(wú)刷直流電機(jī)，考慮到X軸的仰視電機(jī)在平臺(tái)最底端，需要相對(duì)較大的力矩，所以X軸向電機(jī)選用2804型號(hào)的無(wú)刷直流電機(jī)，其他兩個(gè)選用2212型號(hào)的電機(jī).電機(jī)驅(qū)動(dòng)模塊均選用SI4599型號(hào)芯片，由于電機(jī)均為三相電機(jī)，針對(duì)一個(gè)電機(jī)的三相分別使用3個(gè)SI4599型號(hào)芯片進(jìn)行驅(qū)動(dòng).為了驅(qū)動(dòng)3個(gè)無(wú)刷電機(jī)，共需要9個(gè)SI4599芯片，以X軸俯視電機(jī)的A相驅(qū)動(dòng)電路如圖7所示.

當(dāng)PitchA為高電平、PitchAN為低電平時(shí)，PitchAout輸出12 V的高電平；當(dāng)PitchA為低電平、PitchAN為高電平時(shí)，PitchAout輸出0 V的低電平.通過(guò)芯片SI4599的轉(zhuǎn)換，0～3 V的方波轉(zhuǎn)換為0～12 V的方波，實(shí)現(xiàn)了驅(qū)動(dòng)能力的增強(qiáng).

2.2.4 電機(jī)測(cè)速模塊

為了實(shí)現(xiàn)跟蹤平臺(tái)的閉環(huán)控制，在電機(jī)轉(zhuǎn)動(dòng)機(jī)構(gòu)上安裝小型磁鋼，采用型號(hào)為A3144的霍爾傳感器感應(yīng)磁鋼的轉(zhuǎn)動(dòng)信息，實(shí)現(xiàn)電機(jī)轉(zhuǎn)速的監(jiān)測(cè).由于霍爾傳感器輸出的信號(hào)為正弦信號(hào)，需要通過(guò)LM393電壓比較器整形為方波信號(hào)，具體電路如圖8所示.

2.2.5 電源供電模塊

為保證跟蹤平臺(tái)的精確穩(wěn)定性，對(duì)控制系統(tǒng)供電電源的質(zhì)量要求較高.因此,針對(duì)供電電源進(jìn)行穩(wěn)壓處理，系統(tǒng)電源模塊采用3S電池12 V供電，經(jīng)過(guò)78D33轉(zhuǎn)換3.3 V為控制電路供電，另一路經(jīng)過(guò)IRF9328為電機(jī)驅(qū)動(dòng)電路提供12 V的電源，穩(wěn)壓電路如圖9所示.

圖8 霍爾測(cè)速原理Fig.8 Hall speed principle diagram

圖9 電源整定模塊Fig.9 The principle diagram of power setting module

圖10 基于PID算法的穩(wěn)定跟蹤平臺(tái)Fig.10 The principle diagram of power setting module

3 基于PID算法的穩(wěn)定跟蹤平臺(tái)

跟蹤穩(wěn)定控制平臺(tái)采用PID算法進(jìn)行跟蹤控制，控制原理如圖10所示.

u(t)為控制器的輸出(即為電機(jī)驅(qū)動(dòng)信號(hào))，e(t)為控制器的輸入，它跟蹤給定值r(t)與穩(wěn)定平臺(tái)姿態(tài)傳感器檢測(cè)信號(hào)y(t)的偏差，即

e(t)=r(t)-y(t) .

(1)

PID的控制量u(t)是由e(t)的比例(P)、積分(I)、微分(D)通過(guò)一系列的線(xiàn)性組合而成，即

(2)

其傳遞函數(shù)為

(3)

式中：KP為比例系數(shù)；Ti為積分時(shí)間常數(shù)；Td為微分時(shí)間常數(shù).

4 仿真分析與實(shí)驗(yàn)驗(yàn)證

控制系統(tǒng)分別針對(duì)X軸、Y軸、Z軸采用PID閉環(huán)控制算法，PID控制參數(shù)選擇的原則為系統(tǒng)能迅速、準(zhǔn)確、穩(wěn)定地跟蹤平臺(tái)的姿態(tài)變化，利用Ziegler-Nichols方法確定KP，Ti，Td的初始值.由于針對(duì)X軸、Y軸、Z軸的控制算法類(lèi)似，故只闡述針對(duì)平臺(tái)X軸(俯仰角)的控制仿真過(guò)程.設(shè)定穩(wěn)定俯仰角為1°，采樣時(shí)間設(shè)置為1 ms，經(jīng)過(guò)不斷調(diào)整，最終選定KP=3.1，Ti=0.23，Td=0.08.為了測(cè)試系統(tǒng)的抗干擾能力，在第800個(gè)采樣時(shí)間加入一個(gè)隨機(jī)干擾信號(hào)，仿真測(cè)試如圖11和圖12所示.

圖11 無(wú)干擾狀態(tài)下的PID控制系統(tǒng)Fig.11 The PID control system without interference

圖12 有干擾狀態(tài)下的PID控制系統(tǒng)Fig.12 The PID control system with interference

圖13 電子穩(wěn)定平臺(tái)Fig.13 Photos of electronic stability platform

圖11為在無(wú)干擾狀態(tài)下的仿真結(jié)果.當(dāng)姿態(tài)發(fā)生偏離時(shí)，系統(tǒng)只需要0.3 s就趨于穩(wěn)定.根據(jù)有干擾狀態(tài)下的仿真結(jié)果(見(jiàn)圖12)可知，系統(tǒng)處于穩(wěn)定狀態(tài)時(shí)，突然出現(xiàn)一個(gè)外界擾動(dòng)信號(hào)，系能夠統(tǒng)很快恢復(fù)穩(wěn)定.由此可見(jiàn)，控制系統(tǒng)的抗干擾能力強(qiáng).

依據(jù)上述硬件及軟件設(shè)計(jì)，搭建了電子穩(wěn)定跟蹤平臺(tái)，如圖13所示.經(jīng)過(guò)實(shí)物測(cè)試，穩(wěn)定平臺(tái)系統(tǒng)轉(zhuǎn)向角(Z軸)的修正量為0°～360°、橫滾角(Y軸)和俯仰角(X軸)的修正量為±45°；轉(zhuǎn)向角的測(cè)量精度為1°，俯仰角和橫滾角為0.1°，機(jī)械控制精度優(yōu)于0.1°，快速響應(yīng)時(shí)間小于10 ms.

5 結(jié)語(yǔ)

以STM32微處理器為核心，設(shè)計(jì)了一個(gè)電子機(jī)械穩(wěn)定跟蹤平臺(tái)，針對(duì)系統(tǒng)構(gòu)架、各重要模塊的硬件和軟件設(shè)計(jì)進(jìn)行了詳細(xì)的闡述，給出了PID姿態(tài)控制算法.然后，通過(guò)軟件仿真及試驗(yàn)測(cè)試，驗(yàn)證了本設(shè)計(jì)的可行性，為專(zhuān)用航空精密穩(wěn)定跟蹤平臺(tái)的開(kāi)發(fā)提供了重要的參考.

[1] 王平,張國(guó)玉,劉家燕，等.機(jī)載光電平臺(tái)內(nèi)框架拓?fù)鋬?yōu)化設(shè)計(jì)[J].機(jī)械工程學(xué)報(bào),2014,50(13):135-141.

[2] 魏偉,戴明,李嘉全，等.航空光電穩(wěn)定平臺(tái)的自抗擾控制系統(tǒng)[J].光學(xué)精密工程,2015,23(8):2296-2304.

[3] 周向陽(yáng)，賈媛.航空遙感慣性穩(wěn)定平臺(tái)模糊/PID復(fù)合控制[J].儀器儀表學(xué)報(bào),2016,37(11):2545-2554.

[4] 侯丹丹，王樂(lè)勇，毛武軍，等.航空相機(jī)穩(wěn)定平臺(tái)系統(tǒng)設(shè)計(jì)[J].機(jī)電技術(shù),2013(2):44-47.

[5] 張強(qiáng),吳云東,張超.低空遙感小型三軸陀螺穩(wěn)定平臺(tái)的設(shè)計(jì)與實(shí)現(xiàn)[J].測(cè)繪科學(xué)技術(shù)學(xué)報(bào),2012,29(4):276-284.

[6] 李全超，譚淞年，李蕾，等.某紅外相機(jī)穩(wěn)定平臺(tái)框架結(jié)構(gòu)設(shè)計(jì)與分析[J].紅外技術(shù),2016,38(9):728-732.

[7] 朱倚嫻,陸源,許江寧.一種陀螺穩(wěn)定平臺(tái)自適應(yīng)模糊-PID復(fù)合控制方法[J].中國(guó)慣性技術(shù)學(xué)報(bào),2014,22(3):317-321.

[8] 李賢濤,張葆,孫敬輝，等.航空光電穩(wěn)定平臺(tái)擾動(dòng)頻率自適應(yīng)的自抗擾控制[J].紅外與激光工程,2014,43(5):1574-1581.

Design of electron-mechanism stable platform based on MPU6050

HUANG Quanzhen, ZHANG Ning, LIN Weipeng, SUN Qingyuan，CHEN Suxia

(SchoolofElectricalInformationEngineering,HenanUniversityofEngineering,Zhengzhou451191,China)

The stable platform has been widely used in many fields with the characteristics of isolation carrier perturbation, and keeps the dynamic attitude standard from time to time. In this paper, a 3-DOF electro-mechanical stability tracking platform has been designed by the MPU-6050 attitude sensor. Using the attitude change of the MPU-6050 monitor platform, the attitude controller outputs the control signals according to the control algorithm, which is used to control the three motors turning and rotating speed. The motor drives the mechanical transmission mechanism to realize the stable platform in theX,YandZaxis. In order to achieve stable stability of the platform. The simulation and experimental results show that the control effect of the stable platform is good, which provides an important reference value for the development of precise and stable tracking platform for aviation.

stabilization and tracking platform；attitude sensor；PID control algorithm

2016-10-11

國(guó)家自然科學(xué)基金(61403123)；河南省高?？萍紕?chuàng)新人才支持計(jì)劃(17HASTIT020)；河南省科技攻關(guān)項(xiàng)目(162102210099, 172102210205)；河南省高等學(xué)校青年骨干教師培養(yǎng)計(jì)劃(2016GGJS153)

黃全振(1979-)，男，河南駐馬店人,副教授，博士，主要從事先進(jìn)智能控制方面的研究.

TP237

A

1674-330X(2017)02-0046-05