【机器人学】平面2R机器人(四)——正动力学

前情回顾

 

【机器人学】平面2R机器人(一)——正运动学

【机器人学】平面2R机器人(二)——逆运动学

【机器人学】平面2R机器人(三)——速度雅可比矩阵

解答

列写动力学方程

问题: 以图示 \theta_1 和 \theta_2 为广义坐标,\tau_1 和 \tau_2 为关节驱动力矩,求机器人的动力学方程,给出推导过程,并验证惯性矩阵和机器人动能的关系。

计算动能

由于杆件质量均匀分布,根据柯尼希定理:质点系的总动能等于质心的动能,加上各质点相对于质心平动坐标系运动所具有的动能。简而言之,动能可以分解为杆件质心的平动动能和杆件绕自身质心的转动动能。机器人连杆质心分布如下图 1 所示。

图 1  机器人连杆质心分布

首先计算质心的平动动能:

\left\{ \begin{array}{lcl} x_{c1}=\frac{l}{2}\sin\theta_1\\y_{c1}=-\frac{l}{2}\cos\theta_1\end{array} \right.\left\{ \begin{array}{lcl} \dot{x}_{c1}=\frac{l}{2}\cos\theta_1 \ \dot{\theta}_1\\ \dot{y}_{c1}=\frac{l}{2}\sin\theta_1 \ \dot{\theta}_1\end{array} \right.

v_1^2=\dot{x}_{c1}^2+\dot{y}_{c1}^2=\frac{l^2}{4}\dot{\theta}_1^2

所以,

T_{1p}= \frac{1}{2}mv_1^2= \frac{ml^2}{8} \dot{\theta}_1^2

而绕自身的转动动能为: 

T_{1r}= \frac{1}{2}J \omega_1^2= \frac{1}{2} \cdot \frac{ml^2}{12} \dot{\theta}_1^2= \frac{ml^2}{24}\dot{\theta}_1^2

从而得到连杆 1 的动能, 

T_1=T_{1p}+T_{1r}= \frac{ml^2}{6}\dot{\theta}_1^2

同样地,

\left\{ \begin{array}{lcl} x_{c2}= l\sin\theta_1 + \frac{l}{2}\sin(\theta_1+\theta_2)\\y_{c2}=-l\cos\theta_1-\frac{l}{2}\cos(\theta_1+\theta_2)\end{array} \right.\left\{ \begin{array}{lcl} \dot{x}_{c2}=l\cos\theta_1 \dot{\theta}_1+\frac{l}{2}\cos(\theta_1+\theta_2)\ (\dot{\theta}_1+\dot{\theta}_2)\\ \dot{y}_{c2}= l\sin\theta_1 \dot{\theta}_1 + \frac{l}{2}\sin(\theta_1+\theta_2) \ (\dot{\theta}_1+\dot{\theta}_2) \end{array} \right.

v_2^2=\dot{x}_{c2}^2+\dot{y}_{c2}^2=l^2\dot{\theta}_1^2+ \frac{l^2}{4}(\dot{\theta}_1+\dot{\theta}_2)^2+l^2\cos\theta_2\ \dot{\theta}_1(\dot{\theta}_1+\dot{\theta}_2)

v_2^2= \frac{5l^2}{4} \dot{\theta}_1^2+ \frac{l^2}{4}\dot{\theta}_2^2+ \frac{l^2}{2} \dot{\theta}_1 \dot{\theta}_2 + l^2\cos\theta_2\ \dot{\theta}_1(\dot{\theta}_1+\dot{\theta}_2)

所以,

T_{2p}= \frac{1}{2}mv_2^2= \frac{5ml^2}{8} \dot{\theta}_1^2 + \frac{ml^2}{8}\dot{\theta}_2^2 + \frac{ml^2}{4} \dot{\theta}_1\dot{\theta}_2 + \frac{ml^2}{2}\cos\theta_2\ \dot{\theta}_1(\dot{\theta}_1+\dot{\theta}_2)

而绕自身的转动动能为: 

T_{2r}= \frac{1}{2}J \omega_2^2= \frac{1}{2} \cdot \frac{ml^2}{12} (\dot{\theta}_1 +\dot{\theta}_2)^2 = \frac{ml^2}{24}(\dot{\theta}_1^2+\dot{\theta}_2^2+2\dot{\theta}_1\dot{\theta}_2)

 从而得到连杆 2 的动能, 

T_2=T_{2p}+T_{2r}=\frac{2ml^2}{3} \dot{\theta}_1^2 + \frac{ml^2}{6}\dot{\theta}_2^2 + \frac{ml^2}{3} \dot{\theta}_1\dot{\theta}_2 + \frac{ml^2}{2}\cos\theta_2\ \dot{\theta}_1(\dot{\theta}_1+\dot{\theta}_2)

机器人总的动能为:

T=T_1+T_2=\frac{5ml^2}{6} \dot{\theta}_1^2 + \frac{ml^2}{6}\dot{\theta}_2^2 + \frac{ml^2}{3} \dot{\theta}_1\dot{\theta}_2 + \frac{ml^2}{2}\cos\theta_2\ \dot{\theta}_1(\dot{\theta}_1+\dot{\theta}_2)

计算势能

以 y=0 作为零势面,分别得到连杆 1 和 2 的重力势能。

U_1=mgy_{c1}=-\frac{mgl}{2} \cos \theta_1

U_2=mgy_{c2}=-mgl\cos\theta_1-\frac{mgl}{2}\cos(\theta_1+\theta_2)

机器人总的势能为:

U=U_1+U_2=-\frac{3mgl}{2}\cos\theta_1-\frac{mgl}{2}\cos(\theta_1+\theta_2)

结果

对于任何机械系统,有拉格朗日函数:

L=T-U

动力学方程可由第二类拉格朗日方程求得:

\tau_i= \frac{d}{dt} \frac{\partial L}{\partial \dot{q}_i}-\frac{\partial L}{\partial q_i}=\frac{d}{dt}\frac{\partial T}{\partial \dot{q}_i}-\frac{\partial T}{\partial q_i}+\frac{\partial U}{\partial q_i}

所以对关节1有:

\tau_1=\frac{d}{dt}\frac{\partial T}{\partial \dot{\theta}_1}-\frac{\partial T}{\partial \theta_1}+\frac{\partial U}{\partial \theta_1}

\begin{array}{lcl} \tau_1= (\frac{5ml^2}{3}+ml^2\cos\theta_2)\ddot{\theta}_1+(\frac{ml^2}{3}+\frac{ml^2}{2}\cos\theta_2)\ddot{\theta}_2-\frac{ml^2}{2}\sin\theta_2\ \dot{\theta}_2^2 - ml^2 \sin\theta_2\ \dot{\theta}_1 \dot{\theta}_2 + \frac{3mgl}{2} \sin\theta_1+ \frac{mgl}{2} \sin(\theta_1 + \theta_2) \end{array}

对关节2有:

\tau_2=\frac{d}{dt}\frac{\partial T}{\partial \dot{\theta}_2}-\frac{\partial T}{\partial \theta_2}+\frac{\partial U}{\partial \theta_2}

\tau_2=(\frac{ml^2}{3}+\frac{ml^2}{2}\cos\theta_2) \ddot{\theta}_1 + \frac{ml^2}{3} \ddot{\theta}_2 + \frac{ml^2}{2} \sin\theta_2\ \dot{\theta}_1^2+\frac{mgl}{2} \sin(\theta_1+\theta_2)

将两式合并后得机器人动力学方程为:

\pmb{\tau}=\begin{bmatrix} M_{11} & M_{12} \\ M_{21} & M_{22}\end{bmatrix} \begin{bmatrix} \ddot{\theta}_1\\ \ddot{\theta}_2\end{bmatrix} + \begin{bmatrix} D_{11} & D_{12} \\ D_{21} & D_{22}\end{bmatrix} \begin{bmatrix} \dot{\theta}_1^2\\ \dot{\theta}_2^2\end{bmatrix} + \begin{bmatrix} C_1\\C_2 \end{bmatrix} +\begin{bmatrix} G_1\\G_2 \end{bmatrix}

其中,

M_{11}=\frac{5ml^2}{3}+ml^2\cos\theta_2,M_{12}=M_{21}=\frac{ml^2}{3}+\frac{ml^2}{2}\cos\theta_2,M_{22}= \frac{ml^2}{3}

D_{11} = D_{22} = 0,D_{12}=-\frac{ml^2}{2}\sin\theta_2,D_{21}=\frac{ml^2}{2}\sin\theta_2

C_1=-ml^2\sin\theta_2 \ \dot{\theta}_1\dot{\theta}_2,C_2=0

G_1=\frac{3mgl}{2}\sin\theta_1+\frac{mgl}{2}\sin(\theta_1+\theta_2),G_2=\frac{mgl}{2}\sin(\theta_1+\theta_2)

也可将动力学方程的表达形式进一步简化:

\pmb{\tau}=M(\pmb{\theta})\pmb{\ddot{\theta}}+C(\pmb{\theta},\pmb{\dot{\theta}})\pmb{\dot{\theta}}+N(\pmb{\theta},\pmb{\dot{\theta}})

验证惯性矩阵和机器人动能的关系在此处不作解答,因为机器人的动能计算方法使用的就是柯尼希定理,也就是使用了需要验证的关系来计算得到。

未完待续...

【机器人学】平面2R机器人(五)——逆动力学

你可能感兴趣的:(MATLAB,机器人学,matlab)