第 2 课 / 共 30 课📐 运动学阶段

正运动学

🎯 正运动学的目标

正运动学（FK）解决：已知关节变量，求末端位姿。这是机械臂控制的基础。

T_0^n = T_0^1(q₁)·T_1^2(q₂)·...·T_{{n-1}}^n(q_n)

📐 2R平面臂解析推导

正运动学解：

x = L₁cosθ₁ + L₂cos(θ₁+θ₂)
y = L₁sinθ₁ + L₂sin(θ₁+θ₂)
φ = θ₁ + θ₂

💻 通用正运动学求解器

Python
import numpy as np
def dh_matrix(theta,d,a,alpha):
    ct,st=np.cos(theta),np.sin(theta); ca,sa=np.cos(alpha),np.sin(alpha)
    return np.array([[ct,-st*ca,st*sa,a*ct],[st,ct*ca,-ct*sa,a*st],[0,sa,ca,d],[0,0,0,1]])
class RobotArm:
    def __init__(self,dh,jt=None):
        self.dh=dh; self.n=len(dh); self.jt=jt or ['R']*self.n
    def fk(self,q):
        T=np.eye(4); transforms=[T]
        for i in range(self.n):
            a,al,d_off,th_off=self.dh[i]
            if self.jt[i]=='R': theta=q[i]+th_off; d=d_off
            else: theta=th_off; d=q[i]+d_off
            Ti=dh_matrix(theta,d,a,al); T=T@Ti; transforms.append(T)
        return T,transforms
    def pos(self,q): T,_=self.fk(q); return T[:3,3]
arm2r=RobotArm([(1.0,0,0,0),(0.8,0,0,0)])
configs=[[0,0],[np.pi/4,np.pi/6],[np.pi/2,-np.pi/4],[np.pi,np.pi/2]]
print("2R平面臂正运动学"); print("="*50)
for q in configs:
    p=arm2r.pos(q); T,_=arm2r.fk(q)
    print(f"θ=({q[0]:.4f},{q[1]:.4f})→pos=({p[0]:.4f},{p[1]:.4f},{p[2]:.4f})")
L1,L2=1.0,0.8; r_max=L1+L2; r_min=abs(L1-L2)
area=np.pi*(r_max**2-r_min**2)
print(f"\n工作空间: r∈[{r_min:.2f},{r_max:.2f}]m, 面积≈{area:.2f}m²")
np.random.seed(42); ok=0
for _ in range(1000):
    q=np.random.uniform(-np.pi,np.pi,2); r=np.sqrt((L1*np.cos(q[0])+L2*np.cos(q[0]+q[1]))**2+(L1*np.sin(q[0])+L2*np.sin(q[0]+q[1]))**2)
    if r_min<=r<=r_max: ok+=1
print(f"随机采样验证: {ok}/1000点在可达范围内")

运行结果

2R平面臂正运动学 ================================================== θ=(0.0000,0.0000)→pos=(1.8000,0.0000,0.0000) θ=(0.7854,0.5236)→pos=(0.9142,1.4798,0.0000) θ=(1.5708,-0.7854)→pos=(0.5657,1.5657,0.0000) θ=(3.1416,1.5708)→pos=(-1.0000,-0.8000,0.0000) 工作空间: r∈[0.20,1.80]m, 面积≈10.05m² 随机采样验证: 1000/1000点在可达范围内
正运动学验证通过：所有位置计算正确，随机采样100%在可达范围

📌 正运动学性质

  1. 唯一性：给定q→唯一T
  2. 连续性：FK是q的连续函数
  3. 非线性：三角函数使FK非线性
  4. 计算O(n)：n次矩阵乘法

🔬 工程实践要点

运动学实施注意事项

📖 延伸阅读

推荐资源

🔬 扩展实验：工作空间蒙特卡洛采样

Python
import numpy as np
L1,L2=1.0,0.8; N=10000
np.random.seed(0); q1=np.random.uniform(-np.pi,np.pi,N); q2=np.random.uniform(-np.pi,np.pi,N)
x=L1*np.cos(q1)+L2*np.cos(q1+q2); y=L1*np.sin(q1)+L2*np.sin(q1+q2)
r=np.sqrt(x**2+y**2)
print(f"蒙特卡洛采样(N={N})")
print(f"  x:[{x.min():.3f},{x.max():.3f}] y:[{y.min():.3f},{y.max():.3f}]")
print(f"  r:[{r.min():.3f},{r.max():.3f}] 理论:[{abs(L1-L2):.3f},{L1+L2:.3f}]")

蒙特卡洛采样通过大量随机关节角探索工作空间形状，是可视化复杂机械臂工作空间的标准方法。

💡 知识图谱

本课在知识体系中的位置

本课程按"运动学→动力学→力控制→感知规划→实战项目"组织，每课都建立在前面知识基础上。理解本课后，你将：

🎯 学习目标回顾

完成本课后，你应该能够：

📋 本课小结

关键要点

  1. 本课的核心概念和公式已在仿真中验证
  2. Python实现提供了从理论到代码的完整路径
  3. 课后练习将帮助你深化理解和应用能力

下一步

完成练习后，继续下一课的学习。本课内容将在后续课程中被反复使用和扩展。

📚 运动学理论基础

齐次变换矩阵

齐次变换矩阵是描述三维空间中刚体位姿的数学工具。一个4×4的齐次变换矩阵T同时包含了旋转信息（3×3旋转矩阵R）和平移信息（3×1位置向量p）：

T = [R p]
[0 1]

旋转矩阵R是正交矩阵（RᵀR=I，det(R)=1），有3个自由度；位置向量p有3个自由度。因此一个刚体在三维空间中有6个自由度。

欧拉角与旋转表示

旋转有多种等价表示：

连杆坐标系的建立规则

DH参数法为每个连杆建立一个坐标系，建立规则如下：

  1. zi轴沿关节i+1的运动轴方向
  2. xi轴沿zi-1和zi的公垂线方向
  3. yi轴由右手定则确定
  4. 原点Oi在xi与zi的交点

当相邻z轴平行或相交时，需要特殊处理。

运动学标定

实际机器人的运动学参数与设计值有偏差，需要通过运动学标定来补偿。标定流程：

  1. 移动机器人到多组已知位姿
  2. 用外部测量设备（激光跟踪仪/视觉）测量末端实际位姿
  3. 建立参数与末端位姿的误差模型
  4. 用最小二乘法辨识DH参数修正量
  5. 更新控制器中的运动学参数

标定后定位精度可从毫米级提升到0.1mm级别。

🏭 工业应用实例

本课知识在实际工程中的应用

本课涉及的技术在以下场景中有重要应用：

理解理论与实践的差距，是成为优秀机器人工程师的关键。

❓ 常见问题解答

Q1: 本课的算法在实际机器人上能直接使用吗？

仿真验证的算法通常需要以下适配才能部署到实际机器人：参数标定、实时性优化、异常处理、安全保护。仿真与现实的差距（sim-to-real gap）是机器人领域的重要挑战。

Q2: Python实现的效率够用吗？

Python适合算法验证和原型开发，但生产级控制器通常用C++实现。Python的NumPy/SciPy计算效率约为C++的1/10到1/100，对于1kHz控制频率可能不够。可以使用Cython、Numba或直接C++重写来加速。

Q3: 如何从仿真过渡到实际机器人？

推荐步骤：(1) Python仿真验证算法正确性；(2) 添加传感器噪声和延迟模型测试鲁棒性；(3) 用C++重写核心计算模块；(4) 在低速度下实际测试；(5) 逐步提高速度和负载。安全永远是第一位的。

🐛 调试技巧

本课代码的常见问题和调试方法

📝 课后练习

练习 1：绘制2R臂姿态图（matplotlib画连杆和关节）
练习 2：验证3R空间臂特定位形的末端位置
练习 3：推导6R臂正运动学
练习 4：验证home位姿(θ=0)的末端位置
练习 5：设计4DOF SCARA的DH参数
🏆

成就解锁：正运动学大师

掌握从关节空间到笛卡尔空间的映射！

🔐 安全提示

在实际机器人上实验时，请始终遵循以下安全规则：