機器人動力學分析——拉格朗日方程

　　通常，機器人系統是一個非線性的複雜的動力學系統，在機器人動態實時控制系統中，必須分析其動力學特性。在機器人的動力學研究中，主要應用拉格朗日方程建立機器人的動力學方程。
　　拉格朗日力學的基礎是系統能量對系統變量及時間的微分，隨着系統複雜程度的增加，運動拉格朗日力學將變得相對簡單。拉格朗日力學主要以下面兩個基本方程爲基礎：一個針對直線運動，另一個針對旋轉運動。

一、拉格朗日方程

　　對於任何機械系統，拉格朗日函數L$L$ （又稱拉格朗日算子）定義爲系統總動能Ek$E_k$ 與總勢能Ep$E_p$ 之差，即：
　　

L=Ek−Ep$$L=E_k-E_p$$

　　由拉格朗日函數L$L$ 所描述的系統動力學狀態的拉格朗日方程爲：
　　

Fi=ddt(∂L∂q˙i)−∂L∂qi(i=1,2,...,n)$$F_i=\frac{d}{dt}\left(\frac{\mathrm{\partial }L}{\mathrm{\partial }{\dot{q}}_i}\right)-\frac{\mathrm{\partial }L}{\mathrm{\partial }{q}_i}\left(i=1,2,...,n\right)$$

　　式中，L$L$ 爲拉格朗日函數，n$n$ 爲連桿數目，qi$q_i$ 爲系統選定的廣義座標（直線座標還是轉角座標，即平移關節還是旋轉關節），q˙i${\dot{q}}_i$ 爲廣義速度，Fi$F_i$ 爲作用在第i$i$ 個座標系上的廣義力或力矩。代入拉格朗日函數可得：
　　

Fi=ddt∂Ek∂q˙i−∂Ek∂qi+∂Ep∂qi$$F_i=\frac{d}{dt}\frac{\mathrm{\partial }{E}_k}{\mathrm{\partial }{\dot{q}}_i}-\frac{\mathrm{\partial }{E}_k}{\mathrm{\partial }{q}_i}+\frac{\mathrm{\partial }{E}_p}{\mathrm{\partial }{q}_i}$$

　　(1)$(1)$ 系統的勢能Ep$E_p$ 僅是qi$q_i$ 的函數（由勢能公式可知），而動能Ek$E_k$ 是qi,q˙i$qi,{\dot{q}}_i$ 及時間t$t$ 的函數。
　　(2)$(2)$ 若qi$q_i$ 是線位移，則q˙i${\dot{q}}_i$ 是線速度，對應的廣義力Fi$F_i$ 就是力，若qi$q_i$ 是角位移，則q˙i${\dot{q}}_i$ 是角速度，對應的廣義力Fi$F_i$ 就是力矩。

二、平面關節機器人動力學分析

　　機器人是結構複雜的連桿系統，一般採用其次變換的方法，用拉格朗日方程建立系統動力學方程，對其位姿和運動狀態進行描述。其推導過程如下：
　　(1)$(1)$ 選取座標系，選定完全而且獨立的廣義變量關節qi,i=1,2,...,n$q_i,i=1,2,...,n$ ；
　　(2)$(2)$ 選定相應關節上的廣義力Fi$F_i$ ，當qi$q_i$ 是位移變量時，Fi$F_i$ 爲力；當qi$q_i$ 爲角度變量時，Fi$F_i$ 爲力矩；
　　(3)$(3)$ 求出機器人各構件的動能和勢能，構造拉格朗日函數；
　　(4)$(4)$ 代入拉格朗日方程，求得機器人系統的動力學方程。
　　下面以平面二自由度機器人爲例，推導機器人動力學方程。
　　

　　
　　

　　步驟1：選取廣義關節變量及廣義力
　　選取笛卡爾座標系。連桿1和連桿2的關節變量分別爲轉角θ1${\theta }_1$ 和θ2${\theta }_2$ ，關節1和關節2相應的力矩是τ1${\tau }_1$ 和τ2${\tau }_2$ 。連桿1和連桿2的質量分別是m1$m_1$ 和m2$m_2$ ，杆長分別爲l1$l_1$ 和l2$l_2$ ，重心分別在C1$C_1$ 和C2$C_2$ 處，離關節中心的距離分別爲d1$d_1$ 和d2$d_2$ 。
　　因此，杆1重心C1$C_1$ 的位置座標爲：

x1=d1sinθ1y1=−d1cosθ1$$\begin{gathered} x_1=d_{1}sin{\theta }_1 \\ {y}_1=-d_{1}cos{\theta }_1 \end{gathered}$$

　　則速度的平方和爲：
　　

x˙21+y˙21=(d1θ˙1)2$${\dot{x}}_1^2+{\dot{y}}_1^2={\left(d_1{\dot{\theta }}_1\right)}^2$$

　　杆2重心C2$C_2$ 的位置座標爲：

x2=l1sinθ1+d2sin(θ1+θ2)y2=−l1cosθ1−d2cos(θ1+θ2)$$\begin{gathered} x_2=l_{1}sin{\theta }_1+d_{2}sin\left({\theta }_1+{\theta }_2\right) \\ {y}_2=-l_{1}cos{\theta }_1-d_{2}cos\left({\theta }_1+{\theta }_2\right) \end{gathered}$$

　　則速度的平方和爲：
　　

x˙22+y˙22=l21θ˙21+d22(θ˙1+θ˙2)2+2l1d2(θ˙21+θ˙1θ˙2)cosθ2$${\dot{x}}_2^2+{\dot{y}}_2^2=l_1^2{\dot{\theta }}_1^2+d_2^2{\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)}^2+2l_1{d}_2\left({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2\right)cos{\theta }_2$$

　　步驟2 求系統動能

Ek=∑Eki(i=1,2)Ek1=12m1d21θ˙21Ek2=12m2l21θ˙21+12m2d22(θ˙1+θ˙2)2+m2l1d2(θ˙21+θ˙1θ˙2)cosθ2$$\begin{gathered} E_k=\sum E_{ki}(i=1,2) \\ {E}_{k1}=\frac{1}{2}{m}_1{d}_1^2{\dot{\theta }}_1^2 \\ {E}_{k2}=\frac{1}{2}{m}_2{l}_1^2{\dot{\theta }}_1^2+\frac{1}{2}{m}_2{d}_2^2{\left({\dot{\theta }}_1+{\dot{\theta }}_2\right)}^2+m_2{l}_1{d}_2\left({\dot{\theta }}_1^2+{\dot{\theta }}_1{\dot{\theta }}_2\right)cos{\theta }_2 \end{gathered}$$

　　速度即之前求的速度的平方和。
　　步驟3 求系統的勢能

Ep=∑Epi(i=1,2)Ep1=m1gd1(1−cosθ1)Ep2=m2gl1(1−cosθ1)+m2gd2(1−cos(θ1+θ2))$$\begin{gathered} E_p=\sum E_{pi}(i=1,2) \\ {E}_{p1}=m_{1}g{d}_1\left(1-cos{\theta }_1\right) \\ {E}_{p2}=m_{2}g{l}_1\left(1-cos{\theta }_1\right)+m_{2}g{d}_2\left(1-cos\left({\theta }_1+{\theta }_2\right)\right) \end{gathered}$$

　　計算勢能時，算的是相對的高度，即重心C1$C_1$ 轉動的高度。
　　步驟4 建立拉格朗日函數
　　

L=Ek−Ep$$L=E_k-E_p$$

　　步驟5 求系統的動力學方程
　　根據拉格朗日方程式計算各關節上的力矩，求系統動力學方程。
　　(1)$(1)$ 計算關節1$1$ 上的力矩τ1${\tau }_1$
　　

τ1=ddt∂L∂θ˙1−∂L∂θ1$${\tau }_1=\frac{d}{dt}\frac{\mathrm{\partial }L}{\mathrm{\partial }{\dot{\theta }}_1}-\frac{\mathrm{\partial }L}{\mathrm{\partial }{\theta }_1}$$

　　(2)$(2)$ 計算關節2$2$ 上的力矩τ2${\tau }_2$
　　

τ2=ddt∂L∂θ˙2−∂L∂θ2$${\tau }_2=\frac{d}{dt}\frac{\mathrm{\partial }L}{\mathrm{\partial }{\dot{\theta }}_2}-\frac{\mathrm{\partial }L}{\mathrm{\partial }{\theta }_2}$$

三、關節空間動力學方程

　　將章節二中得到的式子整理成矩陣形式：
　　

τ=D(q)q¨+H(q,q˙)+G(q)$$\tau =D\left(q\right)\ddot{q}+H\left(q,\dot{q}\right)+G\left(q\right)$$

　　其中τ=[τ1τ2]T$\tau ={\left[{\tau }_1{\tau }_2\right]}^T$ ;q=[θ1θ2]T$q={\left[{\theta }_1{\theta }_2\right]}^T$ ;q˙=[θ˙1θ˙2]T$\dot{q}={\left[{\dot{\theta }}_1{\dot{\theta }}_2\right]}^T$ ;q¨=[θ¨1θ¨2]T$\ddot{q}={\left[{\ddot{\theta }}_1{\ddot{\theta }}_2\right]}^T$

D(q)=[m1d21+m2(l21+d22+2l1d2cosθ2)m2(d22+l1d2cosθ2)m2(d22+l1d2cosθ2)m2d22]$$D\left(q\right)=\left[\begin{array}{cc}{m}_1{d}_1^2+m_2\left(l_1^2+d_2^2+2l_1{d}_{2}cos{\theta }_2\right)& m_2\left(d_2^2+l_1{d}_{2}cos{\theta }_2\right)\\ m_2\left(d_2^2+l_1{d}_{2}cos{\theta }_2\right)& m_2{d}_2^2\end{array}\right]$$

H(q,q˙)=[−m2l1d2sinθ2θ˙22−2m2l1d2sinθ2θ˙1θ˙2m2l1d2sinθ2θ˙21]$$H\left(q,\dot{q}\right)=\left[\begin{array}{c}-m_2{l}_1{d}_{2}sin{\theta }_2{\dot{\theta }}_2^2-2m_2{l}_1{d}_{2}sin{\theta }_2{\dot{\theta }}_1{\dot{\theta }}_2\\ m_2{l}_1{d}_{2}sin{\theta }_2{\dot{\theta }}_1^2\end{array}\right]$$

G(q)=[(m1d1+m2l1)gsinθ1+m2d2gsin(θ1+θ2)m2d2gsin(θ1+θ2)]$$G\left(q\right)=\left[\begin{array}{c}\left(m_1{d}_1+m_2{l}_1\right)gsin{\theta }_1+m_2{d}_{2}gsin\left({\theta }_1+{\theta }_2\right)\\ m_2{d}_{2}gsin\left({\theta }_1+{\theta }_2\right)\end{array}\right]$$

　　它反映了關節力矩與關節變量、速度、加速度之間的函數關係，對於n$n$ 個關節的操作臂，D(q)$D\left(q\right)$ 是n×n$n\times n$ 正定對稱矩陣，是q$q$ 的函數，稱爲慣性矩陣；H(q,q˙)$H\left(q,\dot{q}\right)$ 是n×1$n\times 1$ 離心力和科氏力；G(q)$G\left(q\right)$ 是n×1$n\times 1$ 重力矢量。