rpr三自由度机械臂雅可比矩阵的推导
时间: 2023-11-03 07:06:23 浏览: 73
rpr三自由度机械臂的运动学模型可以通过雅可比矩阵来描述,其推导过程如下:
首先,我们需要确定机械臂末端执行器的位置和姿态。假设机械臂的基座坐标系为 $O_0$,末端执行器的坐标系为 $O_3$,则末端执行器的位置可以用三维向量表示为:
$$
\boldsymbol{p} = \begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
$$
末端执行器的姿态可以用欧拉角表示为 $\boldsymbol{\theta} = [\theta_1, \theta_2, \theta_3]$,其中 $\theta_1$、$\theta_2$、$\theta_3$ 分别表示绕 $x$、$y$、$z$ 轴的旋转角度。
接下来,我们需要确定机械臂各关节的坐标系和相对运动关系。假设机械臂的三个关节分别为 $J_1$、$J_2$、$J_3$,它们的坐标系分别为 $O_1$、$O_2$、$O_3$,则它们之间的相对位置和姿态关系可以用以下变换矩阵表示:
$$
\boldsymbol{T}_{01} = \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},
\quad
\boldsymbol{T}_{12} = \begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & \cos\theta_2 & -\sin\theta_2 & 0 \\
0 & \sin\theta_2 & \cos\theta_2 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix},
\quad
\boldsymbol{T}_{23} = \begin{bmatrix}
\cos\theta_3 & -\sin\theta_3 & 0 & 0 \\
\sin\theta_3 & \cos\theta_3 & 0 & 0 \\
0 & 0 & 1 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$
其中,$\boldsymbol{T}_{01}$ 表示从 $O_1$ 到 $O_0$ 的变换矩阵,$\boldsymbol{T}_{12}$ 表示从 $O_2$ 到 $O_1$ 的变换矩阵,$\boldsymbol{T}_{23}$ 表示从 $O_3$ 到 $O_2$ 的变换矩阵。
有了以上变换矩阵,我们可以通过正向运动学计算出末端执行器的位置和姿态。但是,反向运动学需要求解关节变量 $\theta_1$、$\theta_2$、$\theta_3$ 对末端执行器位置和姿态的导数,因此需要求出雅可比矩阵。
雅可比矩阵可以用以下公式计算:
$$
\boldsymbol{J} = \begin{bmatrix}
\frac{\partial p_x}{\partial \theta_1} & \frac{\partial p_x}{\partial \theta_2} & \frac{\partial p_x}{\partial \theta_3} \\
\frac{\partial p_y}{\partial \theta_1} & \frac{\partial p_y}{\partial \theta_2} & \frac{\partial p_y}{\partial \theta_3} \\
\frac{\partial p_z}{\partial \theta_1} & \frac{\partial p_z}{\partial \theta_2} & \frac{\partial p_z}{\partial \theta_3} \\
\frac{\partial \theta_1}{\partial t} & \frac{\partial \theta_2}{\partial t} & \frac{\partial \theta_3}{\partial t}
\end{bmatrix}
$$
其中,$\frac{\partial p}{\partial \theta}$ 表示末端执行器位置对关节变量的导数,$\frac{\partial \theta}{\partial t}$ 表示关节变量对时间的导数,即关节速度。
根据链式法则,我们可以将 $\frac{\partial p}{\partial \theta}$ 分解为以下三个部分的乘积:
$$
\frac{\partial p}{\partial \theta} = \frac{\partial p}{\partial O_3} \cdot \frac{\partial O_3}{\partial O_2} \cdot \frac{\partial O_2}{\partial \theta}
$$
其中,$\frac{\partial p}{\partial O_3}$ 表示末端执行器位置对 $O_3$ 坐标系的变换矩阵的导数,$\frac{\partial O_3}{\partial O_2}$ 表示 $O_3$ 坐标系对 $O_2$ 坐标系的变换矩阵的导数,$\frac{\partial O_2}{\partial \theta}$ 表示 $O_2$ 坐标系对关节变量的导数。
根据变换矩阵的求导公式,可以得到:
$$
\frac{\partial p}{\partial O_3} = \begin{bmatrix}
\frac{\partial x}{\partial O_{3_{11}}} & \frac{\partial x}{\partial O_{3_{12}}} & \frac{\partial x}{\partial O_{3_{13}}} \\
\frac{\partial y}{\partial O_{3_{21}}} & \frac{\partial y}{\partial O_{3_{22}}} & \frac{\partial y}{\partial O_{3_{23}}} \\
\frac{\partial z}{\partial O_{3_{31}}} & \frac{\partial z}{\partial O_{3_{32}}} & \frac{\partial z}{\partial O_{3_{33}}}
\end{bmatrix}
$$
$$
\frac{\partial O_3}{\partial O_2} = \begin{bmatrix}
\frac{\partial O_{3_{11}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\
\frac{\partial O_{3_{21}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\
\frac{\partial O_{3_{31}}}{\partial O_{2_{11}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{12}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{13}}} & 0 & 0 & 0 \\
0 & 0 & 0 & \frac{\partial O_{3_{11}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{11}}}{\partial O_{2_{23}}} \\
0 & 0 & 0 & \frac{\partial O_{3_{21}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{21}}}{\partial O_{2_{23}}} \\
0 & 0 & 0 & \frac{\partial O_{3_{31}}}{\partial O_{2_{21}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{22}}} & \frac{\partial O_{3_{31}}}{\partial O_{2_{23}}}
\end{bmatrix}
$$
$$
\frac{\partial O_2}{\partial \theta} = \begin{bmatrix}
\frac{\partial O_{2_{11}}}{\partial \theta_1} & \frac{\partial O_{2_{11}}}{\partial \theta_2} & \frac{\partial O_{2_{11}}}{\partial \theta_3} \\
\frac{\partial O_{2_{21}}}{\partial \theta_1} & \frac{\partial O_{2_{21}}}{\partial \theta_2} & \frac{\partial O_{2_{21}}}{\partial \theta_3} \\
\frac{\partial O_{2_{31}}}{\partial \theta_1} & \frac{\partial O_{2_{31}}}{\partial \theta_2} & \frac{\partial O_{2_{31}}}{\partial \theta_3} \\
0 & 0 & 0 \\
0 & 0 & 0 \\
0 & 0 & 0
\end{bmatrix}
$$
将以上三个矩阵相乘,得到 $\frac{\partial p}{\partial \theta}$。
同理,我们可以计算出关节变量对时间的导数 $\frac{\partial \theta}{\partial t}$,即关节速度。最终,将 $\frac{\partial p}{\partial \theta}$ 和 $\frac{\partial \theta}{\partial t}$ 组合成雅可比矩阵 $\boldsymbol{J}$,即可完成反向运动学的求解。