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

题目

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

图 1  平面2R机器人操作手

以上述机器人为对象,完成如下任务:

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。利用计算力矩控制规则实现机器人末端执行器的线速度指令运动,画出机器人的关节力矩、角度、角速度、角加速度、末端执行器笛卡尔坐标和线速度的时间轨迹图;以及画出机器人雅可比矩阵的行列式值的时间轨迹图,分析机器人靠近奇异位形的程度。

解答

计算转动惯量

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

【机器人学】平面2R机器人(一)——正运动学_第1张图片 图 2  连杆模型

单个连杆模型如图 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 的建系。

图 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}

未完待续...

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

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