简介蔡自兴《机器人学》配套课后练习题答案面向机器人工程、自动化等专业学生用于攻克坐标系变换、机械手运动学、旋转矩阵与变换矩阵等课程核心内容。文档精选多道典型习题给出坐标变换的旋转矩阵推导、3自由度机械手运动方程式求解并通过D-H参数法与两种坐标系建立方式的对比演示齐次变换顺序及矩阵展开过程微分变换部分还涉及等效微分平移与旋转的计算能帮助理解机器人微分运动本质。资源共1个doc文件约592KBWord编排的公式与文字便于自由查阅、打印和二次修改。目前已吸引843人浏览学习可配合教材课后复习、考研专业课梳理和自学查漏补缺是一份经典的配套练习解析。1. 坐标变换习题的价值蔡自兴教材里最容易被低估的一章蔡自兴《机器人学》课后习题中坐标变换和 D-H 建模的题目看起来只是数值计算实际却是运动学分析里最容易出错的环节。旋转矩阵左乘还是右乘、坐标系建在连杆远端还是关节处、D-H 参数表里的角度相对哪个轴量取任何一步判断错了后面的变换矩阵都会跟着错。楔块重排的变换序列、三自由度机械手的运动学与雅可比矩阵这些习题恰好构成一条完整的解题链从坐标变换的基本规则到两种 D-H 建模约定再到微分变换法构造雅可比矩阵。把这条链上的每一步都用可运行的代码验证一遍比埋头刷十道同类题更有帮助。适合正在学习机器人学的学生也适合需要快速回顾运动学基础知识的机器人工程师。2. 旋转矩阵与齐次变换动系右乘、定系左乘的判定规则2.1 相对变换右乘、绝对变换左乘从一道旋转矩阵题说起教材配套习题里有这样一道题坐标系{B}初始与{A}重合让{B}先绕自身 Z 轴旋转 α 角再绕自身 X 轴旋转 β 角求把矢量 PB 的描述变为对 PA 描述的旋转矩阵。这道题的关键在于“绕自身”三个字。坐标系{B}的第二次旋转发生在第一次旋转之后新形成的坐标系里也就是相对自身当前坐标系的变换。此时变换顺序为依次右乘T_AB Rot(z, α) · Rot(x, β)注意 Rot(x, β) 要写在 Rot(z, α) 的右边。直观理解向量先被 Rot(z, α) 作用之后坐标系跟着旋转第二次 Rot(x, β) 再作用在新坐标系的 X 轴上矩阵连乘时后发生的变换写在右边。反过来如果第二次旋转是绕基座标系的 X 轴那就是绝对变换要左乘T_AB Rot(x, β) · Rot(z, α)。两种写法的结果完全不同。很多初学者做这类题时容易把“绕自身”和“绕基座标系”弄混本质上是对变换矩阵的乘法顺序缺少直观验证。2.2 楔块重排的两种变换序列坐标系固联位置决定矩阵写法教材中楔形物体重排问题的两种解法更能说明坐标系选择对变换序列的影响。方法 1 把坐标系 {1} 和 {2} 直接固联在两个楔块上这样变换描述的是物体自身如何从初始位姿运动到目标位姿。对楔块 1变换序列为T1 Rot(y, 90°) · Rot(z, 90°)对楔块 2需要经历四次变换先绕自身 X 轴旋转 -90°再绕新形成的 Z 轴旋转 -180°然后绕定系的 Z 轴旋转 -90°最后沿定系平移 (3, 0, -4)。注意这个序列里既有动系变换又有定系变换写成的矩阵链是T2 Trans(3, 0, -4) · Rot(z, -90°) · Rot(x, -90°) · Rot(z, -180°)方法 2 则把两个坐标系放在与参考坐标系重合的位置变换描述的是从参考位置到目标位置的直接映射。同样的楔块 2变换序列变为先沿定系平移 (2, 9, 0) 和 (4, 0, 0)再依次绕定系 Y 轴转 90°、X 轴转 180°、Z 轴转 90°。两种方法最终的摆放效果一样但矩阵形式完全不同。教材在这里特意加了一句备注当建立的相对坐标系位置不同时到达理想位置的变换矩阵不同。这句话值得划重点——坐标系建在哪里直接决定了变换矩阵里每个因子的顺序和数值。维度方法 1方法 2坐标系位置与楔块固联与参考系重合变换参考系动系为主混合定系从参考位置到目标矩阵写法动系变换右乘定系变换左乘按从右到左读取步骤适用场景描述物体自身运动描述位姿映射关系2.3 用 NumPy 验证矩阵乘法的不可交换性左右乘顺序的判断容易凭感觉走偏干脆用代码把两种结果打印出来对比import numpy as np from math import cos, sin, radians def rot_x(theta): th radians(theta) return np.array([ [1, 0, 0], [0, cos(th), -sin(th)], [0, sin(th), cos(th)] ]) def rot_z(theta): th radians(theta) return np.array([ [cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1] ]) # 相对变换先绕自身Z轴转30°再绕新X轴转45°右乘 T_rel rot_z(30) rot_x(45) # 绝对变换先绕基座标系X轴转45°再绕基座标系Z轴转30°左乘 T_abs rot_x(45) rot_z(30) print(相对变换右乘:\n, np.round(T_rel, 4)) print(绝对变换左乘:\n, np.round(T_abs, 4)) print(两个结果是否相同:, np.allclose(T_rel, T_abs))这里 rot_z 和 rot_x 接收的角度参数对应题目中的 α 和 β分别取 30° 和 45° 做演示。两个矩阵在乘法顺序上只是交换了位置结果差异却非常大。执行后可以看到 T_rel 和 T_abs 的数值完全不同这说明在构造变换时必须明确每一次旋转的参考系不能只盯着角度看。代码里用 np.round 只是为了打印方便实际数值计算不必取整。习惯上用弧度制传参到 NumPy 三角函数里这里为了直观改用角度再在函数内部转换成弧度。3. 三自由度机械手运动学D-H 参数表与两种建模约定3.1 方法 1 与方法 2 的坐标系差异连杆远端还是关节处教材里 3 自由度机械手的运动学求解给出了两种坐标系建立方式。方法 1 把坐标系固联在连杆远端也就是下一个关节处方法 2 把坐标系放在当前关节的转轴上。两种方式推导出的末端变换矩阵完全一致但中间的 D-H 参数表差异明显。下面是方法 1标准 D-H和方法 2改进 D-H的连杆参数对比建模方式连杆 ia(i-1)α(i-1)d(i)θ(i)方法 11L190°0θ1方法 12L20°0θ2方法 1300°0θ3方法 2100°0θ1方法 22L190°0θ2方法 23L20°0θ3方法 1 的参数表里轴 1 与轴 2 垂直这一几何约束体现在 α1 90°方法 2 里同样的几何约束落在 α2 90° 上。如果建模时把坐标系位置放反了参数表的这一列就会跟着偏移后续计算 A 矩阵时容易张冠李戴。方法 1 对应标准 D-H 参数约定坐标系 {i} 固联在连杆远端方法 2 更接近改进 D-H连杆坐标系依次建立在关节轴上且 α、a 的下标语义不同。教材没有刻意区分这两种约定但在实际工程里UR、KUKA 等主流机器人厂商的标定文档通常都基于其中一种混用会导致标定结果对不上。3.2 A 矩阵的通用公式与 Python 实现标准 D-H 的 A 矩阵由四个子变换连乘得到A_i Rot(z, θi) · Trans(0, 0, di) · Trans(ai, 0, 0) · Rot(x, αi)展开后[ ct -st*ca st*sa a*ct ] [ st ct*ca -ct*sa a*st ] [ 0 sa ca d ] [ 0 0 0 1 ]其中 ct cosθist sinθica cosαisa sinαi。这个公式可以直接写成 Python 函数import numpy as np def dh_standard(theta, d, a, alpha): 标准D-H参数到齐次变换矩阵。 参数: 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] ])对旋转关节theta 是关节变量d、a、alpha 是固定参数对移动关节d 是变量。在程序里通常把 D-H 参数整理成列表每个元素是 (d, a, alpha)然后循环调用 dh_standard。改进 D-H 的 A 矩阵写法不同A_i Rot(x, α(i-1)) · Trans(a(i-1), 0, 0) · Rot(z, θi) · Trans(0, 0, di)。两种公式都实现一份在建模时按选定的约定调用对应函数可以减少混淆。3.3 用 SymPy 验证两种建模的运动学方程等价方法 1 和方法 2 的 A 矩阵虽然不同但连乘后的末端变换 T03 应该相同。用 SymPy 做符号推导可以验证这一点import sympy as sp th1, th2, th3 sp.symbols(th1 th2 th3) L1, L2 sp.symbols(L1 L2) def dh_standard_sym(theta, d, a, alpha): ct, st sp.cos(theta), sp.sin(theta) ca, sa sp.cos(alpha), sp.sin(alpha) return sp.Matrix([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) def dh_modified_sym(theta, d, a, alpha): ct, st sp.cos(theta), sp.sin(theta) ca, sa sp.cos(alpha), sp.sin(alpha) return sp.Matrix([ [ct, -st, 0, a], [ca*st, ca*ct, -sa, -sa*d], [sa*st, sa*ct, ca, ca*d], [0, 0, 0, 1] ]) # 方法1标准D-Hα190°a1L1a2L2 A1 dh_standard_sym(th1, 0, L1, sp.pi/2) A2 dh_standard_sym(th2, 0, L2, 0) A3 dh_standard_sym(th3, 0, 0, 0) T1 sp.simplify(A1 * A2 * A3) # 方法2改进D-Hα290°a2L1a3L2 B1 dh_modified_sym(th1, 0, 0, 0) B2 dh_modified_sym(th2, 0, L1, sp.pi/2) B3 dh_modified_sym(th3, 0, L2, 0) T2 sp.simplify(B1 * B2 * B3) print(T1与T2是否相同:, T1 T2)dh_standard_sym 和 dh_modified_sym 分别是两种 D-H 约定的符号实现参数含义与前面的 dh_standard 一致。两种方式下 T03 符号表达式相同说明坐标系建在哪里只影响中间参数不影响最终末端位姿。实际应用中T1 或 T2 矩阵的结果里包含 sinθ1、cos(θ2θ3) 等组合也就是教材给出的运动学方程形式。3.4 零位校验关节变量为零时的变换矩阵检查教材里有一道题专门指定所有关节处于零位要求求解各坐标系间的变换矩阵。零位校验是验证 D-H 参数表最直接的手段把 θi 0 代入 A 矩阵这时 Rot(z, 0) 退化为单位矩阵A_i 等于平移和旋转的简单组合计算结果可以直接用手算核对。对于方法 1 的第 1 个连杆θ1 0 时 A1 变成绕 X 轴旋转 90° 并沿 X 平移 L1 的组合矩阵里大部分元素是 0 或 1很容易判断正误。如果零位下某个矩阵与几何直觉不符基本可以肯定是 D-H 参数表的 a、d 或 α 填错了。这个方法在调试真实机械臂时同样适用转动关节到零位对比实际末端位置与正运动学计算值偏差通常来自 D-H 参数的初始偏置未对准。4. 微分变换与雅可比矩阵从基座标系到工具坐标系的推导4.1 微分变换矩阵的构造当机械手末端发生微小平移 d 和微小旋转 δ 时在基座标系下用微分变换矩阵描述[ 0 -δz δy dx ] [ δz 0 -δx dy ] [-δy δx 0 dz ] [ 0 0 0 0 ]这个矩阵左乘当前变换矩阵 T得到微分运动后的变换矩阵变化量dT Δ · T。这里的 dx、dy、dz 是沿基座标系三个轴的平移分量δx、δy、δz 是绕基座标系三个轴的旋转分量。如果要在某个工具坐标系 {C} 下表达同样的微分运动就把 Δ 变换到 {C} 坐标系下Δc C^(-1) · Δ · C。C 是 {C} 相对基座标系的齐次变换矩阵。这种相似变换保持了微分运动的物理意义只是换了表达参考系。4.2 等效微分平移与旋转在指定坐标系下表达微运动假设 C 的旋转部分为 R平移部分为 p那么 {C} 坐标系下的等效微分平移 d_c 和微分旋转 δ_c 可以写成d_c R^T · (δ × p d) δ_c R^T · δ其中 × 是三维向量叉积。这个公式在教材习题里直接给出了数值计算过程。用代码验证如下import numpy as np # 示例C的旋转部分R与平移部分p R np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]]) p np.array([4.0, 3.0, 1.0]) # 基座标系下的微分平移与旋转 d np.array([0.5, 0.0, 1.0]) delta np.array([0.1, 0.2, 0.0]) d_c R.T (np.cross(delta, p) d) delta_c R.T delta print(等效平移:, d_c) print(等效旋转:, delta_c)计算时先做叉积 δ × p再加上平移分量 d最后用旋转矩阵的转置完成坐标变换。这个顺序不能颠倒微分运动先耦合了旋转对位置的影响然后才投影到目标坐标系。实际做手眼标定或力控时经常要把基座标系下的微小运动量换算到相机或工具坐标系下用的就是这个公式。4.3 用微分变换法构造雅可比矩阵求解教材里三自由度机械手的雅可比矩阵目标是找到关节速度到末端速度的线性映射v_e J(θ) · θ̇其中 v_e 是末端的六维速度三个线速度加三个角速度。对旋转关节雅可比矩阵的第 i 列由两条公式给出J_i [ z(i-1) × (p_e - p(i-1)) ; z(i-1) ]z(i-1) 是关节 i 轴线在基座标系下的方向向量p(i-1) 是关节 i 坐标系原点p_e 是末端位置。用 SymPy 推导三自由度机械手的雅可比# 接前面的T1取旋转矩阵与位置向量 T T1 # 方法1的末端变换 p_e T[:3, 3:4] z0 sp.Matrix([0, 0, 1]) p0 sp.Matrix([0, 0, 0]) z1 sp.simplify(A1[:3, :3] z0) p1 A1[:3, 3:4] z2 sp.simplify((A1 * A2)[:3, :3] z0) p2 (A1 * A2)[:3, 3:4] def jac_col(z, p): v sp.simplify(z.cross(p_e - p)) return sp.Matrix.vstack(v, z) J sp.Matrix.hstack(jac_col(z0, p0), jac_col(z1, p1), jac_col(z2, p2)) print(J)这段代码把三条雅可比列向量拼接成 6×3 矩阵。注意第三列的位置向量来自 A1·A2而不是 A3因为第三个关节坐标系的原点在 A1·A2 的平移部分。教材中方法 1 和方法 2 因为坐标系位置不同各自求出的雅可比矩阵中会出现不同的 sin/cos 组合但描述的是同一个速度映射。使用时要保持 D-H 建模与雅可比计算的约定一致否则末端速度与关节速度会对应不上。4.4 奇异位形与雅可比矩阵的秩雅可比矩阵的秩直接反映机械手在当前位形下的运动能力。对三自由度机械手把特定关节角代入 J 后求秩如果秩小于 3说明末端在某个方向上无法运动也就是进入了奇异位形。典型的奇异位形发生在 θ3 0 或 θ3 π 时此时第二个连杆与第三个连杆完全伸展或折叠末端无法沿垂直于连杆平面的方向移动。在数学上表现为雅可比矩阵的某些行线性相关。工程上做轨迹规划时需要实时估计雅可比矩阵的条件数条件数过大就说明接近奇异要提前调整轨迹。5. 用数值方法验证运动学与逆解实现5.1 有限差分法验证雅可比矩阵解析推导雅可比矩阵容易出错可以用有限差分法做交叉验证。基本思路是给每个关节一个微小扰动用正运动学差分近似速度映射def jacobian_numerical(theta, dh_params, delta1e-6): n len(theta) J np.zeros((6, n)) for i in range(n): dtheta np.zeros(n) dtheta[i] delta T_plus forward_kinematics(theta dtheta, dh_params) T_minus forward_kinematics(theta - dtheta, dh_params) J[:3, i] (T_plus[:3, 3] - T_minus[:3, 3]) / (2 * delta) dR (T_plus[:3, :3] - T_minus[:3, :3]) / (2 * delta) J[3:, i] np.array([dR[2, 1], dR[0, 2], dR[1, 0]]) return J求解时先用解析法得到雅可比矩阵然后在多个随机位形下对比有限差分结果。上面代码中 dR 的反对称元素提取需要与小角度近似配合当 delta 足够小时dR 的反对称部分近似等于旋转向量。如果两个方法的结果一致解析推导就是可信的。这一步在实际项目中是标准的数学验证流程。5.2 基于阻尼最小二乘的牛顿迭代逆解雅可比矩阵最直接的应用是逆运动学求解。给定期望末端位姿用牛顿-拉弗森迭代把位姿误差降到零def ik_dls(T_des, theta_init, dh_params, lam0.1, tol1e-6, max_iter100): theta theta_init.copy() for _ in range(max_iter): T_cur forward_kinematics(theta, dh_params) e compute_error(T_des, T_cur) if np.linalg.norm(e) tol: return theta, True J jacobian(theta, dh_params) JtJ J.T J dtheta np.linalg.solve(JtJ lam**2 * np.eye(len(theta)), J.T e) theta theta dtheta return theta, False其中 lam 是阻尼系数取 0.01 到 0.1 之间。阻尼项让迭代在奇异位形附近仍能稳定收敛代价是收敛速度变慢、精度略降。初值越接近真实解收敛越快实际使用时常配合多组随机初值或关节限位投影做二次优化。迭代到收敛后把求出的关节角代回正运动学得到的末端位姿与期望位姿的误差通常在 1e-6 以下整个流程就可以放心交给后面的轨迹规划模块。本文还有配套的精品资源点击获取
简介蔡自兴《机器人学》配套课后练习题答案面向机器人工程、自动化等专业学生用于攻克坐标系变换、机械手运动学、旋转矩阵与变换矩阵等课程核心内容。文档精选多道典型习题给出坐标变换的旋转矩阵推导、3自由度机械手运动方程式求解并通过D-H参数法与两种坐标系建立方式的对比演示齐次变换顺序及矩阵展开过程微分变换部分还涉及等效微分平移与旋转的计算能帮助理解机器人微分运动本质。资源共1个doc文件约592KBWord编排的公式与文字便于自由查阅、打印和二次修改。目前已吸引843人浏览学习可配合教材课后复习、考研专业课梳理和自学查漏补缺是一份经典的配套练习解析。1. 坐标变换习题的价值蔡自兴教材里最容易被低估的一章蔡自兴《机器人学》课后习题中坐标变换和 D-H 建模的题目看起来只是数值计算实际却是运动学分析里最容易出错的环节。旋转矩阵左乘还是右乘、坐标系建在连杆远端还是关节处、D-H 参数表里的角度相对哪个轴量取任何一步判断错了后面的变换矩阵都会跟着错。楔块重排的变换序列、三自由度机械手的运动学与雅可比矩阵这些习题恰好构成一条完整的解题链从坐标变换的基本规则到两种 D-H 建模约定再到微分变换法构造雅可比矩阵。把这条链上的每一步都用可运行的代码验证一遍比埋头刷十道同类题更有帮助。适合正在学习机器人学的学生也适合需要快速回顾运动学基础知识的机器人工程师。2. 旋转矩阵与齐次变换动系右乘、定系左乘的判定规则2.1 相对变换右乘、绝对变换左乘从一道旋转矩阵题说起教材配套习题里有这样一道题坐标系{B}初始与{A}重合让{B}先绕自身 Z 轴旋转 α 角再绕自身 X 轴旋转 β 角求把矢量 PB 的描述变为对 PA 描述的旋转矩阵。这道题的关键在于“绕自身”三个字。坐标系{B}的第二次旋转发生在第一次旋转之后新形成的坐标系里也就是相对自身当前坐标系的变换。此时变换顺序为依次右乘T_AB Rot(z, α) · Rot(x, β)注意 Rot(x, β) 要写在 Rot(z, α) 的右边。直观理解向量先被 Rot(z, α) 作用之后坐标系跟着旋转第二次 Rot(x, β) 再作用在新坐标系的 X 轴上矩阵连乘时后发生的变换写在右边。反过来如果第二次旋转是绕基座标系的 X 轴那就是绝对变换要左乘T_AB Rot(x, β) · Rot(z, α)。两种写法的结果完全不同。很多初学者做这类题时容易把“绕自身”和“绕基座标系”弄混本质上是对变换矩阵的乘法顺序缺少直观验证。2.2 楔块重排的两种变换序列坐标系固联位置决定矩阵写法教材中楔形物体重排问题的两种解法更能说明坐标系选择对变换序列的影响。方法 1 把坐标系 {1} 和 {2} 直接固联在两个楔块上这样变换描述的是物体自身如何从初始位姿运动到目标位姿。对楔块 1变换序列为T1 Rot(y, 90°) · Rot(z, 90°)对楔块 2需要经历四次变换先绕自身 X 轴旋转 -90°再绕新形成的 Z 轴旋转 -180°然后绕定系的 Z 轴旋转 -90°最后沿定系平移 (3, 0, -4)。注意这个序列里既有动系变换又有定系变换写成的矩阵链是T2 Trans(3, 0, -4) · Rot(z, -90°) · Rot(x, -90°) · Rot(z, -180°)方法 2 则把两个坐标系放在与参考坐标系重合的位置变换描述的是从参考位置到目标位置的直接映射。同样的楔块 2变换序列变为先沿定系平移 (2, 9, 0) 和 (4, 0, 0)再依次绕定系 Y 轴转 90°、X 轴转 180°、Z 轴转 90°。两种方法最终的摆放效果一样但矩阵形式完全不同。教材在这里特意加了一句备注当建立的相对坐标系位置不同时到达理想位置的变换矩阵不同。这句话值得划重点——坐标系建在哪里直接决定了变换矩阵里每个因子的顺序和数值。维度方法 1方法 2坐标系位置与楔块固联与参考系重合变换参考系动系为主混合定系从参考位置到目标矩阵写法动系变换右乘定系变换左乘按从右到左读取步骤适用场景描述物体自身运动描述位姿映射关系2.3 用 NumPy 验证矩阵乘法的不可交换性左右乘顺序的判断容易凭感觉走偏干脆用代码把两种结果打印出来对比import numpy as np from math import cos, sin, radians def rot_x(theta): th radians(theta) return np.array([ [1, 0, 0], [0, cos(th), -sin(th)], [0, sin(th), cos(th)] ]) def rot_z(theta): th radians(theta) return np.array([ [cos(th), -sin(th), 0], [sin(th), cos(th), 0], [0, 0, 1] ]) # 相对变换先绕自身Z轴转30°再绕新X轴转45°右乘 T_rel rot_z(30) rot_x(45) # 绝对变换先绕基座标系X轴转45°再绕基座标系Z轴转30°左乘 T_abs rot_x(45) rot_z(30) print(相对变换右乘:\n, np.round(T_rel, 4)) print(绝对变换左乘:\n, np.round(T_abs, 4)) print(两个结果是否相同:, np.allclose(T_rel, T_abs))这里 rot_z 和 rot_x 接收的角度参数对应题目中的 α 和 β分别取 30° 和 45° 做演示。两个矩阵在乘法顺序上只是交换了位置结果差异却非常大。执行后可以看到 T_rel 和 T_abs 的数值完全不同这说明在构造变换时必须明确每一次旋转的参考系不能只盯着角度看。代码里用 np.round 只是为了打印方便实际数值计算不必取整。习惯上用弧度制传参到 NumPy 三角函数里这里为了直观改用角度再在函数内部转换成弧度。3. 三自由度机械手运动学D-H 参数表与两种建模约定3.1 方法 1 与方法 2 的坐标系差异连杆远端还是关节处教材里 3 自由度机械手的运动学求解给出了两种坐标系建立方式。方法 1 把坐标系固联在连杆远端也就是下一个关节处方法 2 把坐标系放在当前关节的转轴上。两种方式推导出的末端变换矩阵完全一致但中间的 D-H 参数表差异明显。下面是方法 1标准 D-H和方法 2改进 D-H的连杆参数对比建模方式连杆 ia(i-1)α(i-1)d(i)θ(i)方法 11L190°0θ1方法 12L20°0θ2方法 1300°0θ3方法 2100°0θ1方法 22L190°0θ2方法 23L20°0θ3方法 1 的参数表里轴 1 与轴 2 垂直这一几何约束体现在 α1 90°方法 2 里同样的几何约束落在 α2 90° 上。如果建模时把坐标系位置放反了参数表的这一列就会跟着偏移后续计算 A 矩阵时容易张冠李戴。方法 1 对应标准 D-H 参数约定坐标系 {i} 固联在连杆远端方法 2 更接近改进 D-H连杆坐标系依次建立在关节轴上且 α、a 的下标语义不同。教材没有刻意区分这两种约定但在实际工程里UR、KUKA 等主流机器人厂商的标定文档通常都基于其中一种混用会导致标定结果对不上。3.2 A 矩阵的通用公式与 Python 实现标准 D-H 的 A 矩阵由四个子变换连乘得到A_i Rot(z, θi) · Trans(0, 0, di) · Trans(ai, 0, 0) · Rot(x, αi)展开后[ ct -st*ca st*sa a*ct ] [ st ct*ca -ct*sa a*st ] [ 0 sa ca d ] [ 0 0 0 1 ]其中 ct cosθist sinθica cosαisa sinαi。这个公式可以直接写成 Python 函数import numpy as np def dh_standard(theta, d, a, alpha): 标准D-H参数到齐次变换矩阵。 参数: 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] ])对旋转关节theta 是关节变量d、a、alpha 是固定参数对移动关节d 是变量。在程序里通常把 D-H 参数整理成列表每个元素是 (d, a, alpha)然后循环调用 dh_standard。改进 D-H 的 A 矩阵写法不同A_i Rot(x, α(i-1)) · Trans(a(i-1), 0, 0) · Rot(z, θi) · Trans(0, 0, di)。两种公式都实现一份在建模时按选定的约定调用对应函数可以减少混淆。3.3 用 SymPy 验证两种建模的运动学方程等价方法 1 和方法 2 的 A 矩阵虽然不同但连乘后的末端变换 T03 应该相同。用 SymPy 做符号推导可以验证这一点import sympy as sp th1, th2, th3 sp.symbols(th1 th2 th3) L1, L2 sp.symbols(L1 L2) def dh_standard_sym(theta, d, a, alpha): ct, st sp.cos(theta), sp.sin(theta) ca, sa sp.cos(alpha), sp.sin(alpha) return sp.Matrix([ [ct, -st*ca, st*sa, a*ct], [st, ct*ca, -ct*sa, a*st], [0, sa, ca, d], [0, 0, 0, 1] ]) def dh_modified_sym(theta, d, a, alpha): ct, st sp.cos(theta), sp.sin(theta) ca, sa sp.cos(alpha), sp.sin(alpha) return sp.Matrix([ [ct, -st, 0, a], [ca*st, ca*ct, -sa, -sa*d], [sa*st, sa*ct, ca, ca*d], [0, 0, 0, 1] ]) # 方法1标准D-Hα190°a1L1a2L2 A1 dh_standard_sym(th1, 0, L1, sp.pi/2) A2 dh_standard_sym(th2, 0, L2, 0) A3 dh_standard_sym(th3, 0, 0, 0) T1 sp.simplify(A1 * A2 * A3) # 方法2改进D-Hα290°a2L1a3L2 B1 dh_modified_sym(th1, 0, 0, 0) B2 dh_modified_sym(th2, 0, L1, sp.pi/2) B3 dh_modified_sym(th3, 0, L2, 0) T2 sp.simplify(B1 * B2 * B3) print(T1与T2是否相同:, T1 T2)dh_standard_sym 和 dh_modified_sym 分别是两种 D-H 约定的符号实现参数含义与前面的 dh_standard 一致。两种方式下 T03 符号表达式相同说明坐标系建在哪里只影响中间参数不影响最终末端位姿。实际应用中T1 或 T2 矩阵的结果里包含 sinθ1、cos(θ2θ3) 等组合也就是教材给出的运动学方程形式。3.4 零位校验关节变量为零时的变换矩阵检查教材里有一道题专门指定所有关节处于零位要求求解各坐标系间的变换矩阵。零位校验是验证 D-H 参数表最直接的手段把 θi 0 代入 A 矩阵这时 Rot(z, 0) 退化为单位矩阵A_i 等于平移和旋转的简单组合计算结果可以直接用手算核对。对于方法 1 的第 1 个连杆θ1 0 时 A1 变成绕 X 轴旋转 90° 并沿 X 平移 L1 的组合矩阵里大部分元素是 0 或 1很容易判断正误。如果零位下某个矩阵与几何直觉不符基本可以肯定是 D-H 参数表的 a、d 或 α 填错了。这个方法在调试真实机械臂时同样适用转动关节到零位对比实际末端位置与正运动学计算值偏差通常来自 D-H 参数的初始偏置未对准。4. 微分变换与雅可比矩阵从基座标系到工具坐标系的推导4.1 微分变换矩阵的构造当机械手末端发生微小平移 d 和微小旋转 δ 时在基座标系下用微分变换矩阵描述[ 0 -δz δy dx ] [ δz 0 -δx dy ] [-δy δx 0 dz ] [ 0 0 0 0 ]这个矩阵左乘当前变换矩阵 T得到微分运动后的变换矩阵变化量dT Δ · T。这里的 dx、dy、dz 是沿基座标系三个轴的平移分量δx、δy、δz 是绕基座标系三个轴的旋转分量。如果要在某个工具坐标系 {C} 下表达同样的微分运动就把 Δ 变换到 {C} 坐标系下Δc C^(-1) · Δ · C。C 是 {C} 相对基座标系的齐次变换矩阵。这种相似变换保持了微分运动的物理意义只是换了表达参考系。4.2 等效微分平移与旋转在指定坐标系下表达微运动假设 C 的旋转部分为 R平移部分为 p那么 {C} 坐标系下的等效微分平移 d_c 和微分旋转 δ_c 可以写成d_c R^T · (δ × p d) δ_c R^T · δ其中 × 是三维向量叉积。这个公式在教材习题里直接给出了数值计算过程。用代码验证如下import numpy as np # 示例C的旋转部分R与平移部分p R np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]]) p np.array([4.0, 3.0, 1.0]) # 基座标系下的微分平移与旋转 d np.array([0.5, 0.0, 1.0]) delta np.array([0.1, 0.2, 0.0]) d_c R.T (np.cross(delta, p) d) delta_c R.T delta print(等效平移:, d_c) print(等效旋转:, delta_c)计算时先做叉积 δ × p再加上平移分量 d最后用旋转矩阵的转置完成坐标变换。这个顺序不能颠倒微分运动先耦合了旋转对位置的影响然后才投影到目标坐标系。实际做手眼标定或力控时经常要把基座标系下的微小运动量换算到相机或工具坐标系下用的就是这个公式。4.3 用微分变换法构造雅可比矩阵求解教材里三自由度机械手的雅可比矩阵目标是找到关节速度到末端速度的线性映射v_e J(θ) · θ̇其中 v_e 是末端的六维速度三个线速度加三个角速度。对旋转关节雅可比矩阵的第 i 列由两条公式给出J_i [ z(i-1) × (p_e - p(i-1)) ; z(i-1) ]z(i-1) 是关节 i 轴线在基座标系下的方向向量p(i-1) 是关节 i 坐标系原点p_e 是末端位置。用 SymPy 推导三自由度机械手的雅可比# 接前面的T1取旋转矩阵与位置向量 T T1 # 方法1的末端变换 p_e T[:3, 3:4] z0 sp.Matrix([0, 0, 1]) p0 sp.Matrix([0, 0, 0]) z1 sp.simplify(A1[:3, :3] z0) p1 A1[:3, 3:4] z2 sp.simplify((A1 * A2)[:3, :3] z0) p2 (A1 * A2)[:3, 3:4] def jac_col(z, p): v sp.simplify(z.cross(p_e - p)) return sp.Matrix.vstack(v, z) J sp.Matrix.hstack(jac_col(z0, p0), jac_col(z1, p1), jac_col(z2, p2)) print(J)这段代码把三条雅可比列向量拼接成 6×3 矩阵。注意第三列的位置向量来自 A1·A2而不是 A3因为第三个关节坐标系的原点在 A1·A2 的平移部分。教材中方法 1 和方法 2 因为坐标系位置不同各自求出的雅可比矩阵中会出现不同的 sin/cos 组合但描述的是同一个速度映射。使用时要保持 D-H 建模与雅可比计算的约定一致否则末端速度与关节速度会对应不上。4.4 奇异位形与雅可比矩阵的秩雅可比矩阵的秩直接反映机械手在当前位形下的运动能力。对三自由度机械手把特定关节角代入 J 后求秩如果秩小于 3说明末端在某个方向上无法运动也就是进入了奇异位形。典型的奇异位形发生在 θ3 0 或 θ3 π 时此时第二个连杆与第三个连杆完全伸展或折叠末端无法沿垂直于连杆平面的方向移动。在数学上表现为雅可比矩阵的某些行线性相关。工程上做轨迹规划时需要实时估计雅可比矩阵的条件数条件数过大就说明接近奇异要提前调整轨迹。5. 用数值方法验证运动学与逆解实现5.1 有限差分法验证雅可比矩阵解析推导雅可比矩阵容易出错可以用有限差分法做交叉验证。基本思路是给每个关节一个微小扰动用正运动学差分近似速度映射def jacobian_numerical(theta, dh_params, delta1e-6): n len(theta) J np.zeros((6, n)) for i in range(n): dtheta np.zeros(n) dtheta[i] delta T_plus forward_kinematics(theta dtheta, dh_params) T_minus forward_kinematics(theta - dtheta, dh_params) J[:3, i] (T_plus[:3, 3] - T_minus[:3, 3]) / (2 * delta) dR (T_plus[:3, :3] - T_minus[:3, :3]) / (2 * delta) J[3:, i] np.array([dR[2, 1], dR[0, 2], dR[1, 0]]) return J求解时先用解析法得到雅可比矩阵然后在多个随机位形下对比有限差分结果。上面代码中 dR 的反对称元素提取需要与小角度近似配合当 delta 足够小时dR 的反对称部分近似等于旋转向量。如果两个方法的结果一致解析推导就是可信的。这一步在实际项目中是标准的数学验证流程。5.2 基于阻尼最小二乘的牛顿迭代逆解雅可比矩阵最直接的应用是逆运动学求解。给定期望末端位姿用牛顿-拉弗森迭代把位姿误差降到零def ik_dls(T_des, theta_init, dh_params, lam0.1, tol1e-6, max_iter100): theta theta_init.copy() for _ in range(max_iter): T_cur forward_kinematics(theta, dh_params) e compute_error(T_des, T_cur) if np.linalg.norm(e) tol: return theta, True J jacobian(theta, dh_params) JtJ J.T J dtheta np.linalg.solve(JtJ lam**2 * np.eye(len(theta)), J.T e) theta theta dtheta return theta, False其中 lam 是阻尼系数取 0.01 到 0.1 之间。阻尼项让迭代在奇异位形附近仍能稳定收敛代价是收敛速度变慢、精度略降。初值越接近真实解收敛越快实际使用时常配合多组随机初值或关节限位投影做二次优化。迭代到收敛后把求出的关节角代回正运动学得到的末端位姿与期望位姿的误差通常在 1e-6 以下整个流程就可以放心交给后面的轨迹规划模块。本文还有配套的精品资源点击获取
