02@LeDummy——逆运动学

2026年9月30日 · 2344 字 · 5 分钟具身智能 LeDummy

📋 目录 ▼

逆运动学解决的问题与正运动学相反：已知末端参考点的位置和朝向，求六个关节应取的角度。

Dummy 当前固件使用的是解析法。它利用这台机械臂确定的几何结构，直接通过三角函数和旋转矩阵产生候选关节角，而不是从一个初始姿态出发反复迭代逼近目标。

本文仍以当前固件的 SolveIK() 为准。正运动学中已经建立过坐标系、home 和连杆向量，本文直接从目标位姿开始：

({}^{0}\mathbf p_6,{}^{0}R_6) \longrightarrow [q_1,q_2,q_3,q_4,q_5,q_6]

以下是 Dummy 结构与运动学的可视化展示：独立打开交互模型。

1. 六轴拆分

Dummy 的逆解并不是一次同时解六个角度。它先根据目标末端位姿求出腕心，再拆成两部分：

J1、J2、J3 决定腕心的位置。
J4、J5、J6 决定腕心之后的末端朝向。

这个拆分成立，是因为固件将腕心到末端参考点的长度写为局部向量：

{}^{6}\mathbf L_{wrist}=[0,0,L_{WRIST}]^T

因此，已知目标末端位置和朝向后，可以沿末端局部 Z 轴回退：

{}^{0}\mathbf p_w={}^{0}\mathbf p_6-{}^{0}R_6\,{}^{6}\mathbf L_{wrist}

$\mathbf p_w$ 就是腕心。前三轴只需将它送到正确位置；后三轴只需把末端转到正确姿态。

2. 腕心位置：J1、J2、J3

2.1 J1 先确定腕心方位

将腕心投影到基座 XY 平面，J1 的一支解为：

q_1=\operatorname{atan2}(p_{w,y},p_{w,x})

另一支与其相差 $\pi$。这不是末端的朝向，而是腕心相对于 J1 转轴的方位。

确定 J1 后，固件把腕心转换到 $F_1$，并减去基座到肩部的固定偏置 L1_base。余下的 J2/J3 问题就落在一个平面三角形中：肩点 S、肘点 E、腕心 W。

2.2 J2/J3 是肩—肘—腕心三角形

这里有一个容易忽略的细节：前臂不只是 L_FOREARM=115 mm。肘部还存在 D_ELBOW=52 mm 的横向偏置，固件将它们合成为等效长度：

L_{EW}=\sqrt{L_{FOREARM}^2+D_{ELBOW}^2}

同时定义偏置角：

\delta=\operatorname{atan2}(D_{ELBOW},L_{FOREARM})

于是，已知 $L_{SE}=L_{ARM}$、$L_{EW}$ 和 $L_{SW}$ 后，可由余弦定理求三角形的两个内角。本文将肩部内角记为 $\gamma$，将换算到 J3 需要的肘部角记为 $\epsilon$。

关键不在于死记某一个公式，而在于分清三种角：

$\phi$：肩点指向腕心的方向；
$\gamma,\epsilon$：三角形由余弦定理得到的内角；
$\delta$：肘部横向偏置带来的固定几何修正。

2.3 辨识 $q$：看下一坐标系的 X 轴

三角形的内角不是关节角本身。更直观的判断方法是：转动当前关节后，看下一个关节坐标系的 X 轴相对其零位转了多少；这个变化才是应用层的 $q$。

转动 J2 时，观察 $F_2$ 的 $X_2$。$X_2$ 沿上臂方向，三角形中由 $\phi$ 与 $\gamma$ 得到的方向变化，换算为 $X_2$ 相对 L 姿态的转角后，才是 $q_2$。
转动 J3 时，观察 $F_3$ 的 $X_3$；它是 J4 所使用的参考 X 轴。应当匹配的是 $X_3$ 的变化，而不是直接把三角形中某条前臂边的夹角当作 $q_3$。

J3 还需要特别注意 L 姿态基准：L 姿态下 $q_3=\frac{\pi}{2}$，而不是零。若三角形给出的 $X_3$ 偏转为：

\epsilon-\left(\frac{\pi}{2}-\delta\right)

则应将它加回 L 姿态原有的 $\frac{\pi}{2}$，因为其 L 姿态下 $X_3$ 就是 $\frac{\pi}{2}$ ：

\begin{aligned} q_3&=\frac{\pi}{2}+\epsilon-\left(\frac{\pi}{2}-\delta\right)\\ &=\epsilon+\delta \end{aligned}

这正是 $q_3=\delta\pm\epsilon$ 的一支；另一种肘部构型取对应的负号。这里的 L 姿态基准，与后续将应用层 $q$ 转换为矩阵几何角 $\theta$ 时使用的 home 固定偏置，是两个不同层次的量。

三角形天然给出两种肘部构型：肘上和肘下。它们腕心位置相同，但中间连杆的姿态不同。

3. 末端姿态：J4、J5、J6

前三轴确定后，固件已经可以得到它们累计形成的朝向 ${}^{0}R_3$。目标末端朝向为 ${}^{0}R_6$，于是腕部应完成的相对旋转为：

{}^{3}R_6=({}^{0}R_3)^T{}^{0}R_6

旋转矩阵满足 $R^{-1}=R^T$，因此这里不是普通意义上的矩阵求逆，而是把目标姿态从基座系转换到 $F_3$。在固件中，这个结果叫 R36。

腕部三轴对应：

{}^{3}R_6= R_z(q_4)R_x(-\frac{\pi}{2}) R_z(q_5)R_x(+\frac{\pi}{2}) R_z(q_6)

展开后，固件选择其中几个元素反求关节角：

q_5=\pm\arccos(R_{36}[8])

q_4=\operatorname{atan2}(R_{36}[5],R_{36}[2])

q_6=\operatorname{atan2}(R_{36}[7],-R_{36}[6])

后两式对应选取 $\sin q_5>0$ 的一支；若选取负的 $q_5$，固件会同步翻转 atan2 两个输入的符号，得到同一姿态下的另一组 $q_4,q_6$。直观地说，$q_5$ 先决定腕部是否翻转；在选定 $q_5$ 的分支后，$q_4,q_6$ 才能分别由矩阵元素确定。

4. 一个目标姿态为什么有八组逆解

同一个末端位姿通常不只对应一组关节角。Dummy 的解析逆解最多产生：

分支	数量	含义
J1 方位	2	腕心相对基座转轴的两种朝向
J2/J3 肘部构型	2	肘上、肘下
J4/J5/J6 腕部构型	2	腕翻、腕不翻

腕部两支的关系尤其值得记住：

(q_4,q_5,q_6) \Longleftrightarrow (q_4+\pi,-q_5,q_6+\pi)

两组角度对应相同的末端姿态，但手腕机械结构处于不同构型。实际执行时，关节限位和当前姿态会决定保留哪一组。

5. 奇异位姿：不是不可达，而是不唯一

奇异位姿不是“逆解公式失效”，而是机构在该姿态下无法从末端位姿唯一地区分某些关节角。

5.1 J1 奇异

若腕心刚好落在 J1 转轴上：

\sqrt{p_{w,x}^2+p_{w,y}^2}=0

则任何 J1 方位都会给出同一个腕心位置，$\operatorname{atan2}(0,0)$ 没有唯一的物理含义。

5.2 J2/J3 三角形退化

当上臂和等效前臂完全伸直或折叠时：

L_{SW}=L_{SE}+L_{EW} \quad\text{或}\quad L_{SW}=|L_{SE}-L_{EW}|

肘上、肘下两支会合并。若实机关节限位不允许到达这两个边界，日常使用时一般不会遇到它；但固件仍需处理该数学边界。

5.3 腕部奇异

当：

q_5=0\quad\text{或}\quad q_5=\pi

J4 与 J6 的转轴共线。以 $q_5=0$ 为例，姿态只与组合角有关：

R_{36}=R_z(q_4+q_6)

此时无法单独确定 J4、J6 应各转多少，只能确定 $q_4+q_6$。例如 $(20^\circ,0^\circ,40^\circ)$ 与 $(50^\circ,0^\circ,10^\circ)$ 给出相同的腕部姿态。

6. 固件如何选择最终解

SolveIK() 生成 8 组 config 后，MoveL() 会：

检查六个关节是否均在角度限位内；
淘汰任一轴越界的候选解；
对余下候选，计算它与 currentJoints 的最大单轴角度差；
选择该最大差值最小的一组；
调用 MoveJ() 下发目标。

它的选择标准不是六轴总移动量，而是：

\underset{i}{\operatorname{argmin}}\ \max_j|q_{i,j}-q_{current,j}|

7. `MoveL()` 目前并不保证 TCP 直线

名字容易让人误解。当前 MoveL() 的实际流程是：

\text{目标 TCP 位姿} \rightarrow \text{一次 IK} \rightarrow \text{选一组关节目标} \rightarrow \texttt{MoveJ()}

MoveJ() 让六轴按比例同步到达，所以末端能到达目标，但中间通常走的是空间曲线，不是严格直线。

真正的笛卡尔直线运动需要进一步做路径插补：将起点到终点分成多个 TCP 中间点，对每个点连续求 IK，再按时间顺序执行。这是轨迹规划层的工作，而不只是一次逆运动学求解。

Comments