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

1、简易旋转倒立摆及控制装置摘要：本文设计并制作了一套旋转倒立摆及其控制装置，通过单片机控制直流电机带动旋转臂转动，实现了快速起摆、倒立保持、抗扰动等功能。通过将摆杆倒立处的物理方程线性化得到了该系统的状态空间方程。用能量补偿方法实现了起摆，起摆后利用线性二次最优控制(LQR)算法对模型进行稳定控制。实际测试结果表明该系统起摆速度快、稳定时间长、具有良好的抗干扰能力。 关键字：旋转倒立摆；起摆控制；稳定控制；线性二次型最优控制一、 系统方案系统由stm32单片机、增量式编码器、直流伺服电机及其驱动电路、人机交互接口等部分构成。直流伺服电机上的编码器可以测量旋转臂转过的角度，摆杆和旋转臂之间安装的编

2、码器可以测量摆杆转过的角度。利用测量到的两个角度和角速度，计算得到驱动直流电机所需的占空比和转动方向，通过旋转臂带动摆杆摆动使摆杆完成要求的动作，系统原理方框图如图 1所示。图 1系统框架1. 电机选择方案方案一： 采用步进电机。步进电机是开环控制的，输入一个脉冲步进电机就会转过一个固定的角度，驱动相对方便。但是步进电机响应速度不够快，另外控制不当可能引起共振和失步。方案二：采用直流电机。直流电机响应速度快，转矩比较大。但是需要额外的编码器来反馈得到电机的转速和位置，不能像步进电机那样通过脉冲个数来控制转过的角度。考虑到旋转倒立摆在启动和维持倒立状态时都需要较快的反应速度和较大扭矩，扭矩和响应

3、速度是关键的因素，因此选择了方案二。并且在直流电机上安装了编码器用于获取转过的角度。2. 角度传感器选择方案一：电位器式的角度传感器。这类传感器线性精度高且稳定，温度系数小，而且输出量是绝对式的，使用前不需要额外的校准调零，成本较低。但是这类传感器输出信号为模拟量需要经AD采集后才能转换成相应的角度，分辨率受AD限制，高频特性较差。方案二：加速度式的角度传感器。这类传感器依靠测重力加速度在某个轴上的分量测得角度。只有读取重力加速度在某个轴上的分量，通过就可以计算出角。这类传感器只适合测量静态的角度，在有振动或转动的场合下会引入额外的加速度，使得测量不准确。 方案三：增量式编码器。这类

4、传感器输出信号为两路正交的矩形波，通过单片机的正交编码器接口即可得到一个相对的角度，测量的精度取决于光栅码盘上的线数。这类传感器稳定性好，精度高。综合考虑系统的稳定性和精度，选择了方案三。二、 理论分析与计算1. 电机选型 选用MAXON RE25电机，该电机为额定电压24V，额定工作电流1.17A，能够输出额定功率为28.08W。由于旋转杆绕中心旋转带动摆杆摆动，则系统所需最大力矩为(物理参数见附录2)，小于电机的额定转矩。该电机最大效率为86%，带减速比电机转速为，速度较快，能够满足题目响应时间要求。所以选择该电机。2. 摆杆状态检测摆杆状态检测采用1000线的增量式编码器，其输出信号为两

5、路正交的矩形波，根据相位对矩形波脉冲个数计数就可以得到转过的相对角度。由于输出的是相对角度，使用前需要确认零点位置。经4倍频处理后，转动一周计数器计数到4000，最小可分辨角为。若当前计数值为a，则转过的角度就是。利用单片机定时器产生一个定时间隔，记录定时器开始和定时结束时码盘计算器的值和，则角速度为。3. 数学模型及稳定控制算法旋转倒立摆由一个旋转臂和一个摆杆组成，其参考坐标系见下图 2。根据图 2 旋转倒立摆数学模型参考坐标系刚体转动惯量可以直接写出旋转臂和B处角度传感器的动能，分别计为和，利用微分法可以计算出摆杆的动能。系统的总动能就可以计为： 公式(1)以摆杆自然下垂时质心位置为势能零

6、点，则系统总势能也可以直接写出，记为。记拉格朗日算子，系统广义坐标，广义坐标上的非有势力对应广义外力为电机输出转矩和摩擦力，上非有势力对应广义外力为摩擦力，由拉格朗日方程： 公式(2)由拉格朗日方程结合系统的动能和势能方程可以得到系统的运动方程。系统在处是平衡的，将运动方程在线性化，忽略高次项并令，整理后得到系统的状态方程： 公式(3)限于篇幅，详细推导过程在附录1中。系统的物理参数和A、B、C矩阵求解结果见附录2。系统可控性矩阵和可观性矩阵都是满秩的，所以系统是可观可控的。因此可以设计一个基于状态反馈的控制器对系统进行极点配置，这里采用基于最优控制的线性二次状态调节器(LQR)来设计控制器。

7、LQR控制的意义是求取控制量u，使得二次型的性能指标： 公式(4)达到最优，其中Q和R分别是状态向量和控制向量的权重矩阵。LQR的问题归结于求解Riccatti方程： 公式(5)求得矩阵，就可以得到状态反馈形式的控制量，K称为是最优反馈增益矩阵。结合上面求出的A、B矩阵，利用matlab的lqr命令可以方便地求得矩阵。加入反馈矩阵K后的方框图如图 3，在摆杆处于稳定倒立情况下输入r为0。图 3 闭环控制方框图4. 起摆控制算法倒立摆的起摆是一个复杂的非线性系统控制问题。本设计通过控制摆的能量来直接实现起摆控制。在图 2所示的坐标系中，定义摆杆在倒立位置处能量为： (公式6)其导数 (公式7)由

8、于 (公式8)则 (公式9)应用李亚普诺夫方法，令李亚普诺夫函数，则令，则由于摆杆不可能一直在水平位置，即不会一直为0，因此函数V会逐渐减小，直到，此时摆杆起摆，实现了摆杆倒立。在不影响李亚普诺夫函数递减的情况下对控制率进行调整。调整原则是控制力矩尽可能大，以使摆杆在最短时间内起摆。取驱动电压为，k是一个常数，sign是符号函数。三、 电路与程序设计1. 编码器电平转换电路编码器型号为E40S6-1000-3-T-24，该编码器供电电压为1224V。如果采用12V供电编码器输出高电平为+12V，需要采用电平转换来输入单片机。HCPL 2601是双路光耦，能将+12V转换成+5V的逻辑电平，且光

9、耦能够滤去数字信号上的噪声。数字光电编码器驱动电路如下图 4。图 4 编码器电平转换电路2. 电机驱动电路电机驱动电路由一片L6205和相关外围元件组成，L6205为双路电机驱动。在本题目中电机需要消耗很大的电流，为了避免电流大幅波动影响到供电电压，因此在电机驱动板上安装了大电容，具体电路见图 5。图 5 电机驱动电路3. 系统软件设计及流程图系统整体框图如下图 6，单片机复位后，初始化各个任务模块然后根据按键输入执行相应任务，共有4个任务模块分别是往复运动、松手稳定、下垂起摆和其它功能。图 6 系统整体框图下面重点描述起摆及维持稳定的功能，先输入一个触发信号，让摆杆开始摆动，然后利用能量补偿

10、算法计算需要输出的电压让摆杆起摆，判断摆杆的状态决定在何时开始进入维持倒立的任务。维持倒立状态采用了LQR算法进行控制，如果在外界扰动下，不能满足，那么就重新进入起摆任务让摆杆重新起摆。图 7 起摆及维持稳定流程图四、 测试方案与测试结果1. 测试工具表 1 测试工具表名称型号数量1直流稳压电源DF1743003C12量角器ZG1981(精度1度)13钢尺无(精度1mm)14秒表无(精度0.01s)15电子天平JT1001N (精度0.1g)12. 测试方案与结果1) 起摆用时测试摆杆从下垂状态开始，驱动电机带动旋转臂作往复转动，记录从开始摆动到摆角超过60度和摆杆完成圆周运动所用的时间。表

11、2 起摆用时测试结果第1次测试第2次测试第3次测试平均用时超过60度用时/s0.1s0.2s0.2s0.17s完成圆周运动用时/s0.7s0.6s0.8s0.7s2) 倒立状态恢复测试在摆杆处于自然下垂状态下，外力拉起摆杆至接近 165°位置，外力撤除同时，启动控制旋转臂。用秒表记录恢复到倒立状态所用的时间和旋转臂转过的最大角度。表 3 倒立状态恢复测试结果摆杆角度/度160162165167170旋转臂最大角度/度3533302015时间（s）15s1.51.4s1.4s1.2s3) 摆杆自下垂到倒立测试让摆杆处于自然下垂状态开始，控制旋转臂作往复旋转运动，直至摆杆倒立，记录从开始

12、摆动到倒立状态和倒立状态维持的时间表 4 摆杆自下垂到倒立测试结果第1次测试第2次测试第3次测试平均时间到达倒立状态用时/s0.7s0.8s0.6s0.7s倒立状态维持时间/s240s以上240s以上240s以上240s以上4) 摆杆倒立时旋转臂作圆周运动测试在摆杆保持倒立状态的前提下，旋转臂作圆周运动，记录旋转臂作一次圆周运动所用时间和摆杆与倒立位置间的最大角度表 5 摆杆倒立时旋转臂作圆周运动测试结果第1次测试第2次测试第3次测试平均结果圆周运动用时/s3s4s4s5s摆杆最大偏移角/度108963. 结果分析摆杆起摆速度很快，能够在第一次或第二次摆动后就维持在倒立状态，响应时间很短，且倒

13、立状态能够维持很长时间，摆杆倒立旋转臂做圆周运动，耗时少且摆杆偏离倒立状态角度小，较好地满足了题目要求。五、 结论与收获倒立状态下采用基于状态空间反馈及LQR极点配置方法，系统能够及时且精确地控制摆杆的运动，具有较强的抗干扰能力。在起摆过程采用了基于能量补偿的方法，起摆速度很快。参考文献1康华光.电子技术基础（模拟部分）M.高等教育出版社,2003. 2张培仁等.机器人系统设计与算法M.中国科学与技术大学出版社,2008. 3王正林.MATLAB/Simulink与控制系统仿真M.电子工业出版社,2012.4王成元.现代电机控制技术M.机械工业出版社,2009.5刘军.例说STM32M.北京航

14、空航天大学出版社,2011.附录1：旋转倒立摆状态空间方程推导建立如图 8所示坐标系，设旋转臂长为，质量为，摆杆长为，质量为，B位置处角度传感器质量为，为旋转臂与轴正向夹角，为摆杆与轴正向夹角。旋转臂的总动能为： 公式(F-1) 由变分法求得摆杆的总动能为： 公式(F-2)摆杆与旋转臂连接处编码器动能为： 公式(F-3)系统总动能为： 公式(F-4)以摆杆自然下垂时质心位置为势能零点，则系统总势能为：图 8旋转数学模型参考坐标系以摆杆自然下垂时质心位置为势能零点，则系统总势能为： 公式(F-5)计拉格朗日算子，系统广义坐标，广义坐标上非有势力对应广义外力为电机输出转矩和摩擦力，上非有势力对应广义外力为摩擦力，由拉格朗日方程： 公式(F-6)由公式(1)(6)可得到系统的运动方程：其中为电机输出转矩,为电机输入电压，为电机力矩系数，为电机反电势系数，、分别为旋转臂和摆杆绕轴转动的阻尼系