倒立摆仿真与实验报告

1、最优控制实验报告9 / 30目录第1章一级倒立摆实验41.1 一级倒立摆动力学建模41.1.1 一级倒立摆非线性模型建立 41.1.2 一级倒立摆线性模型建立 61.2 一级倒立摆t工状态调节器仿真71.3 一级倒立摆t*状态调节器实验111.4 一级倒立摆t*输出调节器仿真141.5 一级倒立摆t*输出调节器实验161.6 一级倒立摆非零给定调节器仿真181.7 一级倒立摆非零给定调节器实验 20第2章二级倒立摆实验202.1二级倒立摆动力学模型20二级倒立摆非线性模型建立 21二级倒立摆线性模型建立222.2二级倒立摆t*状态调节器仿真232.3二级倒立摆t*状态调节器实验272.4二级倒

2、立摆t*输出调节器仿真282.5二级倒立摆t*输出调节器实验292.6二级倒立摆非零给定调节器仿真 292.7二级倒立摆非零给定调节器实验 30第1章一级倒立摆实验1.1 一级倒立摆动力学建模在忽略了空气阻力和各种摩擦之后，可将直线一级倒立摆系统抽象成小车和匀质杆组成的系统，如下图M小车质量1.096 kg ；m摆杆质量0.109 kg ；b小车摩擦系数 0 .1N/m/sec ；l摆杆转动轴心到杆质心的长度 0.25m ;I摆杆惯量0.0034 kg m2;摆杆与垂直向上方向的夹角，规定角度逆时针方向为正;x小车运动位移，规定向右为正。1.1.1 一级倒立摆非线性模型建立采用拉格朗日方法，系

3、统的拉格朗日方程为:L q,q T q,q V q,q (1.1)其中，L为拉格朗日算子，q为系统的广义坐标，T为系统的动能，V为系统的势能。拉格朗日方程由广义坐标d_Ldt qq和L表示为:Lqifi (1.2)fi为系统沿该广义坐标方向上的外力， 在本系统中，系统的两个广义坐标分别为和x。系统动能：-Mx2 Im/ m1l1 X' cos2 22 2 23 讪(1.3)系统的势能V m1gl1cos (1.4)由于在广义坐标1上应用拉格朗日方程，由于此广义坐标上无广义力，那么d L0(1.5)dt得到： mix cos mgl sinI ml2在simulink中建立非线性仿真动力

4、学模型图12 一级倒立摆非线性动力学模型其中MATLAB Function模块中代码如下：fun cti on dw = f(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) )/(l+m*l*l );1.1.2 一级倒立摆线性模型建立1 2由(1.6)，且对于质量均匀分布的摆杆有I -ml ,将l 0.25m代入有33(xcosgsin )(1.7)将其在平衡位置0处进展线性化，cos1,sin，且有g 9.831m/s2得到29.493 3x(1.8)输入u x，将系统写为如下状

5、态空间描述形式x0100 x0x0000 x1u000100029.49303(1.9)xx10 00 x0yu00 100在simulink中建立线性仿真动力学模型，只需将里建立的非线性模型中MATLAB Function模块代码更改为dw = 29.493*phi+3*u;1.2 一级倒立摆t拥犬态调节器仿真对于线性定常系统的状态方程为X(t) Ax(t) Bu(t)(1.10)给定初始条件X toXo，终端时间tf。求最优控制U* t使系统的二次型性能指标1Jx (t)Qx(t) u (t)Ru(t) dt(1.11)2 to取极小值。式中 A, B,Q, R常数矩阵；Q 半正定对称阵；

6、R 正定对称矩阵。控制不受约束，最优控制存在且唯一，即1u (t) R B Px(t)Kx(t) (1.12)式中，P为n n维正定常数矩阵，满足里卡提矩阵代数方程PA AtP PBR1BtP Q 0(1.13)对于线性定常系统无限时间状态调节器问题，要求系统完全能控。求解出上 方程，即可得到最优控制u*(t)。试验中的一级倒立摆模型可以线性化为定常系统，其中系数矩阵为010000000110000A;B;C;D00010001000029.49303公式(1.11)中选定不同的Q，R值，Q4 >4为半正定矩阵，R1 X1为正定矩阵, 通过求解代数黎卡提方程利用 Matlab里面的lqr

7、函数可以得到最优控系数K lqr A,B,Q,R (1.14)控制率为u(t) Kx(t)(1.15)Q、R的形式可设计为QiiQ22Q, R 1(1.16)Q33Q44因为二次型最优控制是使得二次型性能指标取极小值，故只需改变Q矩阵中元素的值即可，不用改变 R的取值，即只要保证 Q与R的相对大小即可。其 中，Q矩阵中Q11代表小车位置的权重，Q22代表小车速度的权重，Q33代表摆 杆角度的权重，Q44为摆杆角速度的权重。仿真实验模型如下图13仿真实验模型设定角度初始值为10。，角速度与小车速度初值均为0。下面按照一定的依 据选取Q中非零元素的值进展仿真实验，并进展分析。取一组标准值方便比照

8、Q11=Q 22=Q 33=Q 44=2。响应曲线如下列图，在后续研究中，假设无特殊说明Q 中元素分别取此标准值。考虑到实际系统中小车轨道长度有限， 取上述参数时发 现位置相对零点波动的绝对值最大到达了 0.3m以上，这在实际系统中是难以正 常进展试验的，所以要对参数进展调整改良，下面分别研究各个参数变化时对系 统响应的影响。角度变化曲线位置变化曲线t(s)时角度与位置变化曲线图 14Q 11 =Q 22 =Q 33 =Q 44=2(1)分析小车位置的权重对于响应曲线的影响。其他参数不变的情况下，小车位置权重Q11分别取为2、20、200、1000时观察角度与位置变化曲线如图11图15所示。Q

9、11=2Q11=20Q11=200Q11=1000£ J; /r角度变化曲线100-5-10t(s)图) m x0.05Q11=2Q11=20Q11=200Q11=1000y j位置变化曲线0-0.05-0.1-0.15-0.2-0.25-0.3-0.35t(s)15位置权重对响应的影响由图15可以看出，随着Q11的增加，角度变化曲线的稳态时间缩短，但超 调量有所增大；位置变化曲线特性改良明显，稳态时间与绝对的超调值都显著减 小，可见增大Q11的值会改良系统特性。分析小车速度的权重对响应曲线的影响小车速度权重Q22分别取为2、20、200、1000时得到角度与位置随时间 变化曲线如图

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

11、角度与位置随时间位置变化曲线图17摆杆角度权重对响应曲线的影响随着Q33的增大，角度曲线的绝对超调减小，但是相应的导致了稳态时间的 增加；小车位置相应曲线超调减小，同样的也是稳态时间增加了。而且可以看出, Q33对小车位置曲线的影响远不如 Q11和Q22对小车位置响应的影响。(4)分析摆杆角速度的权重对响应曲线的影响11 / 30小车速度权重Q44分别取为2、20、200、1000时得到角度与位置随时间 变化曲线如图18所示由图18可知，随着Q44的增大，角度变化曲线稳态时间有一定程度的增加， 曲线变化稍见平缓，即曲线斜率的最大值变小了，但绝对超调根本没变；小车位 置的响应特性随Q44的增大而

12、变坏，绝对超调大幅上升，稳态时间也明显变长。 所以Q44的值不能取的太大。要注意的是，Q44取值变化过程中Q矩阵其他元素 取的均为上文所提标准值，标准值取的是很小的，所以在确定参数时，只要保证 Q44的值不能比Q33大即可，图18只是提供了分析的依据，不能直接根据上图的曲线进展选择10Q44=2Q44=20Q44=200Q44=1000-1.1| *1角度变化曲线86420-2-4t(s)位置变化曲线t(s)图18摆杆角速度权重对响应曲线的影响以上分析为Q矩阵中非零元素的选取提供了一定的依据，总的来说Q11与Q33的值越大越好，但过大的话可能会对执行器即电机提出过高的要求，而Q22与Q44的取

13、值尽量不能比其他两个元素值大。1.3 一级倒立摆t川犬态调节器实验根据以上分析，选取几组实物实验 Q矩阵中的元素值，并将仿真结果与之 比照方图19至图111所示，比照仿真结果与实验结果的异同， 分析产生此现象 的原因。由于仿真与实物实验的初始条件很难做到完全一致，如对于实物实验来说，由于编码器为一相对式码盘，所以倒立摆稳定状态为-而不是仿真实验中 的0 rad，而且由于实物实验中倒立摆是由下垂状态人为慢慢上摆至满足倒立摆 稳定系统起控条件的，在缓慢移动过程中，很难做到倒立摆起控时摆杆的角速度13 / 30为0，即初始条件难以准确确定。所以只需比拟仿真与实物实验得到的曲线特性 中如绝对超调，稳态

14、时间即可。此外，在实验过程中，可以发现倒立摆摆杆转动 到大概为平衡位置附近10。时，倒立摆起控，这样在处理实验数据时可以将起控 之前的无控状态去掉，只将有效的局部画出来即可，方便观察曲线特性。角度变化曲线（仿真）) m x-0.05-0.1-0.15-0.210,/V位置变化曲线（仿真）角度变化曲线（实验）t（s）位置变化曲线（实验）0510t (s)表11状态调节Q矩阵中非零元素不同取值Q11Q22Q33Q44第一组10001000100100第二组10010010001000第三组1000100010001000图19第一组状态调节器参数下响应图角度变化曲线（仿真）位置变化曲线（仿真）05

15、10t (s)t (s)15 / 30图110第二组状态调节器参数下响应图角度变化曲线(仿真)°t(s)位置变化曲线(仿真)0 -0.05)m -0.1 x /-0.15-165-170-175-18010t (s)r1角度随时间变化曲线-0.2 0510t (s)17 / 30图111第三组状态调节器参数下响应图第一组参数下，仿真与实物实验得到的曲线特性吻合较好， 稳态时间与绝对 超调量都比拟相近；但第二组参数位置曲线的超调相差较大， 分析原因可能是在 将倒立摆扶至起控位置左右时没有缓缓转动导致起控时摆杆有一定的角速度，初始条件相差较大导致曲线相差较大；第三组参数下实物实验得到的角

16、度与位置曲 线都存在稳态误差，尤其是位置误差为 5cm左右，误差比拟大，分析原因可能 是系统的硬件问题，因为就算法来说，状态调节器是不可能将末态稳定在非零点 出的。1.4 一级倒立摆t输出调节器仿真对于线性定常系统x Ax t Bu t(1.17)y Cx t给定初始条件X(t°) xg，终端时间tf 。求最优控制U*(t)，使系统的二次型性能指标为1 J t y (t)Q(t)y(t) u(t)R(t)u(t)dt(i.i8)2 to要求系统完全能观测，且控制不受约束。那么可求解代数黎卡提方程得到正 定对称矩阵PPA AtP PBR1BtP CtQC 0(1.19)最优控制存在且唯

17、一* 1u (t) R B Px(t) (1.20)(1.21)Q33此时倒立摆系统的Q为2 X2阶的，假设设计Q那么Q11(1.22)1 C 0ctqcQ330所以在给定Q的上述形式后可以发现，输出调节器和的代数黎卡提方程的形式与状态调节器时是一致的，只需将状态调节器中Q的第二行第二列和第四行第四列的元素值设置为零，调节Q11和Q33计算出的反应比例系数既是输出调 节器下的反应系数。设定标准状态为Qn=Q 33=100，选取不同的参数进展仿真并比照曲线特性。 如至所示21 / 30t(s)t(s)图112输出调节器下小车位置权重对曲线的影响上图为当Q33=100，时Q11分别取10、100和

18、1000时的响应曲线，可以 看出，增大小车位置的权重可有效缩短稳态时间，并减小小车位置变化曲线的绝 对超调，但是会增加摆杆变化曲线的绝对超调。角度变化曲线t(s)位置变化曲线t(s)图113输出调节器下摆杆角度权重对曲线的影响Q11=100，Q33分别取10、100、1000时摆杆角度和小车位置的响应曲线 如上图，提高Q33的值可减响应曲线的超调，对角度曲线的稳态时间无大的影响， 但会增加位置响应的稳态时间。1.5 一级倒立摆t *输出调节器实验选取几组实物实验Q矩阵中的元素值，如表12所示。得到各组参数下摆杆 角度和小车位置响应如至所示。表12输出调节实验选定参数180175170165t

19、(S)角度变化曲线（实验）位置变化曲线（实验）图114输出调节器第一组参数下响应曲线角度变化曲线（实验）位置变化曲线（实验）图115输出调节器第二组参数下响应曲线Q11Q33第一组参数1000100第二组参数1001000第三组参数10001000角度变化曲线(实验)位置变化曲线(实验)图116输出调节器第三组参数下响应曲线由图114至图116可以看出，各组参数下响应的稳态时间与绝对超调指标 都相当好，比照发现甚至优于仿真结果，分析原因与上一样即在手动将摆杆转动 至起控位置时可能没有把握好摆杆角速度的变化，导致角速度初值过大，分析可以发现当摆杆角速度有一定初值且方向与手动摆起的旋向一致时，是利

20、于倒立摆的摆起的，所以响应曲线性能变好。1.6 一级倒立摆非零给定调节器仿真从本质上来说，非零给定点调节器是基于传统的传递函数的角度来分析的。非零给定调节器指的是给定一个位置信息，使得倒立摆稳定后小车稳定在给定的 位置上。那么可以将小车位置作为输出，小车加速度作为输入，系统要做的是使 输出值与输入值相等。引入状态反应后的系统传递函数矩阵为(s) C(sl A BK) 1B(1.23)注意到其为一 2 1的矩阵，对应的为单输入双输出系统，输入是小车加速度, 输出是小车位置及摆杆角度。当系统稳定即时间趋于无穷时，由拉普拉斯终值定 理可知传递函数变为1(0) C( A BK) B (1.24)此可以

21、视作闭环系统的直流增益，简单的说就是一比例系数，第一行第一列 为输出到小车位置的直流增益，一般情况下是不为1的，这就引出了非零给定调23 / 30节器的问题。当利用Iqr算法求出反应系数矩阵K时，计算出(0)，并将第一行第一列的元素取倒数表示为111 (0)，将给定的位置与此数相乘后再作为输入来控制小车，这样既可以到达非零给定的目的，图13已经simulink模块实现展示，1；(0)即为图13中的Wc(0)A-1 。由上文中对状态调节器和输出调节器的仿真及实物实验可以看出，当改变Q 矩阵中代表小车速度和摆杆角速度元素的值为非零时，可以改善响应的阻尼特性，但同时会使稳态时间特性受到较大影响。非零

22、给定点的仿真中采用输出调节器的形式。选取Qn=1000，Q33=200，期望小车稳定位置yd=0.2m。计算出状态反应矩阵 K=-31.6228-20.130472.821013.1537，代入公式(1.24)求得(0)-0.031635 / 30*(0)31.62280.30.2)0.1 mx 0-0.1r 1 *j-U.201234位置变化曲线(仿真)t(s)图117非零给定输出调节器响应曲线小车位置稳定在距离原点0.2m处。可以发现求出的(0)中21(0)为零，且 21(S)代表输入为加速度，输出为摆杆角度的传递函数，传递函数有零点，21(0)没有意义，这在实际物理系统中 也是明确的，即

23、不能使二次型最优控制下的倒立摆系统摆杆稳定在不平衡的位 置。细心观察还可以发现，1；(0)就等于反应矩阵K中的第一个元素。1.7 一级倒立摆非零给定调节器实验参数与仿真中一致，得到实物实验下非零给定调节器的小车位置及摆杆角度-165角度变化曲线（实验）-170-175°ftX-18011X/012345曲线如图118所示t G)位置变化曲线（实验）图118非零给定调节器实物实验响应曲线第2章二级倒立摆实验2.1二级倒立摆动力学模型为简化系统，我们在建模时忽略了空气阻力和各种摩擦，并认为摆杆为刚体二级倒立摆的组成如图21所示。4图21直线两级倒立摆物理模型倒立摆参数定义如下:M 小车质

24、量；m 1-一摆杆1的质量，为0.05kg ;m2-一摆杆2的质量，为0.13kg ;m3-一质量块的质量，为0.236kg ;I1 -摆杆1中心到转动中心的距离，为0.0775m ;12摆杆2中心到转动中心的距离，为寸 0.25m ;1摆杆1与竖直方向的夹角，规疋逆时针为正；2摆杆2与竖直方向的夹角，规疋逆时针为正；F 作用在系统上的外力;二级倒立摆非线性模型建立利用拉格朗日方程推导运动学方程:拉格朗日方程为：L(q,q)T(q,q)V(q,q)d_L _L f(2.1)dt qiqii其中，L为拉格朗日算子，q为系统的广义坐标，T为系统的动能，V为系统的势能。i 1,2,., n , fi

25、为系统沿该广义坐标方向上的外力，在本系统中，设系统的三个广义坐标分别是x, 1, 2。系统动能T Tm Tm1 Tm2 Tm3 (2.2)其中，Tm为小车动能，Tm1为摆杆1的动能，Tm2为摆杆2动能，Tm3为质量块动能。系统势能V Vm1 Vm2 Vm3gghcos 1 2m3gl1 cos 1 m?g(2l1cos 1 l2cos 2) (2.3)经过推导，可得用！，I, 2, I'X表示的；如下:1 3 2migsin 1 4m>gsin 16m2l1 cos( 21)sin( 14m2x'cos 1 4m3xcos 12l1 ( 4mi 12m2 12m33m2g

26、cos( 22) 22/4m3gs in 122) 14%l2 si n( 13m2 cos( 12)cos9m> cos ( 12)Jsin 22mcos !阳)4 9口2【03(m2|m2l12l2 cos( 12 11 2m3)l112 3gsin 2 6l1 1 sin( 12) 3Xcos 222)6m2l2 2 sin( 12) 3(叶 2m2 2m3)(gsin 1xcos 1)/m2(m, 3 m, Bmjlll； 4mfl12l| cos2( 1 2) 9(2.5)2.1.2二级倒立摆线性模型建立将其在平衡位置处进展泰勒展开并线性化，可以得到状态空间方程如下K22K23

27、K27x00011000020000x000010K12K13020K22K230x1 000 010 100 020 010 03(2gm)14gm24gmi3)2(4mi3m212m3)h9m)2g2(4m13m212m3 )l13(2m1m24m<J2(4mi3m212m3 )l100100100000000y021.626.642g(mh 2mh 2mh)K17K1286.6964m2l2(mi 3m2 3m3) /1294g(g 3m)23 m3)34叫12(叶92(m| 2m2 2rni3)K17K27(2.6)40.3139.45(2.7)3m2 3m3)/l2164m2l

28、2(g 3m2 33 )l294(g 3 m2 m3)30.0882.2二级倒立摆t拥犬态调节器仿真二级倒立摆的状态调节器在原理上是与一级倒立摆一样的，故在此不再赘 述。关键还是求解代数黎卡提方程 P,进而求得反应矩阵K。二级倒立摆系统的 状态空间方程已由公式(2.6)给出。状态调节器的二次型最优性能指标中的Q矩阵与R矩阵形式如为Q11Q22Q, R 1(2.8)Q44Q55Q66只需改变Q矩阵中非零元素的值，即可求得不同性能指标下的反应矩阵。其中Q11代表小车位置的权重，Q22代表摆杆1角度的权重，Q33代表摆杆2角 度的权重，Q44代表小车速度的权重，Q55代表摆杆1角速度的权重，Q66代

29、表 摆杆2角速度的权重。系统的仿真模型如图22所示，设定角度初值103，205，小车位置，速度即摆杆1、2角速度初值均为0。图22二级倒立摆仿真模型其中动力学子模块如下图23动力学子模块Iip2_lqr_f.m为用m文件建立的动力学方程，即公式（2.4）、（2.5）。容如下:fun cti ondw = Iip2_lqr_f(par) ml = 0.05;m2 = 0.13;m3 = 0.236;11 = 0.0775;12 = 0.25;g = 9.831;theta1 = par(1); d_theta1 = par(2); theta2 = par(3);d_theta2 = par(4

30、); u = par(5);cl = (2* l1 * ( -4*m1 - 12*m2 - 12*m3 + 9*m2*cos(theta1-theta2)A2); al = - 2*m1*g*si n(thetal);a2 = - 4*m2*g*si n(theta1);a3 = - 4*m3*g*si n(theta1);a4 =3*m2*g*cos(theta2-theta1)*si n(theta2);a5 =6*m2*l1*cos(theta1-theta2)*si n(theta1-theta2)*d_theta1A2;a6 =4*m2*l2*si n(theta1-theta2)*

31、d_theta2A2;a7 =-2*m1*u*cos(theta1);a8 =-4*m2*u*cos(theta1);a9 =-4*m3*u*cos(theta1);a10= 3*m2*u*cos(theta1-theta2)*cos(theta2);dw(1)=3*(a1+a2+a3+a4+a5+a6+a7+a8+a9+a10)/c1;c2 = -16/9*m2*( m1+3*(m2+m3) )*l1A2*l2A2 + 4*(m2*l1*l2)A2*cos(theta1-theta2)A2;b1 =-4/9*m2*(m1+3*m2+3*m3)*l1A2*l2*(-3*g*si n(theta

32、2)-6*l1*d_theta1A2*si n(theta1-theta2)-3*u*cos(theta2);b2 = 2/3*m2*l1A2*l2*cos(theta1-theta2)*(6*m2*l2*d_theta2A2*si n(theta1-theta2);b3 =2/3*m2*l1A2*l2*cos(theta1-theta2)*(-3*(m1+2*m2+2*m3)*(g*si n(theta1)+u*cos(theta1)；dw(2) = -(b1+b2+b3)/c2;由于在一级倒立摆时已经对Q矩阵中各个非零元素取值不同对响应曲线造成的影响进展了充分的分析，二级摆与一级摆原理一样

33、，也遵循同样的结论。可 以确定的是速度项权重应比位置或角度的权重要小，在更看重稳态时间而对超调相对放松的情况下，Q44Q 66可以比Ql1Q 33小一个量级。而且摆杆角度的权 重取值过大对响应特性影响不大，故而可以不取太大的值。综上，可取 Q11=600，Q22=200，Q33=200，Q44 = 10，Q55 = 10，Q66 = 10。 计算得到反应矩阵K= 14.1421126.2368 -209.390816.68074.0884-34.0530。响应曲线如图24所示:角度变化曲线位置变化曲线图24二级倒立摆状态调节器仿真可以看出，即使在初始值设定比拟小时，角度的超调也是比拟大的，而且

34、摆 杆1即下面的摆杆角度绝对超调很大，这意味着在进展实物实验时，将摆杆竖起 至起控这个过程实现的可能比一级倒立摆时更加困难些， 而且摆杆1的竖起状况 对实验成功与否有很大影响。所以在进展实物实验时，应尽量将摆杆 1先竖直, 然后再缓缓转动摆杆2至系统起控。2.3二级倒立摆t胡犬态调节器实验将2.2中计算出的反应矩阵K应用于实物实验，得到角度与位置变化曲线如 图25所示。可以看出，最终角度和位置都稳定下来。摆杆1到达稳态时仍有12 ° 的角度波动，相比而言摆杆2的角度波动就很小了。这是由二级摆本身复杂的动 力学特性及实验硬件条件所决定的； 而位置变化曲线稳态时距离零点处较远， 稳 态偏差约为0.05m，与教师交流后认为这并不是算法问题，而是硬件导致的，只要二级倒立摆最终稳定且角度波动幅度不大，就可以认为