01@LeDummy——正运动学

2026年9月27日 · 2836 字 · 6 分钟

📋 目录 ▼

正运动学解决一个问题：已知六个关节角，求末端参考点在基坐标系中的位置与朝向。

本文以 Dummy 当前固件的 SolveFK() 为准，沿着“关节角 → 连杆方向 → 末端位姿”的思路理解计算过程。以下是 Dummy 结构与运动学的可视化展示： 独立打开交互模型

1. 坐标系与角度

1.1 构建关节坐标系

采用右手坐标系，第 $i$ 个关节 $J_i$ 绕 $F_{i-1}$ 的 $+Z$ 轴正向转动，正方向遵循右手定则。

Dummy 关节参考坐标系

网页标记坐标系原点Z 轴的含义
基座 / J1 · F₀基座参考原点J1 转轴
J2 · F₁肩部 J2 轴心J2 转轴
J3 · F₂肘部 J3 轴心J3 转轴
J4 · F₃肘部偏置后的 J4 轴位J4 转轴
J5 · F₄、J6 · F₅固件理想腕心，两帧同原点分别为 J5、J6 转轴
末端 · F₆固件末端参考点末端局部 Z 方向

坐标系的原点位置与轴的朝向要分开理解。例如，$F_1$ 的原点在 J2，但其朝向由 J1 转角和固定扭转决定；J2 转动不会带着 $F_1$ 转动。

1.2 从电机读数到关节角 $q$

主控接收驱动板回传的电机位置圈数 $n_i$。以驱动板已经设定的零点为参考，应用层关节角为：

$$ q_i=s_i\frac{360n_i}{r_i}+\mathrm{initPose}_i $$

这里 $q_i$ 的单位为度。

  • $r_i$：电机到关节的有效减速比。当前 J1–J6 均配置为 50，电机转一圈，关节转 $7.2^\circ$。
  • $s_i$：方向修正。inverseDirection 为真时取 $-1$，否则取 $+1$。
  • initPose：驱动板零读数对应的应用层关节角，它把驱动零点与机械臂规定的姿态联系起来。

本文把加 initPose 之前的读数记为 motorAngle。虽然名称带 motor，它已经经过减速比和方向换算，是关节侧角度，不是电机轴原始转角。

调用 ApplyPositionAsHome() 是把当前位置设为驱动参考零点，本身不是运动到零位的指令。具体标定流程见 第 5 节：标定流程 。

1.3 从关节角 $q$ 到几何角 $\theta$

固件还需要把关节角 $q_i$ 转为坐标变换使用的角度：

$$ \theta_i=q_i\frac{\pi}{180}+h_i $$

$h_i$ 是 DH 表第一列的固定偏置，本文称为 home，在上式中以弧度计。它表示：当应用层 $q_i=0$ 时，这一节坐标系在几何上已经转了多少。

因此，initPose 与 home 是两层不同的对应关系：

$$ \begin{aligned} q&=\mathrm{motorAngle}+\mathrm{initPose}\\ \theta&=q\frac{\pi}{180}+h \end{aligned} $$

第一层得到应用层关节角（度），第二层得到矩阵使用的几何角（弧度）。

关节J1J2J3J4J5J6
home $h_i$0°−90°+90°0°0°0°

Dummy 中性姿态与 DH 几何角

如上图所示，以 $h_2$ 为例，按照 DH 角度 中第一次旋转的定义，J2 首先绕 $Z_1$ 轴旋转，使 $X_1$ 对齐 $X_2$。这一步需要顺时针旋转 90°，即 $\theta_2=-90^\circ$。又因为当前 $q_2=0^\circ$，所以 $h_2=-90^\circ$。

1.4 $\theta$、$\alpha$ 分别描述什么

从 $F_{i-1}$ 建立 $F_i$ 的朝向，需要两次旋转：

  1. 绕 $Z_{i-1}$ 转 $\theta_i$，将 $X_{i-1}$ 对齐到 $X_i$。
  2. 绕已经对齐的 $X_i$ 转 $\alpha_i$，将 $Z_{i-1}$ 对齐到 $Z_i$。

$\theta_i$ 随关节运动变化；$\alpha_i$ 是固定扭转角。两者都按旋转轴正方向使用右手定则。

关节J1J2J3J4J5J6
home $h_i$0°−90°+90°0°0°0°
$\alpha_i$-90°0°+90°-90°+90°0°

选定统一参考姿态后，$q_i=0$ 时就有 $\theta_i=h_i$。

2. 连杆向量：末端位置是四段位移之和

明确角度后，再看机械臂的长度。固件把基座到末端的路径合并成四段位移，每段最初都在指定的局部坐标系中表达。

代码变量位移路径局部分量（mm）
L1_base基座 → J2${}^{1}\mathbf L_{base}=[35,-109,0]^T$
L2_armJ2 → J3${}^{2}\mathbf L_{arm}=[146,0,0]^T$
L3_elbowJ3 → 腕心，经过 J4${}^{3}\mathbf L_{elbow}=[-52,0,115]^T$
L6_wrist腕心 → 末端参考点${}^{6}\mathbf L_{wrist}=[0,0,72]^T$

上标表示分量使用哪组坐标轴。例如，${}^{1}\mathbf L_{base}$ 表示从基座到 J2 的向量在 $F_1$ 坐标系中表示，如下图所示。

基座到 J2 的局部位移向量

这些分量来自六个几何参数：

参数L_BASED_BASEL_ARMD_ELBOWL_FOREARML_WRIST
数值（mm）109351465211572

特别注意，L3_elbow 合并了沿 $-X_3$ 的 52 mm 偏置和沿 $+Z_3$ 的 115 mm 前臂长度。它一直指向腕心，不止于 $F_3$/J4 原点。

Dummy 连杆向量示意

要把四段位移相加，必须先统一到基坐标系 $F_0$。记 ${}^{0}R_i$ 为把 $F_i$ 中的向量分量转换到 $F_0$ 的旋转矩阵：

$$ {}^{0}\mathbf v={}^{0}R_i\,{}^{i}\mathbf v $$

其中，旋转矩阵三列分别是 $F_i$ 的 X、Y、Z 单位轴在 $F_0$ 中的分量。于是，末端位置可以先写成：

$$ \begin{aligned} {}^{0}\mathbf p_{end}={}& {}^{0}R_1\,{}^{1}\mathbf L_{base} +{}^{0}R_2\,{}^{2}\mathbf L_{arm}\\ &+{}^{0}R_3\,{}^{3}\mathbf L_{elbow} +{}^{0}R_6\,{}^{6}\mathbf L_{wrist} \end{aligned} $$

长度已经知道，剩下的问题就是：这些旋转矩阵如何由关节角得到？

3. 旋转矩阵：把局部方向变成基座方向

3.1 相邻坐标系之间的旋转

第 1 节中的两次旋转，对应：

$$ C_i\equiv{}^{i-1}R_i=R_z(\theta_i)R_x(\alpha_i) $$

它把 $F_i$ 中表达的向量转换为 $F_{i-1}$ 中的分量。两个基础矩阵为：

$$ R_z(\theta)= \begin{bmatrix} \cos\theta&-\sin\theta&0\\ \sin\theta&\cos\theta&0\\ 0&0&1 \end{bmatrix} $$
$$ R_x(\alpha)= \begin{bmatrix} 1&0&0\\ 0&\cos\alpha&-\sin\alpha\\ 0&\sin\alpha&\cos\alpha \end{bmatrix} $$

相乘得到固件中的每节旋转矩阵 R[i]：

$$ C_i= \begin{bmatrix} \cos\theta_i&-\sin\theta_i\cos\alpha_i&\sin\theta_i\sin\alpha_i\\ \sin\theta_i&\cos\theta_i\cos\alpha_i&-\cos\theta_i\sin\alpha_i\\ 0&\sin\alpha_i&\cos\alpha_i \end{bmatrix} $$

“先 $\theta$、后 $\alpha$”描述的是逐步建立局部坐标系，第二次旋转绕的是转动后的 X 轴。矩阵乘以列向量时，代数计算顺序仍是从右向左。

3.2 沿关节链累计旋转

${}^{i-1}R_i$ 只描述相邻两帧；要转换到 $F_0$，需要把前面的旋转逐节乘起来：

$$ {}^{0}R_i=C_1C_2\cdots C_i $$

例如：

$$ \begin{aligned} {}^{0}R_2&=C_1C_2\\ {}^{0}R_3&=C_1C_2C_3\\ {}^{0}R_6&=C_1C_2C_3C_4C_5C_6 \end{aligned} $$

所以，J2 运动会改变从 ${}^{0}R_2$ 开始的后续矩阵；只动 J3 时，${}^{0}R_2$ 不变，肩到肘的那段也就不动。

4. initPose 与实机标定

主控始终按 $q=\mathrm{motorAngle}+\mathrm{initPose}$ 解释关节角。CalibrateHomeOffset() 的流程如下，角度均以度计：

  1. 在 L 记录临时零点：调用前手动摆好 L，执行 ApplyPositionAsHome()。它会将驱动板的 encoderHomeOffset 改为当前编码位置的单圈偏置并保存；不改变机械姿态，也不修改 initPose。

  2. 从 L 运动到 REST：将 initPose 和 currentJoints 都设为 $[0,0,90,0,0,0]$，再执行 Resting()。目标关节角为 $[0,-73,180,0,0,0]$，减去临时 initPose 后，下发的关节侧目标为 $[0,-73,90,0,0,0]$。

  3. 在 REST 记录最终零点：再次执行 ApplyPositionAsHome()，用 REST 时的单圈编码偏置覆盖 encoderHomeOffset。此时 initPose 仍是 L，尚未恢复。

  4. 重启完成对应：执行 Reboot()，重启驱动板和主控。驱动板加载新的偏置，主控恢复 initPose=REST_POSE，即 $[0,-73,180,0,0,0]$。保持在标定的 REST 时，motorAngle 应接近零，故 $q\approx\mathrm{REST\_POSE}$。

注意：ApplyPositionAsHome() 保存单圈偏置，不直接清空多圈计数，因此不能保证调用后反馈立即全零。

正确完成标定后，再从 REST 回到 L，关节侧读数应为：

$$ \begin{aligned} \mathrm{motorAngle}&=q-\mathrm{initPose}\\ &=[0,73,-90,0,0,0]^T\ \mathrm{deg} \end{aligned} $$

5. 规范 DH 参数

正运动学的基础不是 DH 表本身，而是相邻坐标系之间的刚体变换。DH（Denavit-Hartenberg）参数把每一节变换统一写成四个量，便于建模、推导和复用：

参数含义是否随运动变化
$\theta_i$绕 $Z_{i-1}$ 的转角是；对转动关节通常为 $q_i+h_i$
$d_i$沿 $Z_{i-1}$ 的位移否；对本机械臂为固定结构尺寸
$a_i$沿 $X_i$ 的位移否；对本机械臂为固定结构尺寸
$\alpha_i$绕 $X_i$ 的固定扭转角否

采用标准 DH 约定时，从 $F_{i-1}$ 到 $F_i$ 的齐次变换为：

$$ {}^{i-1}T_i= R_z(\theta_i)\,T_z(d_i)\,T_x(a_i)\,R_x(\alpha_i) $$

其中，$\theta_i$ 负责关节转动，$d_i,a_i,\alpha_i$ 则编码这一节机构的固定几何关系。连续相乘即可得到末端相对基座的完整位姿：

$$ {}^0T_6={}^0T_1{}^1T_2\cdots{}^5T_6 $$

Dummy 固件保存了同一套 DH 风格的角度偏置、长度和扭转参数：$\theta_i=q_i+h_i$，$\alpha_i$ 见第 1.4 节。实现时没有直接连乘六个 $4\times4$ 齐次矩阵，而是将其拆成两部分：先连乘 $R_z(\theta_i)R_x(\alpha_i)$ 得到各帧朝向，再将四段局部连杆向量旋转到 $F_0$ 后相加得到位置。因此它与 DH 的几何含义一致，只是更贴合这台机械臂的固定结构，也更节省 MCU 运算。

Comments