机器人建模与控制习题答案：DH参数、雅可比与动力学推导校准-平芜编程栈

简介：这份《机器人建模和控制》习题答案面向学习机器人学、自动控制及相关课程的学生与研究人员，用于对照教材课后题检验运动学、动力学与控制理论的掌握情况。压缩包共1个PDF文件，约6.46MB，集中提供Spong等所著《Robot Modeling and Control》的习题解答，便于按章节查阅。内容覆盖正逆运动学求解、牛顿—欧拉与拉格朗日动力学建模、轨迹规划、PID与滑模/自适应控制、Lyapunov稳定性分析以及传感器数据处理等典型题目，可帮助读者核对推导过程、验证计算结果并梳理解题思路。目前已有3110人学习下载，适合课程复习、作业对照和考研或科研入门阶段查漏补缺。

1. 手推三遍对不上，问题往往不在推导本身

二连杆的正运动学手推了三遍，结果和书上差一个符号，翻回去检查才发现是 DH 参数表里 α 的正负号抄错了一行——自学机器人学时，这类事发生的频率比想象中高得多。手里这份《机器人建模和控制》习题答案，真正有价值的地方不在“直接看答案”，而在于它把每道题的推导链路完整摊开：齐次变换怎么摆、雅可比怎么拼、拉格朗日方程里那一堆偏导怎么收拾、动力学力矩的量级怎么判断合理。它面前的人是正在啃教材课后题的那批：本科高年级、研究生一年级，或者从控制方向转过来补运动学底子的工程师。想找一份能直接照抄的解答，这份机器人建模和控制习题答案帮不上太多忙；但如果想在自己算完之后有个对照物，把符号约定、多解筛选、动力学量级这些最容易翻车的环节校准一遍，它值得放在手边。

2. 先把记号对齐：DH 参数表怎么读、齐次变换怎么验

2.1 三套记号最容易打架：标准 DH、改进 DH 与位姿描述

同一个机械臂，用标准 DH 和用改进 DH 写出来的参数表完全不是一回事，而习题答案往往只给其中一种，不给说明。标准 DH 的变换顺序是绕 z 转 θ、沿 z 移 d、沿 x 移 a、绕 x 转 α，也就是先关节后连杆；改进 DH 把 x 轴的平移和旋转提到了 z 轴之前，顺序变成绕 x 转 α、沿 x 移 a、绕 z 转 θ、沿 z 移 d。这两种约定的差别看起来只是矩阵相乘顺序，但落到具体题目上，连杆长度挂在哪一列的第四行、α 属于第 i 个还是第 i-1 个坐标系，全部会跟着变。我见过最常见的错误是：答案用的是改进 DH，读者按标准 DH 的公式代进去，前三个关节还能凑出个像样的结果，到第四、第五个关节误差越滚越大，最后整条链的末端位姿完全对不上，还以为是代码写错了。

位姿描述本身还有第二层分叉。旋转矩阵进可以走 ZYZ 欧拉角，退可以走 RPY 固定角，还能拆成轴角或者四元数。习题答案里写“姿态为 (φ, θ, ψ)”时，如果不说明是哪种参数化、内旋还是外旋，读者代入后大概率会得到一组相差一个转置的解。实操里的做法很朴素：整份答案只认一种约定，从第一题到最后一题不改口径，遇到答案里没写清楚的地方，直接用齐次矩阵而不是角度去对——矩阵是唯一的，角度不是。

提示：先把教材目录后面的附录或者符号表翻一遍，把作者用的 DH 约定抄在纸上贴在屏幕边，比反复猜要省事得多。

2.2 用 sympy 把一道正运动学题从头算到尾

符号推导是这类习题答案最容易出细节问题的地方，手工化简到第三步就开始丢项。用 sympy 把变换矩阵逐级乘起来，好处是每一步都能打印出来跟答案对照，而且可以随时把符号换成数值做抽查。

import sympy as sp def dh(theta, d, a, alpha): """标准 DH：A_i = Rot_z(theta) * Trans_z(d) * Trans_x(a) * Rot_x(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], ]) # 平面二连杆：两个关节都绕 z 轴，连杆沿 x 方向伸出 t1, t2, l1, l2 = sp.symbols('theta1 theta2 l1 l2', real=True) A1 = dh(t1, 0, l1, 0) A2 = dh(t2, 0, l2, 0) T02 = sp.simplify(A1 * A2) print(sp.simplify(T02[:3, 3])) # 末端位置向量 print(sp.simplify(T02[:3, :3])) # 末端姿态矩阵

这段代码的关键在dh()这个函数本身：它的四个参数顺序就是约定，一旦定下来，后面所有题目都调它，不再手写矩阵。d和a两个长度的区别要盯住，d是沿 z 轴的偏移（移动关节或者关节高度），a是沿 x 轴的连杆长度，两者在参数表里经常被填反，填反之后位置向量会出现一个本不该有的 z 分量。alpha是相邻两个 z 轴之间的夹角，平面臂全是 0，带肩部偏置的空间臂会在第一个关节后出现 ±π/2，这个符号决定后面所有姿态的左右手性。

符号结果出来之后别急着信。再写一份完全独立的数值实现做交叉验证，是排查同源错误的唯一办法：

import numpy as np def fk_numeric(q, l1=0.4, l2=0.3): # 不复用上面的符号函数，直接用旋转矩阵连乘重写一遍 t1, t2 = q R01 = np.array([[np.cos(t1), -np.sin(t1), 0], [np.sin(t1), np.cos(t1), 0], [0, 0, 1]]) p01 = np.array([l1 * np.cos(t1), l1 * np.sin(t1), 0.0]) R12 = np.array([[np.cos(t2), -np.sin(t2), 0], [np.sin(t2), np.cos(t2), 0], [0, 0, 1]]) p12 = np.array([l2 * np.cos(t2), l2 * np.sin(t2), 0.0]) return p01 + R01 @ p12 # 位置：父连杆平移 + 旋转后的子连杆平移 print(fk_numeric([0.3, -0.7]))

两份实现只要在一组随机关节角上把末端位置对到小数点后十位，符号推导基本就可以确认无误了。这种“同一件事写两遍”的习惯在动力学题上更值钱，后面还会用到。

2.3 参数与校验表：每个符号错在哪会出什么后果

把常见符号错误和它引发的现象列成一张表，遇到结果不对时按表排查，比从头重算快得多。

出错位置	典型现象	定位方法
α 符号反了	末端位置对，姿态的手性相反	取 θ 全为 0，看姿态矩阵是否为单位阵
d 与 a 填反	位置向量多出本不该有的 z 分量	只看位置向量的第三行
标准/改进 DH 混用	前三关节勉强对，后段误差累积	用 3 关节以内的特例分别验两种约定
θ 偏置没加	整条链的结果恒定旋转一个角度	令所有 θ=0，比对零位姿态
旋转顺序颠倒	单关节时正确，多关节时错乱	用两关节同轴的特例测试

表里的每一条都是从实际对不上的结果里倒推出来的。最省时间的一步是“零位检查”：把所有关节角置零，此时正运动学的末端位姿应该能用尺子在实物或者机构简图上直接量出来，量不出来就说明参数表的某一列读错了。零位过了，再取单关节转动 90° 的特例，检查那条连杆是不是真的转到了预期位置。这两个特例跑通，中段的符号推导基本不会有大问题。

3. 逆运动学与雅可比：解析解、数值解和八个分支怎么选

3.1 解析解什么时候存在：几何分解与腕心分离

习题集里的逆运动学题目绝大多数是“设计过”的：要么最后三个关节轴交于一点，要么前三轴平行，这样才能拆出闭式解。拿到题目第一件事不是列方程，而是看结构——把腕心位置单独拎出来，用前三个关节的位置方程先解出腕心坐标，这一步通常只涉及平面几何或者球面三角，比直接解六个联立方程简单一个量级。腕心解出来之后再反推前三个关节角，最后用姿态矩阵反解腕部三个角，这是标准的分解流程。

几何分解的坑在于多解。一个六自由度臂在一般位形下有 8 组解，对应肩左、肩右、肘上、肘下、腕翻、腕不翻的三重组合。习题答案如果只给其中一组，那它多半默认了某种“手臂朝前、肘部朝下”的工程惯例，而读者手上的机构未必符合这个惯例。筛选逻辑通常是三层：先按关节限位砍掉越界解，再按远离奇异位形排序，最后按关节行程最短挑一个。这三层顺序不能颠倒——先挑行程最短的，往往会选中一个贴着限位边界的解，实际下发时直接触发保护。

反三角函数的写法也要固定。求关节角时优先用atan2(y, x)而不是acos或asin，后者只能返回 [0, π] 或 [-π/2, π/2] 区间，遇到第二、第三象限的解会静默丢解，而且不报错，等到实机上转反了才发现。

3.2 数值兜底：阻尼最小二乘的收敛判据与步长

结构不满足解析条件时，只能上数值解。最朴素的是牛顿-拉夫逊，但它在奇异位形附近会把关节增量放大到不可控，实际工程里更常用的是阻尼最小二乘。

import numpy as np def ik_dls(fk, jac, q0, target, tol=1e-8, max_iter=300, lam=1e-2, dq_max=0.2): q = np.array(q0, dtype=float) for k in range(max_iter): err = target - fk(q) # 位置误差，3 维向量 if np.linalg.norm(err) < tol: return q, k, True J = jac(q) # 3xN 几何雅可比 # 阻尼项 lam 保证 J@J.T + lam^2*I 始终可逆 dq = J.T @ np.linalg.solve(J @ J.T + lam ** 2 * np.eye(3), err) n = np.linalg.norm(dq) if n > dq_max: # 步长上限，防止一步跳进限位区 dq *= dq_max / n q = q + dq return q, max_iter, False

三个参数的作用要分清。lam是阻尼系数，取值越大迭代越稳但收敛越慢，一般在 1e-3 到 1e-1 之间按条件数动态调；dq_max是单步关节增量的上限，单位是弧度，设成 0.2 意味着单步不超过约 11°，这个值直接对应“会不会跳过解”；tol是位置误差容差，单位米，1e-8 对教学题足够，实机上取 1e-4 更现实，因为编码器分辨率本身就在那个量级。

返回值里的迭代次数k比收敛标志更有诊断价值。正常位形下 5 到 15 步收敛；如果逼近 300 次上限还没收敛，八成是目标点在工作空间之外，或者初始猜测落在了另一个分支的吸引域里。这时候别急着调lam，先换几个初始值试，能把问题定性成分支选择而不是数值问题。

3.3 雅可比矩阵：几何雅可比、解析雅可比与奇异判据

雅可比在习题里经常只作为“求出来就完事”的中间结果，但它其实是整套推导的枢纽。几何雅可比的第 i 列由两部分拼成：线速度部分等于关节轴方向叉乘从关节 i 到末端的向量，角速度部分就是关节轴方向本身。转动关节和移动关节的区别只在于角速度那一块是轴向量还是零向量。写成公式就是 J = [J_v; J_w]，六行 N 列。

解析雅可比和几何雅可比不是一回事，前者描述的是姿态参数（比如欧拉角）对关节角的偏导，两者之间差一个把角速度映射到欧拉角变化率的矩阵。习题答案里如果混用，会出现“位置部分对得上、姿态部分的量纲不对”的怪现象。判断方法很简单：几何雅可比的角速度部分是纯方向向量，模长恒为 1，解析雅可比不具备这个性质。

奇异判据用条件数比用行列式稳。行列式只在方阵时才有定义，冗余臂上没法用；条件数 σ_max/σ_min 对任意形状都成立，而且能直接读出“离奇异还有多远”。工程上一般把条件数超过 50 就当作接近奇异处理，超过 200 直接停机。这个阈值和机构尺寸有关，教学题里取 1e3 只是为了说明问题，别直接搬到实机上。

4. 动力学与轨迹规划：拉格朗日方程推到能用的力矩曲线

4.1 从动能到 M(q)：惯性矩阵的组装顺序

拉格朗日建模的第一步是把总动能写成 1/2 q̇ᵀM(q)q̇ 的形式，难点全在 M 的组装上。逐连杆累加的思路最不容易乱：对每个连杆，先求出它质心的线速度雅可比 J_v 和角速度雅可比 J_w，然后贡献 m·J_vᵀJ_v + J_wᵀ·R·I·Rᵀ·J_w 到总矩阵。这里 R 是连杆姿态矩阵，I 是连杆在自身坐标系下的惯性张量，两者夹在中间做的是坐标系变换。

import numpy as np def inertia_matrix(links, q): """links 中每个元素提供 m、I、以及三个可调用对象 Jv/Jw/R""" n = len(q) M = np.zeros((n, n)) for lk in links: Jv = lk['Jv'](q) # 质心线速度雅可比 3xN Jw = lk['Jw'](q) # 角速度雅可比 3xN R = lk['R'](q) # 连杆姿态 3x3 I_world = R @ lk['I'] @ R.T # 惯性张量转到世界系 M += lk['m'] * Jv.T @ Jv + Jw.T @ I_world @ Jw return M

I的填写是这一步的高发错误区。教材给的惯性张量通常以质心为原点，而习题里有时以连杆坐标系原点给出，两者之间要过一遍平行轴定理：I_o = I_c + m·(‖d‖²·E - d·dᵀ)，其中 d 是从新原点到质心的向量。漏掉这一项，细长杆的转动惯量会小一大截，算出来的力矩自然偏小。判断方法很直接：把 M 的特征值打出来，如果某个特征值小到接近零，多半是某个连杆的质量或者惯量没加进去。

M 必须对称正定，这是最省事的自检。随机取几组关节角，检查 M 与它的转置是否相等，再看最小特征值是否为正。这两条任意一条不过，说明组装逻辑有结构性问题，不用往下算 C 和 g 了。

4.2 牛顿-欧拉递推做交叉验证：三个随机向量就能抓错

拉格朗日法推出来的闭式表达式往往长到没法人工核对，这时候牛顿-欧拉递推就是最好的对照物。递推分两趟：外推从基座到末端，逐连杆算角速度、角加速度、质心加速度；内推从末端回基座，算关节力和力矩。两趟都是递推式，代码短，逻辑清晰，和拉格朗日那套偏导展开完全不共享任何中间量。

交叉验证的流程固定：随机生成 q、q̇、q̈ 三组向量（注意要在关节限位内取值，q̈ 量级控制在 1 rad/s² 左右），分别代入两种方法算 τ，比较范数相对误差。误差在 1e-10 量级以内，说明两份实现都对；误差大但两个结果量级相当，多半是某个连杆的质心位置或者惯量写错了；误差大到量级不同，先查重力项 g 的符号——这是最高频的错误，g 向量在基座系里指向下，转到连杆系时符号会跟着姿态变。

还有一条性质比数值比对更灵敏：Ṁ - 2C 必须斜对称，等价于对任意向量 ξ 都有 ξᵀ(Ṁ - 2C)ξ = 0。这条性质对付“M 和 C 各自单看都对、合起来不对”的情况特别有效，因为它检验的是两者的耦合关系。具体做法是取几组随机 q̇，用 Christoffel 符号组装出 C，再数值微分得到 Ṁ，做一次二次型检验，结果应该在 1e-10 量级内为零。

4.3 轨迹规划：三次、五次多项式与梯形速度的取舍

轨迹规划题的答案经常只给一条曲线，不给选型理由，读者照着抄换个场景就不能用。三种常用方案的分界其实很清楚：

方案	约束条件	适用场景	主要限制
三次多项式	起止位置、起止速度	点到点、对加速度无要求	加速度在端点跳变
五次多项式	位置、速度、加速度共 6 个	需要平滑启停	峰值加速度偏高
梯形速度	速度上限、加速度上限	长距离、有明确限位	拐点处加速度不连续

import numpy as np def quintic(q0, q1, T, t): """五次多项式：起止位置、速度、加速度共 6 个约束全部置零""" s = t / T return q0 + (q1 - q0) * (10 * s**3 - 15 * s**4 + 6 * s**5) t = np.linspace(0, 1.0, 500) q = quintic(0.0, np.pi / 2, 1.0, t) qd = np.gradient(q, t) # 峰值速度，与关节速度限位比对 qdd = np.gradient(qd, t) # 峰值加速度，与电机转矩限位比对 print(qd.max(), qdd.max())

这段代码真正要看的不是曲线形状，而是qd.max()和qdd.max()两个数。五次多项式的峰值速度是平均速度的 1.875 倍，峰值加速度更大，如果直接把 T 设成“看起来合理”的 1 秒，很容易在实机上触发过流保护。正确顺序是先按加速度限位反推最小 T，再看这个 T 下的速度是否超限，两个都过才算数。np.gradient在这里只是教学用的近似，真要做工程验证得用解析导数，否则采样点数不够时峰值会被削平。

5. 常见问题排查：五类“答案对不上”的现象与定位顺序

5.1 位置对、姿态错：DH 约定混用

现象很典型：正运动学算出来的末端位置和答案一致到小数点后四位，姿态矩阵却差一个转置或者某个元素的符号相反。

原因基本锁定在 DH 约定的混用上。标准 DH 把连杆参数挂在远端坐标系，改进 DH 挂在近端，两者的 α 和 a 分别对应不同的关节序号。另一种可能是旋转变换的相乘顺序写反了，Rot_z 和 Rot_x 交换位置后，位置向量因为只在 xy 平面内活动可能看不出差别，姿态却会整体偏转。

解决方式是做一次单关节隔离测试：只让第一个关节转动，其余全零，此时末端位姿只由 A1 决定，逐项比对 A1 的九个旋转元素。这一步能把问题压缩到单个矩阵。确认 A1 无误后再逐个关节往上加，第一个出错的关节号就是问题所在。整份答案从头到尾只用一种约定，是我从这类题里学到的最实用的一条纪律。

5.2 逆解代回残差大：分支选择与象限丢失

现象：逆运动学解出一组关节角，代回正运动学后位置误差在厘米级，而不是预期的 1e-8。

原因通常有两个。一是反三角函数用了acos或asin，只覆盖了半个值域，第二、第三象限的解被静默丢掉；二是多解筛选时按行程最短挑，结果挑中了一个贴着关节限位的解，代回时因为限位截断产生了偏差。

解决办法是把所有 8 组解全部算出来，逐组代回正运动学算残差，按残差排序而不是按行程排序。残差在前三位的解才算候选，再在候选里按限位和远离奇异筛选。全部改用atan2之后，这类问题基本消失。另外要留意腕部奇异：当第四轴和第六轴共线时，腕部三个角有无穷多组解，此时任何一组代回都能对上，但实际下发会导致关节突然大幅转动，工程上要在这一步加一个“腕部奇异检测 + 保持上一周期解”的保护。

5.3 力矩量级离谱：质心、惯量与重力符号

现象：动力学题算出来的关节力矩比答案大一个数量级，或者符号完全相反。

原因按出现频率排序：重力向量 g 在基座系和连杆系之间转换时符号搞反；惯性张量的平行轴定理漏了；质心位置用的是连杆坐标系原点而不是质心坐标。这三种错误都会让力矩量级失真，但失真方式不同——重力符号反了是整体翻转，平行轴漏了是转动惯量偏小、加速段力矩偏低，质心位置错了则表现为重力力矩的力臂长度不对。

最快的定位手段是静力学退化：令 q̇ = q̈ = 0，此时 τ 只剩下重力项 g(q)，可以手工用尺子和重量算一遍。手算结果和代码结果对不上，问题一定在重力项；对得上再恢复速度项，逐项加回去。我一般在写完动力学代码后第一件事就是跑这个退化检查，比后面调半天参数省事得多。

5.4 雅可比接近奇异，迭代震荡

现象：数值逆解在接近工作空间边界时开始来回震荡，残差不再单调下降，迭代次数飙升到上限。

原因是雅可比的最小奇异值趋近于零，伪逆把微小的误差放大成了巨大的关节增量，下一步又因为超出限位被截断，来回反复。

解决办法是三步：加阻尼项，把伪逆换成阻尼最小二乘；给单步增量设上限；把位置误差和姿态误差分开加权，姿态误差在接近奇异时权重降低。这三步里最有效的是阻尼，但阻尼系数不能设死，按当前条件数动态调整效果更好——条件数小于 20 时用小阻尼保证精度，超过 100 时把阻尼放大到 0.1 量级保证稳定。另外，迭代失败时不要只看收敛标志，把每一步的残差范数打出来存成数组，画一条曲线，是单调下降后卡住还是从一开始就震荡，两种情况的处理方向完全不同。

5.5 闭环仿真步长发散：刚性带来的数值问题

现象：控制器参数按教材公式算出来是稳定的，仿真跑几百步后误差突然指数增长，最后报数值溢出。

原因多半是显式积分器的步长撑不住系统的刚性。机器人关节的闭环带宽通常在几十赫兹，位置环增益设到 100 以上时，系统最快的时间常数会掉到毫秒级，步长 1e-3 秒的显式欧拉法直接越过了稳定边界。

解决办法是先把步长降到 1e-5 秒试一次，如果收敛了，说明就是数值问题，接着换成变步长的积分器再确认一遍；如果降步长后仍然发散，那就是增益本身给大了，跟数值无关。判断这两种情况还有个更巧的办法：在没有阻尼的模型上跑一次，看总机械能是否近似守恒，能量持续增长就是积分器在注入能量，能量衰减则是数值耗散。这个检查在调试任何闭环仿真时都值得先做一遍。

6. 控制律别只看公式：用数值 Lyapunov 单调性反验收敛结论

控制部分的习题答案通常止步于“构造出 Lyapunov 函数 V，验证 V̇ ≤ 0，因此系统渐近稳定”。这套推导在纸面上无懈可击，但落到自己写的代码上，经常出现“公式明明满足条件、仿真就是不收敛”的尴尬。我的习惯是：推导完之后，把 V 当成一个可观测量在仿真里逐帧打出来，看它是不是真的单调不增。

import numpy as np def pd_check(mass=1.0, kp=100.0, kd=20.0, dt=1e-4, steps=20000): q, qd = 0.5, 0.0 # 初始位置误差与零初速 V = np.zeros(steps) for k in range(steps): g = 9.81 * np.cos(q) # 退化成单关节后的重力项 u = -kp * q - kd * qd # PD 控制律，符号对应误差定义 qdd = (u - g) / mass qd += qdd * dt q += qd * dt V[k] = 0.5 * mass * qd**2 + 0.5 * kp * q**2 return V V = pd_check() dv = np.diff(V) print(dv.max(), V[-1]) # 最大单步增量、末态能量

这段代码的价值全在dv.max()这个数上。理论上 V 单调不增，dv.max()应该是零或者极小负数；实际跑出来通常在 1e-3 量级有正有负，这是积分误差导致的抖动，只要不发散就正常。如果dv.max()明显为正并且随时间增大，说明增益和步长的组合已经越过稳定边界，此时该调的是dt而不是kp。

进一步做区分：把dt缩小十倍再跑一次，如果抖动幅度跟着缩小，就是纯数值问题；如果不缩，说明控制律本身在这个模型上不满足稳定性条件，回去检查 V̇ 的推导里是不是漏了科氏力项或者重力补偿项。这个判据比看误差曲线收敛与否灵敏得多，因为误差曲线在临界稳定时也会缓慢下降，而 V 的单调性会立刻暴露问题。