基于MPU6050和步進電機的高精度轉動控制方法

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

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

關鍵詞：?轉動控制;步進電機;MPU6050傳感器;閉環控制

中圖分類號： 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??引言

隨著科技的不斷發展與人類社會的進步，姿態檢測系統在人工智能、車輛船舶、航空航天等領域越來越多地被運用，姿態數據成為其中最重要的參數之一。MPU6050由于其體積小巧，功能強大，精度較高，得以被廣泛應用。同時，兩相混合式步進電機由于結構簡單，運行可靠性高，驅動器成本低等的特點，仍廣泛應用于工業和自動化領域[1]。

本文的目的在于將MPU6050姿態傳感器和步進電機相結合，設計一種高精度的轉動控制方法，實現在動態環境中完成目標的快速穩定和定位。通過搭建一個以自由擺運動為模型的目標平衡定位系統，以STM32F407ZGT6為控制核心，利用MPU6050傳感器采集擺桿和控制目標的姿態數據，步進電機執行控制信號，構成閉環控制系統，驗證了基于MPU6050和步進電機的轉動控制方法具有精度高、響應快的特征。

1??硬件設計

1.1??系統方案

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

1.2 ?角度測量模塊

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

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

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

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

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

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

（2）將MPU6050輸出的姿態數據作為插值點;

（3）將擬合后的數據作為新的姿態數據，提高精確度。

通過擬合可以使姿態數據更加平穩、更加接近真實值，消除電機抖動造成的運行不穩定。角度數據經擬合后的臺監控如圖4所示。

1.3 ?電機控制模塊

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

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

2 ?系統軟件設計

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

2.1??主程序模塊

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

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

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

3 ?轉動控制系統目標平衡性能研究

3.1 ?理論分析

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

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

3.2??實驗驗證

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

4??轉動控制系統目標定位性能研究

4.1??理論分析

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

4.2 ?實驗驗證

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

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

5??結束語

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

參考文獻

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

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

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

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

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

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

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

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