很多人看《机器人学导论》前六章最容易出现的状态就是每一步推导都能看懂合上书做题就一片空白。我自己的体会是这门课的前六章不是六个独立话题而是一条非常完整的链路——先用空间变换把机械臂的姿态讲清楚做正运动学把关节角映射到末端位姿再用逆运动学把末端位姿拆回关节角接着通过雅可比矩阵把关节速度映射到末端速度并分析奇异位形再往下就是力与力矩的互相换算最后升级到动力学。这篇总结就按这条链路把 Intro to Robotics 第1到第6部分的核心内容串起来也把我当时在作业和仿真里反复踩过的坑一起写出来给正在看这门课的朋友做个参考。1. 位姿描述与坐标变换所有运动学计算的地基1.1 位置、姿态与位姿机械臂怎么描述“我在哪”机器人学里的第一件事是搞清楚怎么在数学上描述一个刚体。刚体上任意一点可以用一个三维位置向量表示但只有位置不够因为机械臂末端夹爪是有方向的你不能光说“它到了某个点”还得说“它的开口朝着哪个方向”。所以完整描述空间中的一个物体通常用“位姿”这个词它包含两个部分位置以及姿态。位置很好理解就是一个3×1的坐标向量比如在一个基坐标系下写成 [x, y, z]ᵀ。姿态就没那么直接了常见表示方式是3×3的旋转矩阵。旋转矩阵的每一列其实就是被描述坐标系各个坐标轴在原坐标系里的投影。比如 {B} 系相对 {A} 系的旋转矩阵 R_A^B它的三列分别是 {B} 系的 x 轴、y 轴、z 轴在 {A} 系下的方向向量。这个解释看似简单但几乎所有后续推导都要靠它兜底。把位置和姿态合在一起就可以写成4×4的齐次变换矩阵也就是把3×1的位置向量塞进旋转矩阵的右上角最后一行补成 [0 0 0 1]。好处是原本旋转和平移是两种不同运算合并成矩阵后可以统一用矩阵乘法处理多连杆串联时也能一直乘下去。这个“把坐标变换表达成矩阵链”的思路是整门课最核心的思想没有之一。1.2 旋转矩阵的多种理解方式列、行和欧拉角旋转矩阵可以从三个角度理解分别对应不同的使用场景。第一列向量解读。R_A^B 的三列分别是 {B} 的三个坐标轴在 {A} 下的方向向量所以如果已知某个点在 {B} 下的坐标直接乘 R_A^B 就能转到 {A} 下。这个用法最常见比如激光雷达的测量点要转到车体坐标系本质上就是一次旋转矩阵相乘。第二行向量解读。因为旋转矩阵是正交矩阵它的逆就是它的转置。所以 R_B^A (R_A^B)ᵀ。当你要把 {A} 下的坐标转到 {B} 下不需要费劲求逆转置一下就行。很多初学者会在这里绕晕明明旋转变换“往回走”了为什么不是求逆而是转置其实正交矩阵的逆就是转置你求的和转置的是同一个东西。第三欧拉角解读。旋转矩阵本身九个元素实际自由度只有三个所以工程上常用绕固定轴或自身轴的三个角度来描述姿态。这里有个特别容易踩的坑绕固定轴 x-y-z 顺序旋转和绕自身轴 z-y-x 顺序旋转最终得到的矩阵恰好是一样的但中间步骤很容易搞混。我学的时候用过一个笨办法任何旋转都先写成绕某个坐标系的轴转再判断这个轴属于哪个坐标系。绕固定轴就是左乘绕自身轴就是右乘也就是所谓的“左乘右乘”法则。1.3 齐次变换矩阵的链式写法与相对位置齐次变换矩阵 T_A^B 写起来像这样T_A^B [R_A^B, p_A^B; 0 0 0 1]其中 p_A^B 是 {B} 系原点在 {A} 系下的位置。这个矩阵本身不复杂复杂的是多个矩阵连乘时的含义。假设机器人基座是 {0}末端是 {n}那么从基座到末端的变换 T_0^n T_0^1 T_1^2 … T_{n-1}^n。这个链式规则要求前一个矩阵的“目标系”必须是后一个矩阵的“参考系”也就是下标要首尾相连。实际操作中常见的错误是坐标变换顺序搞反。比如已知“工具坐标系相对末端的位置”和“末端相对基座的位姿”要求“工具坐标系相对基座的位姿”正确做法一定是先基座到末端再末端到工具也就是 T_base_tool T_base_end * T_end_tool。如果有人写成 T_end_tool * T_base_end结果虽然矩阵乘得动但物理意义完全错了。这种前缀后缀的追踪能力是机器人学的基本功前几章不好好练后面正逆运动学一定出问题。2. 正运动学从关节变量到末端位姿的一条龙2.1 DH参数四个数字为什么能描述一个连杆正运动学的目标很明确给出一组关节角度算出末端在基座标系下的位姿。如果每个关节之间都用任意坐标变换去写矩阵会乱到没法维护。于是教材里会引入DH参数用四个量描述相邻两个关节轴之间的几何关系。这四个量分别是连杆长度、连杆扭转角、关节偏距、关节角。按常见约定连杆长度是沿上一关节的x轴方向测量的距离连杆扭转角是绕该x轴从上一个z轴转到当前z轴的角度关节偏距是沿当前关节z轴方向的距离关节角则是绕当前z轴转过的角度。前两个参数描述的是连杆本身的几何形状后两个参数描述的是相邻连杆之间的相对位置。这种拆分方式让每个连杆的建模都变得非常机械照着机器人结构图把四个数填进表格然后套公式出矩阵就行。用DH表建模时最考验人的是坐标系怎么摆。关节轴是旋转轴线x轴要定义为相邻z轴的公垂线方向z轴通常沿着关节运动方向。刚开始做练习时我总忍不住把坐标系画得“很舒服”结果建出来的DH表对不上公式。后来我养成了一个习惯先不管美观按“z轴顺着旋转轴、x轴沿着公垂线、y轴用右手定则补完”的步骤一步步摆坐标系画得再丑也没关系参数反而不会错。2.2 相邻构件变换矩阵的构造逻辑有了四个DH参数相邻坐标系变换就可以写成固定套路。按某一种常用的D-H约定从一个连杆坐标系变换到下一个连杆坐标系需要依次完成绕x轴旋转连杆扭转角、沿x轴平移连杆长度、绕z轴旋转关节角、沿z轴平移关节偏距。把这四个动作各自写成4×4矩阵再按顺序相乘就得到相邻坐标系变换矩阵。这里必须提醒一句目前常见的D-H约定有两种写法细节有所差异有的教材是先绕z轴转再沿x轴移有的是先绕x轴转再沿z轴移。如果你同时看两本书千万别直接把公式抄混。一个简单有效的办法是先看教材里对a、d、α这几个符号的定义再推导一遍它的乘积顺序确认无误后再用。我以前就是因为混用约定导致同一个机械臂建出来的末端位置差了好几厘米。有了相邻变换矩阵正运动学就变成纯粹的矩阵连乘。从基座到第一个关节再到第二个关节一路乘到末端工具坐标系整个过程在编程上就是一个for循环。很多同学觉得正运动学简单因为公式固定、结果唯一但真正到第三章逆运动学你就会发现正运动学如果不过关逆解连调试的基准都没有。2.3 平面两连杆机械臂的手算示范举一个最经典的例子平面两连杆机械臂两个旋转关节连杆长度分别为 L1 和 L2。假设关节变量是 θ1 和 θ2第二个关节角度是相对第一个连杆的角度那么末端位置很容易写成x L1 cosθ1 L2 cos(θ1θ2) y L1 sinθ1 L2 sin(θ1θ2)这个结果既可以用几何投影直接看出来也可以用齐次变换矩阵算出来。把 T_0^1 Rot(z, θ1)T_1^2 Trans(x, L1) * Rot(z, θ2)然后乘到末端点 [L2,0,0,1]ᵀ得到的x、y表达式和上面完全一致。别看这个例子简单它可以说是整个正运动学的缩影关节角是输入末端位姿是输出中间一切变换都靠矩阵连乘实现。我在仿真里做运动学验证时经常先用这种平面两连杆算一个数值例子再放到三维机械臂模型里跑。因为二维情况可以手工验证结果如果程序结果和手算不一致说明问题大概率出在DH参数或矩阵顺序上而不是后面的大系统。3. 逆运动学从末端位姿倒推关节角3.1 解的存在性、多重解与奇异点逆运动学是正运动学的反向问题已知末端位姿求关节变量。看似只是反过来算实际上难了不少因为同一个末端位姿可能对应着好几组关节角也可能一组都没有。先说解的存在性。机械臂每个关节都有行程限制末端能到达的位置集合称为可达空间在这个空间内部的点也未必能以任意姿态到达需要考虑姿态可达的灵巧空间。如果目标位姿跑到可达空间之外那逆解无解。判断一个点是否可达最简单的方式就是看它到基座的距离是否在两个极限长度范围内。再说多重解。平面两连杆就是一个非常直观的例子给定末端位置肘部可以向上弯也可以向下弯对应两组解。有些六轴机械臂甚至会有八组解。解多不代表任意选一组都能用你还要考虑关节限位、避障、以及运动连续性。一般做法是有选择性地从多组解里挑最合适的一组比如“离当前姿态最近的那组”避免机器人从大老远绕一圈过去。逆解里还有一个很麻烦的概念叫奇异位形。在某些关节角组合下末端速度方向会受到限制某些方向怎么动都动不了这就是奇异。奇异点附近逆解会非常敏感微小的末端位移会导致关节角突变。这个内容其实更接近后面的雅可比章节但在逆运动学里第一次遇到时就要有心理准备数值迭代时如果机械臂走到奇异点附近收敛就会出问题。3.2 平面两连杆的几何法求解流程对于平面两连杆机械臂逆解用几何法就能算不需要解高次方程。过程我建议背下来因为它是很多六轴解法的雏形。已知末端坐标 (x, y)两杆长 L1、L2。令末端到基座的距离平方为 r² x² y²。根据余弦定理第二个关节角满足cosθ2 (r² - L1² - L2²) / (2 L1 L2)由此得到 θ2 atan2(±√(1 - cos²θ2), cosθ2)正负号对应“肘上”和“肘下”两种解。然后求第一个关节角从基座指向末端的方向角是 atan2(y, x)但第一个连杆不一定正好指向末端因为第二根杆会偏转一定角度这个偏转角可以算出来。于是θ1 atan2(y, x) - atan2(L2 sinθ2, L1 L2 cosθ2)注意全程都用 atan2而不是 arctan因为 atan2 能正确处理四个象限。我见过不少同学偷懒写成普通反正切结果角度落在象限边界时莫名其妙差 180°。这个多花两秒的小细节能省下大量调试时间。3.3 六关节机械臂的逆解策略与数值迭代方法实际工业机械臂大多有六个旋转关节。如果最后三个关节的轴线交于一点也就是所谓“球形腕”问题会被大大简化。原因是末端姿态可以拆成腕心位置和腕部姿态两部分腕心位置由前三个关节决定腕部姿态由后三个关节决定。于是六自由度逆解被拆成两个三自由度问题前三个关节负责把腕心送到目标点后三个关节负责把末端坐标系转到目标姿态。这种结构上的可解性不是所有机器人都有如果你的机器人构型不满足或者目标位姿比较复杂那就得靠数值迭代。数值迭代的基本思路和牛顿法解方程类似先给一组关节角初值用它算正运动学得到末端位姿再计算当前末端位姿和目标位姿之间的差然后用雅可比矩阵的逆或伪逆把这个差值换算成关节角修正量反复迭代直到误差小于阈值。数值逆解的坑主要集中在初值上。初值离真实解太远迭代很容易收敛到另一组解甚至发散。我在仿真里处理过一台构型比较特殊的机械臂换一组初始角同样目标位姿能收敛到完全不同的姿态。后来我的做法是先用解析法求一组粗略解作为初值再用数值法细化如果没有解析解就保存上一时刻的关节角作为当前时刻的初值保证连续轨迹上的解是平滑的而不是跳来跳去。4. 雅可比矩阵微分运动、速度与奇异位形4.1 雅可比矩阵到底在映射什么如果说正逆运动学描述的是“位置级”的关系那雅可比矩阵描述的就是“速度级”的关系。它把一个关节速度向量映射到末端空间速度向量数学上写成v_e J(q) q̇其中 v_e 是末端的线速度和角速度组合成的六维向量q̇ 是关节速度向量。雅可比矩阵是关节角 q 的函数不是常数矩阵所以机械臂在不同姿态下同样的关节速度会产生完全不同的末端速度。从计算角度理解更有意思雅可比矩阵的每一列就是某个关节单独运动时末端产生的速度。对一个旋转关节它产生末端线速度的方向是 z_{i-1} × (p_e - p_{i-1})也就是关节轴方向向量和从关节轴指向末端的位置向量做叉积产生角速度的方向就是 z_{i-1} 本身。对移动关节它的贡献会更直接相当于沿移动轴方向产生一个线速度。按列拼起来就是完整的雅可比矩阵。这个矩阵还有一个和静力分析直接相关的性质关节力矩和末端力之间的映射用的是雅可比矩阵的转置不是逆。这一点非常反直觉刚开始我经常搞混以为力要从末端到关节就应该求逆。实际上关系是 τ Jᵀ F具体推导会放到静力分析那一节讲这里先记住这个对应关系后面用起来就顺了。4.2 平面两连杆机械臂的雅可比矩阵与奇异位形继续用两连杆平面臂算一下雅可比矩阵。末端位置前面已经写过x L1 cosθ1 L2 cos(θ1θ2) y L1 sinθ1 L2 sin(θ1θ2)对 θ1、θ2 分别求偏导得到2×2的雅可比矩阵J [-L1 sinθ1 - L2 sin(θ1θ2), -L2 sin(θ1θ2) L1 cosθ1 L2 cos(θ1θ2), L2 cos(θ1θ2)]这个矩阵的行列式算出来是 L1 L2 sinθ2。当 sinθ2 0也就是 θ2 0° 或 180° 时行列式等于0雅可比矩阵不可逆这时候机械臂处于奇异位形。直观理解就是当两臂完全伸直或完全折叠时末端只能在某个方向上运动另一个方向不管关节怎么转都动不了或者说想在一个受限方向运动需要无穷大的关节速度。奇异位形对控制的影响非常大。位置控制里如果逆雅可比恰好遇到奇异关节速度会跑到极大轻则轨迹偏移重则触发保护停机。力控制里奇异又会带来完全相反的问题有些方向施加力会被放大到机构不能承受的程度。所以在实际规划里我一般会先算当前姿态的可操作度再决定是否要避开这个区域。4.3 可操作度与路径规划中的应用心得工程上不能只靠“行列式有没有变成0”来判断奇异因为很多姿态只是接近奇异还没到0但末端运动已经非常迟钝。为了量化这种“距离奇异有多近”可以用可操作度指标 w sqrt(det(J Jᵀ))对非冗余机械臂其实就等于行列式的绝对值。这个值越大末端在各方向上的运动能力越均衡越接近0机械臂越接近奇异。我从实际调试里学到的经验是路径规划阶段就要把可操作度纳入约束。不要等到控制器把轨迹跑到一半才发现某个位姿下运动卡顿。如果一条轨迹必然要穿越奇异点通常会用两种方式处理一是调整末端位姿让机械臂换一种臂形绕过奇异二是降低通过奇异点附近的速度让关节速度不至于冲到限幅。实在不行才考虑增加一个冗余自由度也就是利用多出来的关节自由度在不改变末端轨迹的前提下改变臂形但这已经超出前六章的讨论范围了。5. 静力分析与力传递末端推一下关节要出多少力5.1 从虚功原理到 τ Jᵀ F静力分析这一章很多人会觉得突然因为它前一半还在讲速度和微分运动后一半突然跳到力。但只要抓住“虚功原理”这四个字其实非常顺。假设机械臂末端受到一个广义力 F它包含末端力和力矩同时关节需要产生力矩 τ 来保持平衡。如果我们给关节一个极小的虚位移 δq那么末端会产生一个对应的虚位移 δp J δq。静力平衡时关节力矩做的虚功必须等于末端力做的虚功即τᵀ δq Fᵀ δp把 δp J δq 代入得到 τᵀ δq Fᵀ J δq对任意 δq 都要成立于是必然有 τ Jᵀ F。这就是为什么力映射用的是雅可比矩阵转置。这个式子看起来简单但它直接改变了很多人对“机械臂省力”的理解。关节力矩和末端力之间不是简单的杠杆关系而是和当前位姿强相关。同一个力机械臂伸得远时需要的关节力矩大缩回来时需要的关节力矩小。静力分析就是在为这种变化提供量化工具。5.2 平面两连杆臂的静力计算示例继续用平面两连杆臂算一次静力映射。假设末端受到水平方向力 fx 和垂直方向力 fy那么第一个关节的力矩 τ1 和第二个关节的力矩 τ2 可以由 τ Jᵀ F 得到τ1 (-L1 sinθ1 - L2 sin(θ1θ2)) fx (L1 cosθ1 L2 cos(θ1θ2)) fy τ2 (-L2 sin(θ1θ2)) fx (L2 cos(θ1θ2)) fy从式子里能清楚看到第二个关节主要承担末端力带来的局部负载而第一个关节还要额外克服整个手臂结构产生的力矩。所以工程上选第一个关节的电机和减速器时往往要比第二个关节“大得多”。很多小型桌面机械臂看起来关节尺寸差不多但内部减速比差别很大原因就在这里。实际做静力分析时我总是建议把末端执行器的自重也算进去。比如夹爪本身有一定重量它作用在末端而不是关节上就需要把重力当作一个等效末端力再通过雅可比矩阵转置折算到关节。如果忽略了这一步很可能出现“仿真里力矩够装上夹爪后第一个关节直接报警”的情况。5.3 力椭球与选型中的实用经验力椭球是静力分析里一个很直观的可视化概念。假设末端在各个方向上施加单位力同时限制关节力矩的模长等于1那么末端能在不同方向产生多大的力画出来就是一个椭球这个椭球就叫力椭球。椭球长轴方向表示机械臂在这个方向上有较好的出力能力短轴方向则表示出力能力差。这里有一个很有意思的对称关系速度上灵活的方向恰恰是出力上被放大的方向反过来速度上迟钝的方向往往能产生更大的末端力。这从矩阵的奇异性就能理解在奇异方向附近关节速度会被放大所以同样关节力矩产生的末端力反而很小。因此在规划“重物搬运”这类任务时不能只看工作空间是否覆盖还要看力椭球在目标位置的方向是否合适。我选型时一般会先做一次静力分析把任务中最大力矩出现的位置和姿态标出来再反推电机减速比与峰值扭矩免得设备到手才发现某些姿态根本带不动负载。6. 动力学入门从静态平衡到动态耦合6.1 拉格朗日方程从能量角度建立运动方程静力分析假设机械臂不动或平衡但真实机器人一直在加速、减速关节之间还会互相影响。动力学要回答的问题就是给定关节力矩机械臂会产生什么样的加速度或者说要让机械臂末端走某条高速轨迹每个关节需要输出多少力矩拉格朗日方程是从能量角度处理动力学问题很方便的方法。它的核心是先写出系统的动能 T 和势能 V再令拉格朗日量 L T - V最后代入方程d/dt (∂L/∂q̇) - ∂L/∂q τ这套方法看起来有点绕但它最大的好处是不用像牛顿第二定律那样逐个连杆画受力图。只要你能把动能和势能表达式写出来求导之后就能得到动力学方程。平面两连杆机械臂的完整动力学方程写出来会是M(q) q̈ C(q, q̇) q̇ G(q) τ其中 M(q) 是惯性矩阵C(q, q̇) q̇ 包含离心力和科氏力G(q) 是重力项。这些项各有各的物理意义惯性项对应加速度引起的力矩离心力和科氏力项对应速度耦合带来的力矩重力项则和静力分析里的重力折算一致。6.2 惯性矩阵与耦合项为什么关节之间会“互相影响”很多第一次接触机器人动力学的人最容易忽略 M(q) 不是对角阵这一点。非对角元素表示一个关节加速度会在另一个关节上产生力矩这就是所谓的耦合效应。比如第二个关节快速摆动会使第一个关节承受一个额外的反作用力矩。转动越快耦合效应越明显所以高速机器人比低速机器人难控制得多。C 矩阵里的离心力和科氏力更反直觉。即使机械臂某个时刻关节加速度为零只要关节在匀速转动C 项也不为零。尤其是多关节同时运动时科氏力会在关节之间互相传递能量。控制上如果不补偿这些速度相关项机械臂就会在高速运动时出现“飘”或“跟着抖”的现象。我做仿真时的体会是在没有动力学补偿之前用一个很普通的PD位置控制器去跑高速轨迹末端轨迹会明显滞后和振荡。一旦把重力项补偿进去静态定位误差立刻小很多再把惯量项补偿进去动态跟随精度才有明显提升。这也解释了为什么前六章明明在讲理论实际调试里却全是这些理论在兜底。6.3 牛顿-欧拉迭代法更适合程序实现的动力学算法与拉格朗日法相比牛顿-欧拉法更适合编程因为它只需要两个循环。第一个循环从基座向末端递推计算每个连杆的角速度、角加速度、质心线加速度以及每个连杆本身需要的惯性力第二个循环从末端向基座递推把每个连杆受到的力换算成关节力和力矩。虽然推导过程比拉格朗日法繁琐但算法结构非常规整几乎没有多余的符号运算。实际工程中的动力学参数往往不是直接从CAD模型抄出来的因为还存在摩擦、柔性、间隙等非线性因素。所以计算得到的力矩一般要再叠加一个摩擦模型简单的可以用库仑摩擦加黏性摩擦更精细的还会有Stribeck效应。我在做轨迹跟踪时还发现那些从CAD模型直接拿的惯性张量经常和实际设备差不少一个有效的手段是做一次系统辨识实验通过让每个关节单独运动来估计惯量和摩擦参数。6.4 仿真与实测中的单位、摩擦与增益调整要点最后说几个非常实务、但教材里几乎不讲的问题。第一单位必须统一。同一个项目里如果有的模块用毫米、有的用米质量和力矩的数值会差到离谱。我见过一个动力学仿真连杆长度用毫米惯性张量却用千克·平方米结果算出来的关节力矩大得离谱查了很久才发现是单位混搭。稳妥做法是建模一开始就统一用SI单位最后输出到可视化模块时才转换成毫米。第二摩擦不能完全忽略。纯理论动力学模型往往不含摩擦但实际设备的低速跟踪和停机保持几乎都靠摩擦和重力补偿撑着。如果你发现低速时关节响应有明显的“爬行”先不要怀疑控制算法多半是摩擦模型没建对。第三增益阻尼要分步调。先调重力补偿让机械臂在静止时能稳定保持再调PD增益让关节能快速跟随目标最后才加入动力学前馈。每一步都验证完再做下一步不然出问题了很难定位是参数问题还是算法问题。整门课学到这里你会发现前六章其实是在反复讲同一件事如何用数学统一描述机器人的几何、速度、力和运动。刚开始觉得矩阵多、推导乱但只要自己在仿真里从头到尾搭一遍哪怕只是一台平面两连杆这些知识就会从“教材上的公式”变成“你手里的工具”。我自己学下来最大的感受是不要追求一次把所有推导都背下来先把“正运动学—逆运动学—雅可比—静力—动力学”这条主线刻在脑子里再回头补细节会顺畅很多。