【机器人学】平面2R机器人（四）——正动力学

机器人学，平面2R机器人完整建模及控制。

Guo_Zhanyu

4078人浏览 · 2021-08-15 22:46:11

Guo_Zhanyu · 2021-08-15 22:46:11 发布

前情回顾

【机器人学】平面2R机器人（一）——正运动学

【机器人学】平面2R机器人（二）——逆运动学

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

解答

列写动力学方程

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

计算动能

由于杆件质量均匀分布，根据柯尼希定理：质点系的总动能等于质心的动能，加上各质点相对于质心平动坐标系运动所具有的动能。简而言之，动能可以分解为杆件质心的平动动能和杆件绕自身质心的转动动能。机器人连杆质心分布如下图 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}$