倒立摆仿真与实验报告

1、最优控制实验报告二零一五年一月目录第 1 章 一级倒立摆实验31.1 一级倒立摆动力学建模31.1.1 一级倒立摆非线性模型建立31.1.2 一级倒立摆线性模型建立51.2 一级倒立摆 t 状态调节器仿真51.3 一级倒立摆 t 状态调节器实验91.4 一级倒立摆 t 输出调节器仿真111.5 一级倒立摆 t 输出调节器实验131.6 一级倒立摆非零给定调节器仿真141.7 一级倒立摆非零给定调节器实验16第 2 章 二级倒立摆实验162.1 二级倒立摆动力学模型162.1.1 二级倒立摆非线性模型建立172.1.2 二级倒立摆线性模型建立182.2 二级倒立摆 t 状态调节器仿真192.3

2、二级倒立摆 t 状态调节器实验212.4 二级倒立摆 t 输出调节器仿真222.5 二级倒立摆 t 输出调节器实验222.6 二级倒立摆非零给定调节器仿真232.7 二级倒立摆非零给定调节器实验24第1章 一级倒立摆实验1.1 一级倒立摆动力学建模在忽略了空气阻力和各种摩擦之后， 可将直线一级倒立摆系统抽象成小车和匀质杆组成的系统，如图所示图 1-1 直线一级倒立摆模型M 小车质量1.096 kg；m 摆杆质量0.109 kg；b 小车摩擦系数 0 .1N/m/sec；l 摆杆转动轴心到杆质心的长度0.25m；I 摆杆惯量2；0.0034 kg m·摆杆与垂直向上方向的夹角，规定角度

3、逆时针方向为正；x 小车运动位移，规定向右为正。1.1.1 一级倒立摆非线性模型建立采用拉格朗日方法，系统的拉格朗日方程为：L q, qT q, qV q, q(1.1)其中， L为拉格朗日算子，q 为系统的广义坐标，T 为系统的动能，V 为系统的势能。拉格朗日方程由广义坐标qi 和 L表示为：dLL(1.2)dtqifiqifi 为系统沿该广义坐标方向上的外力， 在本系统中， 系统的两个广义坐标分别为 和 x。系统动能：T TM Tm11 Mx 21 m1 x2m1l1 x cos2 m1l12 2(1.3)223系统的势能V m1gl1 cos(1.4)由于在广义坐标1 上应用拉格朗日方程

4、，由于此广义坐标上无广义力，则dLLdt得到：0(1.5)mlx cosmgl sinIml 2在 simulink 中建立非线性仿真动力学模型图 1-2 一级倒立摆非线性动力学模型其中 MATLAB Function模块中代码如下：functiondw = fcn(u,phi)I = 0.0034;m = 0.109;l = 0.25;g = 9.8;dw = ( m*g*l*sin(phi)+m*l*u*cos(phi) )/( I+m*l*l );(1.6)1.1.2 一级倒立摆线性模型建立由(1.6)，且对于质量均匀分布的摆杆有I1 ml 2，将 l0.25m 代入有33(x cosg

5、 sin )(1.7)将其在平衡位置0处进行线性化， cos1,sin，且有 g 9.831m / s2得到29.4933x(1.8)输入 ux ，将系统写为如下状态空间描述形式x0100x0x0000x10001u00029.49303(1.9)xyx1000x00010u0在 simulink 中建立线性仿真动力学模型，只需将1.1.1 里建立的非线性模型中 MATLAB Function 模块代码更改为dw = 29.493*phi+3*u;1.2 一级倒立摆 t状态调节器仿真对于线性定常系统的状态方程为x(t ) Ax(t) Bu(t )(1.10)给定初始条件 x t0x0 ，终端时

6、间 t f。求最优控制 u*t 使系统的二次型性能指标J1x (t )Qx(t)u (t) Ru(t) dt(1.11)2t0取极小值。式中A, B, Q, R 常数矩阵；Q半正定对称阵；R正定对称矩阵。控制不受约束，最优控制存在且唯一，即u (t)R 1B Px(t )Kx(t)(1.12)式中， P 为 nn维正定常数矩阵，满足里卡提矩阵代数方程PA ATP PBR 1BTP Q 0(1.13)对于线性定常系统无限时间状态调节器问题，要求系统完全能控。 求解出上方程，即可得到最优控制u* (t ) 。试验中的一级倒立摆模型可以线性化为定常系统，其中系数矩阵为010000000; B1100

7、00A001; C0010; D0000029.49303公式 (1.11)中选定不同的 Q，R 值，Q4×4 为半正定矩阵， R1×1 为正定矩阵，通过求解代数黎卡提方程（利用 Matlab 里面的 lqr 函数）可以得到最优控系数K lqrA,B,Q,R(1.14)控制率为u(t )Kx (t )(1.15)Q、R 的形式可设计为Q11QQ22, R 1(1.16)Q33Q44因为二次型最优控制是使得二次型性能指标取极小值，故只需改变Q 矩阵中元素的值即可，不用改变R 的取值，即只要保证 Q 与 R 的相对大小即可。其中，Q 矩阵中 Q11 代表小车位置的权重， Q22

8、 代表小车速度的权重， Q33 代表摆杆角度的权重， Q44 为摆杆角速度的权重。仿真实验模型如下图 1-3 仿真实验模型设定角度初始值为 10°，角速度与小车速度初值均为 0。下面按照一定的依据选取 Q 中非零元素的值进行仿真实验，并进行分析。取一组标准值方便对比Q11=Q22=Q33=Q44=2。响应曲线如下图，在后续研究中，若无特殊说明Q 中元素分别取此标准值。 考虑到实际系统中小车轨道长度有限， 取上述参数时发现位置相对零点波动的绝对值最大达到了 0.3m 以上，这在实际系统中是难以正常进行试验的，所以要对参数进行调整改进， 下面分别研究各个参数变化时对系统响应的影响。10角

9、度变化曲线0位置变化曲线8-0.056-0.1)4)-0.15°m(2x-0.20-0.25-2-0.3-4246810-0.3524681000t(s)t(s)图 1-4 Q 11=Q22=Q33=Q 44=2 时角度与位置变化曲线(1) 分析小车位置的权重对于响应曲线的影响。其他参数不变的情况下，小车位置权重 Q11 分别取为 2、20、200、1000 时观察角度与位置变化曲线如图 1-1 图 1-5 所示。角度变化曲线位置变化曲线10Q11=2Q11=205Q11=200Q11=1000)°0m(x0.050Q11=2-0.05Q11=20Q11=200-0.1Q1

10、1=1000-0.15-0.2-5-0.25-0.3-10246810-0.3524681000t(s)t(s)图 1-5 位置权重对响应的影响由图 1-5 可以看出，随着 Q11 的增加，角度变化曲线的稳态时间缩短，但超调量有所增大； 位置变化曲线特性改进明显， 稳态时间与绝对的超调值都显著减小，可见增大 Q11 的值会改进系统特性。(2) 分析小车速度的权重对响应曲线的影响小车速度权重 Q22 分别取为 2、20、 200、1000 时得到角度与位置随时间变化曲线如图 1-6 所示角度变化曲线位置变化曲线10Q22=28Q22=20Q22=2006Q22=1000)4)°m(2x

11、0-2-42468100t(s)0-0.05-0.1Q22=2-0.15Q22=20Q22=200-0.2Q22=1000-0.25-0.3-0.350246810t(s)图 1-6 小车速度权重对响应曲线的影响随着 Q22 的增大，角度曲线特性得到一定改善，绝对超调减小，且稳态时间减小；但对于小车位置曲线来说， 虽然绝对超调变小了， 但很明显稳态时间大大增加了，由于 Q22 代表的是小车的速度权重，可以类比为引入了阻尼项，减小超调的同时会增大稳态时间，这是我们并不希望的。故而 Q22 的值不能太大，要保证 Q22 取值不超过 Q11。(3) 分析摆杆角度的权重对响应曲线的影响小车速度权重 Q

12、33 分别取为 2、20、 200、1000 时得到角度与位置随时间变化曲线如图 1-7 所示角度变化曲线位置变化曲线10Q33=28Q33=20Q33=2006Q33=1000)4)°m(2x0.050-0.05Q33=2-0.1Q33=20Q33=200-0.15Q33=1000-0.20-0.25-2-0.3-4246810-0.3524681000t(s)t(s)图 1-7 摆杆角度权重对响应曲线的影响随着 Q33 的增大，角度曲线的绝对超调减小，但是相应的导致了稳态时间的增加；小车位置相应曲线超调减小， 同样的也是稳态时间增加了。 而且可以看出，Q33 对小车位置曲线的影响

13、远不如Q11 和 Q22 对小车位置响应的影响。(4) 分析摆杆角速度的权重对响应曲线的影响小车速度权重 Q44 分别取为 2、20、 200、1000 时得到角度与位置随时间变化曲线如图 1-8 所示由图 1-8 可知，随着 Q44 的增大，角度变化曲线稳态时间有一定程度的增加，曲线变化稍见平缓， 即曲线斜率的最大值变小了， 但绝对超调基本没变； 小车位置的响应特性随 Q44 的增大而变坏，绝对超调大幅上升，稳态时间也明显变长。所以 Q44 的值不能取的太大。要注意的是， Q44 取值变化过程中 Q 矩阵其他元素取的均为上文所提标准值， 标准值取的是很小的， 所以在确定参数时， 只要保证Q4

14、4 的值不能比Q33 大即可，图1-8 只是提供了分析的依据，不能直接根据上图的曲线进行选择。角度变化曲线10Q44=28Q44=20Q44=2006Q44=1000)4)°m(2x0-2-42468100t(s)位置变化曲线0.20-0.2Q44=2-0.4Q44=20Q44=200-0.6Q44=1000-0.8-1-1.22468100t(s)图 1-8 摆杆角速度权重对响应曲线的影响以上分析为 Q 矩阵中非零元素的选取提供了一定的依据，总的来说Q11 与33 的值越大越好，但过大的话可能会对执行器即电机提出过高的要求，而Q22Q与 Q44 的取值尽量不能比其他两个元素值大。1

15、.3 一级倒立摆 t状态调节器实验根据以上分析，选取几组实物实验 Q 矩阵中的元素值，并将仿真结果与之对比如图 1-9 至图 1-11 所示，对比仿真结果与实验结果的异同，分析产生此现象的原因。由于仿真与实物实验的初始条件很难做到完全一致， 如对于实物实验来说，由于编码器为一相对式码盘，所以倒立摆稳定状态为 - 而不是仿真实验中的 0 rad，而且由于实物实验中倒立摆是由下垂状态人为慢慢上摆至满足倒立摆稳定系统起控条件的， 在缓慢移动过程中， 很难做到倒立摆起控时摆杆的角速度为 0，即初始条件难以精确确定。所以只需比较仿真与实物实验得到的曲线特性中如绝对超调，稳态时间即可。此外，在实验过程中，

16、可以发现倒立摆摆杆转动到大概为平衡位置附近 10°时，倒立摆起控，这样在处理实验数据时可以将起控之前的无控状态去掉，只将有效的部分画出来即可，方便观察曲线特性。表 1-1 状态调节Q 矩阵中非零元素不同取值Q11Q22Q33Q44第一组10001000100100第二组10010010001000第三组100010001000100010角度变化曲线(仿真)0位置变化曲线(仿真)5-0.05)°m-0.1(x0-0.15-5510-0.251000t(s)t(s)角度变化曲线(实验)位置变化曲线(实验)-1650.15-1700.1)°m0.05(-175x0-1

17、80-0.0505100510t (s)t (s)图 1-9第一组状态调节器参数下响应图角度变化曲线(仿真)位置变化曲线(仿真)1005-0.1)°m-0.2(x0-0.3-5510-0.451000t(s)t(s)角度随时间变化曲线位置随时间变化曲线-1650.15-1700.1)°m(0.05-175x0-1800510-0.055100t (s)t (s)图 1-10 第二组状态调节器参数下响应图0角度变化曲线(仿真)位置变化曲线(仿真)1005-0.05)°m-0.1(x0-0.15-5510-0.251000t(s)t(s)-165角度随时间变化曲线位置

18、随时间变化曲线0.04-170)0.02)°m(0(-175x-0.02-180-0.040510-0.065100t (s)t (s)图 1-11 第三组状态调节器参数下响应图第一组参数下， 仿真与实物实验得到的曲线特性吻合较好，稳态时间与绝对超调量都比较相近； 但第二组参数位置曲线的超调相差较大，分析原因可能是在将倒立摆扶至起控位置左右时没有缓缓转动导致起控时摆杆有一定的角速度， 初始条件相差较大导致曲线相差较大； 第三组参数下实物实验得到的角度与位置曲线都存在稳态误差，尤其是位置误差为 5cm 左右，误差比较大，分析原因可能是系统的硬件问题， 因为就算法来说， 状态调节器是不可

19、能将末态稳定在非零点出的。1.4 一级倒立摆 t输出调节器仿真对于线性定常系统xAx tBu t(1.17)yCx t给定初始条件 x(t0 ) x0，终端时间 t f。求最优控制 u* (t) ，使系统的二次型性能指标为1y (t )Q (t) y(t)u (t) R(t)u(t ) dt(1.18)J2t 01要求系统完全能观测， 且控制不受约束。 则可求解代数黎卡提方程得到正定对称矩阵 PPA AT P PBR 1BT P CTQC 0(1.19)最优控制存在且唯一u* (t ) R 1 B Px(t )(1.20)此时倒立摆系统的Q 为 2×2 阶的，若设计QQ11(1.21

20、)Q33则C10000010Q11(1.22)0CTQCQ330所以在给定 Q 的上述形式后可以发现，输出调节器和的代数黎卡提方程的形式与状态调节器时是一致的，只需将状态调节器中Q 的第二行第二列和第四行第四列的元素值设置为零，调节Q11 和 Q33 计算出的反馈比例系数既是输出调节器下的反馈系数。设定标准状态为Q11 33，选取不同的参数进行仿真并对比曲线特性。=Q =100如至所示角度变化曲线位置变化曲线100.150)°m(0(-0.1xQ11=10Q11=10-5Q11=100-0.2Q11=100Q11=1000Q11=1000-10510-0.351000t(s)t(s)

21、图 1-12 输出调节器下小车位置权重对曲线的影响上图为当 Q33=100，时 Q11 分别取 10、100 和 1000 时的响应曲线，可以看出，增大小车位置的权重可有效缩短稳态时间，并减小小车位置变化曲线的绝对超调，2但是会增加摆杆变化曲线的绝对超调。角度变化曲线位置变化曲线100.0550) -0.05°0m(x-0.1Q33=10Q33=10-5Q33=100-0.15Q33=100Q33=1000Q33=1000-10510-0.251000t(s)t(s)图 1-13 输出调节器下摆杆角度权重对曲线的影响Q11=100，Q33 分别取 10、100、1000 时摆杆角度和

22、小车位置的响应曲线如上图，提高 Q33 的值可减响应曲线的超调，对角度曲线的稳态时间无大的影响，但会增加位置响应的稳态时间。1.5 一级倒立摆 t输出调节器实验选取几组实物实验 Q 矩阵中的元素值， 如表 1-2 所示。得到各组参数下摆杆角度和小车位置响应如至所示。表 1-2 输出调节实验选定参数Q11Q33第一组参数1000100第二组参数1001000第三组参数10001000角度变化曲线(实验)位置变化曲线(实验)0.061800.04)° 175m(0.02(x170016512345-0.021234500t (s)t (s)图 1-14 输出调节器第一组参数下响应曲线31

23、80)175°(1701650180)175°(1701650角度变化曲线(实验)位置变化曲线(实验)0.05)m0(x-0.051234502468t (s)t (s)图 1-15 输出调节器第二组参数下响应曲线角度变化曲线(实验)位置变化曲线(实验)0.05)m0(x-0.0512345012345t (s)t (s)图 1-16 输出调节器第三组参数下响应曲线由图 1-14 至图 1-16 可以看出，各组参数下响应的稳态时间与绝对超调指标都相当好，对比发现甚至优于仿真结果， 分析原因与上相同即在手动将摆杆转动至起控位置时可能没有把握好摆杆角速度的变化， 导致角速度初值

24、过大， 分析可以发现当摆杆角速度有一定初值且方向与手动摆起的旋向一致时， 是利于倒立摆的摆起的，所以响应曲线性能变好。1.6 一级倒立摆非零给定调节器仿真从本质上来说，非零给定点调节器是基于传统的传递函数的角度来分析的。非零给定调节器指的是给定一个位置信息， 使得倒立摆稳定后小车稳定在给定的位置上。则可以将小车位置作为输出， 小车加速度作为输入， 系统要做的是使输出值与输入值相等。引入状态反馈后的系统传递函数矩阵为(s) C (sI A BK ) 1 B(1.23)注意到其为一 2 1 的矩阵，对应的为单输入双输出系统， 输入是小车加速度，输出是小车位置及摆杆角度。 当系统稳定即时间趋于无穷时

25、， 由拉普拉斯终值定4理可知传递函数变为(0) C( A BK) 1B(1.24)此可以视作闭环系统的直流增益，简单的说就是一比例系数， 第一行第一列为输出到小车位置的直流增益，一般情况下是不为1 的，这就引出了非零给定调节器的问题。 当利用 lqr 算法求出反馈系数矩阵K 时，计算出(0) ，并将第一行第一列的元素取倒数表示为111(0) ，将给定的位置与此数相乘后再作为输入来控制小车，这样既可以达到非零给定的目的，图1-3 已经 simulink 模块实现展示，111(0) 即为图 1-3 中的 Wc(0)-1 。由上文中对状态调节器和输出调节器的仿真及实物实验可以看出， 当改变 Q 矩阵

26、中代表小车速度和摆杆角速度元素的值为非零时， 可以改善响应的阻尼特性，但同时会使稳态时间特性受到较大影响。 非零给定点的仿真中采用输出调节器的形式。选取 Q11=1000， Q33=200，期望小车稳定位置 yd=0.2m。计算出状态反馈矩阵 K=-31.6228 -20.1304 72.8210 13.1537，代入公式(1.24)求得-0.0316(0)0111(0)31.6228仿真结果如图 1-17 所示。角度变化曲线(仿真)位置变化曲线(仿真)100.350.2)0)0.1°m(-5x0-10-0.1-151234-0.2123400t(s)t(s)图 1-17 非零给定输

27、出调节器响应曲线小车位置稳定在距离原点0.2m 处。可以发现求出的(0) 中21 (0)为零，且已知21 (s) 代表输入为加速度，输出为摆杆角度的传递函数， 传递函数有零点， 211(0)没有意义，这在实际物理系统中也是明确的，即不能使二次型最优控制下的倒立摆系统摆杆稳定在不平衡的位置。细心观察还可以发现， 111(0) 就等于反馈矩阵 K 中的第一个元素。51.7 一级倒立摆非零给定调节器实验参数与仿真中一致，得到实物实验下非零给定调节器的小车位置及摆杆角度曲线如图 1-18 所示)°(角度变化曲线(实验)位置变化曲线(实验)-165-170)0.2m(-175x0.1-1800

28、012345012345t (s)t (s)图 1-18 非零给定调节器实物实验响应曲线第2章 二级倒立摆实验2.1 二级倒立摆动力学模型为简化系统，我们在建模时忽略了空气阻力和各种摩擦，并认为摆杆为刚体。二级倒立摆的组成如图2-1 所示。图 2-1 直线两级倒立摆物理模型倒立摆参数定义如下：6M 小车质量；m1摆杆 1 的质量，为 0.05kg；m2摆杆 2 的质量，为 0.13kg；m3质量块的质量，为 0.236kg；l1 摆杆 1 中心到转动中心的距离，为0.0775m；l2 摆杆 2 中心到转动中心的距离，为0.25m；1 摆杆 1 与竖直方向的夹角，规定逆时针为正；2 摆杆 2 与

29、竖直方向的夹角，规定逆时针为正；F 作用在系统上的外力；2.1.1 二级倒立摆非线性模型建立利用拉格朗日方程推导运动学方程：拉格朗日方程为：L(q, q)T (q, q)V (q, q)dLL(2.1)dtqifiqi其中， L 为拉格朗日算子， q 为系统的广义坐标， T 为系统的动能， V 为系统的势能。 i 1,2,., n ， fi 为系统沿该广义坐标方向上的外力，在本系统中，设系统的三个广义坐标分别是x, 1 , 2 。系统动能T TM Tm1 Tm2 Tm3(2.2)其中， TM 为小车动能， Tm1 为摆杆 1 的动能， Tm2 为摆杆 2 动能， Tm3 为质量块动能。系统势能

30、VVm1 Vm 2 Vm 3m1 gl1 cos 12m3gl1 cos 1m2 g(2l1 cos 1 l 2 cos 2 ) (2.3)经过推导，可得用1 ,1,2 , 2 , x 表示的1, 2 如下：13 2m1 g sin14m2 g sin 14m3 g sin 13m2 g cos( 21)sin26m lcos(21)sin(12)24m l2sin(12)22m x cos12 11221(2.4)4m2 x cos 14m3 x cos 1 3m2 cos( 12 )cos2 /2l1 ( 4m112m212m39m2 cos2 ( 12 )724m2 m13(m2m3 )

31、l12l 23g sin 26l1 12 sin( 1 2 )3x cos 2 92223m2l1 l 2cos( 12 )6m2 l22sin( 12 ) 3(m1 2m22m3 )( g sin 1 xcos 1)/16m2 ( m13m23m3 )l12 l224m22l12l22 cos2 ( 1 2 )9(2.5)2.1.2 二级倒立摆线性模型建立将其在平衡位置处进行泰勒展开并线性化，可以得到状态空间方程如下x000100x0100001010200000120x000000x110 K12K130 0 01K1720 K22K 230 0 02K 27x(2.6)x10000010

32、2y0100000u1x00100002123(2gm14gm2 4gm3 )86.69K124m13m212m3 )l12(K139m2 g21.624m13m212m3 )l12(3( 2m1m24m3 )6.64K174m13m22(12m3 )l1K222 g(m12m22m3 )40.316 (m14m2l23m23m3 ) / l2(2.7)9K234g( m13m23m3 )39.4516 ( m134m2l23m23m3 ) / l2 92( m1 2m22m3 )4 (m13m2m3 )K2716 (m130.0884m2l 23m23m3 )l2982.2 二级倒立摆 t状态调节器仿真二级倒立摆的状态调节器在原理上是与一级倒立摆相同的， 故在此不再赘述。关键还是求解代数黎卡提方程 P，进而求得反馈矩阵 K 。二级倒立摆系统的状态空间方程已由公式 (2.6)给出。状态调节器的二次型最优性能指标中的 Q 矩阵与 R 矩阵形式如为Q11Q22QQ33, R 1(2.8)Q44Q55Q66只需改变 Q 矩阵中非零元素的值，即可求得不同性能指标下的反馈矩阵。其中 Q11 代表小车位置的权重， Q22 代表摆杆 1 角度的权重， Q33代表摆杆 2 角度的权重， Q44 代表小车速度的权重， Q55 代表摆杆 1 角速度的权重， Q66 代表摆杆2