基于LQR球杆系统的控制方法研究与设计仿真(4)
其中J_l为球杆的转动惯量，且J_l=1/3 ML^2
故系统的动能总和为：
T=T_1+T_2+T_3 (2.5)
其次，求系统的势能。小球沿r方向位移引起的势能V_1=mgrsinα, 此外，球杆的势能也要考虑进去，球杆的质量为M，故得到
V_2=1/2 MgLsinα
最后分析作用在广义坐标下各坐标方向的外力，小球位移r的坐标系没有外力作用，而小球转角α的坐标系只受到外力施加的扭矩τ。
综上，得到拉格朗日函数：
L=T-V=T_1+T_2+T_3-V_1-V_2
即L=1/2 m(r ̇^2+r^2 α ̇^2) +1/2 J r ̇^2/R^2 +1/2 J_l σ ̇^2-mgrsinα-1/2 MgLsinα
令广义坐标(q_1,q_2)=(r,α),因此可以得到
d/dt(δL/(δ(q_j ) ̇ ))=(((J/R^2 +m)r ̈)¦(〖(J〗_l+mr^2)α ̈+2mrr ̇α ̇ )),δL/(δq_j ) ((mrα ̇^2-mgsinα)¦(-(mgr+1/2 MgL)cosα)),τ_j=(0¦τ).
由d/dt(δL/(δ(q_j ) ̇ ))- δL/(δq_j )=τ_j得到如下球杆系统的非线性数学模型：
{█((J/R^2 +m)r ̈-mrα ̇^2+mgsinα=0@〖(J〗_l+mr^2)α ̈+2mrr ̇α ̇+(mgr+1/2 MgL)cosα=τ)┤ (2.6)
假设期望角度α在0附近，则可以在平衡点0附近对其进行线性化，并代入式（2.6），得到线性方程：
r ̈=mg/((J/R^2 +m) ) α=-mgd/L(J/R^2 +m) θ (2.7)
由齿轮间的关系可得
θ=R_1/R_2 β (2.8)
2.2.2 角度关系模型
横杆的倾斜角α和大齿轮转角θ之间的关系如图2.2示：
图2.2电机转角与球杆转角关系图
从图2.2，可以推导出横杆倾角α和大齿轮转角θ的关系，令l为连杆长度，d为连杆下端到大齿轮中心的距离a、b、p、q的长度分别如图2.3因此有如下关系式：
sin〖α=a/L〗 (2.9)
a+b=√(l^2-〖(p-q)〗^2 ) (2.10)
p=d-d*cosθ (2.11)