文章目录
本文的公式推导见于蔡自兴的《机器人学》，目的是得到刘金琨的《机器人控制系统的设计与MATLAB仿真》2.12示例中的动力学模型。
拉格朗日函数
拉格朗日函数 L L L是系统的动能和位能之差 L = K − P L=K-P L=K−P，拉格朗日方程可表示为
F i = d L d t ∂ L ∂ q ˙ − ∂ L ∂ q i , i = 1 , 2 , ⋯ , n F_i=\frac{\mathbf d L}{\mathbf dt}\frac{\partial L}{\partial\dot q}-\frac{\partial L}{\partial q_i}, i=1,2,\cdots, n Fi=dtdL∂q˙∂L−∂qi∂L,i=1,2,⋯,n
式中， q i q_i qi为坐标， ⋅ q i \cdot q_i ⋅qi为速度， F i F_i Fi为力或力矩， n n n为连杆数目。
二关节机器人系统建模
现考虑二连杆机械手，如下图所示， m 1 , m 2 m_1, m_2 m1,m2为两个连杆的质量， d 1 , d 2 d_1,d_2 d1,d2为二者长度， g g g为重力加速度。 θ 1 , θ 2 \theta_1,\theta_2 θ1,θ2为广义坐标，设两杆在 x O y xOy xOy平面内摆动， y y y为竖直坐标轴，重力方向为 − y -y −y，则 θ 1 \theta_1 θ1是杆1和 − y -y −y方向的夹角， θ 2 \theta_2 θ2为杆2和杆1的夹角。
则杆1的动能和位能可表示为
K 1 = 1 2 m 1 v 1 2 = 1 2 m 1 d 1 2 ⋅ θ 1 2 P 1 = m 1 g h 1 = − m 1 g d 1 cos θ 1 \begin{aligned} K_1=&\frac{1}{2}m_1v_1^2&=&\frac{1}{2}m_1d_1^2\cdot\theta_1^2\\ P_1=&m_1gh_1&=&-m_1gd_1\cos\theta_1 \end{aligned} K1=P1=21m1v12m1gh1==21m1d12⋅θ12−m1gd1cosθ1
杆2在水平和竖直方向的位置可以记作
x 2 = d 1 sin θ 1 + d 2 sin ( θ 1 + θ 2 ) y 2 = − d 1 cos θ 1 − d 2 cos ( θ 1 + θ 2 ) x_2=d_1\sin\theta_1+d_2\sin(\theta_1+\theta_2)\\ y_2=-d_1\cos\theta_1-d_2\cos(\theta_1+\theta_2) x2=d1sinθ1+d2sin(θ1+θ2)y2=−d1cosθ1−d2cos(θ1+θ2)
则其速度可以表示为
v 2 2 = x ˙ 2 2 + y ˙ 2 2 = d 1 cos θ 1 θ ˙ 1 + d 2 cos ( θ 1 + θ 2 ) ( θ ˙ 1 + θ ˙ 2 ) + d 1 sin θ 1 θ ˙ 1 + d 2 sin ( θ 1 + θ 2 ) ( θ ˙ 1 + θ ˙ 2 ) v_2^2=\dot x_2^2+\dot y_2^2=d_1\cos\theta_1\dot\theta_1+d_2\cos(\theta_1+\theta_2)(\dot\theta_1+\dot\theta_2)+d_1\sin\theta_1\dot\theta_1+d_2\sin(\theta_1+\theta_2)(\dot\theta_1+\dot\theta_2) v22=x˙22+y˙22=d1cosθ1θ˙1+d2cos(θ1+θ2)(θ˙1+θ˙2)+d1sinθ1θ˙1+d2sin(θ1+θ2)(θ˙1+θ˙2)
从而可以求得其动能和位能
K 2 = 1 2 m 2 v 2 2 = 1 2 m 2 d 1 2 ⋅ θ 1 2 + 1 2 m 2 d 2 2 ( θ ˙ 1 + θ ˙ 2 ) 2 + m 2 d 1 d 2 cos θ 2 ( θ ˙ 1 2 + θ ˙ 1 θ ˙ 2 ) P 2 = m 2 g h 2 = − m 2 g d 2 cos θ 1 − m 2 g d 2 cos ( θ 1 + θ 2 ) \begin{aligned} K_2=&\frac{1}{2}m_2v_2^2&=&\frac{1}{2}m_2d_1^2\cdot\theta_1^2+\frac{1}{2}m_2d_2^2(\dot\theta_1+\dot\theta_2)^2+m_2d_1d_2\cos\theta_2(\dot\theta_1^2+\dot\theta_1\dot\theta_2)\\ P_2=&m_2gh_2&=&-m_2gd_2\cos\theta_1-m_2gd_2\cos(\theta_1+\theta_2) \end{aligned} K2=P2=21m2v22m2gh2==21m2d12⋅θ12+21m2d22(θ˙1+θ˙2)2+m2d1d2cosθ2(θ˙12+θ˙1θ˙2)−m2gd2cosθ1−m2gd2cos(θ1+θ2)
则二连杆机械手系统的总动能和总位能分别为
K = K 1 + K 2 = 1 2 ( m 1 + m 2 ) d ˙ 1 2 θ ˙ 1 2 + 1 2 m 2 d 2 2 ( θ ˙ 1 + θ ˙ 2 ) 2 + m 2 d 1 d 2 cos θ 2 ( θ ˙ 1 2 + θ ˙ 1 θ ˙ 2 ) P = P 1 + P 2 = − ( m 1 + m 2 ) g d 1 cos θ 1 − m 2 g d 2 cos ( θ 1 + θ 2 ) \begin{aligned} K&=K_1+K_2&=&\frac{1}{2}(m_1+m_2)\dot d_1^2\dot\theta_1^2+\frac{1}{2}m_2d_2^2(\dot\theta_1+\dot\theta_2)^2+m_2d_1d_2\cos\theta_2(\dot\theta_1^2+\dot\theta_1\dot\theta_2)\\ P&=P_1+P_2&=&-(m_1+m_2)gd_1\cos\theta_1-m_2gd_2\cos(\theta_1+\theta_2) \end{aligned} KP=K1+K2=P1+P2==21(m1+m2)d˙12θ˙12+21m2d22(θ˙1+θ˙2)2+m2d1d2cosθ2(θ˙12+θ˙1θ˙2)−(m1+m2)gd1cosθ1−m2gd2cos(θ1+θ2)
拉格朗日量 L = K − P L=K-P L=K−P，这个式子太长而且就是简单地做减法，所以就不写了。
构建拉格朗日方程
L L L的偏导数为
∂ L ∂ θ 1 = − ( m 1 + m 2 ) g d 1 sin θ 1 − m 2 g d 2 sin ( θ 1 + θ 2 ) ∂ L ∂ θ 2 = − m 2 d 1 d 2 sin θ 2 ( θ ˙ 1 2 + θ ˙ 1 θ ˙ 2 ) − m 2 g d 2 sin ( θ 1 + θ 2 ) ∂ L ∂ θ ˙ 1 = ( m 1 + m 2 ) d 1 2 θ ˙ 1 + m 2 d 2 2 ( θ ˙ 1 + θ ˙ 2 ) + m 2 d 1 d 2 cos θ 2 ( 2 θ ˙ 1 + θ ˙ 2 ) ∂ L ∂ θ ˙ 2 = m 2 d 2 2 θ ˙ 1 + m 2 d 2 2 θ 2 + m 2 d 1 d 2 cos θ 2 θ ˙ 1 \begin{aligned} \frac{\partial L}{\partial\theta_1}&=-(m_1+m_2)gd_1\sin\theta_1-m_2gd_2\sin(\theta_1+\theta_2)\\ \frac{\partial L}{\partial\theta_2}&=-m_2d_1d_2\sin\theta_2(\dot\theta_1^2+\dot\theta_1\dot\theta_2)-m_2gd_2\sin(\theta_1+\theta_2)\\ \frac{\partial L}{\partial\dot\theta_1}&=(m_1+m_2)d_1^2\dot\theta_1+m_2d_2^2(\dot\theta_1+\dot\theta_2)+m_2d_1d_2\cos\theta_2(2\dot\theta_1+\dot\theta_2)\\ \frac{\partial L}{\partial\dot\theta_2}&=m_2d^2_2\dot\theta_1+m_2d_2^2\theta_2+m_2d_1d_2\cos\theta_2\dot\theta_1\\ \end{aligned} ∂θ1∂L∂θ2∂L∂θ˙1∂L∂θ˙2∂L=−(m1+m2)gd1sinθ1−m2gd2sin(θ1+θ2)=−m2d1d2sinθ2(θ˙12+θ˙1θ˙2)−m2gd2sin(θ1+θ2)=(m1+m2)d12θ˙1+m2d22(θ˙1+θ˙2)+m2d1d2cosθ2(2θ˙1+θ˙2)=m2d22θ˙1+m2d22θ2+m2d1d2cosθ2θ˙1
在拉格朗日方程中，还要得到 d L d t ∂ L ∂ q ˙ \frac{\mathbf d L}{\mathbf d t}\frac{\partial L}{\partial\dot q} dtdL∂q˙∂L，即
d L d t ∂ L ∂ θ ˙ 1 = ( m 1 + m 2 ) d 1 2 θ ¨ 1 + m 2 d 2 2 ( θ ¨ 1 + θ ¨ 2 ) + m 2 d 1 d 2 cos θ 2 ( 2 θ ¨ 1 + θ ¨ 2 ) − m 2 d 1 d 2 sin θ 2 θ ˙ 2 ( 2 θ ˙ 1 + θ ˙ 2 ) d L d t ∂ L ∂ θ ˙ 2 = m 2 d 2 2 θ ¨ 1 + m 2 d 2 2 θ ˙ 2 + m 2 d 1 d 2 ( cos θ 2 θ ¨ 1 − sin θ 2 θ ˙ 2 θ ˙ 1 ) \begin{aligned} \frac{\mathbf d L}{\mathbf d t}\frac{\partial L}{\partial\dot\theta_1}&=(m_1+m_2)d_1^2\ddot\theta_1+m_2d_2^2(\ddot\theta_1+\ddot\theta_2)+m_2d_1d_2\cos\theta_2(2\ddot\theta_1+\ddot\theta_2)-m_2d_1d_2\sin\theta_2\dot\theta_2(2\dot\theta_1+\dot\theta_2)\\ \frac{\mathbf d L}{\mathbf d t}\frac{\partial L}{\partial\dot\theta_2}&=m_2d^2_2\ddot\theta_1+m_2d_2^2\dot\theta_2+m_2d_1d_2(\cos\theta_2\ddot\theta_1-\sin\theta_2\dot\theta_2\dot\theta_1) \end{aligned} dtdL∂θ˙1∂LdtdL∂θ˙2∂L=(m1+m2)d12θ¨1+m2d22(θ¨1+θ¨2)+m2d1d2cosθ2(2θ¨1+θ¨2)−m2d1d2sinθ2θ˙2(2θ˙1+θ˙2)=m2d22θ¨1+m2d22θ˙2+m2d1d2(cosθ2θ¨1−sinθ2θ˙2θ˙1)
将其整理后，带入拉格朗日方程
F 1 = d L d t ∂ L ∂ θ ˙ 1 − ∂ L ∂ θ 1 F 2 = d L d t ∂ L ∂ θ ˙ 2 − ∂ L ∂ θ 2 F_1=\frac{\mathbf d L}{\mathbf dt}\frac{\partial L}{\partial\dot \theta_1}-\frac{\partial L}{\partial\theta_1}\quad F_2=\frac{\mathbf d L}{\mathbf dt}\frac{\partial L}{\partial\dot \theta_2}-\frac{\partial L}{\partial\theta_2} F1=dtdL∂θ˙1∂L−∂θ1∂LF2=dtdL∂θ˙2∂L−∂θ2∂L
将各项参数带入后，这两个式子可以统一表示为矩阵形式
F 1 F 2 \] = \[ D 11 D 12 D 21 D 22 \] \[ θ ¨ 1 θ ¨ 2 \] + \[ D 111 D 122 D 211 D 222 \] \[ θ ˙ 1 2 θ ˙ 2 2 \] + \[ D 112 D 121 D 212 D 221 \] \[ θ ˙ 1 θ ˙ 2 θ ˙ 2 θ ˙ 1 \] + \[ D 1 D 2 \] \\begin{bmatrix}F_1\\\\F_2\\end{bmatrix}= \\begin{bmatrix}D_{11}\&D_{12}\\\\ D_{21}\&D_{22}\\end{bmatrix} \\begin{bmatrix}\\ddot\\theta_1\\\\ \\ddot\\theta_2\\end{bmatrix}+ \\begin{bmatrix}D_{111}\&D_{122}\\\\ D_{211}\&D_{222}\\end{bmatrix} \\begin{bmatrix}\\dot\\theta_1\^2\\\\ \\dot\\theta_2\^2\\end{bmatrix}+ \\begin{bmatrix}D_{112}\&D_{121}\\\\ D_{212}\&D_{221}\\end{bmatrix} \\begin{bmatrix}\\dot\\theta_1\\dot\\theta_2\\\\ \\dot\\theta_2\\dot\\theta_1\\end{bmatrix}+ \\begin{bmatrix}D_1\\\\ D_2\\end{bmatrix} \[F1F2\]=\[D11D21D12D22\]\[θ¨1θ¨2\]+\[D111D211D122D222\]\[θ˙12θ˙22\]+\[D112D212D121D221\]\[θ˙1θ˙2θ˙2θ˙1\]+\[D1D2
其中，作用在二阶导数，即角加速度的矩阵为惯量项；作用在角速度平方的矩阵，为向心加速度系数；作用在 θ ˙ 1 θ ˙ 2 \dot\theta_1\dot\theta_2 θ˙1θ˙2上的矩阵，为科氏力系数；最后的 D 1 , D 2 D_1, D_2 D1,D2则为重力项。其具体表达形式为
D 11 = ( m 1 + m 2 ) d 1 2 + m 2 d 2 2 + 2 m 2 d 1 d 2 cos θ 2 D 22 = m 2 d 2 2 D 12 = m 2 ( d 2 2 + d 1 d 2 cos θ 2 ) D 122 = − m 2 d 1 d 2 sin θ 2 D 121 = m 2 d 1 d 2 sin θ 2 D 122 = D 121 = − m 2 d 1 d 2 sin θ 2 D 1 = ( m 1 + m 2 ) g d 1 sin θ 1 + m 2 g d 2 sin ( θ 1 + θ 2 ) D 2 = m 2 g d 2 sin ( θ 1 + θ 2 ) \begin{aligned} D_{11}&=(m_1+m_2)d_1^2+m_2d_2^2+2m_2d_1d_2\cos\theta_2\\ D_{22}&=m_2d_2^2\\ D_{12}&=m_2(d_2^2+d_1d_2\cos\theta_2)\\ D_{122}&=-m_2d_1d_2\sin\theta_2\\ D_{121}&=m_2d_1d_2\sin\theta_2\\ D_{122}&=D_{121}=-m_2d_1d_2\sin\theta_2\\ D_1&=(m_1+m_2)gd_1\sin\theta_1+m_2gd_2\sin(\theta_1+\theta_2)\\ D_2&=m_2gd_2\sin(\theta_1+\theta_2) \end{aligned} D11D22D12D122D121D122D1D2=(m1+m2)d12+m2d22+2m2d1d2cosθ2=m2d22=m2(d22+d1d2cosθ2)=−m2d1d2sinθ2=m2d1d2sinθ2=D121=−m2d1d2sinθ2=(m1+m2)gd1sinθ1+m2gd2sin(θ1+θ2)=m2gd2sin(θ1+θ2)
其余参数为0。
理想动力学模型
记 Θ = [ θ 1 θ 2 ] , F = [ F 1 F 2 ] \Theta=\begin{bmatrix}\theta_1\\ \theta_2\end{bmatrix}, F=\begin{bmatrix}F_1\\ F_2\end{bmatrix} Θ=[θ1θ2],F=[F1F2]，将向心力和科氏力合二为一，且不考虑重力干扰，则上式可记作
F = D ( Θ ) Θ ¨ + C ( Θ , Θ ˙ ) Θ ˙ F=D(\Theta)\ddot\Theta+C(\Theta, \dot\Theta)\dot\Theta F=D(Θ)Θ¨+C(Θ,Θ˙)Θ˙
记 p 1 = ( m 1 + m 2 ) d 1 2 , p 2 = m 2 d 2 2 , p 3 = m 2 d 1 d 2 p_1=(m_1+m_2)d_1^2, p_2=m_2d_2^2, p_3=m_2d_1d_2 p1=(m1+m2)d12,p2=m2d22,p3=m2d1d2，则
D ( Θ ) = [ p 1 + p 2 + 2 p 3 cos θ 2 p 2 + p 3 cos θ 2 p 2 + p 3 cos θ 2 p 2 ] C ( Θ , Θ ˙ ) = [ − p 3 θ ˙ 2 sin θ 2 − p 3 ( θ ˙ 1 + θ ˙ 2 ) sin θ 2 p 3 θ ˙ 1 sin θ 2 0 ] \begin{aligned} D(\Theta)=\begin{bmatrix} p_1+p_2+2p_3\cos\theta_2&p_2+p_3\cos\theta_2\\ p_2+p_3\cos\theta_2&p_2 \end{bmatrix}\\ C(\Theta,\dot\Theta)=\begin{bmatrix} -p_3\dot\theta_2\sin\theta_2&-p_3(\dot\theta_1+\dot\theta_2)\sin\theta_2\\ p_3\dot\theta_1\sin\theta_2&0 \end{bmatrix} \end{aligned} D(Θ)=[p1+p2+2p3cosθ2p2+p3cosθ2p2+p3cosθ2p2]C(Θ,Θ˙)=[−p3θ˙2sinθ2p3θ˙1sinθ2−p3(θ˙1+θ˙2)sinθ20]
此即 刘金琨的《机器人控制系统的设计与MATLAB仿真》2.12示例中的动力学模型。