机械臂控制01——运动学解算

Blog

机械臂控制01——运动学解算

2026-07-12

1. 运动学含义

下图中为二维平面中的二连杆机械臂示意图，其中

P0、P1 为可旋转关节，P2 为机械臂末端
L1、L2 为两段机械臂的臂长
θ1、θ2 为对应关节相对于上个关节的旋转角度

运动学解析分为两部分：

正解：已知 L1、L2、…、Lx，θ1、θ2、…、θx，求解机械臂末端在全局坐标系下的坐标和角度
逆解：已知机械臂末端在全局坐标系下的坐标和角度，求解 L1、L2、…、Lx，θ1、θ2、…、θx

这里示意图是二维的，实际中一般是多维的。

2. 运动学正解

2.1 三角函数

从上图中可以轻易看出

第一根连杆末端（肘关节）坐标 P1 相对于 P0 也就是全局坐标系的坐标为

(L_1*cos(\theta_1), L_1*sin(\theta_1))

第二根连杆末端（肘关节）坐标 P2 相对于 P0 也就是全局坐标系的坐标为

(L_1*cos(\theta_1)+L_2*cos(\theta_1+\theta_2), L_1*sin(\theta_1)+L_2*sin(\theta_1+\theta_2))

如果多连杆，则继续相加即可。但是就是随着维度和连杆条数增多，这套计算会越来越麻烦。

2.2 矩阵运算基础

2.2.1 旋转矩阵

为了理解矩阵旋转的概念，这里定义我们的基座为全局坐标系 A0，然后基于第一段连杆的起点也就是 P0 为原点，连杆本身为X轴，创建一个新的坐标系，新的坐标系记录为 A1。

那么 P1 这个点在坐标系 A1 下的坐标就很好表示了，因为他固定在 X 轴上，Y 永远是 0，且机械臂长度是固定的 L1，因此 P1 这个点在坐标系 A1 下的坐标永远为

(L_1, 0)

那么如何知道在坐标系 A1 的某个点 P1，在 A0 坐标系下的坐标呢？只需要将坐标系 A1 顺时针旋转 θ1 度，让他和坐标系 A0 的 XY 轴分别对齐就可以了。

那么原有的坐标系下的点通过以下变换就可以对应到新的坐标系下的坐标：

\begin{bmatrix} X_{new} \\ Y_{new} \\ \end{bmatrix} = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \\ \end{bmatrix} * \begin{bmatrix} X_{old} \\ Y_{old} \\ \end{bmatrix}

比如，假设坐标系 A1 和 A0 之间的夹角为 45°，L1 臂长为根号2。则 P1 在 A1 坐标系下的坐标为（根号2，0），带入计算得在 A0 坐标系下的坐标为：

\begin{bmatrix} X_{new} \\ Y_{new} \end{bmatrix} = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \\ \end{bmatrix} * \begin{bmatrix} X_{old} \\ Y_{old} \\ \end{bmatrix} = \begin{bmatrix} \frac{\sqrt{2}}{2} & -\frac{\sqrt{2}}{2} \\ \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} \end{bmatrix} \begin{bmatrix} \sqrt{2} \\ 0 \end{bmatrix} = \begin{bmatrix} 1 \\ 1 \end{bmatrix}

可以看到得到的结果就是正确的，带入其他坐标结果也是一样 OK 的。

画个垂线还是很容易理解的。

2.2.2 平移矩阵

为了理解矩阵平移的概念，这里定义我们的基座为全局坐标系 A0，然后基于第一段连杆的终点也就是 P1 为原点，和全局坐标系平行的 X 轴 Y 轴，创建一个新的坐标系，新的坐标系记录为 A1。

则在 A1 坐标系中的某个坐标，在 A0 坐标系中的坐标可以通过如下公式计算得到

\begin{bmatrix} X_{new} \\ Y_{new} \\ \end{bmatrix} = \begin{bmatrix} L*cos(\theta) \\ L*sin(\theta) \\ \end{bmatrix} + \begin{bmatrix} X_{old} \\ Y_{old} \\ \end{bmatrix}

也就是原有的坐标加上两个坐标系之间原点的偏差。

2.3 旋转平移

上面得到两个结论

坐标系旋转：某个坐标系下的某个点，如果坐标系绕原点顺时针旋转 θ 角度，则在新坐标系下下的坐标可以通过如下公式计算得到。

\begin{bmatrix} X_{new} \\ Y_{new} \\ \end{bmatrix} = \begin{bmatrix} cos(\theta) & -sin(\theta) \\ sin(\theta) & cos(\theta) \\ \end{bmatrix} * \begin{bmatrix} X_{old} \\ Y_{old} \\ \end{bmatrix}

坐标系平移：某个坐标系下的某个点，两个坐标系方向相同，则在新坐标系下下的坐标可以通过如下公式计算得到。

\begin{bmatrix} X_{new} \\ Y_{new} \\ \end{bmatrix} = \begin{bmatrix} L*cos(\theta) \\ L*sin(\theta) \\ \end{bmatrix} + \begin{bmatrix} X_{old} \\ Y_{old} \\ \end{bmatrix}

当我们知道了由上述操作，旋转坐标系、平移坐标系之后，就可以采用以下思路来进行求解多连杆了。

比如下图我们需要求解 P2 在 A0 坐标系下的坐标，这里为了方便理解，先暂时忽略 A2 坐标系。

我们从后往前推导：

第一步：求解 P2 在 A1 坐标系下的位置，很容易解得

(L_2*cos(\theta_2), L_2*sin(\theta_2))

第二步：旋转坐标系 A1 到他的上一级坐标系 A0 方向相同，则需要旋转θ1角度。则 P2 在旋转后的 A1 坐标系下的坐标为

\begin{bmatrix} X_{2\_1} \\ Y_{2\_1} \\ \end{bmatrix} = \begin{bmatrix} cos(\theta_1) & -sin(\theta_1) \\ sin(\theta_1) & cos(\theta_1) \\ \end{bmatrix} * \begin{bmatrix} L_2*cos(\theta_2) \\ L_2*sin(\theta_2) \\ \end{bmatrix}

第三步：平移坐标系 A1 到他的上一级坐标系 A0 原点相同，A0 和 A1 就重合了。则 P2 在平移后的 A0 坐标系下的坐标为

\begin{bmatrix} X_{2\_0} \\ Y_{2\_0} \\ \end{bmatrix} = \begin{bmatrix} X_{2\_1} \\ Y_{2\_1} \\ \end{bmatrix} + \begin{bmatrix} L_1*cos(\theta_1) \\ L_1*sin(\theta_1) \\ \end{bmatrix} = \begin{bmatrix} cos(\theta_1) & -sin(\theta_1) \\ sin(\theta_1) & cos(\theta_1) \\ \end{bmatrix} * \begin{bmatrix} L_2*cos(\theta_2) \\ L_2*sin(\theta_2) \\ \end{bmatrix} + \begin{bmatrix} L_1*cos(\theta_1) \\ L_1*sin(\theta_1) \\ \end{bmatrix}

以上步骤计算图示如下图所示

总结以上步骤，

如果有1个关节，也就是一根杆，则直接就能得到末端坐标；
如果有2个关节，对于二连杆，需要一次旋转和一次平移；
…
如果有N个关节，需要N-1次旋转和N-1次平移。

结论：多连杆，计算出末端在最后一个坐标系下的坐标，然后不断旋转平移，一直到全局坐标系停止即可。

2.4 连乘转换

上面过程中旋转操作是乘法，平移操作是加法，每次最后计算的结果还需要再和下一层的坐标系做乘法和加法，计算上来说会比较复杂。

因此这里想办法把平移也给改成乘法，这样先旋转再平移，就可以修改为把原始坐标先和旋转矩阵相乘，然后再和平移矩阵相乘就可以了，得到的结果再乘上级坐标系的旋转和平移矩阵，就可以实现连乘直接得到最终结果了。

这里将所有的向量扩展一个维度。

坐标向量 $\begin{bmatrix} X \\ Y \\ \end{bmatrix}$ 修改为 $\begin{bmatrix} X \\ Y \\ 1 \\ \end{bmatrix}$ 其中，1 在这里没实际意义。

旋转矩阵 $\begin{bmatrix} cos(\theta_1) & -sin(\theta_1) \\ sin(\theta_1) & cos(\theta_1) \\ \end{bmatrix}$ 修改为 $\begin{bmatrix} cos(\theta_1) & -sin(\theta_1) & 0 \\ sin(\theta_1) & cos(\theta_1) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}$ 其中，多出来的 0 或 1 也没意义。

平移矩阵从加法改为乘法后，旋转矩阵为 $\begin{bmatrix} 1 & 0 & x \\ 0 & 1 & y \\ 0 & 0 & 1 \\ \end{bmatrix}$ 其中 xy 分别为要平移的坐标系原点的差，1 没实际意义。

然后就可以开始计算了，还是以上面那个推导为例。

第一步：确定要计算的点 P2 是在 A1 坐标系下，P2 的坐标为

\begin{bmatrix} L_2*cos(\theta_2) \\ L_2*sin(\theta_2) \\ 1 \\ \end{bmatrix}

第二步：构建旋转矩阵和平移矩阵

要将 A1 旋转的角度为 θ1，因此旋转矩阵为

R(\theta_1) = \begin{bmatrix} cos(\theta_1) & -sin(\theta_1) & 0 \\ sin(\theta_1) & cos(\theta_1) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}

要将 A1 到 A0 需要平移的距离填入平移矩阵，得到平移矩阵为

Trans(\theta_1,L_1) = \begin{bmatrix} 1 & 0 & L_1*cos(\theta_1) \\ 0 & 1 & L_1*sin(\theta_1) \\ 0 & 0 & 1 \\ \end{bmatrix}

此时已知 A1 坐标系下的某个点（X，Y）想要计算到 A0 坐标系下，只需要先和旋转矩阵相乘和平移矩阵相乘即可。

\begin{bmatrix} X_g \\ Y_g \\ 1 \\ \end{bmatrix} = Trans(\theta,L)*R(\theta)*P = \begin{bmatrix} 1 & 0 & L_1*cos(\theta_1) \\ 0 & 1 & L_1*sin(\theta_1) \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} cos(\theta_1) & -sin(\theta_1) & 0 \\ sin(\theta_1) & cos(\theta_1) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} X \\ Y \\ 1 \\ \end{bmatrix}

带入我们需要转换的点 P2 在 A1 下的坐标

\begin{bmatrix} X_g \\ Y_g \\ 1 \\ \end{bmatrix} = \begin{bmatrix} 1 & 0 & L_1*cos(\theta_1) \\ 0 & 1 & L_1*sin(\theta_1) \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} cos(\theta_1) & -sin(\theta_1) & 0 \\ sin(\theta_1) & cos(\theta_1) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} L_2*cos(\theta_2) \\ L_2*sin(\theta_2) \\ 1 \\ \end{bmatrix}

得到的结果和原来的是一样一样的。

计算过程中会用到这两个公式
cos(α+β)=cosαcosβ−sinαsinβ
sin(α+β)=sinαcosβ+cosαsinβ

第三步：连乘计算

然后这里将每个坐标系下的旋转矩阵 R 和平移矩阵 Trans 先乘起来（矩阵乘法结合律），得到转换矩阵 T=R*Trans，这样只需要知道最后一层需要被计算的原始坐标，就可以通过连乘得到最终的计算结果了

这里 2 连杆只需要转换 1 次，最终的转换矩阵

T_0 = Trans(\theta_1,L_1)*R(\theta_1)

如果是 3 连杆则需要转换 2 次，最终的转换矩阵

T_0 = Trans(\theta_1,L_1)*R(\theta_1)*Trans(\theta_2,L_2)*R(\theta_2)

多连杆同上，一直连乘即可得到最终的转换矩阵 T0，则最终的计算方式如下

\begin{bmatrix} X_g \\ Y_g \\ 1 \\ \end{bmatrix} = T_0 * \begin{bmatrix} x \\ y \\ 1 \\ \end{bmatrix}

其中 xy 分别为被计算的点在最后一级坐标系下的坐标，T0 为最终的转换矩阵。

第四步：末端优化

前面为了理解，忽略了 A2 坐标系，上面的 xy 坐标，还是需要带入被测点 P2 在 A1 中的坐标，其实这一步也是可以旋转和平移进行计算的，只不过计算的点是 A2 坐标系下的原点（0，0）在 A1 中的位置。

A2 坐标系如果想和 A1 方向相同，需要旋转的角度为 θ2，因此对应的旋转矩阵为

R(\theta_2) = \begin{bmatrix} cos(\theta_2) & -sin(\theta_2) & 0 \\ sin(\theta_2) & cos(\theta_2) & 0 \\ 0 & 0 & 1 \\ \end{bmatrix}

A2 坐标系如果想和 A1 原点对齐，对应的平移矩阵为

Trans(\theta_2,L_2) = \begin{bmatrix} 1 & 0 & L_2*cos(\theta_2) \\ 0 & 1 & L_2*sin(\theta_2) \\ 0 & 0 & 1 \\ \end{bmatrix}

然后，我们需要计算的点是 A2 坐标系下的原点，因此最终得到二连杆的转换矩阵如下

T_0 = Trans(\theta_1,L_1)*R(\theta_1)*Trans(\theta_2,L_2)*R(\theta_2)=T_1*T_2

带入需要转换的点在 A2 坐标系下的坐标得到的最终计算公式如下

\begin{bmatrix} X_g \\ Y_g \\ 1 \\ \end{bmatrix} = T_0 * \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix}

总结以上步骤

如果有 1 个关节，也就是一根杆，需要 1 次旋转平移；
如果有 2 个关节，对于二连杆，需要 2 次旋转平移；
…
如果有 N 个关节，需要 N 次旋转平移。

结论：N 连杆，可以计算出 N 个旋转平移矩阵，直接将旋转平移矩阵相乘，就可以得到目标点在全局坐标下的坐标。

运动学正解完结撒花！！！

3. 运动学逆解（待定）

待定

4. 三维扩展（待定）

待定

FreeRTOS内核移植02 机械臂控制02——导纳控制和阻抗控制