嵌入式硬件篇---机械臂运动学解算（3自由度）

实际 3 自由度机械臂的解算是机器人控制的核心，涉及运动学正解（关节角度→末端位姿）和逆解（目标位姿→关节角度）。以下从结构建模、解算方法、代码实现和应用场景四个维度详细展开，结合工业级机械臂的典型场景进行说明。

一、机械臂结构与坐标系定义

1. 典型结构（RRR 型串联机械臂）

3 自由度机械臂通常由三个旋转关节组成，例如：

• 关节 1（J1）：基座旋转关节，绕垂直轴旋转；
• 关节 2（J2）：肩关节，绕水平轴旋转；
• 关节 3（J3）：肘关节，绕水平轴旋转。

2. D-H 参数法建模

通过 Denavit-Hartenberg 参数法为每个关节建立坐标系，定义四个参数（单位：弧度 / 米）：

• θᵢ：关节 i 的旋转角度（旋转关节的变量）；
• dᵢ：关节 i 的偏移量（移动关节的变量）；
• aᵢ：连杆 i 的长度（沿 X 轴的距离）；
• αᵢ：连杆 i 的扭转角（绕 X 轴的旋转角）。

例如，某 3 自由度机械臂的 D-H 参数表如下（单位：米，弧度）：

3. 齐次变换矩阵

每个关节的变换矩阵 Tᵢ 由旋转和平移组成，公式为：
Ti​=​cosθi​sinθi​00​−sinθi​cosαi​cosθi​cosαi​sinαi​0​sinθi​sinαi​−cosθi​sinαi​cosαi​0​ai​cosθi​ai​sinθi​di​1​​

将所有关节的变换矩阵连乘，得到末端执行器相对于基座的位姿矩阵 T₀₃：
T03​=T1​⋅T2​⋅T3​

二、正运动学解算（关节角度→末端位姿）

1. 解算流程

1. 输入关节角度：θ₁~θ₃（单位：弧度）；
2. 计算各关节变换矩阵：根据 D-H 参数表生成 T₁~T₃；
3. 矩阵连乘：得到末端位姿矩阵 T₀₃；
4. 提取结果：
   • 位置：T₀₃的前三行第四列（x, y, z）；
   • 姿态：用欧拉角或轴角表示（如绕 X 轴旋转 φ，绕 Y 轴旋转 θ，绕 Z 轴旋转 ψ）。

2. Python 代码示例（含参数解释）

import numpy as np

def forward_kinematics(theta1, theta2, theta3):
    """
    正运动学解算：已知关节角度，计算末端位姿
    参数：
        theta1, theta2, theta3: 关节角度（弧度）
    返回：
        T03: 4x4齐次变换矩阵
    """
    # D-H参数（单位：米，弧度）
    d = [0.1607, 0, 0]
    a = [0, 0.425, 0.393]
    alpha = [0, np.pi/2, 0]

    # 生成各关节变换矩阵
    T1 = np.array([
        [np.cos(theta1), -np.sin(theta1)*np.cos(alpha[0]), np.sin(theta1)*np.sin(alpha[0]), a[0]*np.cos(theta1)],
        [np.sin(theta1), np.cos(theta1)*np.cos(alpha[0]), -np.cos(theta1)*np.sin(alpha[0]), a[0]*np.sin(theta1)],
        [0, np.sin(alpha[0]), np.cos(alpha[0]), d[0]],
        [0, 0, 0, 1]
    ])

    T2 = np.array([
        [np.cos(theta2), -np.sin(theta2)*np.cos(alpha[1]), np.sin(theta2)*np.sin(alpha[1]), a[1]*np.cos(theta2)],
        [np.sin(theta2), np.cos(theta2)*np.cos(alpha[1]), -np.cos(theta2)*np.sin(alpha[1]), a[1]*np.sin(theta2)],
        [0, np.sin(alpha[1]), np.cos(alpha[1]), d[1]],
        [0, 0, 0, 1]
    ])

    T3 = np.array([
        [np.cos(theta3), -np.sin(theta3)*np.cos(alpha[2]), np.sin(theta3)*np.sin(alpha[2]), a[2]*np.cos(theta3)],
        [np.sin(theta3), np.cos(theta3)*np.cos(alpha[2]), -np.cos(theta3)*np.sin(alpha[2]), a[2]*np.sin(theta3)],
        [0, np.sin(alpha[2]), np.cos(alpha[2]), d[2]],
        [0, 0, 0, 1]
    ])

    # 连乘得到T03
    T03 = np.dot(T1, np.dot(T2, T3))
    return T03

# 测试：关节角度θ₁=30°, θ₂=60°, θ₃=45°（转换为弧度）
theta1 = np.deg2rad(30)
theta2 = np.deg2rad(60)
theta3 = np.deg2rad(45)
T03 = forward_kinematics(theta1, theta2, theta3)
print("末端位姿矩阵：\n", T03)

三、逆运动学解算（目标位姿→关节角度）

1. 解析法步骤（以 RRR 型机械臂为例）

1. 分离位置与姿态：前两个关节确定位置，第三个关节调整姿态。
2. 求解 θ₁：θ1​=arctan2(xy​)
3. 求解 θ₂和 θ₃：
   • 计算目标点到基座的距离：D=x2+y2​
   • 用余弦定理求解 θ₂和 θ₃：θ2​=arccos(2⋅D⋅L1D2+L12−L22​)−arctan2(Dz​)θ3​=arccos(2⋅L1⋅L2L12+L22−D2​)（其中 L1、L2 为连杆长度）

2. Python 代码示例（含参数解释）

def inverse_kinematics(x, y, z, L1=0.425, L2=0.393):
    """
    逆运动学解算：已知目标坐标，计算关节角度
    参数：
        x, y, z: 目标坐标（米）
        L1, L2: 连杆长度（米）
    返回：
        theta1, theta2, theta3: 关节角度（弧度）
    """
    # 步骤1：求解θ₁
    theta1 = np.arctan2(y, x)

    # 步骤2：计算目标点到基座的距离D
    D = np.sqrt(x**2 + y**2)

    # 步骤3：求解θ₂和θ₃
    # 计算中间变量
    a = (D**2 + L1**2 - L2**2) / (2 * D * L1)
    b = z / D
    theta2 = np.arccos(a) - np.arctan2(b, np.sqrt(1 - b**2))
    
    c = (L1**2 + L2**2 - D**2) / (2 * L1 * L2)
    theta3 = np.arccos(c)

    return theta1, theta2, theta3

# 测试：目标坐标(x=0.5m, y=0.3m, z=0.2m)
x = 0.5
y = 0.3
z = 0.2
theta1, theta2, theta3 = inverse_kinematics(x, y, z)
print("关节角度（弧度）：θ₁=%.2f, θ₂=%.2f, θ₃=%.2f" % (theta1, theta2, theta3))
print("关节角度（度）：θ₁=%.2f°, θ₂=%.2f°, θ₃=%.2f°" % (np.rad2deg(theta1), np.rad2deg(theta2), np.rad2deg(theta3)))

3. 多解处理与奇异性

• 多解性：3 自由度机械臂通常存在2 组逆解（θ₂的正负解），需根据关节限位和运动路径选择合理解。
• 奇异性：当 θ₂+θ₃=0° 或 180° 时，机械臂失去一个自由度，需采用阻尼最小二乘法避免发散：Δθ=(JTJ+λI)−1JTe其中，J 为雅可比矩阵，e 为位姿误差，λ 为阻尼系数。

四、实际应用与优化

1. 工业场景示例

• 装配机器人：输入装配点的位姿，逆解算得到关节角度，控制机械臂完成零件安装。
• 医疗手术机器人：通过医学影像获取目标位置，逆解算生成微小关节运动，实现精准操作。

2. 实时性优化

• 硬件加速：使用 FPGA 或 GPU 并行计算矩阵乘法。
• 预计算：将常用位姿的逆解存入查表，减少实时计算量。

3. 误差补偿

• 标定：通过激光跟踪仪测量末端实际位置，修正 D-H 参数。
• 柔顺控制：结合力传感器实时调整关节角度，补偿装配误差。

五、核心结论

3 自由度机械臂的解算是理论与工程的结合，需在精度、实时性和鲁棒性之间权衡。实际应用中，通常结合运动规划算法（如 RRT、人工势场法）和控制系统（如 ROS MoveIt!）实现复杂任务。