0 引言

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

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

1 系统数学模型创建

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

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

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

　　Lagrange方程：。其中，q是系统的广义坐标，T为系统总动能，V为系统的势能，L为拉格朗日算子。Lagrange方程表示为：

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

       令，得出系统线性化状态方程：

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

2 控制系统硬件电路设计

　　2.1 控制系统总体框图

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

　　2.2 控制系统电路原理图

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

3 控制系统软件设计

　　3.1 资源分配及程序流程

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

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

　　3.2 控制调节算法

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

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

　　其中，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所示)，软件编程时应延时几秒以避开盲区。

　　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以上。通过一系列实验验证，该旋转倒立摆系统能完成任意角度的摆起，并能实现快速倒立，系统能很好地满足设计要求。

　　参考文献

　　[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.