机械臂运动学 | 寒心Code

本文描述机器人运动学的相关信息，包括旋转矩阵，矢量和机械臂位姿的求解，并使用C#描述相关算法。
。

旋转矩阵

在线性代数中旋转矩阵被用来使矢量在欧拉空间中旋转。在机器人学中，旋转矩阵被用来求解机器人关节的姿态（朝向）。在三维空间中，旋转矩阵的特征值为1。

计算旋转矩阵的方法

在计算旋转矩阵时，通常将它作为3个旋转矩阵的复合。在右手笛卡儿坐标系中，假设旋转方向为右手螺旋方向，则有以下旋转矩阵

绕X轴旋转（横滚）
${\mathcal {R}}_{x}({\theta} _{x})= {\begin{bmatrix} 1&0&0 \\0 & \cos {\theta _{x}}&-\sin {\theta _{x}} \\ 0&\sin {\theta _{x}}&\cos {\theta _{x}}\end{bmatrix}}=\exp \left({\begin{bmatrix}0&0&0\\0&0&-\theta _{x}\\0&\theta _{x} & 0 \end{bmatrix}}\right)$
绕Y轴旋转（俯仰）
${\mathcal {R}}_{y}(\theta _{y})={\begin{bmatrix}\cos {\theta _{y}}&0&\sin {\theta _{y}}\\0&1&0\\-\sin {\theta _{y}}&0&\cos {\theta _{y}}\end{bmatrix}}=\exp \left({\begin{bmatrix}0&0&\theta _{y}\\0&0&0\\-\theta _{y}&0&0\end{bmatrix}}\right)$
绕Z轴旋转（偏航）
${\mathcal {R}}_{z}(\theta _{z})={\begin{bmatrix}\cos {\theta _{z}}&-\sin {\theta _{z}}&0\\\sin {\theta _{z}}&\cos {\theta _{z}}&0\\0&0&1\end{bmatrix}}=\exp \left({\begin{bmatrix}0&-\theta _{z}&0\\\theta _{z}&0&0\\0&0&0\end{bmatrix}}\right)$

为计算刚体最终的旋转矩阵，将三个旋转矩阵相乘，得到${\mathcal {R}}={\mathcal {R}}_{z}(\theta _{z})\,{\mathcal {R}}_{y}(\theta _{y})\,{\mathcal {R}}_{x}(\theta _{x})$

旋转矩阵与欧拉角的互相转换

这里的欧拉角指的是顺序为x-y-z的泰特-布莱恩角(Tait–Bryan angles)，而非经典欧拉角的z-x-z顺序。为与经典欧拉角区分，这里用${\theta}_{x},{\theta}_{y},{\theta}_{z}$表示。

欧拉角转换为旋转矩阵
上节中提到，旋转矩阵可以由三个旋转矩阵相乘获得，因此欧拉角到旋转矩阵的方程为
${\mathbf{R}} = {\begin{bmatrix} 1&0&0 \\0 & \cos {\theta _{x}}&-\sin {\theta _{x}} \\ 0&\sin {\theta _{x}}&\cos {\theta _{x}}\end{bmatrix}}{\begin{bmatrix}\cos {\theta _{y}}&0&\sin {\theta _{y}}\\0&1&0\\-\sin {\theta _{y}}&0&\cos {\theta _{y}}\end{bmatrix}}{\begin{bmatrix}\cos {\theta _{z}}&-\sin {\theta _{z}}&0\\\sin {\theta _{z}}&\cos {\theta _{z}}&0\\0&0&1\end{bmatrix}}$
在这里定义:
${s}_{x} = \sin {{\theta}_{x}}; {c}_{x} = \cos {{\theta}_{x}}; {s}_{y} = \sin {{\theta}_{y}}; {c}_{y} = \cos {{\theta}_{y}}; {s}_{z} = \sin {{\theta}_{z}}; {c}_{z} = \cos {{\theta}_{z}}$
则可以得到：
${\mathbf{R}} = {\begin{bmatrix} {c_y}{c_z} & {c_z}{s_x}{s_y} – {c_x}{s_z} & {s_x}{s_z} + {c_x}{c_z} {s_y} \\ {c_y}{s_z} & {c_x}{c_z}+{s_x}{s_y}{s_z} & {c_x}{s_y}{s_z}-{s_x}{c_z} \\ -{s_y} & {c_y}{s_x} & {c_x}{c_y} \end{bmatrix}}$

[codesyntax lang=”csharp” lines=”normal” title=”点击这里查看相应的C#代码” blockstate=”collapsed”]

/// <summary>
/// Convert the Euler Angle to the Rotation Matrix
/// </summary>
/// <param name="euler">Euler Angle Vector</param>
/// <returns>Rotation Matrix</returns>
public static Matrix3F Euler2RMatrix(Vector3F euler)
{
    double sx = Math.Sin(euler.X), cx = Math.Cos(euler.X),
        sy = Math.Sin(euler.Y), cy = Math.Cos(euler.Y),
        sz = Math.Sin(euler.Z), cz = Math.Cos(euler.Z);
        return new Matrix3F(
            cy * cz, cz * sx * sy - cx * sz, sx * sz + cx * cz * sy,
            cy * sz, cx * cz + sx * sy * sz, cx * sy * sz - sx * cz,
            -sy,     cy * sx,                cx * cy
        );
}

[/codesyntax]

旋转矩阵转换为欧拉角
假设旋转矩阵为
$\mathbf{R} = {\begin{bmatrix} {r_{11}} & {r_{12}} & {r_{13}} \\ {r_{21}} & {r_{22}} & {r_{23}} \\ {r_{31}} & {r_{32}} & {r_{33}} \end{bmatrix}} $
根据上节结果求解得
${{\theta}_{x}} = \operatorname{atan2}({r_{32}},{r_{33}})$
${{\theta}_{y}} = \operatorname{atan2}(-{r_{31}},\sqrt{{r_{32}^2}+{r_{33}^2}})$
${{\theta}_{y}} = \operatorname{atan2}({r_{21}},{r_{11}})$

atan2 为值域在 $\left ( -\pi,\pi \right ] $ 的反正切函数，它的定义如下
$
\operatorname{atan2}(y, x) = \begin{cases}
\arctan\left(\frac y x\right) & \qquad x > 0 \\
\arctan\left(\frac y x\right) + \pi& \qquad y \ge 0 , x < 0 \\
\arctan\left(\frac y x\right) – \pi& \qquad y < 0 , x < 0 \\ +\frac{\pi}{2} & \qquad y > 0 , x = 0 \\
-\frac{\pi}{2} & \qquad y < 0 , x = 0 \\
\text{undefined} & \qquad y = 0, x = 0
\end{cases}$
详细信息请参见维基百科

[codesyntax lang=”csharp” lines=”normal” title=”点击这里查看相应的C#代码” blockstate=”collapsed”]

/// <summary>
/// Convert the rotation matrix to the euler angle
/// </summary>
/// <param name="rmat">The roatation matrix</param>
/// <returns>The Euler Angle vector</returns>
public static Vector3F RMatrix2Euler(Matrix3F rmat)
{
    return new Vector3F(
        Math.Atan2(rmat[2, 1], rmat[2, 2]),
        Math.Atan2(-rmat[2, 0], Math.Sqrt(rmat[2, 1] * rmat[2, 1] + rmat[2, 2] * rmat[2, 2])),
        Math.Atan2(rmat[1, 0], rmat[0, 0]));
}

[/codesyntax]

多次旋转时的旋转矩阵

在机器人学中，为求解机械臂的位姿，通常需要对矢量进行多次旋转。通常，这样的旋转是以前一个机械臂终点的旋转坐标系进行相对旋转。在这个情况下仅需在原旋转矩阵右乘新的旋转矩阵即可得到新的旋转矩阵。

设当前节点的相对前一个节点旋转矩阵为 $\mathbf{R}$, 前一个节点的相对于基座的旋转矩阵为 ${\mathbf{R}}_{i-1}$，则当前节点的相对于基座坐标系的旋转矩阵 $ {\mathbf{R}}_{i}$ 为：
$$ {\mathbf{R}}_{i} = {{\mathbf{R}}_{i-1}}{\mathbf{R}} $$

机械臂位姿的求解

为求解机械臂节点的位置，首先简化机械臂的模型为矢量相加。在此基础上从下向上相加即可求得末端位置。
假设机械臂关节位置 $ {\mathbf{P}}_{i} = {\begin{bmatrix} x \\ y \\ z \end{bmatrix}} $,机械臂连杆长度为 ${L}_{i}$

对于每一个运动关节求解步骤如下

定义机械臂的初始状态
计算初始状态下的旋转矩阵${\mathbf{B}}_{i}$,如果初始状态下，所有关节朝向相同，则${\mathbf{B}}_{i} = {\begin{bmatrix} 1&0&0 \\ 0&1&0 \\ 0&0&1 \end{bmatrix}}$
计算运动时的相对旋转矩阵${\mathbf{A}}_{i}$
使用以下公式计算当前关节相对于基座的旋转矩阵（姿态）
$$ {\mathbf{R}}_{i} = ({\mathbf{X}}_{i},{\mathbf{Y}}_{i},{\mathbf{Z}}_{i}) = {\mathbf{B}_{0}}{\mathbf{A}_{1}}{\mathbf{B}_{1}}{\mathbf{A}_{2}} \cdots {\mathbf{B}_{i-1}} {\mathbf{A}_{i}} $$
当前节点末端位置为
$$ {\mathbf{P}}_{i} = {\mathbf{P}}_{i-1} + {\mathbf{R}}_{i} * ({{L}_{i}}*{{\mathbf{Z}}_{0}}) $$
通过第一节的公式可求得末端的欧拉角

以上方法可用于求解绝大部分机械臂的正向运动学

参考文献

维基百科编者, “旋转矩阵,” 19 1 2018. https://zh.wikipedia.org/wiki/旋转矩阵
高濑国克, “マニピュレータの基礎理論,” 1983.
“旋转矩阵、欧拉角、四元数理论及其转换关系,” 21 5 2017. http://blog.csdn.net/lql0716/article/details/72597719.