# 雅可比矩阵向量叉乘法求解指南 ## 概述 向量叉乘法（几何矢量法）是机器人学中求解雅可比矩阵最常用的工程实现方法，特别适用于 3D 空间多自由度机械臂。 --- ## 核心原理 雅可比矩阵建立了**关节速度**与**末端执行器速度**之间的映射关系： ``` ẋ = J(q) · q̇ ``` 其中： - **ẋ** = [v_x, v_y, v_z, ω_x, ω_y, ω_z]ᵀ：末端执行器速度（6×1） - **q̇** = [q̇₁, q̇₂, ..., q̇ₙ]ᵀ：关节速度（n×1） - **J(q)**：6×n 雅可比矩阵 ### 向量叉乘法的核心思想 对于**旋转关节**： - **线速度**：由关节旋转引起的末端线速度 = 角速度 × 位置向量 - **角速度**：等于关节自身的旋转角速度 --- ## 求解步骤 ### 步骤1：建立DH参数 为每个关节定义DH参数： - **θᵢ**：关节角（旋转） - **dᵢ**：连杆偏移（平移） - **aᵢ**：连杆长度（平移） - **αᵢ**：连杆扭角（旋转） ### 步骤2：计算各关节的变换矩阵 使用DH参数计算每个关节的齐次变换矩阵： ``` ┌ ┐ │ cos(θᵢ) -sin(θᵢ)·cos(αᵢ) sin(θᵢ)·sin(αᵢ) aᵢ·cos(θᵢ) │ Tᵢ = │ sin(θᵢ) cos(θᵢ)·cos(αᵢ) -cos(θᵢ)·sin(αᵢ) aᵢ·sin(θᵢ) │ │ 0 sin(αᵢ) cos(αᵢ) dᵢ │ │ 0 0 0 1 │ └ ┘ ``` ### 步骤3：递推计算各关节位置和旋转轴 从基座到末端，逐关节递推： ``` T₀¹ = T₁ T₀² = T₀¹ · T₂ T₀³ = T₀² · T₃ ... T₀ⁿ = T₀ⁿ⁻¹ · Tₙ ``` 从变换矩阵中提取： - **位置向量 pᵢ**：T₀ⁱ 的第4列前3行 - **旋转轴 zᵢ**：T₀ⁱ 的第3列前3行（Z轴方向） ### 步骤4：计算雅可比矩阵各列 对于第 i 个**旋转关节**： **线速度列（前3行）：** ``` J_v[:,i] = zᵢ × (p_end - pᵢ) ``` **角速度列（后3行）：** ``` J_ω[:,i] = zᵢ ``` 对于第 i 个**移动关节**： **线速度列（前3行）：** ``` J_v[:,i] = zᵢ ``` **角速度列（后3行）：** ``` J_ω[:,i] = [0, 0, 0]ᵀ ``` ### 步骤5：组装完整雅可比矩阵 ``` J = [ J_v ] = [ z₁×(pₑₙd-p₁) z₂×(pₑₙd-p₂) ... zₙ×(pₑₙd-pₙ) ] [ J_ω ] [ z₁ z₂ ... zₙ ] ``` --- ## 2D平面3自由度机械臂示例 ### 机械臂结构 ``` 基座 ──θ₁── 关节1 ──L₁── 关节2 ──θ₂── 关节3 ──L₂── 末端 │ θ₃ │ L₃ ``` ### DH参数表 | 关节i | θᵢ | dᵢ | aᵢ | αᵢ | |:---:|:---:|:---:|:---:|:---:| | 1 | θ₁ | 0 | L₁ | 0 | | 2 | θ₂ | 0 | L₂ | 0 | | 3 | θ₃ | 0 | L₃ | 0 | ### 各关节位置计算 **关节1位置：** ``` p₁ = [L₁·cos(θ₁), L₁·sin(θ₁), 0]ᵀ ``` **关节2位置：** ``` p₂ = [L₁·cos(θ₁) + L₂·cos(θ₁+θ₂), L₁·sin(θ₁) + L₂·sin(θ₁+θ₂), 0]ᵀ ``` **末端位置（关节3）：** ``` p_end = [L₁·cos(θ₁) + L₂·cos(θ₁₂) + L₃·cos(θ₁₂₃), L₁·sin(θ₁) + L₂·sin(θ₁₂) + L₃·sin(θ₁₂₃), 0]ᵀ ``` 其中：θ₁₂ = θ₁+θ₂，θ₁₂₃ = θ₁+θ₂+θ₃ ### 旋转轴 由于是平面机械臂，所有关节旋转轴相同： ``` z₁ = z₂ = z₃ = [0, 0, 1]ᵀ ``` ### 雅可比矩阵各列计算 #### 第1列（关节1） ``` p_end - p₁ = [L₂·cos(θ₁₂) + L₃·cos(θ₁₂₃), L₂·sin(θ₁₂) + L₃·sin(θ₁₂₃), 0] J_v₁ = z₁ × (p_end - p₁) = [0, 0, 1] × [L₂·cos(θ₁₂) + L₃·cos(θ₁₂₃), L₂·sin(θ₁₂) + L₃·sin(θ₁₂₃), 0] = [-(L₂·sin(θ₁₂) + L₃·sin(θ₁₂₃)), L₂·cos(θ₁₂) + L₃·cos(θ₁₂₃), 0] J_ω₁ = z₁ = [0, 0, 1]ᵀ ``` #### 第2列（关节2） ``` p_end - p₂ = [L₃·cos(θ₁₂₃), L₃·sin(θ₁₂₃), 0] J_v₂ = z₂ × (p_end - p₂) = [-L₃·sin(θ₁₂₃), L₃·cos(θ₁₂₃), 0] J_ω₂ = z₂ = [0, 0, 1]ᵀ ``` #### 第3列（关节3） ``` p_end - p₃ = [0, 0, 0] (关节3就在末端) J_v₃ = z₃ × (p_end - p₃) = [0, 0, 0] J_ω₃ = z₃ = [0, 0, 1]ᵀ ``` ### 完整雅可比矩阵 ``` θ₁ θ₂ θ₃ ┌──────────────────────────────────────────────────────┐ v_x │ -(L₂sin₁₂+L₃sin₁₂₃) -L₃sin₁₂₃ 0 │ v_y │ L₂cos₁₂+L₃cos₁₂₃ L₃cos₁₂₃ 0 │ v_z │ 0 0 0 │ ├──────────────────────────────────────────────────────┤ ω_x │ 0 0 0 │ ω_y │ 0 0 0 │ ω_z │ 1 1 1 │ └──────────────────────────────────────────────────────┘ ``` 简化后的2D版本（2×3矩阵）： ``` J = [ -(L₂sin₁₂+L₃sin₁₂₃) -L₃sin₁₂₃ 0 ] [ L₂cos₁₂+L₃cos₁₂₃ L₃cos₁₂₃ 0 ] ``` --- ## Python代码实现 ### 通用6自由度机械臂实现 ```python import numpy as np def dh_transform(theta, d, a, alpha): """ 根据D-H参数计算齐次变换矩阵 参数: theta: 关节角 (rad) d: 连杆偏移 a: 连杆长度 alpha: 连杆扭角 (rad) 返回: 4×4 齐次变换矩阵 """ ct, st = np.cos(theta), np.sin(theta) ca, sa = np.cos(alpha), np.sin(alpha) T = np.array([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) return T def compute_jacobian(dh_params, joint_angles, joint_types=None): """ 使用向量叉乘法计算机械臂雅可比矩阵 参数: dh_params: 列表，每个元素是字典 {'d': ..., 'a': ..., 'alpha': ...} joint_angles: 关节角度列表 [θ₁, θ₂, ..., θₙ] joint_types: 关节类型列表，'R'=旋转, 'P'=移动，默认全为'R' 返回: J: 6×n 雅可比矩阵 """ n = len(joint_angles) if joint_types is None: joint_types = ['R'] * n # 默认为旋转关节 J = np.zeros((6, n)) # 计算各关节的变换矩阵和位置 T = np.eye(4) positions = [] # 存储各关节位置 axes = [] # 存储各关节旋转轴 for i in range(n): # 计算当前关节的变换矩阵 params = dh_params[i] T_i = dh_transform( joint_angles[i], params['d'], params['a'], params['alpha'] ) # 累积变换 T = T @ T_i # 提取位置和Z轴方向 positions.append(T[:3, 3].copy()) axes.append(T[:3, 2].copy()) # Z轴方向 # 末端位置 p_end = positions[-1] # 构建雅可比矩阵 for i in range(n): z_i = axes[i] p_i = positions[i] if joint_types[i] == 'R': # 旋转关节 # 线速度分量: z_i × (p_end - p_i) J[:3, i] = np.cross(z_i, p_end - p_i) # 角速度分量: z_i J[3:, i] = z_i else: # 移动关节 # 线速度分量: z_i J[:3, i] = z_i # 角速度分量: 0 J[3:, i] = 0 return J ``` ### 2D平面3自由度机械臂专用实现 ```python def compute_jacobian_2d_3dof(L1, L2, L3, theta1, theta2, theta3): """ 计算2D平面3自由度机械臂的雅可比矩阵 参数: L1, L2, L3: 连杆长度 theta1, theta2, theta3: 关节角度 (rad) 返回: J: 2×3 雅可比矩阵 (只考虑x,y位置) """ # 计算角度和 theta_12 = theta1 + theta2 theta_123 = theta1 + theta2 + theta3 # 构建雅可比矩阵 J = np.zeros((2, 3)) # 第1列 (关节1) J[0, 0] = -(L2 * np.sin(theta_12) + L3 * np.sin(theta_123)) J[1, 0] = L2 * np.cos(theta_12) + L3 * np.cos(theta_123) # 第2列 (关节2) J[0, 1] = -L3 * np.sin(theta_123) J[1, 1] = L3 * np.cos(theta_123) # 第3列 (关节3) J[0, 2] = 0 J[1, 2] = 0 return J # 使用示例 if __name__ == "__main__": # 定义机械臂参数 L1, L2, L3 = 1.0, 0.8, 0.6 # 连杆长度 theta1, theta2, theta3 = np.pi/4, np.pi/6, np.pi/3 # 关节角度 # 计算雅可比矩阵 J = compute_jacobian_2d_3dof(L1, L2, L3, theta1, theta2, theta3) print("雅可比矩阵 J:") print(J) print() # 验证：给定关节速度，计算末端速度 q_dot = np.array([0.1, 0.2, 0.1]) # 关节角速度 (rad/s) v_end = J @ q_dot print(f"关节速度: {q_dot}") print(f"末端线速度: [{v_end[0]:.4f}, {v_end[1]:.4f}]") ``` --- ## 关键要点总结 ### 1. 向量叉乘的几何意义 ``` v = ω × r ``` - **ω**：角速度向量（方向为旋转轴，大小为角速度） - **r**：从旋转中心到目标点的位置向量 - **v**：目标点的线速度（垂直于ω和r构成的平面） ### 2. 为什么关节3的线速度列为0？ 因为关节3就在末端，它转动不会改变末端的位置，只会改变末端的姿态（朝向）。 ### 3. 2D vs 3D的区别 | 维度 | 雅可比矩阵大小 | 考虑因素 | |:---:|:---:|:---| | 2D平面 | 2×n 或 3×n | x, y位置 (+ 姿态) | | 3D空间 | 6×n | x, y, z位置 + 三个旋转 | ### 4. 旋转关节 vs 移动关节 | 关节类型 | 线速度列 | 角速度列 | |:---:|:---:|:---:| | 旋转关节 (R) | zᵢ × (p_end - pᵢ) | zᵢ | | 移动关节 (P) | zᵢ | [0, 0, 0]ᵀ | --- ## 参考资料 - Siciliano, B., et al. "Robotics: Modelling, Planning and Control" - Craig, J.J. "Introduction to Robotics: Mechanics and Control" - 机器人学导论（第3版），John J. Craig 著 --- *文档生成时间：2026-04-15*