LeRobot SO-101 机械臂从底层控制到 VLA 视觉语言动作模型完整实操复盘

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 = Rot ⁡ ( x , α i − 1 ) Trans ⁡ ( x , a i − 1 ) Rot ⁡ ( z , θ i ) Trans ⁡ ( z , d i ) {}^{i-1}T_i= \operatorname{Rot}(x,\alpha_{i-1}) \operatorname{Trans}(x,a_{i-1}) \operatorname{Rot}(z,\theta_i) \operatorname{Trans}(z,d_i) i−1Ti=Rot(x,αi−1)Trans(x,ai−1)Rot(z,θi)Trans(z,di)

其中：

• α i − 1 \alpha_{i-1} αi−1：连杆扭转角
• a i − 1 a_{i-1} ai−1：连杆长度
• θ i \theta_i θi：关节角
• d i d_i di：连杆偏距

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

---

二、手算变换矩阵

给定：

θ i = 30 ∘ , d i = 0 , a i − 1 = 0.5 , α i − 1 = 90 ∘ \theta_i=30^\circ,\quad d_i=0,\quad a_{i-1}=0.5,\quad \alpha_{i-1}=90^\circ θi=30∘,di=0,ai−1=0.5,αi−1=90∘

代入 Modified DH 公式可得：

i − 1 T i = 0.87 − 0.50 0 0.50 0 0 − 1 0 0.50 0.87 0 0 0 0 0 1 {}^{i-1}T_i= \begin{bmatrix} 0.87 & -0.50 & 0 & 0.50\\ 0 & 0 & -1 & 0\\ 0.50 & 0.87 & 0 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} i−1Ti= 0.8700.500−0.5000.8700−1000.50001

右侧位置向量为：

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

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

---

三、正运动学

SO-101 的 Modified DH 参数如下：

关节	α i − 1 \alpha_{i-1} αi−1	a i − 1 a_{i-1} ai−1	d i d_i di	实际关节角
shoulder_pan	0 0 0	0	0.0624	q 1 q_1 q1
shoulder_lift	− π / 2 -\pi/2 −π/2	0.035	0	q 2 − π / 2 q_2-\pi/2 q2−π/2
elbow_flex	0	0.116	0	q 3 q_3 q3
wrist_flex	0	0.135	0	q 4 q_4 q4
wrist_roll	− π / 2 -\pi/2 −π/2	0	0.061	q 5 q_5 q5

程序根据每一行 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 ∘ -90^\circ −90∘ 的角度偏置，这是实现时最容易遗漏的部分。

以全零关节角为例：

q = 0 , 0 , 0 , 0 , 0 q=0,0,0,0,0 q=0,0,0,0,0

得到末端位置：

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

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

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

水平方向位置为：

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

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

---

四、使用 ikpy 求解逆运动学

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

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

实验设置：

q t r u e = ( 30 ∘ , − 20 ∘ , 45 ∘ , − 30 ∘ , 60 ∘ ) q_{\mathrm{true}} =(30^\circ,-20^\circ,45^\circ,-30^\circ,60^\circ) qtrue=(30∘,−20∘,45∘,−30∘,60∘)

对应目标位置约为：

P t a r g e t = ( 0.0980 , 0.0566 , 0.2991 ) m P_{\mathrm{target}} =(0.0980,\ 0.0566,\ 0.2991)\ \mathrm{m} Ptarget=(0.0980, 0.0566, 0.2991) m

ikpy 求得：

q i k p y = ( 30.00 ∘ , 1.97 ∘ , 4.27 ∘ , 6.51 ∘ , 0 ∘ ) q_{\mathrm{ikpy}} =(30.00^\circ,1.97^\circ,4.27^\circ,6.51^\circ,0^\circ) qikpy=(30.00∘,1.97∘,4.27∘,6.51∘,0∘)

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

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

---

五、使用梯度下降求解 IK

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

Loss ⁡ ( q ) = ∥ P t a r g e t − P c u r r e n t ( q ) ∥ 2 \operatorname{Loss}(q)= \left\| P_{\mathrm{target}}-P_{\mathrm{current}}(q) \right\|^2 Loss(q)=∥Ptarget−Pcurrent(q)∥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 A d a m = ( 30.00 ∘ , 12.42 ∘ , − 15.27 ∘ , 13.67 ∘ , 0 ∘ ) q_{\mathrm{Adam}} =(30.00^\circ,12.42^\circ,-15.27^\circ,13.67^\circ,0^\circ) qAdam=(30.00∘,12.42∘,−15.27∘,13.67∘,0∘)

最终位置误差约为：

0.000874 m m 0.000874\ \mathrm{mm} 0.000874 mm

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

---

六、结果分析

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

q t r u e = ( 30 ∘ , − 20 ∘ , 45 ∘ , − 30 ∘ , 60 ∘ ) q_{\mathrm{true}} =(30^\circ,-20^\circ,45^\circ,-30^\circ,60^\circ) qtrue=(30∘,−20∘,45∘,−30∘,60∘)

q i k p y = ( 30.00 ∘ , 1.97 ∘ , 4.27 ∘ , 6.51 ∘ , 0 ∘ ) q_{\mathrm{ikpy}} =(30.00^\circ,1.97^\circ,4.27^\circ,6.51^\circ,0^\circ) qikpy=(30.00∘,1.97∘,4.27∘,6.51∘,0∘)

q A d a m = ( 30.00 ∘ , 12.42 ∘ , − 15.27 ∘ , 13.67 ∘ , 0 ∘ ) q_{\mathrm{Adam}} =(30.00^\circ,12.42^\circ,-15.27^\circ,13.67^\circ,0^\circ) qAdam=(30.00∘,12.42∘,−15.27∘,13.67∘,0∘)

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

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

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

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

---

七、总结

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

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

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

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