电设

目录 ...................................................................................................................................... 1 摘 要 .................................................................................................................................. 2 第一章 设计任务 ................................................................................................................ 3

技术要求 ....................................................................................................................... 3 1.基本要求 ..................................................................................................................... 3 2.发挥部分 ..................................................................................................................... 3 第二章 方案论证 .................................................................................................................. 4

摆杆选择方案 ................................................................................................................ 4 传感器方案比较 ............................................................................................................ 4 控制方案比较 ................................................................................................................ 5 第三章 理论分析与计算 ....................................................................................................... 5

1 分析原理 .................................................................................................................... 5 2 旋转倒立摆系统数学模型的建立 ............................................................................... 6 第四章 机械结构的设计 ....................................................................................................... 8 第五章 硬件电路的设计 ..................................................................................................... 10

MCU最小系统的设计 .................................................................................................... 10 稳压模块的设计 .......................................................................................................... 10 电机驱动的设计 .......................................................................................................... 11 第六章 程序的设计 ............................................................................................................ 11 第七章 测试结果与误差分析 .............................................................................................. 12

测试方法 ..................................................................................................................... 12 误差分析 ..................................................................................................................... 13 第八章 结论与心得体会 ..................................................................................................... 13 附录 .................................................................................................................................... 13

电路图 ......................................................................................................................... 14 元件清单 ..................................................................................................................... 14 源代码： ..................................................................................................................... 15

1

简易旋转倒立摆及控制装置

摘 要

本题目要求设计并制作一套简易旋转倒立摆及其控制装置，通过两个精密塑料电位器测出的信号返回给MCU，经过处理控制输出PWM信号控制电机转动，利用加速度改变摆杆的姿态来实现要求的任务。经过调试，我们已经可以完全实现基础部分的功能以及发挥部分的前两个功能。整个系统的控制用了LQR控制，使得系统更加便于我们的控制，实现题目要求的功能。

关键词： 旋转倒立摆，LQR控制算法，塑料精密电位器

2

第一章 设计任务

本题目要求设计并制作一套简易旋转倒立摆及其控制装置，旋转倒立摆的结构如图所示。

电动机A固定在支架B上，通过转轴F驱动旋转臂C旋转。摆杆 E 通过转轴D固定在旋转臂 C 的一端，当旋转臂C在电动机A驱动下作往复旋转运动时，带动摆杆E在垂直于旋转臂C的平面作自由旋转。

技术要求 1.基本要求

（1）摆杆从处于自然下垂状态（摆角0°）开始，驱动电机带旋转臂作往复旋转使摆杆动，并尽快使摆角达到或超过 -60°~ +60°；

（2）从摆杆处于自然下垂状态开始，尽快增大摆杆的摆动幅度，直至完成圆周运动 ；

（3）在摆杆处于自然下垂状态下，外力拉起摆杆至接近 165°位置，外力撤除同时，启动控制旋转臂使摆杆保持倒立状态时间不少于5s；期间旋转臂的动角度不大于90 °。

2.发挥部分

（1）从摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，尽快使摆杆摆起倒立，保持倒立状态时间不少于10s；

（2）在摆杆保持倒立状态下，施加干扰后摆杆能继续保持倒立或2s内恢复倒立状态；

3

（3）在摆杆保持倒立状态的前提下，旋转臂作圆周运动，并尽快使单方向转过角度达到或超过360°；

（4）其他。

第二章 方案论证

摆杆选择方案

方案一：选用质量较小的摆杆。我们选用的是较细的碳纤维杆，质量大概在100g左右，

从理论上来说质量较小的物体其惯性较小，更容易改变它的运动状态。那么在控制时，我们改变姿态时旋转臂所需要的加速度会比较小，应该更容易实现。但在使用MATLAB仿真时我们会发现由于质量太小导致的转动惯量小，出现的后果就是整个系统的模型是发散的。

方案二：选用质量较大的摆杆。这次我们在空心碳纤维中加入了填充物使其质量达到我们的要求，通过仿真我们可以看到系统是收敛的。另外我们可以通过孩提时代用手指倒立长杆的例子来感受，较重的杆往往立得会比较稳定，也就是我们现在所说的重杆的鲁棒性更强，更容易适应参数的扰动。因此本设计采用了方案二。 传感器方案比较

方案一：精密电位器和光电编码器同时使用。这里我们是用的是和直流减速电机同轴的一个码盘，这样不仅省去了码盘的固定，还避免了码盘和电机之间用齿轮啮合所带来的空程等问题。而精密电位器我们则作为摆杆的角位移传感器，这样的好处在于精密电位器的质量较小，不会对机械结构本身的平衡带来很大的影响，另外这个是xs128本身决定的，因为其只有一路计数器。缺点在于精密电位器有一定死区，不易控制。

方案二：双光电编码器。光电编码器具有分辨能力强、测量精度高和工作可靠等优点。由于我们使用的是增量式码盘，利用计算系统将旋转码盘产生的脉冲增量针对某个基准数进行加减以求得角位移。这样就需要判断码盘的正反转，但是由于所用freescale的xs128MCU里面没有自带正交解码，所以只能在外部使用DM7474的外部D触发器来进行方向判断。这个方案的缺点在于外围电路会比较复杂，并且旋转器上码盘的固定也是一个问题。

方案三：双精密电位器。即旋转臂和转轴处传感器全部选用精密电位器。该 方案的优点在于精密电位器具有线性精密高、理论上无限的分辨力、平滑性优良、动态噪声小、机械寿命长等优良性能。其作为角度传感器的原理在于把机械角度位移转化为电信号，为了达到这一效果，我们可以外接一个已知电阻与其组成分压电路，然后用I/O口采集电压，由于精密电位器的线性度较好，所以可以通过电压值得出角位移。因此我们选用了方案三。

4

控制方案比较

方案一：PID控制。我们这里用PD控制精密电位器的角位移和角速度，然后通过多次尝试我们发现PD控制向电机发送的PWM波不加反馈的话旋转臂的相应速度不能达到我们要求，无法稳摆。而PID只适用于单输入单输出的系统，所以PID控制无法使用。

方案二：LQR控制。LQR， 即线性二次型调节器 ,其对象是现代控制理论中以状态空间形式给出的线性系统 ,而目标函数为对象状态和控制输入的二次型函数。这里我们用到四个输入量即旋转臂角速度、角加速度和摆杆角速度、角加速度来调节摆杆姿态以达到平衡。

输出PWM MCU处理 精密电位器AD采集 角度 电机驱动 摆杆达到预电机转动

定姿态

图一：原理框图

第三章 理论分析与计算

倒立摆系统是非线形、强耦合、多变量和自然不稳定的系统。鲁棒性和随动性对整个系统起着决定性影响。 1 分析原理

旋转倒立摆系统的实验原理：旋转臂由转轴处的直流力矩电机驱动，可绕转轴在垂直于电机转轴的铅直平面内转动。旋转臂和精密电位器同轴相连，摆杆可绕转轴在垂直于转轴的铅直平面内转动。由电位器测量得到的两个角位移信号(旋转臂与铅直线的夹角，摆杆和旋臂之间的相对角度)，作为系统的两个输出量被送入XS128控制器。由角位移的差分可得到角速度信号，然后根据一定的状态反馈控制算法，计算出控制律，并转化为电压信号提供给驱动电路，以驱动直流力矩电机的运动，通过电机带动旋转臂的转动来控制摆杆的运动。倒立摆的控制目标是使倒立在不稳定平衡点附近的运动成为一个稳定的运动，控制旋转臂和摆杆的两个角位移信号在各自的零点位置附近变化。由于单级旋转式倒立摆系统是一个速度比较快的系统，且线性度很有限，要求的采样时间比较小，所以，用连续系统的设计方法来设计数字控制器是可行且有效的。

5

2 旋转倒立摆系统数学模型的建立

对旋转倒立摆系统进行建模，我们可以得到旋转倒立摆的非线性状态方程 如下。

根据旋转倒立摆系统的数学模型，求出在控制u=0 的非线性系统的阶跃响应对应的程序。可见，系统在平衡点附近的阶跃响应曲线不会达到稳定。因此，该系统是自然不稳定的。另外，同样可以计算出系统的能控性矩阵的秩为4，因而系统可控。因此，单级旋转倒立摆系统在不稳定平衡点附近可控。

zoxyθ 2lθ 1l1l2O1

图二：旋转倒立摆的简化模型

环形倒立摆的简化模型和坐标系如图所示，由一个摆杆和一个转动杆组成，摆杆和旋转杆都质量均匀，为转动杆的杆长，为摆杆的杆长；转动杆的质量为，摆杆的质量为；转动杆和X轴的夹角为，摆杆和竖直的Z轴夹角为，设电机输出力矩为，规定逆时针方向为正。环形倒立摆的系统有两个自由度，转动杆是围绕Z轴水平转动，带动摆杆在竖直平面内转动。系统的工作状态，是从摆相相对于不稳定的平衡点为，摆杆的角保持在了一定的范围内[9]。

距离摆杆摆动轴的距离为处取一小段的，这一小段的坐标是：

可以得到这一小段的动能就为

为转动杆绕Z轴转动惯量： 为摆杆绕转动轴心的转动惯量：

6

u是电机输出转矩。 摆杆的动能为：

转动杆只有转动，其动能为：

则系统总动能为：

以转动杆水平位置为0势能位置，则系统势能等于摆杆势能为：

可得系统的运动方程为：

由拉格朗日方程

设系统广义坐标，则在广义坐标上没有外力作用。 其中拉格朗日算子L=T-V 各个符号的意义如下所示： T----系统的总动能； V----系统的总势能； L----拉格朗日方程算子；

----系统和各个广义坐标的广义力；

----系统选取的广义坐标； i-----1,2。 拉格朗日函数式：

系统非线性数学模型为：

其中：

将上式整理可以得到：

将环形倒立摆的数学模型进行线性化推导：

7

根据非线性模型，令0,0，可将非线性模型线性化，得到：

令，，，。

则有

系统的状态方程： 其中，

，，，

表1参数物理意义及数值

参数 物理含义 转动杆质量 摆杆质量 转动杆长度 摆杆长度 重力加速度 数值大小 0.1kg 0.21kg 0.18m 0.17m 9.8N/kg

0000我们通过MATLAB计算，最后可以得到A=

034.99260-27.9837100010，B=，， -24.08060000 52.9773最终算出的系数矩阵是K=

第四章 机械结构的设计

8

这是我们第一代旋转倒立摆，在这个结构中我们用铝板作为底座，带码盘的直流减速电机为旋转臂的动力，碳纤维杆作为摆杆。在这个结构中，由于电机的空程过大，所以我们的稳摆环节无法令人满意，摆杆在中间位置附近震荡非常严重，，不能满足题目要求，所以我们换为了接下来的另一种方案。

在这种结构中我们使用了与电位器同轴的直流力矩电机，换用了更轻的旋转臂以及更重的摆杆。这个电机的空程非常小，而且响应非常迅速。所以我们使用这个结构即可满足我们大部分要求。缺点就在于精密电位器有死区，给某些位置

9

的控制带来一定难度。

第五章 硬件电路的设计

MCU最小系统的设计

我们所使用的是飞思卡尔公司XS128系列的芯片，本最小系统由电源滤波电 路、时钟电路、复位电路等组成。

主板原理图 主板实物图 稳压模块的设计

电路的稳压模块由两个5V的LM2940和一个3.3V的LM1117组成，将电池7.2V 电压分别稳至5V和3.3V。LM2940为MCU和其他外部电路供电，LM1117是为了陀螺仪准备的，但是后来并未用到陀螺仪。而电机我们将两个飞思卡尔电池串联在一起提供足够的电压。大概在16V左右。另外我们每路稳压模块都增加了独立的开关，一来可以省电，二来可以避免在不需要时稳压模块与其他地方而发生短路的风险。

稳压模块原理图

10

稳压模块PCB 稳压模块实物图

电机驱动的设计

驱动采用MOS管H桥电路，由6679和7832两种MOS管和PNP、NPN三极管，电阻等组成。

它带有三个P沟道的高边MOSFET、两个N沟道的低边MOSFET。P沟道高边开关省去了电荷泵的需求, 因而减小了电磁干扰（EMI）。集成的驱动IC具有逻辑电平输入、电流诊断、斜率调节、死区时间产生和过温、过压、欠压、过流及短路保护的功能。

电机驱动实物图

另外在上面的电路图中我们可以看到拨码开关的存在，拨码开关是我们为了改变方案准备的，四位拨码开关一共可以产生最多16种的方案。

第六章 程序的设计

首先我们利用的最经典的PID控制算法进行控制，只需要通过精密电位器给 单片机一个AD值，之后转化为摆杆此刻的角度值，当然得定一个零位，知道摆杆

11

在零位的那个方向，转化的角度值微分得到角速度，PWM输出为;

输出=KP*（角度值-零位）+KD*加速度 通过调节两个系数来使摆杆稳住，但是电机哪儿没有闭环，很难调节使摆杆平稳，而且开始也没有想到好的办法将电机哪儿的精密电位器融入到系统之中，也就是对电机来说是一个开环系统，不够稳定。而且电机也不给力，有很大的空程，所以我们临时就换了结构，换了一个比较好的电机，随后我们小组经过讨论，电机哪儿又加一个电位器，即用两个环来控制摆杆，一个姿态环，一个位置环，姿态环即摆杆的姿态，位置环为摇杆所在的位置，此时的输出：

输出=KP1*（摆杆的角度-中值）+KD1*摆杆的角速度+KP2*（摇杆的角度-中值）+KD2*摇杆的角速度

这样我们就通过闭环使系统得到稳定。 以下是程序框图：

摆杆角度 摆杆角速度 XS128 输出PWM 电机转动 满足题目要求 摆杆姿态调节

第七章 测试结果与误差分析

我们在链接摆杆的精密电位器上加了一个角度盘，以便于我们观察角度指标 是否达标。 测试方法

1 摆杆从处于自然下垂状态（摆角0°）开始，驱动电机带旋转臂作往复旋转使摆杆动，看是否摆角达到或超过 -60°~ +60°；

2从摆杆处于自然下垂状态开始，看摆杆是否能完成圆周运动；

3在摆杆处于自然下垂状态下，外力拉起摆杆至接近 165°位置，外力撤除同时，启动控制旋转臂看摆杆是否能保持倒立状态时间不少于5s；期间旋转臂的动角度不大于90 °。

4从摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，尽快使摆杆摆起倒立，看摆杆是否能保持倒立状态时间不少于10s；

5在摆杆保持倒立状态下，施加干扰后摆杆能否继续保持倒立或2s内恢复倒立状态；

12

6在摆杆保持倒立状态的前提下，旋转臂作圆周运动，看摆杆单方向转过角度是否能达到或超过360°； 误差分析

控制时电机的抖动会使精密电位器返回的A/D值跳动，会使控制带来误差； 电机自身的死区会让整个系统产生一定的误差，由于电刷与换向器之间存在摩擦，所以电机的起动需要克服一个摩擦转矩才能开始转动，在这个区间内即使给电机PWM信号的，电机也不能转动，这就是电机死区产生的原因。由于电机死区的存在，我们必须在控制的时候考虑电机的死区所造成控制上的影响，这样就会对系统产生一个很大的误差；

精密电位器的死区也会影响系统的控制，我们在在线调试的时候测试精密电位器的特性，发现精密电位器转到360度时（以电阻为零时电位器也置零点），未能直接回到0Ω，而是先跳变到一定的值才能返回0Ω，这样就造成了精密电位器的死区，这个死区就会造成控制时的精度，带来误差；

摆杆和旋转臂自身的转动惯量会对系统造成影响。关于这点，由于我们的旋转臂和摆杆自己加过一定的配重，所以可能造成两者质量不均匀，而我们在建模时用到的是一个较为理想的杆，因此有一定的误差。

第八章 结论与心得体会

经过这几天的调试，我们对整个旋转倒立摆的系统有了一个了解。在短时间 内学会一个系统的控制会对我们以后学习工作产生积极的影响。另外本作品利用XS128控制，只需要加很少一部分外围电路即可实现旋转倒立摆的基本功能，在响应速度上达到了一个很快的速度。而且选择的器件也都是经济性，实用性非常好的器件。整体作品完全能达到一个比较好的水准。

附录

13

电路图

元件清单

元器件 XS128最小系统 1000uF电容 22uF钽电容 104电容 1K电阻 10K电阻 贴片LED 4位拨码开关 塑料精密电位器 直流力矩电动机 船型开关 按键开关 LM2940稳压芯片 LM1117稳压芯片 贴片8050三极管 贴片8550三极管 数量 1 4 3 6 10 1 7 1 2 1 4 1 2 1 6 6 14

4.7K电阻 6679MOS管 7832MOS管 8 6 4 源代码：

int x=0;

float kp1=104.1,kp2=104.5,kd=0.48,kh=2.1,kc=0.23;

float left1,right1,left2,right2,kx=0.478,kx1=0.478; float kx2=2.678,kx3=0; //char str[100]; int dd1=48;

interrupt void Real_Time_Interrupt(void) { int i;

if(jiao>=0&&jiao<=mid) {

if(jiao>=120&&jiao<=mid) {

left1=((mid)*2.84-jiao*2.84)*kp1*0.35+(-ess_jiaodu)*kd*350+(save_speed-400)*kh*0.35+ess_speed*kc*350;

left2=(int)(left1*kx1)+dd1; if(left2>=600) left2=600; if(left2<=0) left2=0;

left=(int)left2; right=0; }

else {left=0;right=0;} }

if(jiao>mid&&jiao<360) { if(jiao>mid&&jiao<300) { left=0;

right1=(jiao*2.84-(mid)*2.84)*kp2*0.35+(ess_jiaodu)*kd*350+(400-save_speed)*kh*0.35+ess_speed*kc*350;

right2=(int)(right1*kx1)+dd1;

15

if(right2>=600) right2=600; if(right2<=0) right2=0;

right=(int)right2; }

else {left=0;right=0;} }

/*if( PORTB_PB0==0) {save_speed1=0;

save_speed=save_speed+PACNTL; speed=PACNTL; }

if( PORTB_PB0==1) { save_speed=0;

save_speed1=save_speed1+PACNTL; speed1=PACNTL;

}

PACNTL=0; */ // save_speed=ATD0DR1;

ess_speed=((double)(ATD0DR1)-save_speed); save_speed=ATD0DR1; sum_speed[x]=ess_speed; jiaodu=ATD0DR0;

jiao=(ATD0DR0/2.84);

ess_jiaodu=((double)(ATD0DR0)-jiaodu1); jiaodu1=ATD0DR0;

save_ess[x]=ess_jiaodu; x++; if(x==30) x=0;

for(i=0;i<20;i++) {

e+=save_ess[i]; f+=sum_speed[i]; }

ess_jiaodu=(e/30); ess_speed=(f/30); e=0; f=0;

16

CRGFLG_RTIF = 1; // 清中断标志位

17