打开文本图片集
摘要 本文研究了基于STM32微控制器的平衡运输小车系统的设计问题，完成了基于STM32微控制器和MPU6050的控制系统的硬件设计;通过离散化PID算法软件实现了对系统的闭环控制。同时利用了DMP滤波器提高了反馈信号的采集精度，提高了系统的稳定性。通过系统分析和实验调试确定了系统的各项参数。实验表明，在输入扰动信号时，小车的承载托盘可以稳定在水平位置，其并且有较好的快速性和准确性。
【关键词】平衡运输小车 STM32 微控制器MPU6050 闭环控制 DMP滤波器
随着现代社会自动化水平的提高，在许多现代化工作场所中，例如生产车间、快递分拣站等，越来越多的智能平衡运输小车和机器人替代了人工搬运，作为运输易碎易损坏零件和易碎物品的运输工具，提高了生产效率，使得智能平衡运输机械成为了现代化工厂的重要标志。目前，对于平衡运输小车的研究主要集中于托盘状态的监测和对轻微振动的消除，所设计出的小车在实际控制中托盘的角度常常存在稳态误差，信号的处理上也存在噪声，使得物品容易掉落。所以，本文将针对平衡运输小车中的控制算法的设计和信号的采集和滤波进行深入研究，以stm32微控制器为核心，使用包含MPU6050和DMP滤波器的采集系统作为反馈角度信号，通过离散化PID控制算法对小车托盘的角度进行闭环控制，解决了目前平衡运输系统所存在的稳态误差和噪声问题，使系统满足其控制要求。所设计出的平衡运输小车具有结构简单，动态性能良好，为平衡运输系统的设计提供了低成本的可行性方案。
1 系统的建立
1.1 机械结构的搭建
1.1.1 运输承载结构的搭建
小车的承载结构由三部分组成。
（1）托盘部分：托盘为20cm*20cm的正方形3D打印模型，分别在托盘的几何中心、x轴负半轴距原点5cm处，y轴正半轴距原点5cm处作为万向节的固定点，托盘的俯视图如图1所示。
（2）连接及传动结构部分：在托盘的几何中心处固定的万向节用于连接托盘和小车平台，在x轴和y轴处的万向节用于连接标准舵机的力臂，使得标准舵机1和标准舵机2的力臂在转动的同时可以分别带动托盘绕x轴和y轴转动，设转动的角度分别为pitch和roll。
（3）标准舵机部分：用于承载托盘的标准舵机采用的是MG995标准金属舵机，5.OV-6.OV、2A供电。标准舵机1的力臂摆动的平面应和托盘x轴所在直线在竖直方向上保持重合，如图2所示。这样的摆放位置使得力臂在摆动的过程中只会改变托盘绕x轴方向的角度pitch;标准舵机2同标准舵机1的原理相同，故不再赘述。
将上述三部分依次安装好，得到承载结构的3D模型，如图3所示。
1.1.2 小车驱动机构的选择
小车使用四轮驱动，每个车轮都搭载一个扭矩为7.5KG/cm的直流圆周舵机作为车轮的驱动。
1.2 电源及驱动电路的选择
1.2.1 电源部分
由于本系统中存在许多舵机，要求电源的放电电压电流要足够大，所以小车的电源采用的是3S航模电池组供电，电池组满电电压为12.6V，容量为11OOmAh 25C，最大放电电流为27.5A，满足了本系统的供电要求。
1.2.2 降压稳压电路部分
该部分共分为两部分。采用LM317可调降压芯片构成，它可将由系统电源输出的12.6V电压降为SV输出，为所有舵机供电;第二部分采用LM1117降压芯片构成，它可将由LM317输出的5V电压降为3.3V电压输出，为微控制器及外围器件供电。
1.3 微控制器及外围器件的选择
本系统的微控制器采用的是意法半导体公司生产的stm32f103c8t6微控制器，它的主频为72MHz，处理速度快，对反馈的响应迅速，能够满足本系统的控制要求传感器采用InvenSense公司生产的MPU6050，它是整合性6轴运动处理器件，内含3轴陀螺仪和3轴加速度传感器，内置DMP滤波器，采用nc总线通信。通讯设备为HC-06型蓝牙串口，使本系统可以通过手机上的上位机进行控制，显示界面采用的是0.9寸OLED双色液晶显示屏，采用IIC总线通信。
2 系统的初始化
初始化的顺序为舵机的初始化，圆周舵机初始化，MPU6050的初始化。
2.1 标准舵机的初始化
第一步，初始化定时器3，根据stm32微控制器的定时器溢出频率为：
设定定时器3的预装载值ARR=799，分频系数PSC=1799，则定时器3的溢出频率为50Hz，周期为20ms，刚好满足MG995舵機的PWM信号脉宽周期。第二步，通过改变定时器3输出通道OC3和004的预装载比较值CPR X=736和CPR Y=740，此时输出的PWM信号正好可以使舵机的角度将托盘的姿态与水平面平行。
2.2 圆周舵机的初始化
圆周舵机的PWM脉宽特性与标准舵机基本一致，所以圆周舵机的初始化同标准舵机的初始化相同。设置预装载比较值CPR= 750时，圆周舵机静止，作为它的初始状态。
2.3 MPU6050的初始化
MPU6050为IIC通信， 由 于stm32f103c8t6的硬件nc在传输数据时不稳定，所以通过软件的方式将普通I/O模拟为nc总线，通过nc总线配置MPU6050的内部寄存器，配置完成后MPU6050就会向nc发送原始数据，具体过程为：初始化nc、MPU6050复位、设置数据量程、设置采样频率，激活MPU6050。
应注意的是，要先将控制托盘角度的舵机初始化，使托盘的pitch角和roll角的初值为O。，避免MPU6050在初始化的时选取错误的参照系。
3 系统的控制要求及控制方法的实现
3.1 系统的控制要求
本系统的要求为当小车的姿态发生偏移时，小车上方的托盘能够迅速响应，调整托盘始终能够与地面保持水平，并且尽量不产生振荡，使得托盘里运送的物品不会掉落，这就要求对托盘pitch角和roll角的控制要求调节时间短，超调量小，并且无稳态误差。3.2 控制方法的实现
3.2.1 闭环控制系统的建立
本系统的控制器为stm32f103c8t6微控制器，执行机构为标准舵机，输出量为托盘的角度，MPU6050输出的角度作为负反馈，根据系统的要求，输入的给定恒为O，系统的扰动输入为路面和地形的改变使得输出角度发生改变，系统的结构图如图4。
3.2.2 控制算法的软件实现
软件编写时，通过中断的方式完成每次的控制，即当MPU6050的中断信号发出后，开始进行本次的PID控制调节，过程如图5所示。
（1）控制规律的建立：在中断服务函数中，每次中断都会调用PID算法对本次采集的偏差进行PID运算，具体为：
biasX=O-pitch
sumX=∑biasX
speedX=biasX-last_ biasX
BalanceX_oiasX*KPX+sumX*KIX+speedX*KDX
其中：BalanceX为X轴PID的输出量;biasX是本次采集的pitch角偏差，其数值等于0 - pitch角;sumX是本次偏差和此前所有偏差的和，作为偏差的积分量;speedX是本次偏差和上一次偏差的差值，作为偏差的微分量;KPX是X轴PID的比例系数;KIX是X轴PID的积分系数KDX是X轴PID的微分系数。
Y轴的PID与X轴的相同，故不再列举。
（2）数据的采集和滤波：由于MPU6050的原始数据通常会含有高频噪声，所以系统在进行PID运算之前，应该先对MPU6050采集到的原始数据进行滤波，本系统使用的是DMP滤波器，它可将原始数据滤波后直接转换为四元数输出，再由简单的运算即可获得系统中托盘的欧拉角，大大减少了微控制器的运算量，提高系统的控制精度。
在本系统中只需得到托盘的俯仰角（pitch）和横滚角（roll），即托盘水平方向上的欧拉角。
（3）设置中断：由于MPU6050可以产生中断信号，所以本系统使用外部中断，将中断引脚与MPU6050的INT引脚连接，设置MPU6050为5ms通过INT引脚中断一次，使微控制器先在中断服务函数中读取数据，再进行PID控制，这样可以使数据的采集和数据的处理同步，使控制效果达到最佳。
3.3
PID输出量的处理
将本次输出BalanceX作为一个增量，使本次标准舵机的输出PWM为上一次的BalanceX与本次BalanceX的和。可将BalanceX理解为本次舵机转动的速度，这个速度有方向，当BalanceX=O时，表示舵机此时刻静止。
4 PID参数的调试
由于系统的X轴和Y轴PID参数完全相同，所以以X轴为例说明PID参数的调试过程。
4.1 平衡位置的确定
在标准舵机初始化部分已经得知，当预装载输出比较值CPR X=736时，pitch=O°，X轴方向处于平衡状态。
4.2 PID参数极性的确定
在调试过程中，按照KPX，KDX，KIX的顺序依次确定满足系统控制要求的PID参数极性，经过多次试验，若满足系统的控制要求，则PID参数应满足KPX>O，KDXo，任何一个参数的极性设置错误都会使系统在扰动信号输入时无法稳定，使响应曲线振荡。
4.3 KPX，KDX，KIX参数大小的确定
在KPX，KDX，KIX参数的极性确定之后，经过多次的试验，对PID参数的大小进行了确定，调试的主要过程如表1。
由此得到KPX=1，KDX= -0.6，KIX= O.l，根据工程经验，将结果乘以0 6，最终确定X轴方向的PID参数：KPX= 0.6，KDX =-0.36，KIX= 0.06.Y轴的PID参数同X轴的相同，在PID算法的控制下，系统的控制效果达到最佳。
5 实际控制效果
经过上述的PID参数调试，当系统经过以KPX= 0.6， KDX =-0.36， KIX= 0.06; KPY=0 6，KDY=-0.36，KIY= 0.06为参数的PID算法控制后，利用小车从水平路面驶入斜面模拟扰动信号的输入，通过上位机绘制pitch角的实时波形观察实际控制效果，曲线的横坐标表示时间，纵坐标表示此时pitch的大小与方向。为了和MPU6050采集的pitch角数据保持一致，规定当托盘的Y轴正半轴部分低于水平面时，pitchO。。
以pirch角为例，在受到外界扰动输入时系统pitch角的响应曲线如图6所示。
在图6中，横坐标一格表示0.5s，纵坐标一格表示20。从图中可以观察到，在横坐标t= 97.46s附近和t=98.96s附近分别输入+27。和_270的扰动信号（即驶入驶出斜面）时，系统均能很快地调节pitch角稳定在O。附近，并且系统的动态性能指标良好，调节时间短，几乎没有稳态误差和超调量，满足了系统的所有控制要求系统的动态性能指标良好。
6 结论
本文设计了一种结构简单易于实现的平衡运输小车系统。运用了离散化PID算法和DMP滤波器使该系统可以满足所有的控制要求，同时还解决了此前平衡运输系统中存在稳态误差的问题。在上述的试验分析中证明，本系统具有良好的快速性和穩定性，系统中所使用的PID算法将输出量设置为PWM信号的增量相较于直接将输出量作为PWM信号的来说，作为增量的算法的实际控制效果更好，更符合控制要求。
参考文献
[1]闫娜，自动化物流配送中心系统设计研究 [J].自动化与仪器仪表，2014 （12）： 22-24.[2]杨丰，周广田，现代物流配送中心信息系统设计分析[J].信息技术，2008 （11）：11-13.
[3]纪浩，吴友强，邹力涵.一种轮式移动机器人运动控制系统的实现[J]
.自动化仪 表，2013，34 （11）： 26-29.
[4]杨莘，刘海涛.基于STM32的两轮自平衡遥控小车[J].数字技术与应用，2014 （05）：151-15 2.
[5]马保政，娄文忠，于秀丽等，基于微加速 度计的易碎物品运输监测微系统设计[J].传感技术学报，2008，21（03）： 539-542.
[6]傅忠云，朱海霞，孙金秋等，基于惯性传感器MPU6050的滤波算法研究[J].压电 与声光，201 5，37 （05）： 821-825.
[7]王君荣，陈名松，数字PID控制算法的研究和仿真[J]，大众科技，2008 （04）： 27-2.[8]郑成.PID参数整定方法的研究[J].机械制造，2 009，47 （03）：35-36.