基于MPU6050和步進(jìn)電機(jī)的高精度轉(zhuǎn)動(dòng)控制方法

徐夏怡　宮瑤　李玉衡　姚磊　易映萍

摘 ?要： 本文設(shè)計(jì)了一種以STM32F407ZGT6為控制核心的高精度轉(zhuǎn)動(dòng)控制系統(tǒng)，該系統(tǒng)采用MPU6050姿態(tài)傳感器作為信號(hào)采集元件，通過(guò)數(shù)字運(yùn)動(dòng)處理器（DMP）對(duì)原始數(shù)據(jù)進(jìn)行四元數(shù)解算得到準(zhǔn)確、可靠的姿態(tài)信息，并以?xún)上嗷旌鲜讲竭M(jìn)電機(jī)作為執(zhí)行元件，結(jié)合增量式閉環(huán)控制算法，實(shí)現(xiàn)高精度的轉(zhuǎn)動(dòng)控制。通過(guò)搭建轉(zhuǎn)動(dòng)控制系統(tǒng)的實(shí)驗(yàn)平臺(tái)，驗(yàn)證了該控制方法的可行性。該方法對(duì)其他需要姿態(tài)檢測(cè)和旋轉(zhuǎn)控制的嵌入式系統(tǒng)提供了一定的參考價(jià)值。

關(guān)鍵詞：?轉(zhuǎn)動(dòng)控制;步進(jìn)電機(jī);MPU6050傳感器;閉環(huán)控制

中圖分類(lèi)號(hào)： TP273+.3????文獻(xiàn)標(biāo)識(shí)碼：?A????DOI：10.3969/j.issn.1003-6970.2020.02.008

【Abstract】： This paper designs a high precision rotation control system with STM32F407ZGT6 as the control core. The system uses MPU6050 gesture sensor as the signal acquisition element， calculates the quaternion of the original data to obtain accurate and reliable attitude information by digital motion processor （DMP）， and uses two-phase hybrid stepping motor as the executive element. Combined with incremental closed-loop control algorithm， the system realizes high-precision rotation control. The feasibility of the control method is verified by setting up an experimental platform of the rotation control system. This method provides some reference value for other embedded systems which need gesture detection and rotation control.

【Key words】： Rotation control; Stepping motor; MPU6050 sensor; Closed-loop control

0??引言

隨著科技的不斷發(fā)展與人類(lèi)社會(huì)的進(jìn)步，姿態(tài)檢測(cè)系統(tǒng)在人工智能、車(chē)輛船舶、航空航天等領(lǐng)域越來(lái)越多地被運(yùn)用，姿態(tài)數(shù)據(jù)成為其中最重要的參數(shù)之一。MPU6050由于其體積小巧，功能強(qiáng)大，精度較高，得以被廣泛應(yīng)用。同時(shí)，兩相混合式步進(jìn)電機(jī)由于結(jié)構(gòu)簡(jiǎn)單，運(yùn)行可靠性高，驅(qū)動(dòng)器成本低等的特點(diǎn)，仍廣泛應(yīng)用于工業(yè)和自動(dòng)化領(lǐng)域[1]。

本文的目的在于將MPU6050姿態(tài)傳感器和步進(jìn)電機(jī)相結(jié)合，設(shè)計(jì)一種高精度的轉(zhuǎn)動(dòng)控制方法，實(shí)現(xiàn)在動(dòng)態(tài)環(huán)境中完成目標(biāo)的快速穩(wěn)定和定位。通過(guò)搭建一個(gè)以自由擺運(yùn)動(dòng)為模型的目標(biāo)平衡定位系統(tǒng)，以STM32F407ZGT6為控制核心，利用MPU6050傳感器采集擺桿和控制目標(biāo)的姿態(tài)數(shù)據(jù)，步進(jìn)電機(jī)執(zhí)行控制信號(hào)，構(gòu)成閉環(huán)控制系統(tǒng)，驗(yàn)證了基于MPU6050和步進(jìn)電機(jī)的轉(zhuǎn)動(dòng)控制方法具有精度高、響應(yīng)快的特征。

1??硬件設(shè)計(jì)

1.1??系統(tǒng)方案

系統(tǒng)硬件結(jié)構(gòu)框圖如圖1所示，該系統(tǒng)由STM32F407ZGT6控制模塊，按鍵檢測(cè)模塊、顯示模塊、步進(jìn)電機(jī)控制模塊、角度檢測(cè)模塊以及電源模塊所組成。將兩塊MPU6050姿態(tài)傳感器分別安裝在擺桿和可旋轉(zhuǎn)平臺(tái)上，實(shí)時(shí)監(jiān)測(cè)擺桿和豎直面之間的夾角以及可旋轉(zhuǎn)平臺(tái)與水平面之間的夾角，數(shù)據(jù)通過(guò)模擬IIC通道傳輸給控制器，控制器經(jīng)過(guò)控制算法處理，產(chǎn)生相應(yīng)的控制量，將控制信號(hào)通過(guò)串口傳送給執(zhí)行器，配以能16細(xì)分的TB6600為電機(jī)驅(qū)動(dòng)芯片，使步進(jìn)電機(jī)以一定的速度轉(zhuǎn)過(guò)對(duì)應(yīng)的角度。

1.2 ?角度測(cè)量模塊

對(duì)于電機(jī)控制來(lái)說(shuō)，對(duì)位置、角速的檢測(cè)尤其重要，傳感器的誤差限制了位置和速度的穩(wěn)定精度。一個(gè)運(yùn)動(dòng)控制系統(tǒng)的整體性能，在很大程度上取決于用來(lái)產(chǎn)生反饋信號(hào)的傳感器類(lèi)型及其質(zhì)量。

因此本系統(tǒng)中的角度測(cè)量模塊選用的是MPU-?6050，MPU6050是InvenSense公司的一款高性能三軸加速度+三軸陀螺儀的姿態(tài)傳感器芯片。該處理器的角速度感測(cè)范圍為±250、±500、±1000與±2000°/s，加速度感測(cè)范圍為±2?g、±4?g、±8?g與±16g?，感測(cè)范圍可通過(guò)編程來(lái)控制[2]。MPU-6050模塊的電路如圖2所示。

本系統(tǒng)中MPU-6050使用SDA（數(shù)據(jù)線）和SCL（時(shí)鐘線）端口和主機(jī)STM32進(jìn)行通信，讀取MPU6050各軸的角速度分量、加速度分量后，利用自帶的數(shù)字運(yùn)動(dòng)處理器（DMP： Digital Motion Processor）硬件加速引擎，輸出四元數(shù)，然后采用q30格式，放大到2的30次方 [3]輸出姿態(tài)解算后的數(shù)據(jù)，測(cè)量精度為0.01度，然后通過(guò)內(nèi)置的16位AD轉(zhuǎn)換器進(jìn)行轉(zhuǎn)換發(fā)送給主機(jī)。

以自由擺為模型，將MPU6050固定在擺桿上，沿?cái)[動(dòng)方向上得到一個(gè)姿態(tài)角，該角度為擺桿與豎直面之間的夾角。對(duì)該角度的后臺(tái)監(jiān)控?cái)?shù)據(jù)如圖3所示。

因?yàn)橛蒑PU6050采集的角度數(shù)據(jù)中有部分突變，若不經(jīng)處理可能會(huì)導(dǎo)致步進(jìn)電機(jī)執(zhí)行的不穩(wěn)定，因此經(jīng)過(guò)多次試驗(yàn)，密集采集1000組數(shù)據(jù)，利用擬合方法實(shí)現(xiàn)數(shù)據(jù)精度提高，步驟如下：

（1）選擇二次多項(xiàng)式作為擬合函數(shù);

（2）將MPU6050輸出的姿態(tài)數(shù)據(jù)作為插值點(diǎn);

（3）將擬合后的數(shù)據(jù)作為新的姿態(tài)數(shù)據(jù)，提高精確度。

通過(guò)擬合可以使姿態(tài)數(shù)據(jù)更加平穩(wěn)、更加接近真實(shí)值，消除電機(jī)抖動(dòng)造成的運(yùn)行不穩(wěn)定。角度數(shù)據(jù)經(jīng)擬合后的臺(tái)監(jiān)控如圖4所示。

1.3 ?電機(jī)控制模塊

本系統(tǒng)采用二相四線步進(jìn)電機(jī)和TB6600步進(jìn)電機(jī)專(zhuān)用驅(qū)動(dòng)器。步進(jìn)電機(jī)是一種能將脈沖信號(hào)轉(zhuǎn)換成角位移或線性位移的執(zhí)行器件，廣泛應(yīng)用在數(shù)控設(shè)備中[4]。步進(jìn)電機(jī)的運(yùn)動(dòng)方式是，每收到一個(gè)脈沖，就旋轉(zhuǎn)指定的角度，因此在步進(jìn)電機(jī)控制系統(tǒng)中可以通過(guò)輸人PWM波的方法來(lái)對(duì)步進(jìn)電動(dòng)的運(yùn)動(dòng)進(jìn)行控制。PWM波的產(chǎn)生可以通過(guò)時(shí)鐘頻率、自動(dòng)重裝數(shù)值等參數(shù)進(jìn)行設(shè)置，從而調(diào)節(jié)PWM波的占空比和輸出頻率。TB6600步進(jìn)電機(jī)驅(qū)動(dòng)器是一款專(zhuān)業(yè)的兩相步進(jìn)電機(jī)驅(qū)動(dòng)，可實(shí)現(xiàn)正反轉(zhuǎn)控制。通過(guò)S1、S2、S3這三個(gè)撥碼開(kāi)關(guān)選擇細(xì)分控制（1、2、4、8、16），采用步進(jìn)電機(jī)作為執(zhí)行機(jī)構(gòu)，配以能16細(xì)分的TB6600為電機(jī)驅(qū)動(dòng)芯片，實(shí)現(xiàn)0.1125°步進(jìn)角。

自由擺平板控制系統(tǒng)由MPU6050傳感器采集擺桿偏離豎直面的角度，根據(jù)算法程序得到電機(jī)需要轉(zhuǎn)動(dòng)的方向和角度，并產(chǎn)生控制量使旋轉(zhuǎn)平臺(tái)達(dá)到平衡或定位的目標(biāo)，但到電機(jī)接收到PWM信號(hào)之間有一定的時(shí)間差，從而會(huì)形成步丟?[5]，導(dǎo)致誤差的產(chǎn)生。為了提高電機(jī)控制精度，需要采用閉環(huán)控制。在此，需要采集轉(zhuǎn)動(dòng)后的平臺(tái)的實(shí)時(shí)角度，再將該角度值反饋到控制器進(jìn)行比較，形成閉環(huán)控制。系統(tǒng)閉環(huán)控制框圖如圖5所示。

2 ?系統(tǒng)軟件設(shè)計(jì)

系統(tǒng)采取模塊化設(shè)計(jì)[6]，整個(gè)系統(tǒng)分為若干模塊，最頂層模塊為主程序模塊，其次為兩個(gè)功能模塊：目標(biāo)平衡模塊、目標(biāo)定位模塊。

2.1??主程序模塊

主程序流程圖如圖6所示，當(dāng)自由擺平板系統(tǒng)接通電源時(shí)，首先對(duì)STM32F407ZGT6進(jìn)行初始化，接著通過(guò)按鍵檢測(cè)進(jìn)入各個(gè)功能模塊。

2.2??目標(biāo)平衡和定位功能模塊

進(jìn)入功能模塊后，采集擺桿和豎直面間的角度，判斷電機(jī)轉(zhuǎn)動(dòng)方向，并通過(guò)程序算法，輸出相應(yīng)的PWM信號(hào)，控制步進(jìn)電機(jī)轉(zhuǎn)動(dòng)，完成步進(jìn)后，檢測(cè)旋轉(zhuǎn)平臺(tái)與水平面的角度作為反饋量，計(jì)算反饋值與理論值之間的差，對(duì)其進(jìn)行補(bǔ)償，構(gòu)成一個(gè)閉環(huán)控制系統(tǒng)。具體功能模塊流程圖如圖7所示。

3 ?轉(zhuǎn)動(dòng)控制系統(tǒng)目標(biāo)平衡性能研究

3.1 ?理論分析

在這個(gè)以自由擺運(yùn)動(dòng)為模型的目標(biāo)平衡定位系統(tǒng)中，為了測(cè)試平衡性能，在擺桿頂部的隨步進(jìn)電機(jī)旋轉(zhuǎn)的平臺(tái)上放置一物體，使該物體在擺桿擺動(dòng)的時(shí)候不會(huì)掉落?，F(xiàn)對(duì)該物體做受力分析，其受到平板對(duì)其的支持力和與平板之間的摩擦力及自身重力。

但步進(jìn)電機(jī)再進(jìn)行角度較小的轉(zhuǎn)動(dòng)時(shí)，存在抖動(dòng)，反而不利于物體的穩(wěn)定，通過(guò)實(shí)驗(yàn)和估算，令α始終等于啟動(dòng)時(shí)β的大小，即在啟動(dòng)后平板迅速轉(zhuǎn)為與桿垂直，物體能夠停留在平板上不會(huì)掉落。由此得到方案：放下擺桿后，電機(jī)快速轉(zhuǎn)動(dòng)使平板與擺桿垂直，此后電機(jī)不再轉(zhuǎn)動(dòng)。我們采用的是二相四線的步進(jìn)電機(jī)的固有步距角為1.8度，選用細(xì)分?jǐn)?shù)為16，所以步距角為1.8°/16=0.1125°，因此步進(jìn)電機(jī)轉(zhuǎn)動(dòng)的脈沖個(gè)數(shù)為n=β/0.1125°，擺桿在右側(cè)，則順時(shí)針轉(zhuǎn)，反之，則逆時(shí)針。

3.2??實(shí)驗(yàn)驗(yàn)證

在旋轉(zhuǎn)平臺(tái)上貼一張白紙，標(biāo)上刻度，單位為cm，用來(lái)測(cè)量物體在擺桿擺動(dòng)過(guò)程中，移動(dòng)的距離。現(xiàn)在平臺(tái)放置一個(gè)物體，分別從30°、40°、50°瞬時(shí)放開(kāi)擺桿讓其自由下落擺動(dòng)，實(shí)驗(yàn)結(jié)果如表1所示，實(shí)驗(yàn)結(jié)果表明轉(zhuǎn)動(dòng)控制系統(tǒng)平衡性能良好。

4??轉(zhuǎn)動(dòng)控制系統(tǒng)目標(biāo)定位性能研究

4.1??理論分析

在這個(gè)以自由擺運(yùn)動(dòng)為模型的目標(biāo)平衡定位系統(tǒng)中，為了測(cè)試目標(biāo)帶激光筆定位性能，將激光筆固定在旋轉(zhuǎn)平臺(tái)上，在距離擺桿1.5?m以外的地方放置一個(gè)靶子，以擺桿豎直靜止時(shí)激光筆照射的位置在靶子上畫(huà)一條水平線，以該線作為中心線，在擺桿擺動(dòng)的過(guò)程中，讓光能夠照靶子上，且光點(diǎn)始終能夠位于靶子的中心線上。根據(jù)自由擺擺動(dòng)時(shí)平板的運(yùn)動(dòng)狀態(tài)，可以建立激光筆定位模型圖，其示意圖如圖9所示。

4.2 ?實(shí)驗(yàn)驗(yàn)證

在距離自由擺1.5?m左右的距離上放置一塊站立的木板。木板上貼有白紙，其上標(biāo)有刻度值，單位為cm，用來(lái)測(cè)量激光筆所指向的位置與中心線的誤差。分別從30°、40°、50°瞬時(shí)放開(kāi)擺桿讓其自由下落擺動(dòng)，在所有測(cè)量過(guò)程中光電距離中心線的最大偏差量為0.8?cm，驗(yàn)證結(jié)果如表2所示，實(shí)驗(yàn)結(jié)果表明轉(zhuǎn)動(dòng)控制系統(tǒng)定位性能良好。

經(jīng)過(guò)測(cè)試，MPU6050傳感器與二相四線步進(jìn)電機(jī)測(cè)試精度達(dá)到設(shè)計(jì)要求，系統(tǒng)可完成精準(zhǔn)控制，達(dá)到設(shè)計(jì)目標(biāo)。

5??結(jié)束語(yǔ)

本文主要從硬件、軟件、算法三個(gè)方面詳細(xì)介紹了一種基于MPU6050和步進(jìn)電機(jī)的高精度轉(zhuǎn)動(dòng)控制方法，通過(guò)擬合和參數(shù)整定，使采集的姿態(tài)數(shù)據(jù)更加精確，通過(guò)閉環(huán)控制提高步進(jìn)電機(jī)的控制精度，在理論分析的基礎(chǔ)上搭建了以自由擺運(yùn)動(dòng)為模型的目標(biāo)平衡定位系統(tǒng)，實(shí)驗(yàn)結(jié)果表明本文所設(shè)計(jì)的控制方法能使轉(zhuǎn)動(dòng)控制系統(tǒng)平衡和定位性能良好，具有一定的工程利用價(jià)值。

參考文獻(xiàn)

楊海光， 劉文怡， 朱思敏， 等. 步進(jìn)電機(jī)對(duì)測(cè)量角度的定位與控制[J].?自動(dòng)化儀表， 2013（02）： 40-46

王素青， 熊維堂. 基于STM32的兩輪自平衡小車(chē)系統(tǒng)設(shè)計(jì)[J]. 實(shí)驗(yàn)室研究與探索， 2016， 35（05）： 146-150.

黃金鵬， 尚俊娜， 岳克強(qiáng). 基于ARM與MPU6050的測(cè)姿系統(tǒng)設(shè)計(jì)[J]. 傳感器與微系統(tǒng)， 2018， 37（4）： 101-103.

徐煜明. 步進(jìn)電機(jī)速度控制的研究與實(shí)現(xiàn)[J].?工礦自動(dòng)化，?2007（02）： 82-85.

王妮. 基于PLC的步進(jìn)電機(jī)誤差修正方案設(shè)計(jì)[J]. 數(shù)字技術(shù)與應(yīng)用， 2012（09）： 168.

慶燕飛. 試論模塊化設(shè)計(jì)方法及其在機(jī)械設(shè)計(jì)中的運(yùn)用[J]. 南方農(nóng)機(jī)， 2019，?50（15）： 121.

居津， 袁海泉. 關(guān)于單擺加速度與擺角關(guān)系的探究[J]. 物理通報(bào)， 2013（01）： 93-96.

劉丹， 鄭賓， 劉乃強(qiáng)， 等. 自由擺平衡控制方法研究[J]. 電子世界， 2014（18）： 356-357.