有趣的过去1，2，zzdzt 3，4，4，郁汉琪1，skdfd1

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

倒立摆的结构很简单，但控制系统很复杂。 介绍了旋转倒立摆的结构原理，利用分析力学中的Lagrange方程建立了旋转倒立摆的线性数学模型，导出了其公式和比较准确的状态方程描述，分析了旋转倒立摆的不稳定性和控制性。 设计了控制系统的硬件电路和软件程序的编制，对系统的全控制过程进行了数据采集、图形展示，分析了摇杆从自然下垂到抬起180实现倒立摆的全过程。 系统采用离散PID调节算法进行控制，保证了系统的稳定性，消除了嵌入卡尔曼滤波算法中可能出现的噪声信号，保证了数据的正确性。

旋转倒立摆； 洛杉矶格兰奇； 数学模型； 卡尔曼滤波器； PID调节

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

图1是旋转倒立摆的结构图。 倒立摆的原理是，通过电机使旋转臂在水平平面内往复运动时，钟摆会因惯性以旋转轴2为中心在垂直平面内往复运动，当能量达到一定值时，摆动幅度超过180成为倒立状态。 单片机系统对摆动杆的快速倒立控制是本研究的主要内容。 首先建立数学模型，然后设计单片机软、硬系统，最后进行调试分析，形成旋转倒立摆单片机控制系统。

1系统数学模型的建立

假设在建立数学模型之前忽略空气阻力。 系统由电机和质量均匀的摆动杆m构成，将从摆动杆的重心到旋转轴2的距离设为l，将从旋转轴2的轴心到旋转轴1的轴心的距离设为r，在某个时刻，旋转臂在水平面上旋转的角度是多少？摆动杆在垂直平面内旋转的角度是多少？ 琢、杆和旋转臂的角度以逆时针方向为正[5]。 其受力分析如图2所示。

回转臂和摆动杆一起动作，摆动杆的重心x、y方向的速度分量分别如下所示。

整个系统的动能t由回旋臂在水平面内的旋转动能、摆动杆在铅直平面内的旋转动能、摆动杆重心沿着x轴方向和y轴方向的动能4个部分构成。 这样就得到了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子程序执行。

如图5所示，计时器T0中断服务子程序的流程图主要执行传感器数据的读取、卡尔曼滤波、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] 无私的手机，twdmg，yedyc.X-Z倒立摆的一种饱和非线性稳定控制方法的研究[J].自动化学报，2013，39(1)：92-96.

[2] 赵建军，直率的羊，善良的发带，等.基于二阶倒立摆的人体运动合成[J].计算机学报，2014，37(10)：2188-2195.

[3] sxdld，nrdhm，勤奋的过客.快速显式预测控制在倒立摆系统中的应用[J].仪器仪表学报，2014，35(9)：2037-2044.

[4] 重要的饼干，马莉，pldhm.一类新的二阶滑模控制方法及其在倒立摆控制中的应用[J].自动化学报，2015，41(1)：194-202.

[5] ygdyx，jxdbm，twdzxc.基于Lagrange方程建模的单级旋转倒立摆控制[J].中国工程科学，2005，7(10)：11-15.

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

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

[8] xhdlz，辛勤的书本，美满的灯泡.PID控制器设计与参数整定方法综述[J].化工自动化及仪表，2005，32(5)：1-7.

[9] wydlh，师宇，纯真的柠檬.基于模糊PID控制器的控制方法研究[J].机械科学与技术，2011，30(1)：166-172.

[10] jzdrg，激情的小蚂蚁.自适应控制及应用[M].西安：西北工业大学出版社，1998.

[11] dqdxhd.模糊系统与模糊控制教程[M].北京：清华大学出版社，2003.

[12] 美好的吐司，懵懂的可乐，csdjr.Kalman滤波理论及其在导航系统中的应用(第二版)[M].北京：科学出版社，2010.

[13] DAUM F.Nonlinear filters 心灵美的小蘑菇 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.

AET会员年终大福利！

极速赛车五码一期全天计划数据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] 无私的手机，twdmg，yedyc.X-Z倒立摆的一种饱和非线性稳定控制方法的研究[J].自动化学报，2013，39(1)：92-96.

[2] 赵建军，直率的羊，善良的发带，等.基于二阶倒立摆的人体运动合成[J].计算机学报，2014，37(10)：2188-2195.

[3] sxdld，nrdhm，勤奋的过客.快速显式预测控制在倒立摆系统中的应用[J].仪器仪表学报，2014，35(9)：2037-2044.

[4] 重要的饼干，马莉，pldhm.一类新的二阶滑模控制方法及其在倒立摆控制中的应用[J].自动化学报，2015，41(1)：194-202.

[5] ygdyx，jxdbm，twdzxc.基于Lagrange方程建模的单级旋转倒立摆控制[J].中国工程科学，2005，7(10)：11-15.

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

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

[8] xhdlz，辛勤的书本，美满的灯泡.PID控制器设计与参数整定方法综述[J].化工自动化及仪表，2005，32(5)：1-7.

[9] wydlh，师宇，纯真的柠檬.基于模糊PID控制器的控制方法研究[J].机械科学与技术，2011，30(1)：166-172.

[10] jzdrg，激情的小蚂蚁.自适应控制及应用[M].西安：西北工业大学出版社，1998.

[11] dqdxhd.模糊系统与模糊控制教程[M].北京：清华大学出版社，2003.

[12] 美好的吐司，懵懂的可乐，csdjr.Kalman滤波理论及其在导航系统中的应用(第二版)[M].北京：科学出版社，2010.

[13] DAUM F.Nonlinear filters 心灵美的小蘑菇 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.

AET会员年终大福利！