最优控制实验报告
二零一五年一月
目录
第1章一级倒立摆实验 (3)
1.1 一级倒立摆动力学建模 (3)
1.1.1 一级倒立摆非线性模型建立 (3)
1.1.2 一级倒立摆线性模型建立 (5)
1.2 一级倒立摆t
状态调节器仿真 (5)
∞
状态调节器实验 (9)
1.3 一级倒立摆t
∞
输出调节器仿真 (11)
1.4 一级倒立摆t
∞
输出调节器实验 (13)
1.5 一级倒立摆t
∞
1.6 一级倒立摆非零给定调节器仿真 (14)
1.7 一级倒立摆非零给定调节器实验 (16)
第2章二级倒立摆实验 (16)
2.1 二级倒立摆动力学模型 (16)
2.1.1 二级倒立摆非线性模型建立 (17)
2.1.2 二级倒立摆线性模型建立 (18)
状态调节器仿真 (19)
2.2 二级倒立摆t
∞
状态调节器实验 (21)
2.3 二级倒立摆t
∞
2.4 二级倒立摆t
输出调节器仿真 (22)
∞
输出调节器实验 (22)
2.5 二级倒立摆t
∞
2.6 二级倒立摆非零给定调节器仿真 (23)
2.7 二级倒立摆非零给定调节器实验 (24)
第1章一级倒立摆实验
1.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一级倒立摆非线性模型建立
采用拉格朗日方法，系统的拉格朗日方程为：
()()()
=-(1.1)
L q q T q q V q q
,,,
其中，L为拉格朗日算子，q为系统的广义坐标，T为系统的动能，V为系
统的势能。拉格朗日方程由广义坐标i q 和L 表示为：
i i i
d L L
f dt q q ??-=?? (1.2)
i f 为系统沿该广义坐标方向上的外力，在本系统中，系统的两个广义坐标分别为φ和x 。系统动能：
()2222111111112
cos 223
M m T T T Mx m x m l x m l φφφ=+=+++
(1.3)
系统的势能
11cos V m gl φ=
(1.4)
由于在广义坐标1θ上应用拉格朗日方程，由于此广义坐标上无广义力，则
0d L L
dt φφ
??-=?? (1.5)
得到：
()
2
cos sin mlx mgl I ml φφ
φ+=
+ (1.6)
在simulink 中建立非线性仿真动力学模型
图1-2 一级倒立摆非线性动力学模型
其中MATLAB Function 模块中代码如下：
function dw = 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.1.2 一级倒立摆线性模型建立
由(1.6)，且对于质量均匀分布的摆杆有21
3I ml =，将0.25l m =代入有
3(cos sin )x g φφφ=+
(1.7)
将其在平衡位置0φ=?处进行线性化，cos 1,sin φφφ==，且有29.831/g m s = 得到
29.4933x φφ=+
(1.8)
输入u x = ，将系统写为如下状态空间描述形式
10000000100
100
029.493031000000100x x x x u x x x y u φφφφφφφ????????????????????????=+????????????????
????????
????
????????==+??????????????
????
(1.9)
在simulink 中建立线性仿真动力学模型，只需将1.1.1里建立的非线性模型中MATLAB Function 模块代码更改为
dw = 29.493*phi+3*u;
1.2 一级倒立摆t ∞状态调节器仿真
对于线性定常系统的状态方程为
()()()x t Ax t Bu t =+ (1.10)
给定初始条件()00x t x =，终端时间f t =∞。求最优控制()*u t 使系统的二次型性能指标
01()()()()2
t J x t Qx t u t Ru t dt ττ∞??=+??? (1.11)
取极小值。 式中 ,,,A B Q R ——常数矩阵；
Q ——半正定对称阵；
R ——正定对称矩阵。
控制不受约束，最优控制存在且唯一，即
1()()()u t R B Px t Kx t τ*-=-=-
(1.12)
式中，P 为n n ?维正定常数矩阵，满足里卡提矩阵代数方程
10T T PA A P PBR B P Q -+-+=
(1.13)
对于线性定常系统无限时间状态调节器问题，要求系统完全能控。求解出上方程，即可得到最优控制*()u t 。
试验中的一级倒立摆模型可以线性化为定常系统，其中系数矩阵为
1
0000000010
029.4930A ?????
?=??????;0103B ??
????=????
-??
;10000010C ??=????;00D ??=????
公式(1.11)中选定不同的Q ，R 值，Q 4×4为半正定矩阵，R 1×1为正定矩阵，通过求解代数黎卡提方程（利用Matlab 里面的lqr 函数）可以得到最优控系数
(),,,K lqr A B Q R =
(1.14)
控制率为
()()u t Kx t =-
(1.15)
Q 、R 的形式可设计为
11223344,1Q Q Q R Q Q ??
????==????
?
? (1.16) 因为二次型最优控制是使得二次型性能指标取极小值，故只需改变Q 矩阵中
元素的值即可，不用改变R 的取值，即只要保证Q 与R 的相对大小即可。其中，Q 矩阵中Q 11代表小车位置的权重，Q 22代表小车速度的权重，Q 33代表摆杆角度的权重，Q 44为摆杆角速度的权重。仿真实验模型如下
图1-3仿真实验模型
设定角度初始值为10°，角速度与小车速度初值均为0。下面按照一定的依据选取Q 中非零元素的值进行仿真实验，并进行分析。取一组标准值方便对比Q 11=Q 22=Q 33=Q 44=2。响应曲线如下图，在后续研究中，若无特殊说明Q 中元素分别取此标准值。考虑到实际系统中小车轨道长度有限，取上述参数时发现位置相对零点波动的绝对值最大达到了0.3m 以上，这在实际系统中是难以正常进行试验的，所以要对参数进行调整改进，下面分别研究各个参数变化时对系统响应的影响。
t(s)
θ (°)
角度变化曲线
t(s)
x (m )
位置变化曲线
图1-4 Q 11=Q 22=Q 33=Q 44=2时角度与位置变化曲线
(1) 分析小车位置的权重对于响应曲线的影响。
其他参数不变的情况下，小车位置权重Q 11分别取为2、20、200、1000时观察角度与位置变化曲线如图1-1图1-5所示。
t(s)θ (°)
角度变化曲线
t(s)
x (m )
位置变化曲线
图1-5 位置权重对响应的影响
由图1-5可以看出，随着Q 11的增加，角度变化曲线的稳态时间缩短，但超调量有所增大；位置变化曲线特性改进明显，稳态时间与绝对的超调值都显著减小，可见增大Q 11的值会改进系统特性。
(2) 分析小车速度的权重对响应曲线的影响
小车速度权重Q 22分别取为2、20、200、1000时得到角度与位置随时间变化曲线如图1-6所示
t(s)θ (°)
角度变化曲线
t(s)
x (m )
位置变化曲线
图1-6 小车速度权重对响应曲线的影响
随着Q 22的增大，角度曲线特性得到一定改善，绝对超调减小，且稳态时间减小；但对于小车位置曲线来说，虽然绝对超调变小了，但很明显稳态时间大大增加了，由于Q 22代表的是小车的速度权重，可以类比为引入了阻尼项，减小超调的同时会增大稳态时间，这是我们并不希望的。故而Q 22的值不能太大，要保证Q 22取值不超过Q 11。
(3)
分析摆杆角度的权重对响应曲线的影响
小车速度权重Q 33分别取为2、20、200、1000时得到角度与位置随时间变化曲线如图1-7所示
t(s)θ (°)
角度变化曲线
t(s)
x (m )
位置变化曲线
图1-7摆杆角度权重对响应曲线的影响
随着Q 33的增大，角度曲线的绝对超调减小，但是相应的导致了稳态时间的增加；小车位置相应曲线超调减小，同样的也是稳态时间增加了。而且可以看出，Q 33对小车位置曲线的影响远不如Q 11和Q 22对小车位置响应的影响。
(4)
分析摆杆角速度的权重对响应曲线的影响
小车速度权重Q 44分别取为2、20、200、1000时得到角度与位置随时间变化曲线如图1-8所示
由图1-8可知，随着Q 44的增大，角度变化曲线稳态时间有一定程度的增加，曲线变化稍见平缓，即曲线斜率的最大值变小了，但绝对超调基本没变；小车位置的响应特性随Q 44的增大而变坏，绝对超调大幅上升，稳态时间也明显变长。
所以Q 44的值不能取的太大。要注意的是，Q 44取值变化过程中Q 矩阵其他元素取的均为上文所提标准值，标准值取的是很小的，所以在确定参数时，只要保证Q 44的值不能比Q 33大即可，图1-8只是提供了分析的依据，不能直接根据上图的曲线进行选择。
t(s)θ (°)
角度变化曲线
t(s)
x (m )
位置变化曲线
图1-8摆杆角速度权重对响应曲线的影响
以上分析为Q 矩阵中非零元素的选取提供了一定的依据，总的来说Q 11与Q 33
的值越大越好，但过大的话可能会对执行器即电机提出过高的要求，而Q 22与Q 44的取值尽量不能比其他两个元素值大。
1.3 一级倒立摆t ∞状态调节器实验
根据以上分析，选取几组实物实验Q 矩阵中的元素值，并将仿真结果与之对比如图1-9至图1-11所示，对比仿真结果与实验结果的异同，分析产生此现象的原因。由于仿真与实物实验的初始条件很难做到完全一致，如对于实物实验来说，由于编码器为一相对式码盘，所以倒立摆稳定状态为-π而不是仿真实验中的0 rad ，而且由于实物实验中倒立摆是由下垂状态人为慢慢上摆至满足倒立摆稳定系统起控条件的，在缓慢移动过程中，很难做到倒立摆起控时摆杆的角速度为0，即初始条件难以精确确定。所以只需比较仿真与实物实验得到的曲线特性中如绝对超调，稳态时间即可。此外，在实验过程中，可以发现倒立摆摆杆转动到大概为平衡位置附近10°时，倒立摆起控，这样在处理实验数据时可以将起控之前的无控状态去掉，只将有效的部分画出来即可，方便观察曲线特性。
表1-1 状态调节Q 矩阵中非零元素不同取值
Q 11 Q 22 Q 33 Q 44 第一组 1000 1000 100 100 第二组 100 100 1000 1000 第三组 1000
1000
1000
1000
5
10-5
05
10
t(s)
θ (°
)
510
-0.2
-0.15-0.1-0.05
0t(s)
x (m )
510
-180
-175
-170
-165角度变化曲线(实验)
t (s)
θ (°
)0
5
10
-0.05
0.050.1
0.15位置变化曲线(实验)
t (s)
x (m )
图1-9 第一组状态调节器参数下响应图
5
10-5
5
10
t(s)
θ (°
)角度变化曲线(仿真)
510
-0.4
-0.3-0.2-0.1
0t(s)
x (m )
位置变化曲线(仿真)
5
10
-180
-175
-170
-165角度随时间变化曲线
t (s)
θ (°
)0
510
-0.05
0.050.1
0.15位置随时间变化曲线
t (s)
x (m )
图1-10 第二组状态调节器参数下响应图
510
-505
10
t(s)
θ (°
)0
5
10
-0.2
-0.15-0.1-0.05
0t(s)
x (m )
5
10
-180
-175
-170
-165
角度随时间变化曲线
t (s)
θ (°
)0510
-0.06
-0.04
-0.0200.02
0.04
位置随时间变化曲线
t (s)
x (m )
图1-11 第三组状态调节器参数下响应图
第一组参数下，仿真与实物实验得到的曲线特性吻合较好，稳态时间与绝对超调量都比较相近；但第二组参数位置曲线的超调相差较大，分析原因可能是在将倒立摆扶至起控位置左右时没有缓缓转动导致起控时摆杆有一定的角速度，初始条件相差较大导致曲线相差较大；第三组参数下实物实验得到的角度与位置曲线都存在稳态误差，尤其是位置误差为5cm 左右，误差比较大，分析原因可能是系统的硬件问题，因为就算法来说，状态调节器是不可能将末态稳定在非零点出的。
1.4 一级倒立摆t ∞输出调节器仿真
对于线性定常系统
()()()
x Ax t Bu t y Cx t =+= (1.17)
给定初始条件00()x t x =，终端时间f t =∞。求最优控制*()u t ，使系统的二次型性能指标为
01()()()()()()2
t J y t Q t y t u t R t u t dt ττ
∞??=
+??? (1.18)
要求系统完全能观测，且控制不受约束。则可求解代数黎卡提方程得到正定对称矩阵P
10T T T PA A P PBR B P C QC -+-+=
(1.19)
最优控制存在且唯一
*1()()u t R B Px t τ-=-
(1.20)
此时倒立摆系统的Q 为2×2阶的，若设计
1133Q Q Q ??
=???
? (1.21)
则
11331000001000T
C Q C QC Q ??=??
??
?????
?=?????
?
(1.22)
所以在给定Q 的上述形式后可以发现，输出调节器和的代数黎卡提方程的形式与状态调节器时是一致的，只需将状态调节器中Q 的第二行第二列和第四行第四列的元素值设置为零，调节Q 11和Q 33计算出的反馈比例系数既是输出调节器下的反馈系数。
设定标准状态为Q 11=Q 33=100，选取不同的参数进行仿真并对比曲线特性。如至所示
510-10-50510t(s)
θ (°
)角度变化曲线
510
-0.3-0.2-0.10
0.1t(s)
x (m )
位置变化曲
线
图1-12 输出调节器下小车位置权重对曲线的影响
上图为当Q 33=100，时Q 11分别取10、100和1000时的响应曲线，可以看出，增大小车位置的权重可有效缩短稳态时间，并减小小车位置变化曲线的绝对超
调，但是会增加摆杆变化曲线的绝对超调。
510-10-50510t(s)
θ (°
)角度变化曲线
510
-0.2-0.15-0.1-0.050
0.05t(s)
x (m )
位置变化曲线
图1-13输出调节器下摆杆角度权重对曲线的影响
Q 11=100，Q 33分别取10、100、1000时摆杆角度和小车位置的响应曲线如上图，提高Q 33的值可减响应曲线的超调，对角度曲线的稳态时间无大的影响，但会增加位置响应的稳态时间。
1.5 一级倒立摆t ∞输出调节器实验
选取几组实物实验Q 矩阵中的元素值，如表1-2所示。得到各组参数下摆杆角度和小车位置响应如至所示。
表1-2 输出调节实验选定参数
Q 11 Q 33 第一组参数 1000 100 第二组参数 100 1000 第三组参数 1000
1000
1
23
4
5
165
170
175
180
角度变化曲线(实验)
t (s)
θ (°
)012345
-0.02
00.020.04
0.06
位置变化曲线(实验)
t (s)
x (m )
图1-14输出调节器第一组参数下响应曲线
1
23
4
5
165
170
175
180
角度变化曲线(实验)
t (s)
θ (°
)0
2
4
6
8
-0.05
0.05
位置变化曲线(实验)
t (s)
x (m )
图1-15输出调节器第二组参数下响应曲线
1
23
4
5
165
170
175
180
角度变化曲线(实验)
t (s)
θ (°
)0
1
23
4
5
-0.05
0.05
位置变化曲线(实验)
t (s)
x (m )
图1-16输出调节器第三组参数下响应曲线
由图1-14至图1-16可以看出，各组参数下响应的稳态时间与绝对超调指标都相当好，对比发现甚至优于仿真结果，分析原因与上相同即在手动将摆杆转动至起控位置时可能没有把握好摆杆角速度的变化，导致角速度初值过大，分析可以发现当摆杆角速度有一定初值且方向与手动摆起的旋向一致时，是利于倒立摆的摆起的，所以响应曲线性能变好。
1.6 一级倒立摆非零给定调节器仿真
从本质上来说，非零给定点调节器是基于传统的传递函数的角度来分析的。非零给定调节器指的是给定一个位置信息，使得倒立摆稳定后小车稳定在给定的位置上。则可以将小车位置作为输出，小车加速度作为输入，系统要做的是使输出值与输入值相等。
引入状态反馈后的系统传递函数矩阵为
1()()s C sI A BK B -Φ=-+
(1.23)
注意到其为一21?的矩阵，对应的为单输入双输出系统，输入是小车加速度，输出是小车位置及摆杆角度。当系统稳定即时间趋于无穷时，由拉普拉斯终值定
理可知传递函数变为
1(0)()C A BK B -Φ=-+
(1.24)
此可以视作闭环系统的直流增益，简单的说就是一比例系数，第一行第一列为输出到小车位置的直流增益，一般情况下是不为1的，这就引出了非零给定调节器的问题。当利用lqr 算法求出反馈系数矩阵K 时，计算出(0)Φ，并将第一
行第一列的元素取倒数表示为1
11(0)-Φ，将给定的位置与此数相乘后再作为输入来
控制小车，这样既可以达到非零给定的目的，图1-3已经simulink 模块实现展
示，1
11
(0)-Φ即为图1-3中的Wc(0)^-1。 由上文中对状态调节器和输出调节器的仿真及实物实验可以看出，当改变Q 矩阵中代表小车速度和摆杆角速度元素的值为非零时，可以改善响应的阻尼特性，但同时会使稳态时间特性受到较大影响。非零给定点的仿真中采用输出调节器的形式。选取Q 11=1000，Q 33=200，期望小车稳定位置y d =0.2m 。
计算出状态反馈矩阵K=[-31.6228 -20.1304 72.8210 13.1537]，代入公式(1.24)求得
-0.0316(0)0??
Φ=??
??
1
11(0)31.6228-Φ=-
仿真结果如图1-17所示。
1
2
3
4
-15-10-505
10t(s)
θ (°
)角度变化曲线(仿真)
01
234
-0.2
-0.1
00.10.2
0.3t(s)
x (m )
位置变化曲线(仿真)
图1-17非零给定输出调节器响应曲线
小车位置稳定在距离原点0.2m 处。
可以发现求出的(0)Φ中21(0)Φ为零，且已知21()s Φ代表输入为加速度，输
出为摆杆角度的传递函数，传递函数有零点，1
21(0)-Φ没有意义，
这在实际物理系统中也是明确的，即不能使二次型最优控制下的倒立摆系统摆杆稳定在不平衡的
位置。细心观察还可以发现，1
11
(0)-Φ就等于反馈矩阵K 中的第一个元素。
1.7 一级倒立摆非零给定调节器实验
参数与仿真中一致，得到实物实验下非零给定调节器的小车位置及摆杆角度曲线如图1-18所示
1
23
4
5
-180-175
-170
-165角度变化曲线(实验)
t (s)
(°
)0
1
23
4
5
00.1
0.2
位置变化曲线(实验)
t (s)
x (m
)
图1-18 非零给定调节器实物实验响应曲线
第2章 二级倒立摆实验
2.1 二级倒立摆动力学模型
为简化系统，我们在建模时忽略了空气阻力和各种摩擦，并认为摆杆为刚体。 二级倒立摆的组成如图2-1所示。
图2-1 直线两级倒立摆物理模型
倒立摆参数定义如下：
M ——小车质量；
m 1——摆杆1的质量，为0.05kg ； m 2——摆杆2的质量，为0.13kg ； m 3——质量块的质量，为0.236kg ；
1l ——摆杆1中心到转动中心的距离，为0.0775m ； 2l ——摆杆2中心到转动中心的距离，为0.25m ；
1θ——摆杆1与竖直方向的夹角，规定逆时针为正；
2θ——摆杆2与竖直方向的夹角，规定逆时针为正；
F ——作用在系统上的外力；
2.1.1 二级倒立摆非线性模型建立
利用拉格朗日方程推导运动学方程： 拉格朗日方程为：
(,)(,)(,)
i
i i
L q q T q q V q q d L L
f dt q q =-??-=?? (2.1)
其中，L 为拉格朗日算子，q 为系统的广义坐标，T 为系统的动能，V 为系统的势能。1,2,...,i n =，i f 为系统沿该广义坐标方向上的外力，在本系统中，设系统的三个广义坐标分别是12,,x θθ。
系统动能
123M m m m T T T T T =+++
(2.2)
其中，M T 为小车动能，1m T 为摆杆1的动能，2m T 为摆杆2动能，3m T 为质量块动能。
系统势能
12311131121122cos 2cos (2cos cos )m m m V V V V m gl m gl m g l l θθθθ=++=+++ (2.3) 经过推导，可得用1122,,,,x θθθθ表示的12,θθ如下：
11121312212
222121121221221121312122211232123[2sin 4sin 4sin 3cos()sin 6cos()sin()4sin()2cos 4cos 4cos 3cos()cos ]/[2(412129cos ())]
m g m g m g m g m l m l m x m x m x m l m m m m θθθθθθθθθθθθθθθθθθθθθθθ=---+-+--+----+----+- (2.4)
22221231221112222212122221212311222222212312212124{[3()][3sin 6sin()3cos ]
92
cos()[6sin()3(22)(sin cos )]}/316[(33)4cos ()]9
m m m m l l g l x m l l m l m m m g x m m m m l l m l l θθθθθθθθθθθθθθθ=--++----+---+++-+++- (2.5)
2.1.2 二级倒立摆线性模型建立
将其在平衡位置处进行泰勒展开并线性化，可以得到状态空间方程如下
11221213171122
23
272212000
10000000100000001000000010000000010000001000000x x x x K K K K K K x y θθθθθθθθθθ????
?????????????
???
????????
????=+?
???
????????????????
????????
????????????????????
????==??????12120010000x u x θθθθ??
????????????????+???????
???????????????
(2.6)
123121231
2131231
123171231
12322221232
12323223(244)
86.69
2(4312)921.62
2(4312)3(24)
6.64
2(4312)2(22)
40.31
6
4(33)/9
4(33)
16
3[4(9
gm gm gm K m m m l m g
K m m m l m m m K m m m l g m m m K m l m m m l g m m m K m l ---==---==-------=
=---++=
=-1-++++=--123212312327221232
39.45
33)/]
4
2(22)(3)
30.088
16
4(33)9m m m l m m m m m m K m l m m m l =++++-++==--++ (2.7)
2.2 二级倒立摆t ∞状态调节器仿真
二级倒立摆的状态调节器在原理上是与一级倒立摆相同的，故在此不再赘述。关键还是求解代数黎卡提方程P ，进而求得反馈矩阵K 。二级倒立摆系统的状态空间方程已由公式(2.6)给出。状态调节器的二次型最优性能指标中的Q 矩阵与R 矩阵形式如为
112233
44
55
66,1Q Q Q Q R Q Q Q ??
??????
==?
?????????
?
? (2.8)
只需改变Q 矩阵中非零元素的值，即可求得不同性能指标下的反馈矩阵。其中Q 11代表小车位置的权重，Q 22代表摆杆1角度的权重，Q 33代表摆杆2角度的权重，Q 44代表小车速度的权重，Q 55代表摆杆1角速度的权重，Q 66代表摆杆2角速度的权重。
系统的仿真模型如图2-2所示，设定角度初值102035θθ==-，，小车位置，速度即摆杆1、2角速度初值均为0。
图2-2 二级倒立摆仿真模型
其中动力学子模块如下
图2-3 动力学子模块
lip2_lqr_fcn.m为用m文件建立的动力学方程，即公式(2.4)、(2.5)。容如下：
function dw = lip2_lqr_fcn(par)
m1 = 0.05;
m2 = 0.13;
m3 = 0.236;
l1 = 0.0775;
l2 = 0.25;
g = 9.831;
theta1 = par(1);
d_theta1 = par(2);
theta2 = par(3);
d_theta2 = par(4);
u = par(5);
c1 = (2* l1 * ( -4*m1 - 12*m2 - 12*m3 + 9*m2*cos(theta1-theta2)^2));
a1 = - 2*m1*g*sin(theta1);
a2 = - 4*m2*g*sin(theta1);
a3 = - 4*m3*g*sin(theta1);
a4 = 3*m2*g*cos(theta2-theta1)*sin(theta2);
a5 = 6*m2*l1*cos(theta1-theta2)*sin(theta1-theta2)*d_theta1^2;
a6 = 4*m2*l2*sin(theta1-theta2)*d_theta2^2;
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) )*l1^2*l2^2 + 4*(m2*l1*l2)^2*cos(theta1-theta2)^2;
b1 =
-4/9*m2*(m1+3*m2+3*m3)*l1^2*l2*(-3*g*sin(theta2)-6*l1*d_theta1^2*sin(theta1-theta2)