简介《机器人建模和控制》Robot Modeling and Control是 Spong、Hutchinson 与 Vidyasagar 合著的机器人学经典教材本书习题答案是配套该书的学习资料主要面向正在系统学习机器人运动学、动力学、传感器建模与控制系统设计的学生、研究者以及自学工程师。压缩包内共1个PDF文件整体大小6.46MB便于直接查阅、打印或导入平板标注文件为原版教材课后习题的答案汇总内容围绕逆向运动学求解、动力学建模、轨迹规划、Lyapunov稳定性分析及传感器数据处理等核心主题展开。该资源已有3098人学习下载是同类教材辅助材料中关注度较高的一份。通过对照解答过程读者可以验证自己的计算与推导理解多关节机器人IK解析或数值方法、驱动力矩计算、平滑路径规划和控制器稳定性判据的实际应用从而巩固理论知识并提升解决工程问题的能力。1. 为什么《机器人建模和控制》的习题答案比解题过程更值得拆解很多初学者拿到《机器人建模和控制》的习题第一反应是找一份现成答案抄完交差结果下次上课还是听不懂。因为这本书的习题几乎全是计算推导题真正卡人的不是“没答案”而是不知道标准解题流程长什么样运动学题考坐标系定义和矩阵复合动力学题考拉格朗日方程要展开到哪一步才算完控制题考稳定性证明的固定套路。这篇笔记把三大类习题的通用解法和自检手段拆开讲配套可直接运行的代码模板。适合正在学机器人学课程、准备考试或者读论文需要补基础的同学也适合所有被课后答案折磨过的自习党。2. 正运动学与DH参数习题先画坐标系再填参数表正运动学是整本书的题根出题形式通常是给一个三到四自由度的机械臂简图和若干杆长尺寸要求写出末端位姿矩阵。这类题公式背起来不难丢分几乎都发生在DH参数表上坐标系画反、z轴方向没对齐、d和a偷换位置最终答案就会差一个负号或者差一段距离。这里的核心不是会背变换公式而是建立一套从结构图到参数表的固定流程。2.1 DH参数的四步法从结构图到参数表拿到任何机械臂结构图我一般不会直接去匹配记忆里的标准答案而是按四步走。第一步确定关节轴每个旋转关节画一条轴线z轴方向沿轴线注意z下标从0开始和教科书保持一致。第二步找x轴方向x轴沿前一个z轴指向当前z轴的公垂线方向如果相邻两个关节轴相交x轴取两轴所在平面的法线方向。第三步把a、alpha、d、theta四个参数按表填好第四步把参数表代进变换矩阵。这里有个必须统一的前提你用的是标准DH还是修正DH。两套定义的差别在于x轴和变换顺序不同同一个机械臂会得到完全不一样的四列参数但最终算出的末端位姿必须一致。我在笔记里统一用标准DH约定变换顺序是绕z转theta、沿z平移d、沿x平移a、绕x转alpha。以最常见的二连杆平面臂为例假设杆长分别是L1和L2DH参数表长这样连杆 itheta_id_ia_ialpha_i1q10L102q20L20这张表填完后构造变换矩阵的代码就是教科书公式的直接翻译。下面用numpy写一个最基础的变换函数它接受每个关节的theta、d、a、alpha返回4x4齐次矩阵import numpy as np def dh_transform(theta, d, a, alpha): 单关节标准DH变换矩阵。 参数单位说明 theta, alpha 弧度 d, a 米 返回 4x4 numpy 数组。 ct np.cos(theta) st np.sin(theta) ca np.cos(alpha) sa 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] ])这段代码的逻辑就是标准DH的矩阵定义前三行前三列是姿态旋转部分第四列前三个元素是末端原点在基坐标系下的位置。注意位置分量的表达里同时出现了theta的三角函数和a的乘积这意味着如果a和alpha填错位置也会跟着错。建议把这个函数保存成单独的工具函数后面正运动学、雅可比、数值逆解都要反复调用。有了单关节变换矩阵正运动学就只是连乘。把每个关节的参数依次传入乘完就是末端位姿def forward_kinematics(dh_params): 正运动学把DH参数表连乘成末端位姿。 dh_params 是 list每个元素是 (theta, d, a, alpha)顺序从基座到末端。 T np.eye(4) for theta, d, a, alpha in dh_params: T T dh_transform(theta, d, a, alpha) return T连乘顺序必须是从基座往末端方向如果写反结果就是另一个运动链的位姿这在多关节机械臂里是最隐蔽的错误之一。参数表里零值特别容易让人大意d为0、alpha为0时矩阵会退化成只有旋转和平移的简单形式但代码里不能省。这个函数本身很短但它把“题意”和“结果”连成了一条可追踪的路径习题答案对不对最终都要回到这里验证。2.2 末端位姿的符号推导与零位自检数值代码能验证结果但习题要求写符号表达式时手推很容易在cos(q1q2)这类复合角展开上出岔子。常见做法是直接上符号计算把答案变成公式再和教材给出的标准形式对照。下面用SymPy做同样的二连杆正运动学推导import sympy as sp q1, q2 sp.symbols(q1 q2, realTrue) L1, L2 sp.symbols(L1 L2, positiveTrue) def T_std(theta, d, a, alpha): 标准DH符号变换矩阵参数说明同前。 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] ]) T01 T_std(q1, 0, L1, 0) T12 T_std(q2, 0, L2, 0) T02 sp.simplify(T01 * T12) print(px , T02[0, 3]) print(py , T02[1, 3])运行后应该得到px L1cos(q1) L2cos(q1 q2)py L1sin(q1) L2sin(q1 q2)这正是习题标准答案的核心部分。符号推导的价值在于它不会遗忘链式法则不会把角度单位搞混还能直接显示每一项的来源。如果你手推的表达式和它不一致先别急着怀疑公式而是回头检查DH表的顺序和零位定义。拿到符号表达式后有一个必须做的动作零位自检。把机械臂所有关节角设成0也就是让它处于完全伸展的零位姿态这时末端位置应该等于所有杆长之和(L1L2, 0)。如果是六轴机器人零位姿态下的末端位置应该和题图里画出的坐标纸完全吻合。这一步能抓住大约三分之一的正运动学错误因为坐标系旋转方向反了、d的正负号反了都会在零位露出马脚。提示符号推导的最终答案通常要用弧度表示但多数教材课后答案的数值样例喜欢写角度。对答案前先统一单位否则会出现“看似差了一个90度”的误解。这一章的收尾动作是把符号表达式保存成代码或JSON文件方便后面逆解和控制仿真直接引用。我自己习惯把DH参数和正运动学函数放在一个模块里所有习题答案都以这个模块为基准。这就像给整个解题流程装了一个黑匣子记录仪哪一步出了问题都能回溯。3. 逆运动学与雅可比习题解析解教你怎么取舍数值解教你兜底正运动学是把关节角映射到位姿逆运动学反过来。习题册里逆解题的标准问法是“已知末端位姿求各关节角”。对于六轴工业臂教科书喜欢让你用解析法但真正写作业和做项目时数值法才是那个永不背叛你的兜底工具。这道题的关键不是背公式而是理解多解、奇异和收敛。3.1 解析逆解的拆分思路与多解取舍解析逆解的常见做法是“腕部解耦”把六自由度机械臂拆成前三个关节定位置、后三个关节定姿态。以习题里出现最多的球腕结构为例先由末端位姿反解出腕部中心在基坐标系下的位置再用前三关节的几何关系求出前三个角最后根据姿态残量解出后三个角。这是一个通用框架几乎所有带球腕的工业机械臂都能套。具体到前三关节二自由度平面臂的逆解可以直接用余弦定理def inverse_kinematics_2d(px, py, L1, L2, elbowdown): 二连杆平面臂解析逆解。 参数 px, py 目标位置末端坐标 L1, L2 杆长 elbow down 或 up决定肘部朝向 返回 (q1, q2) 弧度。 cos_q2 (px**2 py**2 - L1**2 - L2**2) / (2 * L1 * L2) cos_q2 min(max(cos_q2, -1.0), 1.0) # 防止数值越界 if elbow down: q2 -np.arccos(cos_q2) else: q2 np.arccos(cos_q2) q1 np.arctan2(py, px) - np.arctan2( L2 * np.sin(q2), L1 L2 * np.cos(q2) ) return q1, q2这段代码的要点有两个。第一cos_q2需要做数值夹取因为浮点运算可能导致根号内的值略微超出[-1,1]第二arccos给出两个符号相反的解对应“肘部朝下”和“肘部朝上”这是逆解多解性的直观体现。平面二连杆都这样六轴机械臂的解会有更多分支而且分支之间可能对应完全不同的末端姿态。多解取舍的习题考点通常在四条规则先检查每个候选解是否在关节限位内再检查是否避开了奇异位形然后选行程最小的解最后如果目标位姿来自轨迹规划还要考虑跟前一时刻的关节角连续性。教材答案往往只给一组解不少同学问“为什么我算出来另一组也对”答案就是多解性不一定两个都对但都可能满足末点位姿。提示解析解写完之后务必把求出的关节角代回正运动学验证。如果逆解结果和正运动学输出对不上优先怀疑arccos的定义域和分支选择而不是怀疑正运动学本身。解析解的麻烦在于它依赖机械臂形态。非球腕结构、有偏置的腕部、关节轴线不交于一点的构型都需要专门推导。这时候数值解的价值就显现出来了。3.2 雅可比矩阵构造与阻尼最小二乘数值逆解数值逆解的思想很简单给出一个初始关节角算出当前末端位姿与目标位姿的误差用雅可比矩阵的伪逆把误差映射回关节空间逐步迭代直到收敛。习题里最常考的是让你构造雅可比矩阵而真正写程序时用数值雅可比最省事也最不容易推导错。def rotation_vector_from_matrix(R): 从旋转矩阵提取旋转向量小角度近似够用。 参数 R 3x3 旋转矩阵 返回 3x1 旋转向量。 cosine (np.trace(R) - 1.0) / 2.0 cosine max(min(cosine, 1.0), -1.0) angle np.arccos(cosine) if abs(angle) 1e-9: return np.zeros(3) return angle / (2.0 * np.sin(angle)) * np.array([ R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1] ]) def numerical_jacobian(fk, q, eps1e-6): 数值雅可比对每个关节做正负微扰差商近似。 参数 fk 正运动学函数输入关节角list输出4x4矩阵 q 当前关节角list eps 微扰步长太小会引入浮点误差太大会失真 返回 6 x n 雅可比矩阵。 J np.zeros((6, len(q))) for i in range(len(q)): qp q.copy() qm q.copy() qp[i] eps qm[i] - eps Tp fk(qp) Tm fk(qm) # 位置部分对平移向量做差分 J[:3, i] (Tp[:3, 3] - Tm[:3, 3]) / (2 * eps) # 姿态部分用旋转矩阵差得到等效旋转向量 dR Tp[:3, :3] Tm[:3, :3].T J[3:, i] rotation_vector_from_matrix(dR) / (2 * eps) return Jeps的选择是数值雅可比最大的玄学。取1e-3差分逼近误差大取1e-12浮点舍入误差会把结果淹没。经验值在1e-6到1e-7之间角度单位是弧度时稳定性最好。另外姿态部分的差分我在代码里用小角度近似提取旋转向量这在每次迭代误差小于0.1弧度时足够精确如果初始位姿差太多建议先单独用位置误差迭代几轮再切换到完整雅可比。有了雅可比数值逆解就像在爬山每一步往误差减小的方向走。阻尼最小二乘是比纯伪逆可靠得多的选择def damped_least_squares_ik(fk, target, q_init, lam0.01, tol1e-9, max_iter100): 阻尼最小二乘数值逆解。 参数 fk 正运动学函数 target 目标位姿 4x4 q_init 初始关节角 lam 阻尼系数0为纯伪逆数值建议0.005~0.05 tol 收敛阈值按末端位置误差算 max_iter 最大迭代次数 返回 (q, 是否收敛)。 q np.array(q_init, dtypefloat) for _ in range(max_iter): T fk(q) pos_err target[:3, 3] - T[:3, 3] R_err target[:3, :3] T[:3, :3].T rot_err rotation_vector_from_matrix(R_err) e np.concatenate([pos_err, rot_err]) if np.linalg.norm(pos_err) tol: return q, True J numerical_jacobian(fk, q) # J J^T lam^2 I 保证矩阵可逆 dq J.T np.linalg.inv(J J.T lam**2 * np.eye(6)) e q q dq return q, False阻尼系数lam是这题的灵魂。lam0时就是纯伪逆一旦机械臂靠近奇异位形J J^T接近奇异迭代步长会剧烈放大关节角飞出几十上百弧度lam取0.01时相当于在解空间中加了一个软约束步长被压住鲁棒性大幅提升。代价是收敛速度变慢末端精度会略微下降但对习题验证来说这个精度损失完全可以接受。数值逆解还有一个隐蔽陷阱它会收敛到离初始值最近的解而不是唯一解。比如平面二连杆的目标位置在可达空间中初始肩膀朝上最终收敛到肘部朝上的解肩膀朝下就收敛到另一种解。对答案时如果书后给你的是另一组解只要回代后位姿一致就能确认自己是对的。这一点是习题答案环节最容易让初学者怀疑人生的地方也是数值法相对解析法的最大优势它不挑机构形态什么结构的机械臂都能给出一组可行解。4. 动力学与控制习题拉格朗日方程和PD控制的推导与验证动力学题是《机器人建模和控制》里推导量最大的部分常见题型包括“求惯性矩阵”“求重力项”“证明某控制律的稳定性”。这类题难在表达式又长又容易漏项而且书后答案往往只给最终矩阵不给你中间过程。我的做法是用符号计算把拉格朗日方程的机械步骤全部自动化把注意力放到物理含义上。4.1 用符号推导直接拿M矩阵和重力项G二连杆机械臂的动力学方程标准形式是M(q) qdd C(q, qd) qd G(q) tau。习题里最常见的第1问是写出M和G。手推拉格朗日方程要算动能、势能、偏导数步骤多且容易出错用SymPy做符号推导则非常省心。import sympy as sp q1, q2 sp.symbols(q1 q2, realTrue) dq1, dq2 sp.symbols(dq1 dq2, realTrue) L1, L2 sp.symbols(L1 L2, positiveTrue) m1, m2 sp.symbols(m1 m2, positiveTrue) g sp.symbols(g, positiveTrue) # 两个杆的质心位置假设质心在杆中点 x1 (L1 / 2) * sp.cos(q1) y1 (L1 / 2) * sp.sin(q1) x2 L1 * sp.cos(q1) (L2 / 2) * sp.cos(q1 q2) y2 L1 * sp.sin(q1) (L2 / 2) * sp.sin(q1 q2) # 质心位置的雅可比矩阵 J1 sp.Matrix([ [sp.diff(x1, q1), sp.diff(x1, q2)], [sp.diff(y1, q1), sp.diff(y1, q2)] ]) J2 sp.Matrix([ [sp.diff(x2, q1), sp.diff(x2, q2)], [sp.diff(y2, q1), sp.diff(y2, q2)] ]) # 惯性矩阵 M m1 * J1^T J1 m2 * J2^T J2 M sp.simplify(m1 * J1.T * J1 m2 * J2.T * J2) # 势能对关节角的偏导 重力项 G V m1 * g * (L1 / 2) * sp.sin(q1) \ m2 * g * (L1 * sp.sin(q1) (L2 / 2) * sp.sin(q1 q2)) G sp.Matrix([sp.diff(V, q1), sp.diff(V, q2)]) print(M , M) print(G , G)这段代码的逻辑分两层位置雅可比把关节速度映射到质心笛卡尔速度动能用0.5 * v^T m v 表达势能只和高度有关对q求偏导就是重力项。运行时能看到M矩阵里有cos(q2)出现说明第1和第2关节之间存在惯性耦合而且耦合随q2变化这正是动力学习题的标准考点。M矩阵里每一项的物理含义值得花时间看。M[0,0]包含两个杆的等效惯量M[0,1]和M[1,0]相等这是惯性矩阵对称性的体现M[1,1]只和第2杆有关因为第1关节的运动不改变第2杆绕自身关节轴方向的惯量配置。如果你手推的结果不对称几乎可以断定哪里漏了项。注意代码里用质心点的质量矩阵代替了完整的转动惯量。习题里如果标的不是质心而是“质量集中在末端”只要把x1、y1替换成末端坐标方法完全不变但若题目明确给出绕质心的转动惯量Ic还要在J^T m J基础上加入Ic项。科氏项C(q, qd)是动力学习题的另一个高频考点但手推Christoffel符号极易出错。我的建议是先用数值工具验证M的正确性再通过M矩阵元素对时间的导数和C矩阵的关系反推C。对自习来说知道C矩阵不是唯一的就够了只要M_dot - 2C 是反对称阵这一组C就是合法结果。教材答案给C的方式和你的推导不一致时不一定代表你错了。4.2 PD控制与重力补偿习题答案里的前馈项不是摆设控制类习题最典型的题目是“设计一个PD控制器让机械臂稳定到目标位置”。书后答案几乎都会在PD后面加一项重力补偿G(q)但不少同学会疑惑PD本身不就是控制器吗为什么答案里多了一项原因很简单重力是一个恒定扰动纯PD控制的稳态误差等于重力矩除以比例增益只有把增益调到无穷大才能消除而直接前馈补偿重力矩稳态误差会变成零。理解这一点之后仿真验证就成了水到渠成的事。下面用一个单摆模型演示带不带重力补偿的差别这个模型虽然简单但足以复现习题答案背后的设计逻辑import numpy as np # 单摆参数转动惯量J质量m摆长L J 0.05 m 0.2 L 0.25 g 9.81 dt 0.001 t_total 5.0 n_steps int(t_total / dt) def simulate(compensate): 对单摆做PD控制仿真。 compensateTrue 开启重力补偿前馈 compensateFalse 纯PD控制 返回时间序列角度数组。 q 0.0 dq 0.0 q_hist [] qd 0.5 # 目标角度 Kp 5.0 Kd 0.2 for _ in range(n_steps): e qd - q tau Kp * e - Kd * dq if compensate: tau m * g * L * np.sin(q) ddq (tau - m * g * L * np.sin(q)) / J dq dq ddq * dt q q dq * dt q_hist.append(q) return np.array(q_hist) q_no_comp simulate(compensateFalse) q_comp simulate(compensateTrue) print(纯PD终点角度:, q_no_comp[-1]) print(带重力补偿终点角度:, q_comp[-1])参数上Kp取5.0、Kd取0.2是一个比较保守的组合保证系统不过冲目标角度0.5弧度大约28.6度重力矩在这个角度下有明显影响。运行结果会看到纯PD的终点角度离0.5还有一段距离而带重力补偿的终点非常接近0.5。这个差距就是习题答案里加前馈项的原因。做实验题时Kp调大能缩小但不消除稳态误差Kd只影响动态过程不影响稳态如果增大Kp后系统开始震荡说明系统阻尼比太低需要同步增大Kd。实际练习时我建议把上述仿真封装成一个函数输入不同的Kp、Kd、compensate参数输出稳态误差曲线。还能顺便验证一个课后题常问的结论重力前馈不影响闭环系统的稳定性因为前馈项是开环补偿它的加入不会改变特征方程。这个结论说起来简单亲手仿真之后会有更直观的感受。5. 习题答案对不上的5种翻车现场排查清单写作业最痛苦的不是题做不出来而是做完了对答案发现对不上。我把自己和带过的同学踩过的坑梳理成下面五类每一条都按“现象→原因→解决”的顺序写你可以直接拿来当排查清单用。5.1 符号、方向、坐标序号的坑现象一算出来的重力项G和书后答案恰好差一个负号。原因九成在重力方向约定不一致。书后答案通常默认基坐标系z轴竖直向上重力加速度g带负号如果推导时把势能写成-mgh重力项自然反号。解决办法是先在草稿纸上标出基坐标系方向再写势能表达式不要凭习惯写。现象二关节坐标序号差了一位导致符号表达式看起来“差了一个角”。机械臂的关节编号从基座侧算起但有些习题图在标注时把基座固定关节当成0号或1号。比如二连杆题目里把靠近基座的关节叫theta0你按照theta1去套后答案位置完全不对。解决方法是每次动手前先数一遍图上的序号再把编号约定写进代码注释。现象三DH参数表中a和d填反。这两个参数的单位同为长度填反后位置矩阵通常不会全错而是某些分量错。检查技巧当theta_i0时第i个坐标系的原点沿x轴方向移动a_i沿z轴方向移动d_i。用一个简单位置点代进去验立刻能分辨是否填反。5.2 单位、奇异位形、控制器发散的坑现象四数值和答案不一样但形状趋势完全一致。原因十有八九是角度单位。书本习题答案的数值样例喜欢直接用角度制画图或标注而公式推导默认弧度。比如答案写“q2 90°”你用np.pi/2算出来当然对不上。解决办法是把所有计算统一到弧度最后再转成角度制对比。现象五数值逆解迭代发散关节角飞出天际。原因是当前构型接近奇异或者阻尼系数lam取得太小。解决方法是把lam从0.01往上调同时检查目标位姿是否在工作空间之外。用平面二连杆举例目标点距离基座超过L1L2时任何逆解都不可能收敛这时应该先判断可达性而不是调迭代器参数。现象六动力学仿真在运行到某一步时加速度发生跳变之后波形彻底崩溃。这通常不是控制器写错而是机械臂穿越了奇异位形导致M矩阵在数值上接近奇异M逆计算爆炸。解决办法是在M矩阵求逆之前检查条件数或者改用稳健的线性方程求解器。对习题验证来说直接跳过奇异位形的仿真区间也是一种务实方案。我把这五条做成一个固定检查流程先看符号再看单位再查DH表最后检查是否奇异。做完这四步绝大多数答案对不上的问题都能解决剩下的问题往往是书后答案本身的排版错误。不要过度怀疑自己的能力印刷错误在教材里不算罕见这时反而要用正运动学回代来确认谁是对的。6. 用符号计算反向验证书后答案一个能省几小时的检查习惯对答案环节里最值得养成的一个习惯不要只看“答案数值”而是把答案回代到正运动学或动力学方程里让计算机告诉你对没对。这个方法简单到不值得写成工具库却能把“我觉得对”变成“我验证过对”。以正运动学题为例书后答案给出末端位姿矩阵你通过逆解算出一组关节角这时该做的是把这组关节角重新传入forward_kinematics函数把输出的位姿和题目给的目标位姿求差而不是盯着两个矩阵肉眼对比。下面是一个通用的校验函数def verify_pose_against_answer(fk, answer_joints, target_pose, tol1e-6): 回代校验把习题答案里的关节角代回正运动学与目标位姿对比。 参数 fk 正运动学函数 answer_joints 书后答案给的关节角 target_pose 题目目标位姿 4x4 tol 允许的位置误差阈值 返回 (是否一致, 位置误差, 姿态误差)。 T_check fk(answer_joints) pos_err np.linalg.norm(T_check[:3, 3] - target_pose[:3, 3]) R_diff T_check[:3, :3] target_pose[:3, :3].T rot_err np.linalg.norm(rotation_vector_from_matrix(R_diff)) return (pos_err tol and rot_err tol), pos_err, rot_err这段代码直接用前面定义好的两个工具函数逻辑是把书后答案当作“待检验的假设”而不是“必须对照的真理”。一旦输出一致说明你的解法正确如果不一致分两种情况你的解本身错误或者书后答案的手册里只有一组解而漏了另一组等价解。无论哪种回代都能给你一个明确的反馈信号而不是让你继续陷入“我差在哪里”的反复猜测。对于动力学答案的验证逻辑同理。书后给出M矩阵和G矩阵的符号表达式你可以用同一套DH参数做符号推导然后让SymPy把两者相减并化简。如果差了一个恒等式比如0项sin项完全抵消就能确认两者在物理上等价。用数值回代更快的做法是随机取几个关节角代入两个表达式对比数值是否相同。我自己当年做这类题栽过最大的跟头是二连杆动力学题在求导时漏了链式法则对着答案数值看了一个小时也没发现最后用符号计算回代才抓到。从那以后我每写完一道动力学习题收尾前必做两个动作先用符号推导整齐地输出M和G再随机取三个关节角做数值对比。这个习惯帮我省下的时间远远超过它本身所花费的时间。希望帮到你。本文还有配套的精品资源点击获取