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

最新推荐文章于 2022-10-25 12:48:20 发布

Guo_Zhanyu

最新推荐文章于 2022-10-25 12:48:20 发布

阅读量5.6k

点赞数 9

分类专栏： MATLAB 机器人学文章标签： matlab

本文链接：https://blog.csdn.net/Hide_on_Stream/article/details/118631636

版权

MATLAB 同时被 2 个专栏收录

8 篇文章 7 订阅

订阅专栏

机器人学

6 篇文章 8 订阅

订阅专栏

题目

图 1 所示为一个平面 2R 机器人，以及机器人广义坐标和关节力矩的定义。机器人 2 个连杆的质量均匀分布，长度都为 $1m$ ，质量都为 $5kg$ 。末端执行器笛卡尔坐标为 $(x,y)$ 。

以上述机器人为对象，完成如下任务：

1. 求机器人连杆绕其质心的转动惯量。

2. 求机器人的 D-H 参数表，根据 D-H 参数表写出变换矩阵 $_{1}^{0}\textrm{T}$ 、 $_{2}^{1}\textrm{T}$ 、 $_{2}^{0}\textrm{T}$ 矩阵；再用指数积方法计算变换矩阵 $_{2}^{0}\textrm{T}$ ，并和 D-H 方法计算结果作对比。

3. 用矢量积方法、微分变换法、指数积方法中的任一种计算机器人的速度雅可比矩阵。

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

5. 利用 MATLAB 编程仿真上述平面 2R 机器人的动力学系统，完成如下任务：

(1) 当机器人初始姿态为 $\begin{bmatrix} \theta_1 \\ \theta_2 \end{bmatrix} = \begin{bmatrix} 180^{\circ} \\ 0^{\circ} \end{bmatrix}$ ， $\begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}$ 时，分别画出关节力矩 $\begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}N\cdot m$ 和 $\begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = \begin{bmatrix} 2 \\ 0.5 \end{bmatrix}N\cdot m$ 时，机器人的关节角度、角速度、角加速度以及末端执行器笛卡尔坐标的轨迹图（步长0.01s，时长2s的仿真）。

(2) 当机器人初始姿态为 $\begin{bmatrix} \theta_1 \\ \theta_2 \end{bmatrix} = \begin{bmatrix} 100^{\circ} \\ 90^{\circ} \end{bmatrix}$ ，关节力矩 $\begin{bmatrix} \tau_1 \\ \tau_2 \end{bmatrix} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}N\cdot m$ 时，画出机器人的关节角度、角速度、角加速度以及末端执行器笛卡尔坐标的轨迹图；以及画出机器人速度雅可比矩阵的行列式值的时间轨迹图，分析机器人靠近奇异位形的程度（步长0.01s，时长2s的仿真）。

(3) 当机器人初始姿态为 $\begin{bmatrix} \theta_1 \\ \theta_2 \end{bmatrix} = \begin{bmatrix} 100^{\circ} \\ 90^{\circ} \end{bmatrix}$ ，末端执行器在笛卡尔空间的线速度指令为（常数） $\begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix} = \begin{bmatrix} -0.25 \\ -0.5 \end{bmatrix}m/s$ ，仿真时间步长0.01s，仿真时间长度6s。利用计算力矩控制规则实现机器人末端执行器的线速度指令运动，画出机器人的关节力矩、角度、角速度、角加速度、末端执行器笛卡尔坐标和线速度的时间轨迹图；以及画出机器人雅可比矩阵的行列式值的时间轨迹图，分析机器人靠近奇异位形的程度。

解答

计算转动惯量

问题：求机器人连杆绕其质心的转动惯量。

单个连杆模型如图 2 所示，其绕质心的转动惯量计算方法如下：

$J=\int{r^2dm}=\int_{-l/2}^{l/2}x^2\frac{m}{l}dx=\frac{ml^2}{12}$

所以 $J=\frac{5\times 1^2}{12}=\frac{5}{12} kg\cdot m$ 。

两种方法计算变换矩阵

问题：求机器人的 D-H 参数表，根据 D-H 参数表写出变换矩阵 $_{1}^{0}\textrm{T}$ 、 $_{2}^{1}\textrm{T}$ 、 $_{2}^{0}\textrm{T}$ 矩阵；再用指数积方法计算变换矩阵 $_{2}^{0}\textrm{T}$ ，并和 D-H 方法计算结果作对比。

D-H法

首先对机器人进行如下图 3 的建系。

各关节轴线垂直于平面，规定 $z_i$ 轴与关节 $i$ 轴线重合，且方向为从纸面向外；轴线间的公垂线 $a_i$ 沿实体连杆方向，规定 $x_i$ 轴与公垂线 $a_i$ 重合，方向从关节 $i$ 指向关节 $i+1$ ；根据右手法则确定 $y_i$ 轴。规定 $\left \{ 0 \right \}$ 与世界坐标系O-XY重合，末端坐标系 $\left \{ 3 \right \}$ 与 $\left \{ 2 \right \}$ 仅有固定的平移变换。

机器人的 D-H 参数如下表 1 所示。

表1 D-H连杆参数表
$i$	$a_{i-1}$	$\alpha_{i-1}$	$d_i$	$\theta_i$	关节变量取值范围
1	$0$	$0$	$0$	$\beta_1(-90^{\circ})$	$-180^{\circ}\sim 180^{\circ}$
2	$l$	$0$	$0$	$\theta_2(0^{\circ})$	$-180^{\circ}\sim 180^{\circ}$
3	$l$	$0$	$0$	$0$	-

其中 $l=1m$ ， $\beta_1=\theta_1-90^{\circ}$ 。建立变换矩阵如下所示：

$_{1}^{0}\textrm{T} = \begin{bmatrix} \cos \beta_1 & -\sin \beta_1 & 0 & 0\\ \sin \beta_1 & \cos \beta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}=\begin{bmatrix} \sin \theta_1 & \cos \theta_1 & 0 & 0\\ -\cos \theta_1 & \sin \theta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{1}\textrm{T} = \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & l\\ \sin \theta_2 & \cos \theta_2 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{3}^{2}\textrm{T} = \begin{bmatrix}1 & 0 & 0 & l\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

所以，

$_{2}^{0}\textrm{T} =\ _{1}^{0}\textrm{T}\ _{2}^{1}\textrm{T} = \begin{bmatrix} \sin \theta_1 & \cos \theta_1 & 0 & 0\\ -\cos \theta_1 & \sin \theta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & l\\ \sin \theta_2 & \cos \theta_2 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{0}\textrm{T} = \begin{bmatrix} \sin \theta_1 \cos \theta_2 + \cos \theta_1 \sin \theta_2 & \cos \theta_1 \cos \theta_2 - \sin \theta_1 \sin \theta_2 & 0 & l\sin \theta_1\\ -\cos \theta_1 \cos \theta_2 + \sin \theta_1 \sin \theta_2 & \sin \theta_1 \cos \theta_2 + \cos \theta_1 \sin \theta_2& 0 & -l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{0}\textrm{T}=\begin{bmatrix} \sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2) & 0 & l\sin \theta_1\\ -\cos(\theta_1+\theta_2) & \sin(\theta_1+\theta_2) & 0 & -l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

指数积方法

两个关节轴线的线矢量方向和取关节上的一点如下：

$\omega_1=\omega_2=\begin{bmatrix} 0\\ 0\\ 1 \end{bmatrix},r_1=\begin{bmatrix} 0\\ 0\\ 0 \end{bmatrix},r_2=\begin{bmatrix} 0\\ -l\\ 0 \end{bmatrix}$

查表得：

$e^{[V_1]\theta_1}=\begin{bmatrix} R(\theta_1) & p(\theta_1)\\ 0 & 1 \end{bmatrix}=\begin{bmatrix} \cos \theta_1 & -\sin \theta_1 & 0 & 0\\ \sin \theta_1 & \cos \theta_1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$e^{[V_2]\theta_2}=\begin{bmatrix} R(\theta_2) & p(\theta_2)\\ 0 & 1 \end{bmatrix}=\begin{bmatrix} \cos \theta_2 & -\sin \theta_2 & 0 & -l\sin \theta_2\\ \sin \theta_2 & \cos \theta_2 & 0 & -l(1-\cos \theta_2)\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{0}\textrm{T}(0,0)=\begin{bmatrix} 0 & 1 & 0 & 0\\ -1 & 0 & 0 & -l\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

所以，

$_{2}^{0}\textrm{T}(\theta_1,\theta_2)=e^{[V_1]\theta_1}e^{[V_2]\theta_2} \ _{2}^{0}\textrm{T}(0,0)$

$_{2}^{0}\textrm{T}(\theta_1,\theta_2) = \begin{bmatrix} \sin \theta_1 \cos \theta_2 + \cos \theta_1 \sin \theta_2 & \cos \theta_1 \cos \theta_2 - \sin \theta_1 \sin \theta_2 & 0 & l\sin \theta_1\\ -\cos \theta_1 \cos \theta_2 + \sin \theta_1 \sin \theta_2 & \sin \theta_1 \cos \theta_2 + \cos \theta_1 \sin \theta_2& 0 & -l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{2}^{0}\textrm{T}(\theta_1,\theta_2) =\begin{bmatrix} \sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2) & 0 & l\sin \theta_1\\ -\cos(\theta_1+\theta_2) & \sin(\theta_1+\theta_2) & 0 & -l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

可以看到，指数积方法计算的结果 $_{2}^{0}\textrm{T}(\theta_1,\theta_2)$ 与 D-H 方法求解的结果 $_{2}^{0}\textrm{T}$ 相等，即：

$_{2}^{0}\textrm{T}(\theta_1,\theta_2) =\ _{2}^{0}\textrm{T}$

末端执行器变换矩阵

$_{3}^{0}\textrm{T} =\ _{2}^{0}\textrm{T}\ _{3}^{2}\textrm{T} =\begin{bmatrix} \sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2) & 0 & l\sin \theta_1\\ -\cos(\theta_1+\theta_2) & \sin(\theta_1+\theta_2) & 0 & -l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix}1 & 0 & 0 & l\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$

$_{3}^{0}\textrm{T}=\begin{bmatrix} \sin(\theta_1+\theta_2) & \cos(\theta_1+\theta_2) & 0 & l\sin(\theta_1+\theta_2)+l\sin \theta_1\\ -\cos(\theta_1+\theta_2) & \sin(\theta_1+\theta_2) & 0 & -l\cos(\theta_1+\theta_2)-l\cos \theta_1\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}$