从DH参数到逆运动学：机器人建模与控制习题答案避坑指南

简介《机器人建模和控制》习题答案面向机器人学课程学习者、考研复习者及需要核对课后推导的科研人员围绕Spong等所著经典教材中的运动学、动力学、控制与传感器建模等内容提供参考答案与推导核对。压缩包内共1个PDF文件约6.46MB便于在电脑或平板上按章节查阅。已有3110人学习下载说明其在机器人学课后练习与概念巩固中具有一定参考需求。答案可帮助读者检查正向/逆向运动学求解、拉格朗日与牛顿-欧拉动力学建模、轨迹规划、Lyapunov稳定性分析及传感器数据处理等题目的思路适合作为课程作业对照、考前复盘和自学验算的辅助材料但不宜替代独立推导。1. 一套习题答案的价值在于它替你跑完了「哪一步会崩」的实验很多人搜「机器人建模和控制 习题 答案」心里想的是对一下数字。真做下去才发现数字最不重要两连杆逆运动学能解出四组关节角代回正解残差都在 1e-15 量级可只有一组能让机械臂不撞到自己。答案给出的从来不是一个数而是一条被约束剪过的分支。更反直觉的是这类题表面在验证计算实际在验证建模约定。DH 参数的下标、欧拉角的旋转顺序、角度制还是弧度制、质心位置算不算转动惯量——任何一处约定不一致你算得越认真错得越远而且错得毫无报错。把这套推导链条走通等于拿到了机器人学那套符号系统的解释器后面写正解、推雅可比、搭动力学仿真时翻的车十有八九还是同一批约定问题。2. 坐标系与 DH 参数为什么一串 4×4 矩阵能拼出整条机械臂2.1 齐次变换的本质一次乘法同时吃掉旋转和平移刚体变换在三维空间里是p R p t一个线性映射加一个平移。麻烦在于平移不是线性映射你没法用 3×3 矩阵把t塞进去于是复合变换就变成了「矩阵乘一次、向量加一次」的交替操作。n 个关节连起来你就得写 n 次乘法 n 次加法符号推导时会疯。齐次坐标的思路是加一维把点写成[x, y, z, 1]ᵀ变换矩阵变成T [ R t ] [ 0 1 ]这一加平移就变成了四维空间里的线性映射复合变换退化成纯粹的矩阵连乘T_0n T_01 T_12 ... T_{n-1,n}而且顺序天然对应「先关节 1、再关节 2」的物理链路。4×4 里最后一行的0 0 0 1不是冗余它是在告诉乘法这不是任意 16 个数的矩阵是仿射变换群的一个元素连乘之后依然成立。下面这段代码不用任何机器人库纯 numpy 把最小构件搭出来就是为了让你看清「DH 参数最终被展开成了什么」import numpy as np def rot_z(t): c, s np.cos(t), np.sin(t) T np.eye(4) T[0, 0], T[0, 1] c, -s T[1, 0], T[1, 1] s, c return T def rot_x(t): c, s np.cos(t), np.sin(t) T np.eye(4) T[1, 1], T[1, 2] c, -s T[2, 1], T[2, 2] s, c return T def trans_x(a): T np.eye(4); T[0, 3] a; return T def trans_z(d): T np.eye(4); T[2, 3] d; return T def dh_standard(theta, d, a, alpha): # 标准 DH绕 z 转 theta - 沿 z 移 d - 沿 x 移 a - 绕 x 转 alpha return rot_z(theta) trans_z(d) trans_x(a) rot_x(alpha) def fk_2r(q, l1, l2): A1 dh_standard(q[0], 0.0, l1, 0.0) # 连杆 1绕 z 转 q1杆长 l1 A2 dh_standard(q[1], 0.0, l2, 0.0) # 连杆 2绕 z 转 q2杆长 l2 T A1 A2 return T[:3, 3], T[:3, :3] # 末端位置、末端姿态 p, R fk_2r([np.pi/3, np.pi/6], 1.0, 0.8)dh_standard里的四个动作顺序不能换先绕z转关节角因为旋转关节的θ就是绕本关节轴再沿z平移d这一步处理的是两连杆之间的偏置再沿x平移a把坐标系推到下一根轴上最后绕x转α把z轴掰到下一关节的轴线上。四次操作合起来正好把「当前坐标系」搬到「下一个坐标系」这就是所谓「DH 参数只描述相邻两轴的相对位姿」。平面 2R 的α、d全为 0退化成一个纯平面问题但代码路径没变这种不变量在调试时非常值钱。2.2 标准 DH 与改进 DH差的是下标崩的是整条链机器人圈里有两套 DH 约定同时活着。标准 DH也叫 Denavit-Hartenberg 原始形式把连杆参数下标写成a_{i-1}, α_{i-1}, d_i, θ_i变换顺序是「先绕本关节z转、再沿本关节z移、再沿x移、再绕x转」。改进 DHCraig 形式把下标改成a_i, α_i变换顺序是「先绕x转、沿x移、再绕z转、沿z移」。两种写法描述同一个机械臂只是坐标系挂在连杆的近端还是远端但如果你从两篇材料里各抄一半灾难就发生了第一个关节的角度可能没问题到第三个关节时d平移到了错误的轴上末端位置偏移一个常量正解永远对不上逆解永远收敛不了而且因为没有异常抛出你会怀疑是公式推错了最后把所有推导重写一遍。一个能立刻自检的办法拿q 0的零位位形把 DH 表代进去算末端位置和机械臂的机械零位图纸比对。零位对不上百分之百是约定混了跟数学无关。2.3 教材约定与你的代码约定要对齐这本教材的推导用的是标准 DH 体系转动关节的θ_i是变量其余三个是常量所以每个A_i都是q_i的函数。写代码时最容易忽略的是「θ_i到底是不是零位偏移」——很多实际机械臂在机械零位时θ不为零DH 表里会多一个θ_offset如果习题答案里假设offset 0而你的模型没设成 0两边数字就像平行世界。提示每次拿到一张新的 DH 表先只算前三行把每个坐标系的原点位置打印出来和示意图逐个对。这一步花五分钟能省掉后面五小时。3. 逆运动学与雅可比解不是算出来的是被约束剪出来的3.1 解析解的门槛腕心分离与 Pieper 准则六自由度机械臂的逆解为什么有的能写出封闭形式、有的只能迭代答案在几何里。绝大多数六轴工业臂采用「球形腕」后三轴轴线交于一点。这个交点叫腕心它的位置只由前三轴决定于是 6 维问题被拆成 3 维位置子问题和 3 维姿态子问题每个子问题都能单独用几何或代数方法解出来。更一般地Pieper 准则说如果三个相邻关节轴相交于一点或者三轴互相平行那么逆运动学存在封闭解。原因是此时方程组的最高次数会被结构约束降下来。代数路线的做法是把sin θ、cos θ用半角u tan(θ/2)替换三角方程变成多项式方程六轴最多得到 8 组解。两连杆的解析解更直接用余弦定理一步到位import numpy as np def ik_2r_analytic(x, y, l1, l2): c2 (x*x y*y - l1*l1 - l2*l2) / (2*l1*l2) if c2 -1.0 or c2 1.0: return [] # 目标超出工作空间无解 s2_abs np.sqrt(max(0.0, 1.0 - c2*c2)) sols [] for s2 in (s2_abs, -s2_abs): # 肘上 / 肘下两个分支 q2 np.arctan2(s2, c2) q1 np.arctan2(y, x) - np.arctan2(l2*s2, l1 l2*c2) sols.append(np.array([q1, q2])) return solsc2就是余弦定理里的那个余弦值它的范围检查等价于「目标点是否落在可达圆环内」超出就返回空列表而不是让sqrt开负数后得到一个nan继续往下跑。arctan2的选择很关键用arctan(s/c)会丢掉象限信息q1在第二、三象限时直接错 180 度。循环里对s2取正负号就是物理上的肘上肘下两个构型这也是「四组解」的来源两连杆只有 2 个自由度却已有 2 个解分支六轴到 8 组解毫不奇怪。3.2 雅可比矩阵速度映射也是奇异的探测仪雅可比J(q)是正运动学的偏导矩阵ẋ J(q) q̇它把关节速度映射成末端速度。对 2RJ [ -l1 sin q1 - l2 sin(q1q2) -l2 sin(q1q2) ] [ l1 cos q1 l2 cos(q1q2) l2 cos(q1q2) ]行列式det J l1 l2 sin q2。当q2 0或π时det J 0机械臂完全伸直或完全折叠末端在某个方向上的速度能力被彻底抹掉——这就是奇异位形。它的物理含义很实在手臂伸直时你没法再让末端沿手臂方向移动那个方向需要无限大的关节速度实际中表现为关节猛冲、力矩爆表。数值逆解的标准做法是牛顿迭代q_{k1} q_k J⁺ eJ⁺是伪逆Jᵀ(JJᵀ)⁻¹。问题在于接近奇异时JJᵀ的最小奇异值趋近于 0求逆相当于把噪声放大成无穷大迭代一步就飞出去。工程上的解法是阻尼最小二乘Levenberg-Marquardtdef ik_2r_dls(target, q0, l1, l2, lam0.05, iters300, tol1e-8): q np.array(q0, dtypefloat) for k in range(iters): x l1*np.cos(q[0]) l2*np.cos(q[0]q[1]) y l1*np.sin(q[0]) l2*np.sin(q[0]q[1]) e np.array(target) - np.array([x, y]) if np.linalg.norm(e) tol: # 收敛判据任务空间误差 break J np.array([ [-l1*np.sin(q[0]) - l2*np.sin(q[0]q[1]), -l2*np.sin(q[0]q[1])], [ l1*np.cos(q[0]) l2*np.cos(q[0]q[1]), l2*np.cos(q[0]q[1])] ]) # 阻尼项 lam^2 * I 抬高 JJ^T 的最小奇异值避免奇异附近数值爆炸 dq J.T np.linalg.solve(J J.T lam**2 * np.eye(2), e) q q dq return q, klam是阻尼系数取 0 就退化成纯牛顿法取太大则收敛慢但稳经验上先给个0.01~0.1量级接近收敛时再逐步减小这套「先粗后细」的调法在轨迹跟踪里很常见。iters上限和tol要一起看如果迭代到上限还没收敛说明目标可能不可达或者初值落在另一个吸引域里这时候该做的是换初值或多起点搜索不是加大迭代次数。3.3 多解怎么选代价函数比求解算法更重要八组解代回正解全都成立选哪一组答案取决于任务。常见的三层筛选是先按关节限位过滤q_min ≤ q ≤ q_max再按避障约束过滤碰撞检测最后在剩下的解里挑代价最小的。代价函数一般写成关节空间距离的加权和cost Σ w_i (q_i - q_i^prev)²q^prev是上一控制周期的解w_i给不同关节不同权重腰关节转动代价通常比小臂高得多。这一项的意义在于连续性如果每一拍都独立求最优解机械臂会在两个等价解之间来回跳表现为末端不动、关节疯狂抽搐现场看就是「机器人疯了」。习题答案里往往只给一组解实操时要在那组解附近加上这一层筛选逻辑才是能落地的版本。4. 动力学与控制M、C、g 这三个矩阵为什么值得单独算4.1 拉格朗日法从标量能量推到矩阵形式运动学只回答「在哪」动力学回答「需要多大力」。拉格朗日法的好处是绕开了内力分析只要写出动能K和势能P令L K - P套公式d/dt(∂L/∂q̇) - ∂L/∂q τ力就自动出来了。对 n 关节系统这套推导会整理成同一个形状M(q) q̈ C(q, q̇) q̇ g(q) τM是惯性矩阵对称正定C q̇是科氏力与离心力项g是重力项。三个矩阵各有物理含义而且M和g直接决定静态精度C决定高速时的跟踪误差。用 sympy 把两连杆推一遍是最快的建立直觉的方式import sympy as sp t sp.symbols(t) q1, q2 sp.Function(q1)(t), sp.Function(q2)(t) m1, m2, l1, l2, lc1, lc2, g, I1, I2 sp.symbols( m1 m2 l1 l2 lc1 lc2 g I1 I2, positiveTrue) # 质心位置lc1、lc2 是质心到各自关节的距离 x1 lc1*sp.cos(q1); y1 lc1*sp.sin(q1) x2 l1*sp.cos(q1) lc2*sp.cos(q1q2) y2 l1*sp.sin(q1) lc2*sp.sin(q1q2) v1sq sp.diff(x1, t)**2 sp.diff(y1, t)**2 v2sq sp.diff(x2, t)**2 sp.diff(y2, t)**2 K (sp.Rational(1,2)*m1*v1sq sp.Rational(1,2)*I1*sp.diff(q1, t)**2 sp.Rational(1,2)*m2*v2sq sp.Rational(1,2)*I2*sp.diff(q1q2, t)**2) P m1*g*y1 m2*g*y2 qd [sp.diff(q1, t), sp.diff(q2, t)] q [q1, q2] # K 是 q̇ 的二次型对速度分量求二阶偏导直接得到 M M sp.Matrix(2, 2, lambda i, j: sp.simplify(sp.diff(K, qd[i], qd[j]))) gvec sp.Matrix([sp.simplify(sp.diff(P, qi)) for qi in q])这段代码的每一行都对应一个定义没有技巧K是刚体动能的平动项加转动项I那一项是绕各自质心的转动惯量M之所以能靠对速度求二阶导得到是因为动能对速度是严格二次型K ½ q̇ᵀ M q̇。跑出来的M元素是M11 m1 lc1² m2(l1² lc2² 2 l1 lc2 cos q2) I1 I2 M12 m2(lc2² l1 lc2 cos q2) I2 M22 m2 lc2² I2cos q2出现在对角元素里说明手臂伸展时惯性变大——这条直觉比任何公式都好记。重力项g(q)就是势能对q的偏导等于把「重力在每个关节上产生的等效力矩」直接读出来。4.2 牛顿-欧拉递推为什么它是 O(n)拉格朗日法展开成符号表达式时计算量随关节数三次增长六轴以上就很吃力。牛顿-欧拉走另一条路先沿链路从基座向末端外推逐杆算角速度、角加速度、质心线加速度再从末端向基座内推逐杆算力和力矩。两次递推每根杆的运算量恒定整体是 O(n)。两者的关系类似「闭式解」和「递推解」结果完全相同代价不同。工程实现里几乎都用牛顿-欧拉算τ因为控制周期是毫秒级没人有时间做符号化简。下表是个粗略对照。维度拉格朗日法牛顿-欧拉法推导直观性高只需能量低需要逐杆受力分析计算复杂度O(n³)符号展开后O(n)输出形式显式 M、C、g直接输出 τ需额外处理才能拆出 M适合场景控制器设计、稳定性证明实时仿真、逆动力学计算关键取舍在这里做控制律设计时你需要M、C、g的显式结构去构造 Lyapunov 函数所以用拉格朗日写仿真器时你只需要τ所以用牛顿-欧拉。两者必须能对上对不上就是有一边写错了这是最有效的自检手段。4.3 计算力矩法为什么能抵消非线性PD 控制在机器人上单独用结果通常是稳态有偏差、或者根本站不住——因为重力项g(q)始终存在误差不为零时它不会自己消失。计算力矩法也叫反馈线性化的思路是既然我知道非线性项长什么样就直接用模型把它抵掉。τ M̂(q)(q̈d Kv ė Kp e) Ĉ(q, q̇) q̇ ĝ(q)e qd - qė q̇d - q̇带帽子的M̂, Ĉ, ĝ是模型估计值。把它代入动力学方程如果模型完全准确非线性项两两抵消剩下的误差方程是线性的ë Kv ė Kp e 0这就是这套方法为什么「能 work」的全部秘密它把非线性系统的跟踪问题转化成了一个带指定极点的线性二阶系统。Kp决定刚度Kv决定阻尼通常取Kv 2√Kp附近做临界阻尼。现实里模型永远不准确M̂ ≠ M会留下残余项所以稳定性分析要做。选 Lyapunov 函数V ½ q̇ᵀ M(q) q̇ ½ eᵀ Kp ePD 重力补偿的形式求导后交叉项能不能消掉取决于Ṁ - 2C是否反对称。这个性质是拉格朗日动力学的必然结果不是巧合它保证了q̇ᵀ(Ṁ - 2C)q̇ 0于是V̇里只剩-q̇ᵀKv q̇ ≤ 0系统半正定稳定再用 LaSalle 不变集原理推到渐近稳定。注意如果C矩阵是自己手写的、没有严格按 Christoffel 符号从M导出Ṁ - 2C的反对称性会破掉仿真表现为能量缓慢发散末端轨迹越来越野。这是血泪经验里最常见的一类「公式看着没错、跑起来就是不对」。下面是把三个矩阵组装成控制器的一小段代码参数含义逐行标注def computed_torque(q, dq, qd, dqd, ddqd, Kp, Kv, dyn): q/dq: 当前关节位置与速度; qd/dqd/ddqd: 期望轨迹 Kp/Kv: 2x2 对角增益; dyn: 返回 (M, C, g) 的动力学函数 e qd - q de dqd - dq M, C, g dyn(q, dq) # 期望加速度 误差反馈一起过一遍惯性矩阵保证量纲一致 v ddqd Kv de Kp e return M v C dq g # 最后两项抵消科氏力与重力M v是「为了实现这个期望加速度所需的总力矩」C dq g是「抵消当前非线性的补偿力矩」两项相加就是最终输出。注意C dq而不是C dqd用的必须是当前实测速度这个细节错了补偿在加减速阶段会变成反向扰动。5. 避坑排查习题答案对不上的五个高频现场5.1 位置对、姿态错旋转顺序的玄学现象末端位置三个分量精确到小数点后十位姿态矩阵却总是对不上转置一下好像差不多。原因欧拉角约定不同。ZYZ、ZYX、RPY描述同一个姿态会给出完全不同的三个角度值而很多答案只写「姿态角 (α, β, γ)」不写旋转顺序。更隐蔽的是内旋和外旋的差别主动旋转和被动旋转互为转置。解决永远用旋转矩阵或四元数做中间表示只在最后一步转成欧拉角如果必须比对角先把标准答案的矩阵转置一次看看是否吻合吻合就说明是主动、被动搞反了。5.2 逆解代回正解误差很大伪逆在奇异点炸了现象迭代结束后q看着正常但代回正运动学末端偏差好几个厘米或者迭代次数打满也没收敛。原因目标点接近工作空间边界或者手臂接近伸直JJᵀ条件数极大伪逆把浮点误差放大dq一步就能到几千弧度。也可能目标根本不可达算法还在硬迭代。解决先检查目标点在不在工作空间内2R 就是圆环判据六轴可以用数值可达性采样再改成阻尼最小二乘并加关节步长上限最后把q归一化到[-π, π]避免q和q 2π之间的伪差异造成迭代震荡。5.3 仿真能量发散C 矩阵不是从 M 导出来的现象给一个无重力、无力矩的自由摆动末端速度却越来越大能量不守恒。原因C是手写的近似表达式或者用了不同的参数符号导致Ṁ - 2C不反对称。理论上这个反对称性是保证被动性passivity的基础破掉它等于凭空往系统里泵能量。解决让C由 Christoffel 符号从M自动生成写完立刻做数值验证随机取若干组(q, q̇)检查q̇ᵀ(Ṁ - 2C)q̇是否在所有样本上都接近 0。这个检验只要十几行代码比调一整天参数有用。5.4 单位混用90和π/2是同一个角吗现象正解结果整体缩放或旋转了一个固定量怎么改公式都没用。原因DH 表里的α有时填的是度数90代码里np.cos(90)会当弧度处理得到-0.448这种毫无意义的值。同样地电机编码器回来的通常是脉冲数或度不转成弧度直接喂给动力学公式sin的结果会完全错位。解决在参数入口处强制断言单位所有角度进入模型前统一乘π/180在函数签名里写明theta_rad让命名本身成为约束。这条看着很土但它是唯一能在半年后回看代码时救你一命的手段。5.5 控制参数怎么调都振采样周期被忽略了现象仿真里调好的Kp、Kv一上实机就高频抖动降低增益后跟踪又跟不上。原因连续域设计的增益在离散实现里受采样周期约束。采样太慢时相位滞后会吃掉稳定裕度等效于给系统加了一个延迟环节此外实测速度通常带噪声直接乘Kv会把噪声放大成力矩抖动。解决先按连续域算出增益再检查采样频率是否至少是闭环带宽的 20 倍速度信号加速度低通滤波或改用状态观测器最后给输出力矩加上变化率限制防止单个周期的尖峰打坏机构。6. 解析解与数值解的取舍把习题答案当成一份可执行的假设做完这套题最值得留下来的是一个判断习惯什么时候该推公式什么时候该交给迭代。解析解给出的是结构——它告诉你几组解、奇异在哪、哪些参数真正影响结果数值解给出的是一条可用路径——它不保证全局但在局部足够快、足够通用。维度解析解数值解结果完整性一次给出全部分支只给出一条依赖初值奇异处理结构上暴露奇点需要阻尼等手段兜底计算成本上机前可离线推导每周期迭代成本随精度上升适用规模结构特化的机型任意串联链我的建议是拿习题做「假设检验」而不是「对答案」先用符号推导得出结构再用数值仿真验证一致性两者只要有一处不符问题必然出在你的建模约定上而不是运算能力上。这一步的收益远超多做十道题。从那以后我每次写动力学相关代码都会强制走一遍三个自检零位位形对不对、Ṁ - 2C反不反对称、能量守恒不守恒。三关过了再谈控制参数否则调参就是在给错误模型打补丁。希望帮到你。本文还有配套的精品资源点击获取