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

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

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

中圖分類號： TP273+.3????文獻標識碼：?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ā)展與人類社會的進步，姿態(tài)檢測系統(tǒng)在人工智能、車輛船舶、航空航天等領(lǐng)域越來越多地被運用，姿態(tài)數(shù)據(jù)成為其中最重要的參數(shù)之一。MPU6050由于其體積小巧，功能強大，精度較高，得以被廣泛應(yīng)用。同時，兩相混合式步進電機由于結(jié)構(gòu)簡單，運行可靠性高，驅(qū)動器成本低等的特點，仍廣泛應(yīng)用于工業(yè)和自動化領(lǐng)域[1]。

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

1??硬件設(shè)計

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

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

1.2 ?角度測量模塊

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

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

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

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

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

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

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

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

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

1.3 ?電機控制模塊

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

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

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

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

2.1??主程序模塊

主程序流程圖如圖6所示，當自由擺平板系統(tǒng)接通電源時，首先對STM32F407ZGT6進行初始化，接著通過按鍵檢測進入各個功能模塊。

2.2??目標平衡和定位功能模塊

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

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

3.1 ?理論分析

在這個以自由擺運動為模型的目標平衡定位系統(tǒng)中，為了測試平衡性能，在擺桿頂部的隨步進電機旋轉(zhuǎn)的平臺上放置一物體，使該物體在擺桿擺動的時候不會掉落。現(xiàn)對該物體做受力分析，其受到平板對其的支持力和與平板之間的摩擦力及自身重力。

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

3.2??實驗驗證

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

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

4.1??理論分析

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

4.2 ?實驗驗證

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

經(jīng)過測試，MPU6050傳感器與二相四線步進電機測試精度達到設(shè)計要求，系統(tǒng)可完成精準控制，達到設(shè)計目標。

5??結(jié)束語

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

參考文獻

楊海光， 劉文怡， 朱思敏， 等. 步進電機對測量角度的定位與控制[J].?自動化儀表， 2013（02）： 40-46

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

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

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

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

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

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

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