直线两级倒立摆实验指导

∂f 2 | . . . .. ∂x x =0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0 ∂f 2 | . . . .. ∂θ 2 x = 0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0
k 22 =
∂f 2 | . . . .. ∂θ1 x =0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0
' '' ' '
对于系统，设以下变量： xpend1 摆杆 1 质心横坐标； yangle1 摆杆 1 质心纵坐标； xpend2 摆杆 2 质心横坐标； yangle2 摆杆 2 质心纵坐标； xmass 质量块质心横坐标； ymass 质量块质心纵坐标； 又有：
©Googol 2005
136
第 6 章 直线两级倒立摆
L( q, q ) = T ( q, q ) − V ( q , q )
. . .
其中 L 为拉格朗日算子，q 为系统的广义坐标，T 为系统的动能，V 为系统的 势能。
d ∂L ∂L − = fi dt ∂ q. ∂qi i
其中 i＝1,2,3……n， f i 为系统在第 i 个广义坐标上的外力，在二级倒立摆系统 中，系统的广义坐标有三个广义坐标，分别为 x, θ 1 , θ 2 。 首先计算系统的动能：
©Googol 2005
139
第 6 章 直线两级倒立摆
k11 = ∂x@tD a dd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k12 = ∂θ1@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k13 = ∂θ2@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k14 = ∂x'@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. a'@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k15 = ∂θ1'@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k16 = ∂θ2'@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k17 = ∂x''@tD add ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k21 = ∂x@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k22 = ∂θ1@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k23 = ∂θ2@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k24 = ∂x'@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k25 = ∂θ1'@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k26 = ∂θ2'@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; k27 = ∂x''@tD bdd ê. x@tD → 0 ê. θ1@tD → 0 ê. θ2@tD → 0 ê. x'@tD → 0 ê. θ1 '@tD → 0 ê. θ2'@tD → 0 ê. x''@tD → 0; Simplify@k12D Simplify@k13D Simplify@k17D Simplify@k22D Simplify@k23D Simplify@k27D m1 = 0.05 ; m2 = 0.13; m3 = 0.236; l1 = 0.0775; l2 = 0.25; g = 9.8 ; k12 k13 k17 k22 k23 k27
系统的势能为：
V = Vm1 + Vm 2 + Vm 3 = m1 ypend1 + m2 ypend 2 + m3 ymass = m1l1Cosθ1 + m2 (2l1Cosθ1 + l 2 Cosθ 2 ) + 2m3 l1Cosθ1
由于系统在 θ 1 , θ 2 广义坐标下没有外力作用，所以有：
其中
k11 = k13 = ∂f 1 | . . . .. ∂x x =0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0 ∂f 1 | . . . .. ∂θ 2 x =0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0 k12 = ∂f1 | . . . .. ∂θ1 x =0,θ1 =0,θ 2 =0, x =0,θ1 =0,θ 2 =0, x =0 ∂f1 ∂x
倒立摆参数定义如下： M 小车质量
m1 m2
摆杆 1 的质量 摆杆 2 的质量 质量块的质量 摆杆 1 中心到转动中心的距离 摆杆 2 中心到转动中心的距离 摆杆 1 与竖直方向的夹角
m3
l1 l2
θ1
©Googol 2005
135
第 6 章 直线两级倒立摆
θ2
摆杆 2 与竖直方向的夹角
F 作用在系统上的外力 利用拉格朗日方程推导运动学方程： 拉格朗日方程为：
第 6 章 直两级倒立摆由直线运动模块和两级倒立摆组件组成。
6.1 系统物理模型
为简化系统， 我们在建模时忽略了空气阻力和各种摩擦， 并认为摆杆为刚体。 二级倒立摆的组成如图 6-1 所示：
摆杆 2
θ2
质量块 摆杆 1
θ1
F
小
x
车
导轨
图 6-1 直线两级倒立摆物理模型
TM = 1 Mcar ∗ x @tD2 ; 2
PRO 6-1 直线两级倒立摆建模 Mathematics 程序
xpend1 = x@tD − l1 ∗ Sin@θ1@tDD; ypend1 = l1 ∗ Cos@θ1@tDD; tpend1 = 1 ê 2 ∗ m1 ∗ HH∂t xpend1L ^2 + H∂t ypend1L ^2L + 1 ê 2 ∗ H1 ê 3 ∗ m1 ∗ l1 ^2L ∗ θ1'@tD ^2; xpend2 = x@tD − 2 ∗ l1 ∗ Sin@θ1@tDD − l2 ∗ Sin@θ2@tDD; ypend2 = 2 ∗ l1 ∗ Cos@θ1@tDD + l2 ∗ Cos@θ2@tDD; tpend2 = 1 ê 2 ∗ m2 ∗ HH∂t xpend2L ^2 + H∂t ypend2L ^2L + 1 ê 2 ∗ H1 ê 3 ∗ m2 ∗ l2 ^2L ∗ Hθ2'@tDL ^2; xmass = x@tD − 2 ∗ l1 ∗ Sin@θ1@tDD; ymass = 2 ∗ l1 ∗ Cos@θ1@tDD; tmass = 1 ê 2 ∗ m3 ∗ HH∂t xmassL ^2 + H∂t ymassL ^2L; v = m1 ∗ g ∗ ypend1 + m2 ∗ g ∗ ypend2 + m3 ∗ g ∗ ymass; Simplify@vD; lang = TM + tpend1 + tpend2 + tmass − v; Simplify@langD; ldad = ∂θ1'@tD lang; Simplify@ldadD; f1 = ∂t ldad − ∂θ1@tD lang; Simplify@f1D; ldbd = ∂θ2'@tD lang; Simplify@ldbdD; f2 = ∂t ldbd − ∂θ2@tD lang; Simplify@f2D; Solve@8f1 0 , f2 == 0< , 8θ1 ''@tD, θ2 ''@tD<D; add = θ1''@tD ê. %; bdd = θ2''@tD ê. %;
T = TM + Tm1 + Tm 2 + Tm3
其中
TM , Tm1 , Tm 2 , Tm3 分别为小车的动能，摆杆 1 的动能，摆杆 2 的动能和质量
块的动能。 小车的动能：
TM =
' '' ' ' . 2 1 Mx 2
Tm1 = Tm1 + Tm 2 其中 Tm1 , Tm 2 分别为摆杆 1 的平动动能和转动动能。 Tm 2 = Tm 2 + Tm 2 其中 Tm 2 , Tm 2 分别为摆杆 2 的平动动能和转动动能。
.
k14 =
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k15 =
∂f 1 ∂ θ1 ∂f 1 ∂x
.. .
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k16 =
∂f1 ∂θ2
.
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k17 =
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k 21 =
.
.
.
..
k 26 =
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k 27 =
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
在 Mathematics 中计算以上各式： （计算文件请参见“\GLIP2002\建模文件\MathematicaFile”中的“L2DIP.nb”文 件。 ）
..
..
将在平衡位置附近进行泰勒级数展开，并线性化，可以得到：
. . . .. ⎧ .. ⎪θ1 = k11 x + k12θ1 + k13θ 2 + k14 x + k15 θ1 + k16 θ 2 + k17 x ⎨ .. . . . .. ⎪ = + + + + + + θ k x k θ k θ k x k θ k θ k x 21 22 1 23 2 24 25 1 26 2 27 ⎩ 2
k 23 =
©Googol 2005
138
第 6 章 直线两级倒立摆
k 24 =
∂f 2 ∂x ∂f 2 ∂θ2
. .
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
.
.
.
..
k 25 =
∂f 2 ∂ θ1 ∂f 2 ∂x
.. .
|
x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0 ,θ1 = 0 ,θ 2 = 0 , x = 0
为求解状态方程：
. ⎧ ⎪ X = AX + Bu ⎨ ⎪ ⎩Y = CX
需要求解 θ1 和θ 2 因此设：
. . . .. ⎧ .. = ( , , , , , , ) f x x x θ θ θ θ θ ⎪ 1 1 1 2 1 2 ⎨ .. . . . .. ⎪ ⎩θ 2 = f 2 ( x,θ1 , θ 2 , x,θ1 ,θ 2 , x)
则有：
Tm1
' 2 2 ⎞ d xpend d ypend ( 1 ) ( 1 ) 1 ⎛ ⎛ ⎞ ⎛ ⎞ = m1 ⎜ ⎜ ⎟ +⎜ ⎟ ⎟ dt dt 2 ⎜ ⎠ ⎝ ⎠ ⎟ ⎠ ⎝⎝ . 2 . 2 1 1 2 J p1 θ = m1l1 θ 1 2 6
Tm1 =
''
同理：
Tm 2 =
' 2 2 1 ⎛ ⎛ d ( xpend 2) ⎞ ⎛ d ( ypend 2) ⎞ ⎞ + m2 ⎜ ⎜ ⎟ ⎜ ⎟ ⎟ dt dt 2 ⎜ ⎝ ⎠ ⎝ ⎠ ⎟ ⎝ ⎠ . 2 . 2 1 1 2 = J p 2 θ 2 = m2 l 2 θ 2 2 6
©Googol 2005
137
第 6 章 直线两级倒立摆
∂L ⎧ d ∂L ⎪ dt . − ∂θ = 0 1 ⎪ ∂ θ1 ⎨ d ∂L ∂L ⎪ =0 − . ∂θ 2 ⎪ dt ∂ θ 2 ⎩
对于二级倒立摆系统，系统状态变量为：
. . . ⎧ ⎫ ⎨ x,θ1 ,θ 2 , x,θ1 ,θ 2 ⎬ ⎩ ⎭
⎧ xpend1 = x − l1 Sinθ 1 ⎨ ⎩ ypend1 = l1Cosθ 1 ⎧ xpend 2 = x − 2l1 Sinθ 1 − l 2 Sin(θ 2 ) ⎨ ⎩ ypend 2 = 2l1Cosθ 1 + l 2 Cosθ 2 ⎧ xmass = x − 2l1 Sinθ 1 ⎨ ⎩ ymass = 2l1Cosθ 1