基于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.