简介这份《机器人建模和控制》习题答案面向学习机器人学、自动控制及相关课程的学生与研究人员用于对照教材课后题检验运动学、动力学与控制理论的掌握情况。压缩包共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): 标准 DHA_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, realTrue) 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, l10.4, l20.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, tol1e-8, max_iter300, lam1e-2, dq_max0.2): q np.array(q0, dtypefloat) 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 保证 JJ.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 MI的填写是这一步的高发错误区。教材给的惯性张量通常以质心为原点而习题里有时以连杆坐标系原点给出两者之间要过一遍平行轴定理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(mass1.0, kp100.0, kd20.0, dt1e-4, steps20000): 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 的单调性会立刻暴露问题。注意V 的构造要和实际控制律对应。用 PD 控制却拿计算力矩法的 V 去验证得到的单调性结论没有意义。从那以后我每次写完一个控制律的推导都会强制走一遍“退化到单关节 数值 V 单调性 步长减半复测”这三步宁可多花二十分钟也不愿意在六轴模型上调试一整晚才发现是符号问题。希望帮到你。本文还有配套的精品资源点击获取
简介这份《机器人建模和控制》习题答案面向学习机器人学、自动控制及相关课程的学生与研究人员用于对照教材课后题检验运动学、动力学与控制理论的掌握情况。压缩包共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): 标准 DHA_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, realTrue) 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, l10.4, l20.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, tol1e-8, max_iter300, lam1e-2, dq_max0.2): q np.array(q0, dtypefloat) 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 保证 JJ.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 MI的填写是这一步的高发错误区。教材给的惯性张量通常以质心为原点而习题里有时以连杆坐标系原点给出两者之间要过一遍平行轴定理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(mass1.0, kp100.0, kd20.0, dt1e-4, steps20000): 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 的单调性会立刻暴露问题。注意V 的构造要和实际控制律对应。用 PD 控制却拿计算力矩法的 V 去验证得到的单调性结论没有意义。从那以后我每次写完一个控制律的推导都会强制走一遍“退化到单关节 数值 V 单调性 步长减半复测”这三步宁可多花二十分钟也不愿意在六轴模型上调试一整晚才发现是符号问题。希望帮到你。本文还有配套的精品资源点击获取
