机器人建模与控制习题答案：正逆运动学、雅可比与动力学控制校验

简介：《机器人建模与控制》（Mark W. Spong 等三人合著，2006 年 John Wiley & Sons 出版）的课后习题解答，面向机器人学方向的本科生、研究生与自学者，用于在做完习题后对照核验，检验运动学、动力学与控制理论的实际掌握程度。压缩包内只有 1 个 pdf 文件，约 6.46MB，收录全书各章习题的参考答案，覆盖坐标系与坐标变换、正向与逆向运动学（解析法和数值法）、牛顿—欧拉与拉格朗日动力学建模、编码器与视觉等传感器建模，以及 PID、滑模、自适应、最优控制等控制器设计。对轨迹规划、Lyapunov 稳定性分析、传感器噪声处理与状态估计一类典型题目，答案给出了推导过程与结果，便于理清解题思路、排查建模疏漏。目前已有 3106 人学习下载。

1. 从一道算不下去的习题说起：机器人建模和控制习题答案到底解决什么问题

“机器人建模和控制 习题 答案”这几个词凑在一起被搜出来的人，心里多半有一种说不清的虚：课听懂了，笔记也抄了，可一合上书，那道让自己写满三页纸的题到底对不对，没人告诉。就像照着菜谱做菜，步骤一步没落，出锅前还是想有人尝一口说“盐差一点”。机器人这门课别扭的地方在于，每个概念最后都要落成一串能算的数：关节角、变换矩阵、力矩、增益。推导错了不会报错，只会给你一个长得挺像对的矩阵。所以习题答案的价值不在于替你算，而在于当那个“尝一口的人”，让你知道自己是在哪一步把符号写反了。

2. 答案为什么值得单独存在：从“看懂了”到“算对了”的那道坎

2.1 机器人课的题目为什么特别容易“看懂了却做不出”

课堂上的机器人学是用几何直觉讲的：末端位姿是各连杆变换的连乘，速度通过雅可比映射，力矩由动力学方程给出。每一句听起来都天经地义，甚至有点“这还用讲”的感觉。可一旦落到纸面上，同一个概念就换了一副面孔：连乘里有六个变换矩阵，每个矩阵里塞着 sin 和 cos，角度用的是弧度还是度、旋转是绕当前轴还是绕固定轴、平移放在第四列还是转置过来——任何一个约定错了，最后那个 4×4 矩阵依然会算出一个漂亮的结果，只是它是错的。

写代码有编译器兜底，语法错了立刻红字；做数学推导没有任何反馈机制。你算出一个末端位置 (0.42, 0.31, 0.55)，它不像错的样子，甚至可能刚好落在工作空间里，看着特别合理。这种“错得毫无破绽”是机器人习题最坑人的地方，也是答案存在的第一个理由：它提供了一次外部校验，把“我觉得对”变成“确实对”。

血泪经验通常是这样攒出来的：某位同学做两连杆平面臂的正运动学，DH 参数表里把连杆长度和偏距写反了一行，正解还能勉强凑出一个数，等到算雅可比矩阵、算奇异位形时彻底崩了，回头找了两个小时才发现问题在最开始那张表上。如果中间有任何一步能对照答案，这两小时能省下来。

2.2 一份答案里通常藏着四层信息，多数人只用了第一层

翻开一份习题解答，绝大多数人第一眼看的是最后那个结果：矩阵长什么样、数值等于多少、增益取了多少。这只是最外面的一层，也是价值最低的一层。真正让答案值得反复翻的，是后面三层。

结果层：最终的矩阵、数值、表达式。作用是快速自检，对上了心里踏实，对不上就往下找。

过程层：这道题为什么用解析法而不是数值法，为什么先求腕心再求腕部姿态，为什么把某个关节角固定在零位。这一层回答的是“思路从哪来”，也是区分“会做这道题”和“会做这类题”的分水岭。

约定层：这是最容易翻车也最容易被忽略的一层。同一本教材里，DH 参数有标准型和改进型两种写法，差一个下标就整体错位；旋转矩阵有左乘右乘之争；四元数有实部在前还是在后。答案里如果写清了它采用的是哪套约定，你就能把自己的推导和它对齐；如果没写，你就得从结果反推它用了什么约定——这件事本身就很有训练价值。

边界层：每道题背后都藏着几个没说出口的假设——连杆视为刚体、忽略关节摩擦、电机惯量折算到关节侧、传感器噪声视为零均值高斯。答案在这些假设下成立，换个场景就未必。很多“答案和我的结果差一点点”的困惑，根源就在某个假设上：你算了摩擦，它没算。

打个比方，结果层是菜谱上那张成品照片，过程层是步骤，约定层是“少许盐”到底是多少克，边界层是“这个方子适合几个人吃”。只看照片的人，永远做不出同一道菜。

2.3 教材例题、课后习题、习题答案，三者角色完全不同

很多人把这三样东西当成同一类材料，其实它们的定位差得很远。

例题是“示范动作”，教练慢慢做一遍给你看，球是摆好位置的。习题是“实战”，球会乱飞，还会飞出界。答案则是赛后录像回放，让你看自己哪一步重心歪了。

理解了这三者的差别，就能明白为什么“把答案抄一遍”几乎不产生任何学习效果：抄的是结果层，丢掉的是过程层、约定层和边界层，而后者才是这门课真正要训练的东西。机器人建模与控制这门课的考试和工程实践，考的都是“换一道没见过的题，你还能不能推出那个矩阵”。

提示：拿到一份答案，先别急着对最后那个数。花两分钟看它开头写了什么约定——DH 参数表、角度单位、旋转顺序。这两分钟能省掉后面半小时的困惑。

3. 坐标系与运动学：正解是顺着推，逆解是倒着猜

3.1 四套坐标系，一句话分清各自管什么

机器人学里“坐标系”这个词被反复提及，但不同语境下指的不是同一个东西，这是初学者第一道坎。

基座坐标系（世界坐标系）：固定在地面或工作台上，不随机器人动。它回答的是“东西在车间的哪个位置”。

关节坐标系：每个关节自己的参考系，随关节转动而转动。它回答的是“这个电机现在转到几度”。

连杆坐标系（DH 坐标系）：按 DH 规则挂在每根连杆上的中间坐标系，是推导变换矩阵的工具性产物。它回答的是“从这根杆到下一根杆怎么过渡”。

工具/末端坐标系：固连在末端执行器上，随手爪或焊枪的姿态变化。它回答的是“我的手相对工件是什么姿势”。

任务空间：不是某个具体坐标系，而是描述任务的那组变量，比如末端的三维位置加三个姿态角。它回答的是“我要把工件放到哪、朝哪个方向”。

一个生活化的对应：基座坐标系是城市地图，关节坐标系是你手腕转了多少度，工具坐标系是你手里那支笔的笔尖朝向，任务空间是“把笔尖放到纸上那个叉上”。四者可以互相换算，但混着用就会算出一堆自相矛盾的结果。

3.2 正向运动学：连乘变换矩阵为什么能“串”出末端位姿

正向运动学要回答的问题很朴素：已知每个关节转了多少，末端在哪、朝哪。做法是把相邻连杆之间的变换一个个乘起来。

相邻连杆的变换用四个 DH 参数描述：连杆长度 a、连杆扭角 α、连杆偏距 d、关节角 θ。四个参数串起一个 4×4 齐次变换矩阵，它同时打包了旋转和平移——这正是齐次坐标的妙处，一个矩阵顶两个用。

import numpy as np def dh_transform(a, alpha, d, theta): """单个 DH 参数对应的齐次变换矩阵（标准型）""" 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] ]) # 两连杆平面臂：a1=0.5, a2=0.4，扭角和偏距都为 0 T1 = dh_transform(0.5, 0.0, 0.0, np.deg2rad(30)) T2 = dh_transform(0.4, 0.0, 0.0, np.deg2rad(45)) T_0_2 = T1 @ T2 # 连乘顺序：从基座往末端 print(np.round(T_0_2[:2, 3], 4)) # 末端 x, y

这段代码里三个地方最容易错。第一，@是从基座向末端的连乘顺序，写成T2 @ T1就变成了另一台机器人的解。第二，np.deg2rad不能省，直接把 30 塞进np.cos会得到弧度为单位的荒谬结果。第三，dh_transform用的是标准型 DH，如果教材采用改进型，矩阵里的a和alpha位置要对调，d和theta的顺序也会变——这类差异不会报错，只会让你的答案和书上差一个下标。

正向运动学的好处是它永远有唯一解，而且是“顺着算”，没有任何需要猜的地方。这也是为什么它适合作为校验的第一步：如果正解都对不上，后面雅可比、动力学、控制全都不用看了。

3.3 逆向运动学：为什么答案经常不止一组，甚至一组都没有

逆运动学问的是反问题：末端要到达某个位姿，各关节该转到多少。这是机器人学里第一类真正让人头疼的题，原因有三。

多解。一个六自由度机械臂到达同一个末端位姿，通常有 8 组解——肘上肘下两种、腕部翻转两种、肩部左右两种，组合起来就是 2×2×2。答案里如果只给一组，不代表只有一组，很可能只是“默认取肘上、腕不翻转”的那一组。做题时如果只写一组就交，等于丢了一半分。

无解。目标点在工作空间之外，无论怎么转都够不着。解析法会推到一个cos θ > 1的式子，这时候要有意识地判断“这是无解，不是我算错了”。

奇异位形。在奇异位形附近，雅可比矩阵接近不可逆，关节角对末端位姿极其敏感，数值法会给出一个跳变巨大的解。答案里如果出现“此处需特殊处理”的注释，多半就是在提示这个。

解析法和数值法的区别值得单拎出来说。解析法靠几何关系直接推出关节角的封闭表达式，快、精确、能枚举全部解，但只对特定构型（比如球形腕、三轴交于一点）奏效，换个构型就得重推。数值法靠迭代求解，通用性强，但需要初值、可能收敛到局部解、在奇异位形附近不稳。答案里两种方法常同时出现，前者给思路，后者给验证。

3.4 雅可比矩阵：把关节速度和末端速度连起来的那根轴

雅可比矩阵是运动学里承上启下的东西。它做的事就一件：把关节空间的速度映射到任务空间的速度，v = J·q̇。行数是任务空间维数，列数是关节数。

它有两个“副作用”特别重要。其一，雅可比转置把末端力映射成关节力矩，τ = Jᵀ·F，这是静力学的基础，也是力控的起点。其二，雅可比的行列式为零时，机器人处于奇异位形——某个方向突然失去了运动能力，就像人的手臂完全伸直时，再想往回缩只能靠肘，靠肩是推不动的。

答案里的雅可比题通常要求写出 6×n 的完整矩阵，按“线速度部分三行、角速度部分三行”分块。初学者常犯的错是把角速度那三行的参考坐标系搞混：有的教材要求相对基座坐标系，有的要求相对末端坐标系，两者差一个旋转矩阵，答案的数值会完全不同但都“看起来对”。

4. 动力学与控制：答案在验算的其实是“力矩够不够、抖不抖”

4.1 动力学建模：牛顿-欧拉和拉格朗日只是两条路，终点是同一个方程

运动学只关心几何，动力学关心的是力和惯量。要让机器人动起来，得知道每个关节需要多大扭矩，而这个扭矩跟连杆质量、转动惯量、质心位置、科氏力、离心力、重力全有关。

两条经典路线。牛顿-欧拉法走的是递推：先从基座往末端推速度和加速度，再从末端往基座推力和力矩。它像记账，一笔一笔算清楚每个连杆上受了什么力，物理图像直观，适合写代码，计算量随关节数线性增长。拉格朗日法走的是能量：写出总动能 K 和总势能 P，代入 L = K − P，再对每个广义坐标求偏导。它像从高处俯瞰，不用管内部每一个力怎么传递，只关心整体的能量账本，适合手推和理解结构。

两条路算到最后一定是同一个方程：M(q)q̈ + C(q,q̇)q̇ + g(q) = τ。M 是惯性矩阵，描述“加速有多难”；C 项包含科氏力和离心力，描述“转起来之后自己被自己带偏”；g 是重力项，描述“不动的时候也得使劲撑着”。答案里如果只给最终表达式，如果不检查 M 矩阵是否对称正定、C 项是否在 q̇ = 0 时为零，就很难判断这个结果对不对。

4.2 控制题在答案里长什么样：PID、滑模、自适应各自解决什么

动力学算完，下一步是让机器人按预期动。控制题的答案通常分三块：控制器结构、参数取值、稳定性证明。

PID 是最基础的一块。P 项管“现在还差多少”，像拉橡皮筋，差得越远拉得越狠；I 项管“过去一直差多少”，像记忆，专门消灭那些靠 P 消不掉的稳态误差；D 项管“差距变化有多快”，像刹车，防止冲过头。三项放在一起，本质是在“追得快”和“不超调”之间找平衡。

class PID: def __init__(self, kp, ki, kd, dt, tau_max): self.kp, self.ki, self.kd = kp, ki, kd self.dt = dt self.tau_max = tau_max # 关节扭矩上限，防积分饱和 self.integral = 0.0 self.prev_error = 0.0 def step(self, target, actual): error = target - actual self.integral += error * self.dt derivative = (error - self.prev_error) / self.dt self.prev_error = error output = self.kp * error + self.ki * self.integral + self.kd * derivative # 抗积分饱和：输出被限幅时停止积分累加 if abs(output) > self.tau_max: self.integral -= error * self.dt output = max(-self.tau_max, min(self.tau_max, output)) return output

这段代码里tau_max限幅和积分回退是工程上最容易被忽略的两行。纯理论题不会考，但真机上没有这两行，机械臂会在启动瞬间积出一个巨大的积分项，然后猛地甩出去。答案里的 PID 参数往往是“能证明稳定”的一组，不是“实际跑得好”的一组，两者之间的差距要靠仿真和实测填。

滑模控制的思路不一样：先设计一个滑模面，用一个大到足以压住所有不确定性的切换项，把系统强行推到面上，再让它沿着面滑向目标。优点是抗扰强、对模型误差不敏感，代价是切换项带来的抖振——这也是“抖”这个字在控制题里反复出现的原因。自适应控制则更进一步，假设某些参数（比如负载质量）未知，一边在线估计一边控制，答案里会多出一组参数更新律。

4.3 轨迹规划与稳定性：答案里为什么总出现那个 V(x)

轨迹规划题问的是：从 A 点到 B 点，中间怎么走。三次多项式能保证位置和速度连续，五次多项式能再加一条加速度连续——这一点在实机上很关键，加速度不连续意味着力矩突变，力矩突变意味着机械结构要挨一下冲击。

稳定性题问的是：这个控制器到底能不能把误差收到零。李雅普诺夫方法是最常见的工具，思路是构造一个标量函数 V(x)，它代表系统的“广义能量”，只要证明 V 正定、V̇ 负定，系统就稳定。打个比方：碗里放一颗小球，不管从哪个位置松手，它最终都会停在碗底，因为重力势能一直在降低。V 就是这个势能，V̇ 为负就是“能量一直在掉”。

答案里出现的 V 通常长这样：V = ½xᵀPx + ½eᵀΓ⁻¹e，前一项是状态误差的二次型，后一项是参数估计误差的二次型。看到这种结构，基本可以判断这是一道“自适应控制 + 李雅普诺夫”的组合题，解题路径是先求 V̇，再把控制律代进去，让交叉项相互抵消。

5. 常见误区与易混概念：五组长得像、坑却完全不同的词

5.1 运动学解和动力学解，不是同一道题的两个版本

易混点：两者都叫“解”，都会输出关节角或关节力矩，都带一堆三角函数。

为什么混：教材章节挨着排，题目形式又都是“已知某某，求某某”，初学者容易把运动学题当成动力学题的前半段。

怎么区分：看题目里有没有出现质量、惯量、力、力矩、时间。运动学只跟几何有关，输入输出是位置、姿态、速度，跟多重、多快加速完全无关；动力学一旦出现 m、I、τ、q̈ 这些符号，就进入了力的世界。一个直观判据：把机器人放到失重环境里，运动学答案一个字都不用改，动力学答案全变。

5.2 关节空间和任务空间，混着用会算出“对但没用”的结果

易混点：两个空间都能描述机器人的状态，都能画轨迹，都能做插值。

为什么混：正运动学就是把关节空间映射到任务空间，来回换算太顺手的，顺到手滑就忘了自己在哪个空间里。

怎么区分：问自己“这个量是电机直接控制的，还是任务直接要求的”。关节空间里的量是每个电机的角度，插值出来的是电机转速均匀；任务空间里的量是末端的位置姿态，插值出来的是末端走直线。在关节空间做直线插值，末端画出来的是一条弧线，这在焊接、涂胶这类要求末端走直线的场景里是致命的。答案里如果给了轨迹方程却没说是哪个空间，先看变量名：带 q 的是关节空间，带 x、y、z 的是任务空间。

5.3 正运动学和逆运动学，难度是“倒挂”的

易混点：很多人直觉认为“正”是正向推导所以难，“逆”是反推所以简单，或者反过来。

为什么混：“正/逆”这两个字带有方向暗示，让人误以为难度也跟着方向走。

怎么区分：正运动学永远有唯一解，顺着矩阵连乘就行，难度低但繁琐；逆运动学可能多解、可能无解、可能遇到奇异，难度高且需要分情况讨论。答案里如果逆解只写了一个解，八成是省了“另外几组由对称性给出”这句话。判断一道题是不是逆问题，看已知量：已知关节角求位姿是正解，已知位姿求关节角是逆解。

5.4 稳定性、收敛性、鲁棒性，三个词经常被当成同义词用

易混点：三个词都描述“系统表现好不好”，答案里的结论句也常常混着写。

为什么混：在实际系统里它们经常同时成立，导致初学者以为是一回事。

怎么区分：稳定性说的是“受扰后能不能回到平衡点”，收敛性说的是“误差能不能随时间趋于零”，鲁棒性说的是“模型有偏差、有扰动时性能还能不能保住”。一个系统可以稳定但不收敛（在平衡点附近振荡不衰减），可以收敛但不鲁棒（模型稍微一变就发散）。答案里的李雅普诺夫证明通常只证稳定性，如果不额外说明“对参数摄动的界”，就不能顺手写成“鲁棒”。

5.5 传感器建模和状态估计，前者是“描述噪声”，后者是“对抗噪声”

易混点：两者都出现在带传感器的题目里，都涉及概率和噪声。

为什么混：教材常把编码器、激光雷达、视觉的建模和卡尔曼滤波放在相邻章节，题目里也常常一起出现。

怎么区分：传感器建模问的是“这个传感器输出的数据长什么样”，答案是一个带噪声项的观测方程，比如 z = h(x) + v，v 服从某个分布。状态估计问的是“怎么从这些带噪声的数据里把真实状态推出来”，答案是滤波器结构、增益矩阵、更新步骤。前者是问题定义，后者是求解方法。搞反了就会出现“用卡尔曼滤波去建模一个编码器”这种答非所问。

6. 什么时候该翻答案，什么时候该合上：一条可执行的使用边界

答案这东西，用得对是加速器，用得不对是拐杖，而且是一根会让你在考场上原地摔倒的拐杖。下面这张表是判断边界的粗糙但好用的标准。

一个具体的例子。某高校机器人课程里，A 同学做逆运动学作业，六自由度臂，要求给出所有可行解。他推出两组，交之前翻了答案，发现答案列了八组。他没有直接抄，而是回头找自己漏在哪：原来他只考虑了肘上肘下，忘了腕部翻转还有两种，肩部左右又翻了一倍。这个“找漏”的过程花了他四十分钟，但下次遇到七自由度冗余臂的题，他自己就能枚举全。反过来，如果当时他把八组抄上去，考试换一个构型，他还是只会写两组。

另一个方向是“什么时候别用”。当你在建立最初的空间直觉时，答案是最没用的东西。第一次接触雅可比、第一次看李雅普诺夫函数、第一次推动力学方程，这些时刻需要的是把图画出来、把物理意义想明白，而不是对着一个已经写好的矩阵点头。点头的那一刻感觉懂了，其实只是“看懂了别人的结果”，离“自己能写出来”差着十万八千里。

注意：答案里的参数值、增益、初值都是为那道题量身定的，换个工况直接搬过去，翻车是大概率事件。

从那以后我每次用习题答案，都强制走一遍“先推完、再对照、最后复盘差异”的流程，哪怕推得很慢、错得很难看。慢的那半小时，往往比看答案的十分钟记得牢。希望帮到你。

本文还有配套的精品资源，点击获取