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

曾宪阳，杨红莉，郁汉琪，褚南峰

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

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

曾宪阳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°形成倒立状态。通过单片机系统控制摆杆迅速倒立是本文研究的主要内容。首先建立数学模型，然后设计单片机软、硬件系统，最后进行调试分析，形成旋转倒立摆单片机控制系统。

1　系统数学模型创建

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

图1　旋转倒立摆结构图

图2　旋转倒立摆受力分析图

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

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

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

令 α=x1，=x2，θ=y1，=y2，得出系统线性化状态方程：

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

表1　相关物理参数表

2　控制系统硬件电路设计

2.1控制系统总体框图

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

图3　旋转倒立摆系统总体框图

2.2控制系统电路原理图

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

图4　系统传感器及驱动电路原理图

3　控制系统软件设计

3.1资源分配及程序流程

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

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

图5　定时器T0中断子程序流程图

3.2控制调节算法

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

图6　模拟PID控制器的原理框图

系统采用计算量较小的增量式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]：

4　测试结果与结论

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

表2　设定摆起模式运行结果测量表

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

The modeling analysis and system design of rotational inverted pendulum based on single chip microcomputer control

Zeng Xianyang1，2，Yang Hongli3，4，Yu Hanqi1，Chu Nanfeng1
(1.Industry Center，Nanjing Institute of Technology，Nanjing 211167，China；2.Department of Mathematics，Nanjing University，Nanjing 210093，China；3.Department of Mathematics and Physics，Nanjing Institute of Technology，Nanjing 211167，China；4.School of Atmospheric Sciences，Nanjing University，Nanjing 210093，China)

Structure of rotational inverted pendulum is simple,but the control system is more complicated.This paper introduces the structure of rotational inverted pendulum,Lagrange equation in analytical mechanics linear mathematical model of rotational inverted pendulum is established,the formula and a more accurate equation of state description are deduced,and the instability and controllability of rotational inverted pendulum are analyzed.The control system hardware circuit and software programming are designed.The control process of the system for data acquisition,graphic display,swinging rod is analyzed from natural prolapse to set up the whole process of 180°handstand.System control uses the discrete PID algorithm to ensure the stability of the system.And the infusion of the Kalman filtering algorithm filters the possible jamming signal,which ensures the accuracy of the data.

rotational inverted pendulum;Lagrange；mathematical model；Kalman filter；PID control

TK323

A

10.16157/j.issn.0258-7998.2016.09.020

南京工程学院青年基金重点项目（QKJA201404）；南京工程学院 2014年校级教学改革项目（JG201440）；南京工程学院创新基金重大项目（CKJA201410）

2016-04-27)(

2016-05-16)

曾宪阳（1979-），男，博士研究生，主要研究方向：电子技术应用、控制系统及数学建模。

杨红莉（1980-），通信作者，女，博士后，主要研究方向：计算数学，E-mail：yanghongli1016@163.com。

郁汉琪（1961-），男，硕士，教授，主要研究方向：电子技术应用。

中文引用格式：曾宪阳，杨红莉，郁汉琪，等.基于单片机控制的旋转倒立摆建模分析与系统设计[J].电子技术应用，2016，42(9)：76-79.

英文引用格式：Zeng Xianyang，Yang Hongli，Yu Hanqi，et al.The modeling analysis and system design of rotational inverted pendulum based on single chip microcomputer control[J].Application of Electronic Technique，2016，42(9)：76-79.