机器人运动学基础：从坐标变换到轨迹规划的完整学习指南

拿到Introduction to Robotics前六章的学习总结我第一反应不是翻目录而是先想清楚一个问题这门课到底要告诉我什么。六章内容串起来其实就一条主线从一个刚体在空间里怎么表达到机械臂每个关节怎么转再到末端的位姿、速度、轨迹怎么被描述和控制。无论你是在校学生、考研复习还是刚入行的机器人工程师前六章都是绕不过去的基础。我见过不少同学公式背得很熟但一到建系就乱了。这篇总结就是想把第一到第六章的骨架、重点和那些不上课没人告诉你的坑一次说清楚。1. 第一到六章到底在讲什么一张知识地图很多初学者最大的问题不是某一章难而是学完前六章之后脑子里没有形成一条线。所以先别急着啃细节我把这六章的内容重新排了一遍你会发现它们其实是在讲同一个故事的三个环节。1.1 一条主线从“末端要去哪”到“关节怎么转”机器人学要解决的最基本问题是让机械臂末端到达某个位置、摆出某个姿态。而前六章的知识正好对应这个链条上的每一环第一章建立基本概念自由度、刚体、运动链、工作空间让你知道机械臂为什么能用6个关节达到空间的6个自由度。第二章解决“空间描述”怎么用数学语言说清楚一个刚体在哪儿、朝向哪儿这是所有计算的底层语法。第三章解决“正运动学”给定每个关节的角度末端位姿到底是多少也就是从关节空间向笛卡尔空间的正向映射。第四章解决“逆运动学”给定末端目标位姿反向求出每个关节的角度。第五章解决“速度和后力”末端速度和关节速度怎么换算、末端受力和关节力矩怎么换算这就是雅可比矩阵的用武之地。第六章解决“轨迹规划”在起点和终点之间让机械臂沿着一条平滑、可执行、不突变的路径运动起来。这六章就像一条流水线先学会描述空间里的刚体再学会建立机械臂的数学模型然后会正解、会逆解、会算速度和力最后把这一切串起来生成运动轨迹。1.2 学完前六章你应该具备什么能力我自己的经验是判断基础有没有打牢不看你会不会背定义而是看你能不能独立完成下面这个最小闭环给一台两连杆平面机械臂杆长分别是 l1 和 l2你能写出它的DH参数表推出正运动学矩阵反过来由末端坐标解出关节角再写出它的雅可比矩阵判断它在哪些位形会奇异最后在关节空间和笛卡尔空间各规划一条从 A 到 B 的轨迹。如果这个流程你能不看笔记做下来这门课的前六章就等于真正过关了。下面我按章节逐个拆解每个部分都会把原理、公式记忆方法和实操中的坑一起讲。2. 空间描述与刚体变换一切计算的底层语法第二章是整门课的“语法课”也是最容易因为简单而轻视的一章。旋转矩阵、欧拉角、齐次变换矩阵这些概念后面每一章都在用。如果这一章没吃透后面的DH参数、雅可比推导基本就是空中楼阁。2.1 为什么位置和姿态必须分开描述一个刚体在三维空间里有6个自由度3个位置自由度说明它“在哪”3个姿态自由度说明它“朝向哪”。很多人一开始不理解为什么姿态单独要占用3个自由度我常用一个手机的例子解释手机放在桌面上位置可以相同但屏幕朝上还是朝下、旋转90度还是180度完全是不同的状态。所以表达一个坐标系相对于另一个坐标系必须同时给两部分信息位置矢量 p 描述原点偏移旋转矩阵 R 描述三个坐标轴的朝向。R 的每一列分别是被描述坐标系的 x、y、z 轴在参考坐标系里的投影。理解这一点比死记矩阵元素有用得多因为很多题你只要画出坐标轴就能直接看出旋转矩阵长什么样。旋转矩阵有两个非常重要的性质它是正交矩阵R 的转置等于逆行列式等于 1。这两个性质在实际计算中经常用来验算比如你算了一个旋转矩阵转置乘回去不是单位矩阵那说明前面肯定算错了。2.2 旋转矩阵、欧拉角和齐次变换到底记什么旋转矩阵是表达姿态的“标准格式”但人眼看不直观所以教材里引入了欧拉角和固定角。欧拉角的核心要义是“绕动坐标系旋转”常见顺序是 ZYX 或 ZYZ。这里最容易踩坑的是欧拉角的旋转顺序不能随便换R Rz * Ry * Rx一旦调换顺序结果天差地别。还有一个绕不开的概念叫“万向锁”当中间轴旋转到某些特定角度时会丢失一个转动自由度所以工程上在高精度姿态估计里更多使用四元数但在机器人学的导论阶段旋转矩阵仍然是主角。齐次变换矩阵是把旋转和位移统一成 4x4 矩阵的工具形式是T [R p; 0 1]。它最大的好处是能把连续的坐标变换用矩阵乘法串联起来。复合变换的乘法顺序我用一句口诀总结左乘看世界右乘看当前。如果变换是相对于参考坐标系描述的就往左侧乘如果是相对于当前坐标系描述的就往右侧乘。这句话从第二章用到第六章基本不会出错。2.3 一个算例先旋转后平移怎么算举个最简单的例子加深印象。已知 B 坐标系相对于 A 坐标系先绕 A 的 z 轴旋转 90 度再沿 A 的 x 轴平移 5。求 B 中的点 p(1,0,0) 在 A 中的坐标。先写变换矩阵R [0 -1 0; 1 0 0; 0 0 1]p [5;0;0]。A 中的坐标 R * p_B p结果是 [5;1;0]。注意这个过程是“先旋转后平移”所以位移不用乘旋转矩阵。反过来如果是“先平移后旋转”那 p 的表达式就会多一步矩阵乘法。这个细节很多人到了第三章才反应过来然后整个DH表全乱套。实操建议每次做坐标变换先在草稿纸上画出两个坐标系的三根轴标出旋转方向和平移方向再写矩阵。我见过太多同学直接套公式结果正负号搞反回头查半天发现是坐标系画错了。3. 正运动学DH参数就是机器人的“户口本”正运动学是第三章的重头戏核心就一个DH参数法。这一章学得好不好直接决定后面逆运动学和雅可比能不能顺利展开。3.1 DH参数的四个量到底管什么DH参数法用四个量描述相邻两个坐标系之间的关系a连杆长度、alpha连杆扭转角、d连杆偏距、theta关节角。我自己记这四个量的口诀是“a和alpha描述杆的几何d和theta描述关节的连接”。a 是沿着 x 轴方向从当前 z 轴到下一个 z 轴的距离它决定连杆的长短。alpha 是绕 x 轴旋转从当前 z 轴转到下一个 z 轴的角度它决定两个关节轴之间的夹角。d 是沿着 z 轴方向从上一个 x 轴到当前 x 轴的距离。theta 是绕 z 轴旋转从上一个 x 轴转到当前 x 轴的角度。对于旋转关节theta 是变量对于移动关节d 是变量。理解和记忆的关键在于每个参数描述的都是“相邻两个关节轴之间的关系”而不是某个杆件本身的尺寸。很多初学者把 a 直接当成机械臂某根杆的长度这在某些构型下没错但思考方式不通用。3.2 建立坐标系的流程和最容易犯的错DH建系的步骤看起来很简单先给所有关节标出 z 轴方向然后确定 x 轴方向最后补齐 y 轴。但实操中到处是坑最常见的几个我列一下第一个坑x 轴方向选反。x 轴必须沿着从当前 z 轴到下一个 z 轴的公垂线方向选反了会导致 theta 或者 a 的符号变成负的后续全部跟着错。第二个坑z 轴方向不沿关节轴。z 轴必须和关节转动的轴线重合方向可以任选但一旦选了就要全程一致。第三个坑把标准DH和改进DH混在一起用。教材可能是标准DH网上某篇文章用的是改进DH两个约定的坐标系位置不一样混着用末端位姿就差出一大截。我建议新手只选一种约定死磕到底。另外建系时别忘了固定基座坐标系通常记作0系和工具坐标系。0系可以随意放在基座上但最好让第一根轴的 z 轴和基座 z 轴重合能省很多事。3.3 两连杆平面臂从DH表到变换矩阵的完整算例我以最常见的两连杆平面机械臂为例完整走一遍流程。设第一杆长 l1第二杆长 l2两个关节都是旋转关节。DH表如下关节 ia(i-1)alpha(i-1)d(i)theta(i)1000theta12l100theta2然后根据标准DH通式写出相邻变换矩阵 T01 和 T12。最后把两个矩阵乘起来得到 T02末端位置就是大家很熟悉的x l1 * cos(theta1) l2 * cos(theta1 theta2)y l1 * sin(theta1) l2 * sin(theta1 theta2)注意这个结果的来源不是背出来的而是矩阵乘法实打实算出来的。我建议每个人都自己手推一遍推完再和教材对答案。这个过程能帮你把第二章的复合变换、第三章的DH参数一次性打通。提示算完之后可以用特殊角度验证。比如 theta10、theta20 时末端应该在 (l1l2, 0)。如果算出来不是说明或者DH表错了或者矩阵乘反了。4. 逆运动学从末端位姿反推关节角正运动学给定角度算位姿方向明确逻辑简单。但逆向就没那么温柔了——已知末端位姿要求关节角这一步是机器人控制里最麻烦的问题之一。4.1 为什么逆解比正解难这么多逆运动学难有三个原因。第一是多重解。机械臂到达同一个末端位姿通常有多组关节角方案。就拿两连杆平面臂来说给定末端坐标理论上存在肘上、肘下两组解。第二是无解。末端坐标如果超出了机械臂的工作空间比如离基座太远那无论怎么转关节都到不了。在控制程序里从逆解器返回的结果是空集合就必须做异常处理。第三是奇异位形在某些关节角组合下逆解算法会失效因为此时末端在某些方向上无法产生速度这在第五章会专门讲。4.2 解析法和数值法怎么选逆运动学的解法分两类解析法和数值法。解析法就是像中学解方程一样把关节角用末端坐标的显式公式表达出来。它的优点是计算快、精度高、适合实时控制。但不是所有机械臂都有解析解教材里有个很重要的结论叫 Pieper 准则如果机械臂满足相邻三个关节轴交于一点或者相邻三个关节轴平行就存在解析解。绝大多数工业六轴机械臂的设计就是按照满足这个准则来设计的。数值法则基于雅可比矩阵做迭代逼近通用性强任何构型都能用但缺点是有可能陷入局部最优、速度慢、需要初值。实际工程中能用解析解就用解析解数值法更多用于冗余机械臂或特殊构型。4.3 多重解怎么选不是随机挑一个既然逆解有多组解控制器到底该选哪一组实际工程里有一套优先级首先要避开奇异位形。其次要避开碰撞和关节限位。再次要考虑运动连续性选离当前关节角最近的一组解避免机械臂大幅抖动。最后还要考虑能耗优先使用重力矩较小的构型。两连杆平面臂的逆解公式自己也推导一遍cos(theta2) (x^2 y^2 - l1^2 - l2^2) / (2 * l1 * l2)theta2 atan2( sqrt(1 - k^2), k ) 或 atan2( -sqrt(1 - k^2), k )theta1 atan2(y, x) - atan2(l2 * sin(theta2), l1 l2 * cos(theta2))这里的 atan2 是四象限反正切能自动处理正负号。现在想想如果 l1、l2 组合后x^2y^2 大于 (l1l2)^2那么 k 的绝对值会大于 1sqrt(1-k^2) 没有实数解这就是“无解”的数学来源。实操建议逆解拿到多组解之后不要只在纸上算完就完事。可以写一个简单的仿真函数把每组解对应的姿态画出来亲眼看看“肘上”和“肘下”的区别印象会深很多。5. 雅可比矩阵速度和静力的桥梁如果说正逆运动学解决的是“位置”问题第五章就是解决“位置变化率”和“力”的问题。雅可比矩阵是连接关节空间和笛卡尔空间的变速器。5.1 雅可比矩阵的两个面孔雅可比 J 是一个 m x n 矩阵机器人学里通常 mn6表示末端6维速度与关节速度的映射关系末端速度 J * 关节速度这个公式很简单但它背后还有一层重要含义一个同样由雅可比矩阵实现的力映射关节力矩 J^T * 末端力为什么同一个矩阵能既管速度又管力核心是功率守恒。关节功率 关节速度^T * 关节力矩末端功率 末端速度^T * 末端力两者必须相等代进去就得到 tau J^T F。很多教材会把这个关系放在后面章节但我建议学完速度雅可比就顺手把静力映射一起看了因为两者本来就是一对。5.2 几何法构造雅可比矩阵雅可比矩阵有两种构造方式解析法和几何法。几何法更直观记住两条公式就能用对旋转关节 i末端线速度的贡献是 z(i-1) × (p_e - p(i-1))角速度贡献是 z(i-1)。对移动关节 i末端线速度的贡献是 z(i-1)角速度贡献是 0。这里的 z(i-1) 是关节 i 的旋转轴方向在基坐标系下的表达p_e 是末端位置矢量。用这个公式结合刚才两连杆平面臂的结果可以推出它的2x2雅可比矩阵J [-l1sin(theta1) - l2sin(theta1theta2), -l2sin(theta1theta2)l1cos(theta1) l2cos(theta1theta2), l2cos(theta1theta2)]接下来看奇异性。计算行列式det(J) l1 * l2 * sin(theta2)当 theta2 0 或 theta2 180° 时行列式等于0矩阵不可逆机械臂处于奇异位形。theta2 0 是手臂完全伸直theta2 180° 是手臂折叠。在最直观的层面理解在这两个位形下末端在某些方向上动不了比如手臂伸直时简单往前推的力会全部变成杆件的拉力/压力而不是驱动力矩。5.3 奇异位形在实际控制里的影响奇异位形不是“正好在某个角度才会遇到”的理论概念实际运动中非常容易碰到。比如规划一条笛卡尔空间的直线轨迹机械臂可能经过一个接近直角的舒展姿态如果不管奇异关节速度会突然飙到一个很大的数值轻则轨迹偏差重则触发急停。工程上常用两个指标评估奇异性最小奇异值和可操作度。可操作度定义是 sqrt(det(J*J^T))它越接近0说明这个位形离奇异越近。很多控制算法会在可操作度低于阈值时自动在关节空间做平滑过渡避免硬闯奇异点。实操建议如果你在写运动学相关的程序一定要把雅可比矩阵的奇异值判断加进去。哪怕只是写个课程设计也至少要在代码里检测 det(J) 的绝对值是否小于某个阈值否则你永远不知道机械臂为什么走到一半突然卡住。6. 轨迹规划让机械臂“听话地动起来”有了正运动学、逆运动学和雅可比矩阵机械臂已经“知道”自己该怎么到达目标位形了但还差最后一步怎么从 A 点平滑地到 B 点。这就是轨迹规划。6.1 关节空间和笛卡尔空间的轨迹规划怎么选轨迹规划有两条路在关节空间规划或者在笛卡尔空间规划。关节空间规划的思路是先对路径的起点和终点求逆解得到两个关节角向量然后让每个关节从初始角度平滑过渡到目标角度。它的优点是不需要实时插值逆解计算量小也不会因为经过奇异位形而中途失败。缺点也很明显末端的实际路径是弯曲的你无法保证它走一条直线。笛卡尔空间规划则是在末端坐标系里规定一条路径比如直线然后每隔一小段时间取一个点做逆解得到关节轨迹。优点是路径可控适合涂胶、焊接等要求直线轨迹的工艺缺点是要反复做逆解并且可能碰到奇异点导致关节速度爆表。实际工程里两种方式都有应用初学者阶段先把关节空间的梯形速度曲线和多项式轨迹练熟再去看笛卡尔直线插补会更顺手。6.2 三次多项式和五次多项式的边界条件怎么定以关节空间一次点到点运动为例最常见的是三次多项式。设关节角度 q(t)给定四个边界条件起点位置、终点位置、起点速度、终点速度通常都为0。三次多项式是 q(t) a0 a1t a2t^2 a3*t^3四个系数恰好对应四个条件。假设起点 q0终点 q1总时间 T1秒起点速度、终点速度都是0代入后解得q(t) 3t^2 - 2t^3这就是一个平滑的上升曲线。但它有一个问题加速度在起点和终点会突跳。算一下就知道 q_ddot(t) 6 - 12t起点加速度是6终点加速度是-6这种突变的加速度会带来冲击实际运行中可能造成机械臂震动。解决办法是改用五次多项式额外约束起点和终点的加速度为0这样加速度曲线变成连续平滑的曲线机械臂运行会更稳。当然也不是说多项式次数越高越好——次数越高中间路径越不可控而且计算量也上去了。工程里五次多项式已经足够常见。6.3 梯形速度曲线为什么是工程首选除了多项式工程上用得更广的轨迹是梯形速度曲线。它的思路是让关节在加速阶段以恒定加速度加速到最大速度然后匀速运行再以恒定减速度减速到零。速度曲线像一个梯形因此得名。好处是计算简单、执行时间可预估而且很容易控制峰值速度在电机允许范围内。我在实际项目中做点到点运动经常是先算梯形速度曲线因为它的加速度是方波比三次多项式的加速度突变更符合电机驱动器的控制习惯。如果发现梯形速度曲线在加减速切换处冲击比较大再考虑用 S 形速度曲线加加速度受限做平滑。实操建议定轨迹之前先看一下电机的最大速度和最大加速度指标。不然你在仿真里规划得再漂亮实际一跑驱动器和机械结构先抗议了。7. 学习过程中的高频坑与避坑清单最后这部分是我自己掉过坑也看别人反复掉的经典问题。我整理成了一张速查表方便你复习的时候对照。7.1 最典型的五个坑问题后果应对方案坐标系画错变换矩阵全错末端位置偏出天际动笔前先画图标清每根轴的方向标准DH和改进DH混用同一型号机械臂出现两套模型无法调试固定用一套约定所有代码统一注释忽略欧拉角顺序外旋和内旋混着算姿态结果不一致明确标注旋转序列比如ZYX别只写“旋转”逆解多重解直接取第一组机械臂突然大角度抽动加最短关节行程/避奇异判断奇异位形没有检测运行时关节速度突变计算雅可比行列式设置阈值提前规避这些坑不是教材里的考点但全是实际调试中的真问题。尤其是第一条画坐标系这件事看起来过分简单但做不对后面全白费。7.2 考试和面试最常问的几个考点如果是准备考试或面试有几个点出现频率极高值得重点准备解释什么是自由度如何计算一个开链机械臂的自由度。给出两个坐标系求旋转矩阵或齐次变换矩阵。给一个机械臂简图写出DH参数表。解释正运动学和逆运动学的区别以及为什么逆解可能多解。推导一个简单机械臂的雅可比矩阵并说明奇异位形。比较关节空间轨迹和笛卡尔空间轨迹。这些考点看似分散实际都围绕同一个核心你有没有真正理解“坐标变换和运动学链”。只要第二章和第三章地基打得牢后面的考点都能推出来。7.3 从总结到落地建议自己做一次完整闭环看完这篇总结如果你只停留在“懂了”的层面那我还是建议你动手做一轮最小闭环。具体做法很简单拿两连杆平面机械臂用你熟悉的语言写一个小程序完成这几件事输入两个杆长随机给定一组关节角输出末端坐标再输入末端坐标反推所有可能的关节角组合随后计算雅可比矩阵画出奇异位形附近的可操作度变化最后在关节空间用三次多项式规划一条从起点到终点的轨迹输出每个时间步的关节角度、速度和加速度。这个过程花不了多少时间但会把前六章所有公式真正变成你自己的工具。等你做完这轮闭环再去看六轴机械臂的DH表或者看真实工业机器人的控制代码你会发现思路完全打开了。我个人在整个学习过程中体会最深的一件事是不要把机器人学当数学课去背要把它当一门需要“亲手验证”的工程课去学。公式推一遍、代码跑一遍、图看一眼比盯着教材看十遍都有用。如果你现在正卡在某个章节不用急着往前赶把坐标变换和DH参数这两个基本功打牢后面的路会顺很多。