基于欧拉-拉格朗日方程的机器人动力学模型

机器人动力学方程

机器人动力学方程是描述机器人力和运动之间的关系的方程。只描述力和运动的关系，不考虑产生运动的力和扭矩。

欧拉 - 拉格朗日方程

欧拉-拉格朗日方程（OL）描述了处于完整约束下，并且约束力满足虚功原理的机械系统的力和运动随时间的变化。

有两种推导方法，先介绍使用牛顿第二定律的推导方法。

根据牛顿第二定律，某质点的运动方程是：
my¨=f−mgm\ddot y = f-mg my¨=f−mg
先对时间求导，再对y˙\dot yy˙求偏导，方程左侧可以写为：
my¨=ddt(my˙)=ddt∂∂y˙(12my˙2)=ddt∂K∂y˙m \ddot y = {d \over dt}(m \dot y)={d \over dt}{\partial \over \partial \dot y}({1 \over 2}m \dot y^2)={d \over dt}{\partial K \over \partial \dot y} my¨=dtd(my˙)=dtd∂y˙∂(21my˙2)=dtd∂y˙∂K
其中K=12my˙2K = {1 \over 2}m \dot y^2K=21my˙2，是动能。

接着将重力表示为：
mg=∂∂y(mgy)=∂P∂ymg = {\partial \over \partial y}(mgy) = {\partial P \over \partial y} mg=∂y∂(mgy)=∂y∂P
其中P=mgyP = mgyP=mgy表示重力势能。

定义拉格朗日算子LLL，表示系统动能与势能之差：
L=K−P=12my˙2−mgyL = K-P ={1 \over 2}m \dot y^2-mgy L=K−P=21my˙2−mgy
并且有：
∂L∂y˙=∂K∂y˙,∂L∂y=−∂P∂y{\partial L\over \partial \dot y} = {\partial K \over \partial \dot y},{\partial L \over \partial y} =-{\partial P \over \partial y} ∂y˙∂L=∂y˙∂K,∂y∂L=−∂y∂P

则初始的质点运动方程可化为：
ddt∂K∂y˙=f−∂P∂y{d \over dt}{\partial K \over \partial \dot y} = f-{\partial P \over \partial y} dtd∂y˙∂K=f−∂y∂P
即：
ddt∂L∂y˙−∂L∂y=f{d \over dt}{\partial L \over \partial \dot y}-{\partial L \over \partial y} = f dtd∂y˙∂L−∂y∂L=f
此方程被称为欧拉-拉格朗日方程。

推广到n自由度的系统，得到：
ddt∂L∂q˙k−∂L∂qk=τk{d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k dtd∂q˙k∂L−∂qk∂L=τk
其中τk\tau_kτk是与广义坐标qkq_kqk相关的力。

动能与势能

欧拉-拉格朗日方程可以直接用来推导动力学方程，前提是我们能够以一组广义坐标来表示该系统的动能和势能。如果要让这能够得到实际应用，那么我们就必须能够针对一个n连杆机器人计算出他的动能和势能。接下来将推到刚性连杆机器人动能和势能的表达式。

动能表示

刚体的动能可表示为平移动能和关于质心的旋转动能之和：
K=12mvTv+12ωTZωK = {1 \over 2}mv^Tv+{1\over 2}\omega^T Z\omega K=21mvTv+21ωTZω
ZZZ表示物体的惯性张量，是一个3*3的对称矩阵。

Z=RIRTZ = RIR^TZ=RIRT，R是附体坐标系与惯性坐标系之间的姿态变换。

III是附体坐标系中的惯性张量，仅取决于物体的形状和质量分布，与物体运动无关。

速度vvv和角速度ω\omegaω需要转置，因为要考虑多个维度的方向。连杆上任意一点的现速度和角速度可通过雅可比矩阵和关节速度（关节变量的导数）来表示：
vi=Jviqq˙ωi=Jωiqq˙v_i = J_{v_i}q\dot q \\ \omega_i = J_{\omega_i}q\dot q vi=Jviqq˙ωi=Jωiqq˙

机器人总动能可表示为：
K=12q˙T∑i=1n[mi(Jvi(q))TJvi(q)+(Jωi(q))TRi(q)Ii(Ri(q))TJωi(q)]q˙K = {1 \over 2}\dot q^T \sum_{i=1}^n[m_i\ {(J_{v_i}(q))}^T \ J_{v_i}(q) \ + \ (J_{\omega_i}(q))^T \ R_{i}(q) \ I_i \ (R_i(q))^T \ J_{\omega_i}(q)]\dot q K=21q˙Ti=1∑n[mi (Jvi(q))T Jvi(q) + (Jωi(q))T Ri(q) Ii (Ri(q))T Jωi(q)]q˙
用D(q)D(q)D(q)来表示机器人的惯性矩阵：
D(q)=∑i=1n[mi(Jvi(q))TJvi(q)+(Jωi(q))TRi(q)Ii(Ri(q))TJωi(q)]K=12q˙TD(q)q˙D(q) \ = \ \sum_{i=1}^n[m_i\ {(J_{v_i}(q))}^T \ J_{v_i}(q) \ + \ (J_{\omega_i}(q))^T \ R_{i}(q) \ I_i \ (R_i(q))^T \ J_{\omega_i}(q)] \\ K = {1 \over 2}\dot q^T D(q)\dot q D(q) = i=1∑n[mi (Jvi(q))T Jvi(q) + (Jωi(q))T Ri(q) Ii (Ri(q))T Jωi(q)]K=21q˙TD(q)q˙
机器人惯性矩阵D(q)D(q)D(q)有如下特点：

只与机器人构型有关
对称且正定
动能总是非负的

势能表示

在刚体动力学情形下，势能总是来源于重力。假设物体质量集中在质心，计算第iii个连杆的势能：
Pi=migTrciP_i = m_ig^Tr_{ci} Pi=migTrci
ggg是惯性坐标系中的重力向量，rcir_{ci}rci是连杆iii的质心坐标。机器人总势能为：
P=∑i=1nPi=∑i=1nmigTrciP = \sum_{i=1}^n P_i = \sum_{i=1}^n m_ig^Tr_{ci} P=i=1∑nPi=i=1∑nmigTrci
在m、g保持不变的情况下，机器人势能只与广义坐标rcir_{ci}rci有关。

运动方程

上面我们得到了如下结果：

系统动能是关于广义速度（坐标微分）的二次函数：
K=12q˙TD(q)q˙=12∑i,jndi,j(q)q˙iq˙jK = {1 \over 2}\dot q^T D(q)\dot q = {1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j K=21q˙TD(q)q˙=21i,j∑ndi,j(q)q˙iq˙j
系统势能是关于广义坐标的函数，且与广义速度无关：
P=∑i=1nmigTrciP = \sum_{i=1}^n m_ig^Tr_{ci} P=i=1∑nmigTrci
欧拉-拉格朗日算子为：
L=K−P=12∑i,jndi,j(q)q˙iq˙j−P(q)L = K-P ={1 \over 2}\sum_{i,j}^n d_{i,j}(q) \dot q_i \dot q_j-P(q) L=K−P=21i,j∑ndi,j(q)q˙iq˙j−P(q)
欧拉-拉格朗日方程为：
ddt∂L∂q˙k−∂L∂qk=τk{d \over dt}{\partial L \over \partial \dot q_k}-{\partial L \over \partial q_k} = \tau_k dtd∂q˙k∂L−∂qk∂L=τk
其中：
∂L∂q˙k=∑jdkjq˙jddt∂L∂q˙k=∑jdkjq¨j+∑jddtdkjq˙j=∑jdkjq¨j+∑i,j∂dkj∂qiq˙iq˙j∂L∂qk=12∑i,j∂di,j∂qkq˙iq˙j−∂P∂qk{\partial L \over \partial \dot q_k}= \sum_jd_{kj}\dot q_j\\ {d \over dt}{\partial L \over \partial \dot q_k} = \sum_jd_{kj}\ddot q_j+\sum_{j}{d \over dt}d_{kj}\dot q_j \\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ = \sum_jd_{kj}\ddot q_j+\sum_{i,j}{\partial d_{kj}\over \partial q_i}\dot q_i\dot q_j \\ {\partial L \over \partial q_k} = {1 \over 2}\sum_{i,j} {\partial d_{i,j} \over \partial q_k} \dot q_i \dot q_j - {\partial P \over \partial q_k} ∂q˙k∂L=j∑dkjq˙jdtd∂q˙k∂L=j∑dkjq¨j+j∑dtddkjq˙j =j∑dkjq¨j+i,j∑∂qi∂dkjq˙iq˙j∂qk∂L=21i,j∑∂qk∂di,jq˙iq˙j−∂qk∂P
因此对于每个k=1,2,...nk=1,2,...nk=1,2,...n，欧拉-拉格朗日方程可以写成：
∑jdkjq¨j+∑i,j{∂dkj∂qi−12∂di,j∂qk}q˙iq˙j−∂P∂qk=τk\sum_jd_{kj}\ddot q_j+\sum_{i,j}\{ {\partial d_{kj}\over \partial q_i} -{1\over 2}{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k j∑dkjq¨j+i,j∑{∂qi∂dkj−21∂qk∂di,j}q˙iq˙j−∂qk∂P=τk
即：
∑jdkjq¨j+∑i,j12{∂dkj∂qi+∂dkj∂qi−∂di,j∂qk}q˙iq˙j−∂P∂qk=τk\sum_jd_{kj}\ddot q_j+\sum_{i,j} {1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k}\} \dot q_i\dot q_j - {\partial P \over \partial q_k}= \tau_k j∑dkjq¨j+i,j∑21{∂qi∂dkj+∂qi∂dkj−∂qk∂di,j}q˙iq˙j−∂qk∂P=τk
定义Christoffel symbol：
cijk=cjik=12{∂dkj∂qi+∂dkj∂qi−∂di,j∂qk}c_{ijk} = c_{jik} ={1\over 2}\{ {\partial d_{kj}\over \partial q_i} + {\partial d_{kj}\over \partial q_i} -{\partial d_{i,j} \over \partial q_k} \} cijk=cjik=21{∂qi∂dkj+∂qi∂dkj−∂qk∂di,j}
定义广义重力：
gk=∂P∂qkg_k = {\partial P \over \partial q_k} gk=∂qk∂P
最终得到欧拉-拉格朗日方程：
∑jdkj(q)q¨j+∑i,jcijk(q)q˙iq˙j−gk(q)=τk\sum_jd_{kj}(q)\ddot q_j+\sum_{i,j} c_{ijk}(q) \dot q_i\dot q_j - g_k(q)= \tau_k j∑dkj(q)q¨j+i,j∑cijk(q)q˙iq˙j−gk(q)=τk
方程左侧三项分别为：

广义坐标的二阶导数：惯性项
广义坐标一阶导数的二次型：离心力项+哥氏力项
广义位置（0阶导数）重力项

方程可简写为：
D(q)q¨+C(q,q˙)q˙+g(q)=τD(q)\ddot q+C(q,\dot q)\dot q+g(q) = \tau D(q)q¨+C(q,q˙)q˙+g(q)=τ

推导平面2关节机器人的动力学模型

现在考虑下图中带有两个转动关节的平面机械臂。

要使用刚刚得到的欧拉-拉格朗日方程，就要与关节位置和关节速度相关的三个量：D(q),C(q,q˙),g(q)D(q),C(q,\dot q),g(q)D(q),C(q,q˙),g(q)。

首先使用雅可比矩阵来计算动能，计算平移速度：
vc1=Jvc1q˙vc2=Jvc2q˙Jvc1=[−lcsin(q1)0lc1cos(q1)000]Jvc2=[−l1sin(q1)−lc2sin(q1+q2)−lc2sin(q1+q2)l1cos(q1)+lc2cos(q1+q2)lc2cos(q1+q2)00]v_{c1} = J_{v_{c1}}\dot q \\ v_{c2} = J_{v_{c2}}\dot q \\ J_{v_{c1}} = \begin{bmatrix}-l_c sin(q_1) & 0 \\ l_{c1}cos(q_1) & 0\\ 0 & 0 \\\end{bmatrix} \\ J_{v_{c2}} = \begin{bmatrix}-l_1sin(q_1)-l_{c2}sin(q_1+q_2) & -l_{c2}sin(q_1+q_2) \\ l_1cos(q_1)+l_{c2}cos(q_1+q_2) & l_{c2}cos(q_1+q_2)\\ 0 & 0 \\\end{bmatrix} vc1=Jvc1q˙vc2=Jvc2q˙Jvc1=⎣⎡−lcsin(q1)lc1cos(q1)0000⎦⎤Jvc2=⎣⎡−l1sin(q1)−lc2sin(q1+q2)l1cos(q1)+lc2cos(q1+q2)0−lc2sin(q1+q2)lc2cos(q1+q2)0⎦⎤
平移部分对应的动能为：
12m1vc1Tvc1+12m2vc2Tvc2=12q˙{m1Jvc1TJvc1+m2Jvc2TJvc2}q˙{1\over 2}m_1 v^T_{c1}v_{c1}+{1\over2}m_2v^T_{c2}v_{c2} = {1\over 2}\dot q\{ m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} \}\dot q 21m1vc1Tvc1+21m2vc2Tvc2=21q˙{m1Jvc1TJvc1+m2Jvc2TJvc2}q˙
接下来考虑角速度项：
ω1=q˙1kω2=(q˙1+q˙2)k\omega_1 = \dot q_1k \\ \omega_2 = (\dot q_1+ \dot q_2)k ω1=q˙1kω2=(q˙1+q˙2)k
由于ωi\omega_iωi与每个关节坐标系的z轴对齐，旋转动能可以简单表示为12Iiωi2{1\over 2}I_i\omega_i^221Iiωi2，其中IiI_iIi是转动惯量，它的轴线穿过连杆i的质心且平行于ziz_izi轴。因此，就广义坐标而言，整个系统的旋转动能为：
12q˙T{I1[1000]+I2[1111]}q˙{1\over 2}\dot q^T \{ I_1 \begin{bmatrix}1 &0 \\ 0&0 \end{bmatrix} + I_2 \begin{bmatrix} 1&1\\1&1 \end{bmatrix}\}\dot q 21q˙T{I1[1000]+I2[1111]}q˙
惯性矩阵：
D(q)=m1Jvc1TJvc1+m2Jvc2TJvc2+[I1+I2I2I2I2]=[d11d12d21d22]D(q) = m_1J^T_{v_{c1}}J_{v_{c1}} +m_2J^T_{v_{c2}}J_{v_{c2}} + \begin{bmatrix} I_1+I_2&I_2 \\ I_2&I_2 \end{bmatrix} = \begin{bmatrix} d_{11}&d_{12} \\ d_{21}&d_{22} \end{bmatrix} \\ D(q)=m1Jvc1TJvc1+m2Jvc2TJvc2+[I1+I2I2I2I2]=[d11d21d12d22]
计算得：
d11=m1lc12+m2(l12+lc22+2l1lc2cos(q2))+I1+I2d12=d21=m2(lc22+l1lc2cos(q2))+I2d22=m2lc22+I2d_{11} = m_1l_{c1}^2 + m_2(l_1^2+l^2_{c2}+2l_1l_{c2}cos(q_2))+I_1+I_2\\ d_{12} = d_{21} = m_2(l^2_{c2}+l_1l_{c2}cos(q_2))+I_2 \\ d_{22} = m_2l^2_{c2}+I_2 d11=m1lc12+m2(l12+lc22+2l1lc2cos(q2))+I1+I2d12=d21=m2(lc22+l1lc2cos(q2))+I2d22=m2lc22+I2
我们已经得到了惯性矩阵，接下来计算Christoffel符号cijkc_{ijk}cijk：
c111=12∂d11∂q1=0c121=c211=12∂d11∂q2=−m2l1lc2sin(q2)=hc221=∂d12∂q2−12∂d11∂q1=hc112=∂d21∂q1−12∂d11∂q2=−hc122=c212=12∂d22∂q1=0c222=12∂d22∂q2=0c_{111}= {1 \over 2}{\partial d_{11}\over \partial q_1} = 0 \\ c_{121}= c_{211}= {1 \over 2}{\partial d_{11}\over \partial q_2} = -m_2l_1l_{c2}sin(q_2) = h \\ c_{221}= {\partial d_{12}\over \partial q_2}-{1 \over 2}{\partial d_{11}\over \partial q_1} = h \\ c_{112}= {\partial d_{21}\over \partial q_1}-{1 \over 2}{\partial d_{11}\over \partial q_2} = -h \\ c_{122}= c_{212}= {1 \over 2}{\partial d_{22}\over \partial q_1} = 0\\ c_{222}= {1 \over 2}{\partial d_{22}\over \partial q_2} = 0 \\ c111=21∂q1∂d11=0c121=c211=21∂q2∂d11=−m2l1lc2sin(q2)=hc221=∂q2∂d12−21∂q1∂d11=hc112=∂q1∂d21−21∂q2∂d11=−hc122=c212=21∂q1∂d22=0c222=21∂q2∂d22=0

接下来计算势能，机械臂的势能等于两个连杆势能之和。
P1=m1glc1sin(q1)P2=m2g(l2sin(q1)+lc2sin(q1+q2))P=P1+P2=(m1lc1+m2l1)gsin(q1)+m2lc2gsin(q1+q2)P_1 = m_1gl_{c1}sin(q_1) \\ P_2 = m_2g(l_{2}sin(q_1)+l_{c2}sin(q_1+q_2))\\ P = P_1+P_2 =(m_1l_{c1}+m_2l_1)gsin(q_1)+m_2l_{c2}gsin(q_1+q_2) P1=m1glc1sin(q1)P2=m2g(l2sin(q1)+lc2sin(q1+q2))P=P1+P2=(m1lc1+m2l1)gsin(q1)+m2lc2gsin(q1+q2)
之前的广义重力gkg_kgk可变为：
g1=∂P∂q1=(m1lc1+m2l1)gcos(q1)+m2lc2gcos(q1+q2)g2=∂P∂q2=m2lc2gcos(q1+q2)g_1 = {\partial P \over\partial q_1 } = (m_1l_{c1}+m_2l_1)gcos(q_1)+m_2l_{c2}gcos(q_1+q_2) \\ g_2 = {\partial P \over\partial q_2 } = m_2l_{c2}gcos(q_1+q_2) g1=∂q1∂P=(m1lc1+m2l1)gcos(q1)+m2lc2gcos(q1+q2)g2=∂q2∂P=m2lc2gcos(q1+q2)
最后可以写出系统的动力学方程：
d11q¨1+d12q¨2+c121q˙1q˙2+c211q˙2q˙1+c221q˙22+g1=τ1d21q¨1+d22q¨2+c112q˙12+g2=τ2d_{11}\ddot q_1+d_{12} \ddot q_2+c_{121}\dot q_1 \dot q_2 + c_{211}\dot q_2 \dot q_1 +c_{221}\dot q_2^2+g_1=\tau_1 \\ d_{21} \ddot q_1+d_{22}\ddot q_2+c_{112}\dot q_1^2+g_2 = \tau_2 d11q¨1+d12q¨2+c121q˙1q˙2+c211q˙2q˙1+c221q˙22+g1=τ1d21q¨1+d22q¨2+c112q˙12+g2=τ2
在这种情况下，原方程矩阵C(q,q˙)C(q, \dot q)C(q,q˙)由下式给出：
C=[hq˙2hq˙2+hq˙1−hq˙10]C = \begin{bmatrix}h\dot q_2 & h \dot q_2+h\dot q_1 \\ -h\dot q_1 & 0\end{bmatrix} C=[hq˙2−hq˙1hq˙2+hq˙10]