Task 2

本次任务主要学习 SO-101 机械臂的运动学基础，包括：

• Modified DH 参数与齐次变换矩阵
• 使用 NumPy 实现正运动学
• 使用 ikpy 求解位置逆运动学
• 使用 PyTorch 自动微分实现梯度下降 IK

实验代码：

https://github.com/LongtermFcq/lerobot-so101-vla-end2end-demo/tree/main/Task2

Task2 目录主要包含：

test1.py
test2_1.py
test2_2.py

---

一、Modified DH 参数

本题采用改进型 DH，也就是 Craig 约定。相邻坐标系之间的变换为：

$${}^{i-1}{T}_i=\mathrm{Rot}(x,{\alpha }_{i-1})\mathrm{Trans}(x,a_{i-1})\mathrm{Rot}(z,{\theta }_i)\mathrm{Trans}(z,d_i)$$

其中：

• ${\alpha }_{i-1}$：连杆扭转角
• $a_{i-1}$：连杆长度
• ${\theta }_i$：关节角
• $d_i$：连杆偏距

需要注意，标准 DH 与 Modified DH 的参数定义和变换顺序不同，编写程序时不能混用。

---

二、手算变换矩阵

给定：

$${\theta }_i=30^{\circ },d_i=0,a_{i-1}=0.5,{\alpha }_{i-1}=90^{\circ }$$

代入 Modified DH 公式可得：

$${}^{i-1}{T}_i=\left[\begin{array}{cccc}0.87& -0.50& 0& 0.50\\ 0& 0& -1& 0\\ 0.50& 0.87& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

右侧位置向量为：

$$P=(0.5,0,0)$$

与沿 $x$ 轴平移 $0.5$ 的几何含义一致。

---

三、正运动学

SO-101 的 Modified DH 参数如下：

关节	${\alpha }_{i-1}$	$a_{i-1}$	$d_i$	实际关节角
shoulder_pan	$0$	0	0.0624	$q_1$
shoulder_lift	$-\pi /2$	0.035	0	$q_2-\pi /2$
elbow_flex	0	0.116	0	$q_3$
wrist_flex	0	0.135	0	$q_4$
wrist_roll	$-\pi /2$	0	0.061	$q_5$

程序根据每一行 DH 参数生成相邻变换矩阵，并依次连乘：

T = np.eye(4)

for i, (alpha, a, d, offset) in enumerate(DH_TABLE):
    theta = q[i] + offset
    Ti = dh_transform(alpha, a, theta, d)
    T = T @ Ti

其中，第 2 个关节带有 $-90^{\circ }$ 的角度偏置，这是实现时最容易遗漏的部分。

以全零关节角为例：

$$q=[0,0,0,0,0]$$

得到末端位置：

$$P=(0.0960,0,0.3134)\mathrm{m}$$

其高度可以由机械臂尺寸直接验证：

$$0.0624+0.116+0.135=0.3134\mathrm{m}$$

水平方向位置为：

$$0.035+0.061=0.096\mathrm{m}$$

说明 DH 参数与矩阵连乘结果正确。

---

四、使用 ikpy 求解逆运动学

test2_1.py 采用闭环方式验证 IK：

1. 设置一组关节角真值
2. 通过正运动学生成目标位置
3. 使用 ikpy 反解关节角
4. 将求解结果重新代入正运动学
5. 计算末端位置误差

实验设置：

$$q_{\mathrm{true}}=(30^{\circ },-20^{\circ },45^{\circ },-30^{\circ },60^{\circ })$$

对应目标位置约为：

$$P_{\mathrm{target}}=(0.0980,0.0566,0.2991)\mathrm{m}$$

ikpy 求得：

$$q_{\mathrm{ikpy}}=(30.00^{\circ },1.97^{\circ },4.27^{\circ },6.51^{\circ },0^{\circ })$$

虽然求出的关节角与原始关节角不同，但将其代回正运动学后，末端位置误差接近零，因此 IK 求解有效。

这也说明：逆运动学的正确性应通过末端位姿误差判断，而不是要求恢复原始关节角。

---

五、使用梯度下降求解 IK

逆运动学也可以看作一个优化问题：

$$\mathrm{Loss}(q)={\Vert P_{\mathrm{target}}-P_{\mathrm{current}}(q)\Vert }^2$$

test2_2.py 使用 PyTorch 编写可微正运动学，并通过 Adam 更新关节角：

optimizer.zero_grad()

P_current = forward_kinematics(q)
loss = torch.sum((P_target - P_current) ** 2)

loss.backward()
optimizer.step()

实现时，含有梯度的矩阵元素需要通过 torch.stack 组装。若直接使用 torch.tensor([[...]]) 重新构造矩阵，可能切断自动微分计算图。

实验在第 219 次迭代收敛，得到：

$$q_{\mathrm{Adam}}=(30.00^{\circ },12.42^{\circ },-15.27^{\circ },13.67^{\circ },0^{\circ })$$

最终位置误差约为：

$$0.000874\mathrm{mm}$$

说明使用自动微分和梯度优化同样能够完成位置逆运动学求解。

---

六、结果分析

本次实验得到三组不同的关节角：

$$q_{\mathrm{true}}=(30^{\circ },-20^{\circ },45^{\circ },-30^{\circ },60^{\circ })$$

$$q_{\mathrm{ikpy}}=(30.00^{\circ },1.97^{\circ },4.27^{\circ },6.51^{\circ },0^{\circ })$$

$$q_{\mathrm{Adam}}=(30.00^{\circ },12.42^{\circ },-15.27^{\circ },13.67^{\circ },0^{\circ })$$

三组关节角都能使末端到达同一个目标位置，说明位置逆运动学通常具有多解性。

本次机械臂有 5 个关节变量，而目标仅包含三维位置，因此系统存在冗余自由度。

其中，wrist_roll 对当前定义下的末端位置没有影响，所以 $q_5$ 在两种位置 IK 方法中都保持为初始值 $0^{\circ }$。

需要说明的是，本次代码求解的是末端位置 IK，并没有加入末端姿态约束。若要实现完整位姿 IK，还需要在损失函数中增加旋转误差。

---

七、总结

本次 Task 2 完成了 SO-101 机械臂从 Modified DH 建模、正运动学计算，到两种逆运动学数值求解方法的完整流程。

ikpy 的优点是求解速度快、使用方便；基于 PyTorch 的梯度下降方法收敛较慢，但损失函数更加灵活，后续可以继续加入：

• 末端姿态约束
• 关节角限位
• 参考姿态正则项
• 避障约束
• 运动平滑约束

通过本次实验，也进一步验证了 DH 约定统一、独立模型交叉验证以及 IK 结果回代验证的重要性。