如果你正在啃一门叫《Introduction to Robotics》的课，前六讲大概率是你最容易产生“我会不会不适合学机器人”念头的阶段。我最近把第1到第6讲完整整理了一遍，写成了一份可以边学边查的模块化笔记。这篇总结不严格按照某一本教材的目录走，而是按知识模块拆成六块：空间描述与坐标变换、正运动学、逆运动学、雅可比矩阵与静力学、轨迹规划、动力学入门。不管你是在校学生、刚入行的机械臂工程师，还是自学爱好者，这份内容都可以当作一张路线图，帮你搞清楚每章到底在解决什么问题、公式是怎么来的、工程上真正会用到哪些部分。
我的经验是，很多人不是学不会机器人学，而是被第一章的矩阵搞到心态失衡，之后每章都像浮沙建塔。所以这篇总结里，我会把每一块的“为什么”放在“是什么”前面，尽量解释公式背后的物理意义和常见坑。真正动手推公式还是省不了的，但至少你看推导的时候能知道自己在找什么。
1. 空间描述与坐标变换：先学会用矩阵说清“在哪里、朝哪看”
1.1 为什么机器人学的第一课全是矩阵
机器人本质上是一串由连杆和关节串联起来的运动链。要让末端执行器完成某个动作，第一步不是算角度，而是先回答“末端到底在哪里、它的坐标轴朝向哪”。在三维空间里，一个点的位置可以用三个坐标表示，但一个刚体的姿态至少需要三个独立参数，比如欧拉角；机器人的手爪不仅要“到那个点”，还必须“保持某个姿态”，否则抓取、焊接、涂胶都会失败。因此，位置和姿态必须作为一个整体来描述，这个整体就被称为“位姿”。
矩阵是描述位姿最自然的方式。机器人学中常用的齐次变换矩阵T可以写成一个分块形式：
T = [ R p; 0 1 ]
其中R是3x3旋转矩阵，p是3x1平移向量。一个刚体相对基坐标系的位姿，就浓缩在这4x4矩阵里。后面的正运动学、逆运动学、雅可比，全部建立在T和R的基础上。所以第一课出现矩阵，不是故意劝退，而是后面所有内容都离不开这套语言。
1.2 旋转、平移与变换顺序的关键规则
使用齐次变换矩阵的好处是可以把旋转和平移统一成一次矩阵乘法。在一连串变换中，矩阵乘法的顺序极其讲究。假设基坐标系为0，末端坐标系为3，完整变换一般写成：
T_03 = T_01 * T_12 * T_23
从左到右，坐标系索引依次推进。这个写法对应的是“串联链”，意思很明确：先把0系的点变换到1系，再变换到2系，再变换到3系。很多人写反了，往往是交换了相邻两个变换矩阵的左右顺序，结果末端位置差了十万八千里。
一个直观类比：你站在房间里，先向前走1米再向右转90度，和先向右转90度再向前走1米，最后的位置和朝向都不同。连续坐标变换本质上就是这样，每一步的平移和旋转都依赖当前坐标系，顺序不能调换。这里还有一条关键规则：绕固定坐标系的旋转，变换矩阵左乘；绕运动坐标系自身的旋转，变换矩阵右乘。练习时，我建议先把每个关节坐标系画出来，再按相邻关系写矩阵链，最后用特殊角度代入验证，而不是直接心算连乘结果。
1.3 练习坐标变换时的两个实用习惯
第一个习惯：做题前先把坐标系的z轴都画出来。机器人学里通常把关节旋转轴放在z轴方向，所以z轴的朝向基本决定了DH参数里的θ和α。画图时不要只画连杆，要画出每个坐标系的x、y、z轴；确定x轴方向时，尽量顺着相邻z轴的公垂线方向。这样后续写变换矩阵时，每个矩阵的符号都不容易出错。
第二个习惯：用特殊角度验证。随便找一个零位或90度的位形，直接手算出应该到达的坐标，再拿公式代入。如果能对上，基本说明矩阵链没有写反；对不上，就逐级检查平移分量和旋转方向。用Python的时候，可以像我一样写一个极小的矩阵构造函数，每次只验证一个变换。
import numpy as np def rotz(theta): c, s = np.cos(theta), np.sin(theta) return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) def transform(R, p): T = np.eye(4) T[:3, :3] = R T[:3, 3] = p return T # 绕z轴旋转90度，再沿基坐标x轴平移1米 T1 = transform(rotz(np.pi/2), [1, 0, 0]) print(T1)这个例子把“先旋转后平移”的矩阵构造出来，平移p是在基坐标系下的分量。实际项目中，我建议把这类小工具放在自己的工具函数库第一页，后面验证正解、逆解都会用到。
2. 正运动学：从关节角到末端位姿的“唯一解”
2.1 正运动学是在解决什么问题
正运动学的输入是n个关节角，输出是末端坐标系在基坐标系下的位姿。对于任意确定的多连杆结构，只要几何参数已知，从关节角到末端位姿就是一个确定的、唯一的函数。换句话说，给定q，末端位姿只有一个答案，不会出现两种解。这种“唯一性”是正运动学相对逆运动学最简单的地方，也让它成为验证机器人模型的黄金标准。
我在实际工程里第一次用正运动学，是想在示教器上实时显示末端坐标。控制器从电机编码器读回每个关节角，然后调用正运动学接口算出一个4x4矩阵，界面上的x、y、z坐标其实就是矩阵右上角平移向量的三个分量。这个流程每秒要跑几百次，所以正运动学必须算得足够快。这就涉及DH参数一个很大的好处：把复杂的几何关系变成统一的矩阵连乘，代码可以用循环处理，而不是手写每个连杆的几何公式。
2.2 DH参数建模：四个数字讲清一个机器人
DH参数是描述相邻关节轴关系的经典方法。每个连杆有两个参数，每个关节也有两个参数，合起来四个参数：关节角θ、连杆偏距d、连杆长度a、连杆扭转角α。标准DH的变换顺序是沿z轴旋转θ、沿z轴平移d、沿x轴平移a、沿x轴旋转α，写成矩阵是：
T = Rot(z, θ) * Trans(z, d) * Trans(x, a) * Rot(x, α)
这里必须强调一句：不同资料里DH有两种常见规范，标准DH和改进DH。改进DH的变换顺序通常是沿x轴平移a、沿x轴旋转α、沿z轴旋转θ、沿z轴平移d，坐标系定义也不一样。很多同学从网上下载一个机器人的模型文件或DH表，却不知道它是按哪种规范建的，结果正运动学算出来完全对不上。严格做法是看文档里的约定，或者自己用一根已知结构的小机械臂标定一遍。
四个参数可以这样理解：a是相邻两关节轴之间沿公垂线方向的距离，α是两轴之间的扭角，d是沿关节轴方向的偏置，θ就是关节绕轴的旋转角度。直观上，这组参数就是在告诉算法“下一个坐标系该怎么摆”。一旦你拿到一张DH表，正运动学就变成了一段循环代码，不再需要从头推导每个连杆的空间关系。
2.3 手推一个平面二连杆，验证你对建模的理解
用二连杆来练习。设连杆1长度l1，连杆2长度l2，两个关节角分别为q1、q2。平面二连杆的末端位置可以直接用几何法写出来：
x = l1*cos(q1) + l2*cos(q1+q2)
y = l1*sin(q1) + l2*sin(q1+q2)
再用DH法走一遍：固定l1和l2的长度，α都是0，d都是0，θ分别是q1和q2。代入标准DH矩阵，连乘两次，一样能得到上面的x、y。这个推导过程是检验你是否真正理解DH参数的最佳方式。如果算出来多一个符号或差一个平移分量，说明坐标系定义或者x轴方向选错了。
这里的实操建议是：不要只背二连杆的公式，而是练习把式子里的每一项对应到几何图的某一段。比如l2*cos(q1+q2)，就是指第二根连杆在基坐标系x轴上的投影。能画出这个投影关系，后面学习六轴机器人的位置正解就不会那么慌。
3. 逆运动学：从末端位姿反推关节角，难度开始跳变
3.1 多解、无解、奇异：逆解三大拦路虎
逆运动学的问题描述只有一句话：给定末端位姿，求关节角q。但这句话背后是非常麻烦的非线性方程组。首先是多解，同一个末端位置往往对应多个关节组合，平面二连杆就有“肘上”和“肘下”两组解，六轴机器人更是可能有8组甚至更多解。其次是可能无解，目标点如果超出机械臂工作半径，任何一组关节角都到不了。工程上会提前做可达性判断，不再浪费算力求解。最后是奇异位形，机械臂在某些姿势下会丢失某个运动自由度，此时即使末端只有微小移动，也可能对应关节速度的剧烈变化。
所有这些问题都在告诉我们，逆运动学不是一个求唯一答案的问题，而是一个“在所有可行解里做选择”的优化问题。初学者写逆解，最容易犯的错就是把一个解析解当成唯一解，接上轨迹规划后却发现关节在路径中间突然跳一大截。先接受“多解”这个事实，后面会顺很多。
3.2 几何法逆解二连杆：公式与选择逻辑
继续用平面二连杆。已知末端位置(x,y)，两个连杆长度l1、l2。根据余弦定理，可以解得第二个关节角q2：
cos(q2) = (x^2 + y^2 - l1^2 - l2^2) / (2*l1*l2)
注意判别式要满足x^2 + y^2 <= (l1+l2)^2，否则目标不可达。由于cos函数在0到π内对应两个角，可以用q2 = ± arccos(...)得到两组解，正负分别对应肘上肘下。然后第一个关节角由几何关系得到：
q1 = atan2(y, x) - atan2(l2*sin(q2), l1 + l2*cos(q2))
这里使用atan2而不是atan，是为了保留象限信息，避免出现正负结果互相抵消的问题。计算到手后，再根据双解中哪一个更接近当前关节角、哪一个不撞到环境物体，来选择最终解。这个“选择逻辑”比解方程本身更重要，因为它才是工程里真正要写进代码里的部分。
3.3 工程上怎么选逆解，怎么避开奇异
实际控制器的选择策略通常不是全局最优，而是“足够好”：在所有逆解里，选择离当前关节角最近的一组，让每个关节的移动量尽量小。这样做的好处是运动连续，计算也简单。但只做“最近解”还不够，还必须叠加关节限位检查和奇异判断。
关节限位检查相对简单，比较每个解的q是否在传感器允许范围内。奇异判断则要用到雅可比，也就是下一章的内容，这里可以先记住一个现象：在奇异位形附近，用逆解算出的关节速度会非常大，尤其当期望末端运动方向沿着机械臂不可动的方向时。我处理奇异的方式是：在笛卡尔轨迹规划前先扫描路径点，计算可操作性度量；一旦低于阈值，把直线路径拆成两段，中间用一个“绕行”的过渡点来改变末端运动方向，避免迫使关节速度逼近无穷大。这个方式虽然不优雅，但在很多项目里比调半天奇异规避算法更快生效。
4. 雅可比矩阵与静力学：速度与力之间的“桥梁”
4.1 雅可比矩阵：关节速度与末端速度的映射
到了这一章，机器人学才真正从几何进入运动分析。雅可比矩阵J连接的是关节空间速度和操作空间速度：
v = J * dot(q)
其中v通常是一个6维向量，包含末端的线速度和角速度，dot(q)是关节角速度向量。J的维度是6×n。J不是常数，它随当前q变化，所以每到一个新位形，雅可比都要重新求值。
对于平面二连杆，如果只关心末端位置，可以忽略姿态，得到一个2×2的位置雅可比，直接由末端位置关于q1、q2求偏导得到。这个例子特别适合写代码验证：随便给一组关节速度和当前关节角，用雅可比算末端速度，再把两个关节角各加一个微小增量、用正运动学差分算末端速度，两者应该非常接近。这同时也是检验雅可比推导是否正确的通用手段，叫数值雅可比对比。
4.2 静力映射与J^T：虚功原理告诉你的机械增益
雅可比不仅描述速度，还通过转置关系描述了力和力矩的映射。假设末端受到外力F，为了保持平衡，各关节需要施加的力矩tau为：
tau = J^T * F
这个式子来自虚功原理：末端做的虚功等于关节做的虚功。J把关节速度映射到末端速度，J^T则把末端力映射到关节力矩，这是一种对偶关系。可以把它想象成一个机械放大机构：如果机械臂处于不好的位形，J^T的某些分量很小，那么末端受一点力都会在关节上产生很大的力矩需求，电机可能直接过流报警。
我在做力控和负载估算时，经常用这个式子做快速判断：想抓一个10kg的重物，末端力大约已知，然后看当前位形下各关节力矩是否在电机额定范围内。这个方法不需要仿真，计划阶段就能淘汰很多不安全的抓取方案。要注意的是，F必须表示在末端坐标系或者基坐标系，并与关节力矩的维度统一，不能混着用。
4.3 可操作性度量与奇异预警的实操用法
可操作性度量是一个很有用的标量指标，定义通常是：
w = sqrt(det(J * J^T))
在机械臂处于非奇异位形时，w大于0；接近奇异时趋向于0。用这个指标可以在规划轨迹时对每个路径点做一次扫描，如果w小于某个阈值，则提前报警。这个阈值没有标准值，更多是依据具体机构试验标定。比如某实验室的六轴机器人，w低于0.02时会明显感觉末端运动变“硬”，我一般就把阈值设在这里。
使用雅可比还有几个容易踩的坑。一个是单位问题，关节角速度必须统一用弧度每秒，线速度与角速度的单位也各自要一致。另一个是数值雅可比和解析雅可比的差异，如果推导用的是旋转矩阵的微分形式，得到的角速度雅可比可能与常见的几何雅可比差一个坐标系旋转，需要额外转换，不能直接把公式套过来。
我自己的习惯是，在所有需要雅可比的地方都留一个单元级别的小测试：给一个随机关节角和一个随机关节速度，用解析雅可比和数值差分对比，误差在千分之一以内才认为这个雅可比可用。这样能避免很多“算法写着写着不知道错在哪里”的调试时间。
5. 轨迹规划：让机械臂不光能到，还要走得“有规矩”
5.1 关节空间规划：最常用的默认路径方案
轨迹规划的目标可以概括为：给末端或关节设计一条随时间变化的光滑曲线，并保证速度、加速度在允许范围内。最常见的方案是关节空间规划：先对路径点做逆运动学，得到每个关节在一系列路径点上的角度值，然后对每个关节单独规划一条q(t)曲线。
关节空间规划之所以是默认方案，是因为它不用在中间点反复求逆解，也就少了很多“够不到”“跳变”“奇异”的麻烦。而且它的计算量小，非常适合嵌入式控制器。缺点是末端在两点之间走的不一定是空间直线，而是由关节运动合成的曲线。如果工艺要求末端走直线，比如涂胶、焊接，才需要换用笛卡尔空间规划。
一个常用技巧是引入归一化时间s(t)，让所有关节共用同一个时间参数。这样虽然每个关节有自己的q(s)函数，但时间轴完全一致，末端路径不会出现某个关节先到、另一个关节还在晃的情况。实际写代码时，我会先把总运动时间和梯形速度约束算出来，再按统一的时间采样每个关节的位置。
5.2 三次多项式与五次多项式的取舍
三次多项式是最简单的插值方法，刚好用起点、终点的位置和速度四个约束确定四个系数：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3
给定t0、tf时的q和dot(q)，可以解出a0到a3。这种曲线光滑，位置和速度连续，但加速度在起点和终点并不是零，所以机械振动和冲击比较明显。比较在意平稳性的场景，我会用五次多项式，它在三次的基础上还得满足起止加速度约束，一共6个约束确定6个系数：
q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5
五次多项式的加速度连续，轨迹更平滑，代价只是多解几个线性方程，在现在的控制器上完全不是负担。所以除非是在特别老的平台上节省计算，否则我更推荐默认五次多项式。当然，很多工业控制器里真正用的是梯形速度曲线或S型速度曲线，但理解多项式插值是理解这些分段曲线的基础。
5.3 笛卡尔空间直线插值：好看但容易踩坑
笛卡尔空间直线轨迹对用户最友好，指定起点和终点位置，希望末端沿直线运动。但它有三个典型坑，我逐一说明。
第一，中间点需要实时逆解。即使路径点有限，每个中间点都可能出现多解，如果不做连续选择，关节角会突然跳变。第二，直线穿过奇异位形时，某些关节速度会爆炸，控制器限幅后末端轨迹变成一条弯曲曲线，失去“直线”的意义。第三，姿态插值不能用欧拉角直接线性插值，因为欧拉角不是线性空间，中间姿态会绕出不可控的路径。
我实操时，会先在笛卡尔空间以固定步长采样，比如每5毫米一个点，然后对每个采样点进行逆解，并检查关节速度、加速度、可操作性和关节限位。任何一项超限，就缩小步长或改变路径形状。末端姿态的插值则优先用四元数slerp，避免欧拉角的万向锁问题。这套方法虽然比纯关节空间规划多了几步，但能提前发现很多真机上了才会暴露的问题。
6. 动力学入门：从“怎么动”到“怎么发力”
6.1 运动学到动力学，缺了什么
前五章都在讨论几何和速度，机器人仿佛是由无质量连杆组成的理想模型。但现实中，一个机械臂要运动，电机必须输出力矩去克服重力和惯性，而且各关节之间存在耦合：转动第二个关节时，会带动第一个关节额外受力。如果不考虑这些，控制器给的力矩往往不够，或者运动轨迹严重偏离预期。
动力学模型的一般形式可以写成：
tau = M(q) * ddq + C(q, dot(q)) * dot(q) + G(q) + tau_friction
其中M是惯量矩阵，C包含科里奥利力和离心力，G是重力项，tau_friction是摩擦项。这个方程建立了关节力矩和关节运动之间的因果关系，也是很多高级控制算法的基础。
6.2 拉格朗日方程：用能量统一建模
动力学模型有多种方法得到，课程里最常见的是拉格朗日方程。拉格朗日函数L等于系统总动能K减去总势能P，然后对每个关节坐标q_i使用：
d/dt (∂L/∂dot(q)_i) - ∂L/∂q_i = tau_i
这个方法的优点是系统化：只要写出每条连杆的动能和势能，机械求导就能得到运动方程，建模是一套理性流程，不容易漏项。缺点是推导过程烦琐，二连杆都会写满一页纸。我建议至少完整推导一遍平面二连杆的拉格朗日方程，不需要背最后的结果，而是体验“动能-势能-求导”的流程，理解每一项在方程里的来源。比如速度耦合项为什么会出现，为什么转动第二关节会影响第一关节的受力。对于更复杂的机械臂，工程实现一般会用牛顿-欧拉递推法或现成的动力学库，但从理解角度，拉格朗日仍然是入门的最佳载体。
6.3 动力学模型在控制里的现实角色
有了动力学模型，控制就有了“预报”能力。最常见的是重力补偿：在低速或保持位形时，用G(q)计算出当前位形下各关节为克服重力所需的力矩，把它作为前馈叠加到控制器输出上。这样机械臂即使没有电机出力，也不会因为重力往下坠，拖动示教时会非常轻。
更完整的计算力矩控制器可以直接把动力学模型放进控制律：
tau = M(q) * (ddq_des + KD*e_dot + KP*e) + C + G
如果模型足够准确，这个控制律能把非线性机器人系统线性化，实现更好的轨迹跟踪。但现实中模型误差很大，比如摩擦难以精确建模、负载变化、间隙等，所以纯计算力矩控制反而容易抖。我在实践中更倾向于：用动力学做重力补偿和前馈，用PID做反馈误差修正，留有余量。这也是很多商业控制器在“加强”模式里的做法。
学习这章时，不需要急着手写六轴完整动力学，先把二连杆推导弄明白，再用现成工具验证一个已知机器人的动力学，比如让机器人保持静止、给各关节一个很小的力矩斜坡，看重力项是否和实测力矩趋势一致。这样你对“模型”和“真实”之间的差距会有具体的体感。
7. 学习建议与避坑经验
7.1 每个知识点配一个“最小样例”
学完上面六块之后，最有效的巩固方式不是继续啃下一章，而是给每个概念配一个“最小样例”。所谓最小样例，就是参数少到能用手算、但又保留核心特征的模型。平面二连杆几乎可以覆盖正运动学、逆运动学、雅可比、轨迹规划和简单动力学。只要有一个二连杆的仿真代码，你就能反复验证：变化一个关节角，正解结果对不对；给定一个末端轨迹，逆解和轨迹规划能不能跟上；再让末端受力，关节力矩能不能按J^T换算出来。
用最小样例学习的好处是，公式不会成为飘在空中的符号。比如我在学雅可比时，先写了一个二连杆的2×2雅可比，然后随机给关节速度，比较用雅可比算出的末端速度和用正解差分得到的末端速度。看到两者误差小于千分之一，我才敢去碰六轴机器人的雅可比代码。这个习惯看起来慢，实际上省掉了后面大量调试时间。
7.2 三个我反复踩过的坑
第一个坑是坐标变换的左右乘写反。绕固定坐标系旋转时矩阵左乘，绕自身坐标系旋转时右乘，这个规则我至少错了三次。解决办法是每次推完都拿特殊角度验证。
第二个坑是DH参数规范不统一。网上很多机械臂模型的DH表来源混杂，标准DH和改进DH的矩阵顺序、参数含义都有差异。第一次用某个机器人模型前，我会先看模型文档里的约定，或者用零位位形反推坐标系，确认无误后再写代码。
第三个坑是单位混用。角度制和弧度制混用是机器人学里最常见的低级错误，往往表现为逆解结果看起来合理，但关节速度大得离谱。我的做法是代码入口全部统一为弧度制，所有能输出角度的接口都标明rad。写一个断言在计算前后检查数值范围，成本很低但能拦住很多事故。
写到这里，刚好把第1到第6讲的主线过完了。我自己最深的体会是，机器人学不是靠背公式学下来的，而是靠“反复从问题出发推公式、再回到样例验证”学下来的。前六讲的信息量确实大，但当你发现正运动学、逆运动学、雅可比、轨迹规划、动力学这些模块彼此之间都有清晰的接口关系时，这门课的地基就真的牢固了。如果你手里也有一套自己的二连杆样例代码，不妨把这几章全部在上面过一遍，我觉得效果会比读十篇总结都好。
