《电子技术应用》

基于单片机控制的旋转倒立摆建模分析与系统设计

2016年电子技术应用第9期 作者:曾宪阳1,2,杨红莉3,4,郁汉琪1,褚南峰1
2016/11/12 9:50:00

  曾宪阳1,2,杨红莉3,4,郁汉琪1,褚南峰1

  (1.南京工程学院 工业中心,江苏 南京211167;2.南京大学 数学系,江苏 南京210093;3.南京工程学院 数理部,江苏 南京211167;4.南京大学 大气科学学院,江苏 南京210093)

  摘  要: 旋转倒立摆结构简单,但控制系统较为复杂。介绍了旋转倒立摆的结构原理,运用分析力学中的Lagrange方程建立了旋转倒立摆的线性数学模型,推导出其公式及较为准确的状态方程描述,分析了旋转倒立摆的不稳定性和可控性。设计了控制系统硬件电路及软件程序编写,对系统整个控制过程进行了数据采集、图形展示,分析了摆杆从自然下垂到摆起180°实现倒立的整个过程。系统采用离散PID调节算法进行控制,保证了系统的稳定性,融入卡尔曼滤波算法滤除可能出现的干扰信号,保证了数据的准确性。

  关键词: 旋转倒立摆;Lagrange;数学模型;卡尔曼滤波;PID调节

0 引言

  倒立摆是一种研究控制理论的理想平台,其结构简单、成本低,是一个典型的多变量、快速、非线性和自然不稳定系统。旋转倒立摆在外力的作用下,从一个稳定的平衡状态自动转移到另一个平衡状态的过程中,既要求起摆迅速,又要求在到达新的平衡点时能快速进行稳摆控制[1-4]。

  图1为旋转倒立摆结构图。旋转倒立摆的原理为:当电机带动旋转臂在水平平面内来回转动时,摆杆由于惯性将会绕转轴2在竖直平面内来回摆动,当能量达到一定数值时,摆动幅度将超过180°形成倒立状态。通过单片机系统控制摆杆迅速倒立是本文研究的主要内容。首先建立数学模型,然后设计单片机软、硬件系统,最后进行调试分析,形成旋转倒立摆单片机控制系统。

图像 001.png

1 系统数学模型创建

  建立数学模型前,设空气阻力忽略不计。系统由电机和质量均匀的摆杆m组成,摆杆质心到转轴2的距离为L,转轴2轴心到转轴1轴心距离为r,某时刻旋转臂在水平面上转动角度为?兹,摆杆在竖直平面内转动角度为?琢,设摆杆和旋转臂的角度以逆时针方向为正[5]。其受力分析如图2所示。

图像 002.png

  旋转臂和摆杆一起动作,摆杆质心x、y方向的速度分量分别如下:

  QQ图片20161112100342.png

  QQ图片20161112100349.png

  系统总动能T由4个部分组成:旋臂水平面内的转动动能、摆杆竖直平面内的转动动能、摆杆质心沿x轴方向和y轴方向的动能。由此可得到Lagrange函数:

  QQ图片20161112100353.png

  QQ图片20161112100358.png

  Lagrange方程:QQ图片20161112100410.png。其中,q是系统的广义坐标,T为系统总动能,V为系统的势能,L为拉格朗日算子。Lagrange方程表示为:

  QQ图片20161112100415.png

  在倒立摆系统中i=1,2,q=(?兹,?琢),?兹为旋臂角位移,?琢为摆杆角位移,Qi为系统沿该广义坐标方向上的外力,得到以下方程组:

  QQ图片20161112100419.png

  QQ图片20161112100423.png

       令QQ图片20161112100427.png,得出系统线性化状态方程:

  QQ图片20161112100431.png

  根据表1相关参数,当旋转倒立摆系统靠近平衡点时,得到如下线性化数学模型:

图像 008.png

  QQ图片20161112100437.png

2 控制系统硬件电路设计

  2.1 控制系统总体框图

  控制系统主要由单片机系统、驱动模块、直流减速电机、角度传感器、LCD液晶显示模块等组成。单片机通过实时采集角度传感器数据,经分析得出摆杆当前状态,通过计算得出相应控制量来控制电机转动,从而改变摆杆状态,使其达到预期控制的目的。系统总体框图如图3所示。

图像 003.png

  2.2 控制系统电路原理图

  图4为控制系统传感器及驱动原理图。为保证控制精度和速度,采用增强型51单片机STC12C5A60S2[6]作为控制核心;电机驱动模块采用L298N大电流电机驱动芯片,由单片机P2口部分口线控制;角度传感器采用MPU6050 六轴运动处理组件,能检测摆杆在垂直方向上的角度值,由P1口部分口线控制,中断引脚接至单片机外部中断0引脚。此外电路还包括LCD12864显示电路及按键电路,用来实现人机交互及当前状态显示。

图像 004.png

3 控制系统软件设计

  3.1 资源分配及程序流程

  系统主要控制任务包括:角度数据采集与处理、电机驱动控制、按键输入读取、数据显示等。其中数据显示任务为非紧急事件,由主程序循环调用即可完成;角度数据采集与处理、电机驱动控制任务具有连贯性及较高的实时性,属于周期性的紧急事件,应由定时器T0中断服务子程序一并完成;按键输入读取任务属于随机性的紧急事件,应由外部中断0子程序来完成。

  定时器T0中断服务子程序流程图如图5所示,主要执行读取传感器数据、卡尔曼滤波、PID调节任务,为避免中断程序溢出,此三项任务的执行时间应小于5 ms,因此单片机外部晶振频率选为24 MHz方能满足速度需要。

图像 005.png

  3.2 控制调节算法

  PID调节是按偏差的比例、积分、微分进行控制的一种闭环调节控制,原理框图如图6所示,其中r(t)为系统给定值,c(t)为实际输出,u(t)为输出控制量。引入PID调节算法可保证系统处于闭环控制状态,并且能让系统快速准确达到设置的平衡状态[7-11]。

图像 006.png

  系统采用计算量较小的增量式PID算法,由模拟PID离散化得到在k-1时刻的输出:

  QQ图片20161112101206.png

  其中,T为采样时间。单片机控制系统将采用恒定的采样周期T,一旦确定A、B、C,只要使用前后3次测量的偏差值,就可以求出控制量。

  3.3 系统抗干扰设计

  由于角度传感器采集到的数据含有噪声信号,主要来自外界电磁干扰及角度传感器本身的噪声。在软件系统中采用卡尔曼滤波算法[12-15]将噪声滤去。设采样率为200 Hz,对摆杆摆起发整个过程进行角度数据采集,经卡尔曼滤波前后图像对比如图7所示,其展示了摆杆从自然下垂状态开始实现倒立的整过过程。摆杆静止时刻自然下垂为平衡状态Ⅰ,与垂直方向夹角为0°(图7(b)中2段所示)。摆杆在电机带动下获取一定能量来回振荡(图7(b)中3所示),能量达到一定程度摆杆越过180°实现倒立,经过短时间几次小的振荡后(图7(b)中4所示),到达新的平衡状态Ⅱ(图7(b)中5所示),实现了摆杆倒立。停止电机控制,摆杆将回到原平衡状态Ⅰ,并在0°附近自由振荡几次后达到静止(图7(b)中6所示)。由于卡尔曼滤波在初始时刻滤波误差较大,属于盲区(图7(b)中1所示),软件编程时应延时几秒以避开盲区。

图像 007.png

  3.4 主要程序源码

  定时器T0中断子程序代码如下所示[16]:

  void Timer0 interrupt 1  using 1

  {

  TH0=(65536-2500)/256;

  TL0=(65536-2500)%256;//重装初值

  ReadMpu6050();//读传感器数据

  KalmanFilter();//卡尔曼滤波

  if(DelayCounter==2000) //计数判断

  {Motor0=PIDControl();//PID调节控制

  DispBuffUpdata();} //更新显示缓冲区

  else DelayCounter++;

  }

4 测试结果与结论

  表2为摆起模式运行结果测量表。通过按键设置摆起模式,分别选择摆起角度为45°~180°之间的数据,摆杆会在电机带动下做往复摆动,摆角很快达到超过-60°~+60°。当摆杆角度接近180°时,通过PID调节,迅速使摆杆保持稳定倒立状态,时间维持5 s以上。通过一系列实验验证,该旋转倒立摆系统能完成任意角度的摆起,并能实现快速倒立,系统能很好地满足设计要求。

图像 010.png

  参考文献

  [1] 王家军,刘栋良,王宝军.X-Z倒立摆的一种饱和非线性稳定控制方法的研究[J].自动化学报,2013,39(1):92-96.

  [2] 赵建军,魏毅,夏时洪,等.基于二阶倒立摆的人体运动合成[J].计算机学报,2014,37(10):2188-2195.

  [3] 周昆,孙明玮,陈增强.快速显式预测控制在倒立摆系统中的应用[J].仪器仪表学报,2014,35(9):2037-2044.

  [4] 李雪冰,马莉,丁世宏.一类新的二阶滑模控制方法及其在倒立摆控制中的应用[J].自动化学报,2015,41(1):194-202.

  [5] 吴爱国,张小明,张钊.基于Lagrange方程建模的单级旋转倒立摆控制[J].中国工程科学,2005,7(10):11-15.

  [6] 南通国芯.STC12C5A60S2+系列单片机手册[DB/OL].http://www.stcmcu.com.

  [7] 曹敏,徐凌桦.单神经元PID算法在倒立摆控制系统中的应用[J].微计算机信息,2009,25(1-1):70-71.

  [8] 杨智,朱海锋,黄以华.PID控制器设计与参数整定方法综述[J].化工自动化及仪表,2005,32(5):1-7.

  [9] 王述彦,师宇,冯忠绪.基于模糊PID控制器的控制方法研究[J].机械科学与技术,2011,30(1):166-172.

  [10] 陈新海,李言俊.自适应控制及应用[M].西安:西北工业大学出版社,1998.

  [11] 王立新.模糊系统与模糊控制教程[M].北京:清华大学出版社,2003.

  [12] 付梦印,邓志红,闫莉萍.Kalman滤波理论及其在导航系统中的应用(第二版)[M].北京:科学出版社,2010.

  [13] DAUM F.Nonlinear filters beyond the Kalman filter[J].IEEE A&E Systems Magazine,2005,20(8):57-69.

  [14] KIM J,VADDI S S,MENON P K,et al.Comparison between nonlinear filtering techniques forspiraling ballistic missile state estimation[J].IEEE Transactions on Aerospace and Elec-tronic Systems,2012,48(1):313-328.

  [15] KALMAN R E.A new approach to linear filtering and prediction problems[J].Transactions of the AMSE Journal of Basic Engineering,1960,82:35-45.

  [16] 谭浩强.C语言程序设计(第三版)[M].北京:清华大学出版社,2005.

  


继续阅读>>