前言

• 最近我们一直在玩转无人机和 SLAM，再我们继续解读新的算法和代码之前，我们来玩一点不一样的
• 刚好最近 VLA（Vision-Language-Action，视觉-语言-动作）具身智能火得不行，从谷歌的 RT-2、OpenVLA 到 π0，大模型开始直接输出机器人动作，而这一切的物理载体，正是机械臂
• 所以接下来这个系列，我们将逐步上手最近大火的 VLA 具身智能机械臂，计划从 机械臂基础 → MoveIt2 + Gazebo 仿真 → Pinocchio 刚体动力学 → VLA 入门一路推进
• 本期我们先不碰仿真和代码框架，专注把地基打牢——机械臂基础：机械臂的结构组成、坐标系与位姿表示、正逆运动学、雅可比与奇异位形、动力学、运动规划与控制
• 全文较长，分上、下两篇：上篇为机械臂建模（第 1-5 章：结构、位姿、运动学、速度运动学、动力学），下篇为规划与控制（第 6-8 章）+ 全部可视化代码附录
  

---

目录（本文为上篇，覆盖第 1-5 章）

---

1 机械臂基础概念

1-1 机械臂是什么

• 机械臂（Manipulator）本质上是一个由连杆（Link） 和关节（Joint） 串联组成的多刚体链式机构
• 它一端固定在基座（Base） 上，另一端安装末端执行器（End Effector），通过控制各个关节的转动/平动，把末端送到期望的位置和姿态
• 说人话：机械臂就是一条"骨头 + 关节链"——骨头是连杆，关节是骨头之间的"转轴/滑轨"，最前面那只"手"就是末端执行器
• 一台完整的机械臂系统，从硬件到控制，可以拆成下面几层：

层次	组成	回答的问题
机械结构	连杆 + 关节 + 电机 + 减速器	机械臂长什么样
运动学	关节角 → 末端位姿	末端在哪、怎么动
动力学	力矩 ↔ 运动	需要多大的力/力矩
运动规划	轨迹生成	从 A 点到 B 点怎么走
控制	电机伺服 + 力控	怎么让机械臂"听话"

核心思想：机械臂的所有"智能"，本质上都在回答一个问题——“我的关节该转多少”。运动学告诉你关节和末端的映射关系，规划告诉你关节随时间怎么变化，控制负责把这个变化"逼真"地执行出来

1-2 关节 Joint

• 关节是机械臂运动的"发动机"，也是相邻两个连杆之间的运动副
• 运动副（Kinematic Pair）是机械原理（机构学）里的术语，指两个构件直接接触、又能产生相对运动的可动连接
• 在机构学里，任何机构都由两类东西组成：构件（Link）+ 运动副（Pair）——机械臂本质就是一条由连杆（构件）和关节（运动副）组成的开链机构

说人话：运动副就是"两个零件搭在一起、还能相对动"的连接方式。机械臂里叫它"关节"，机构学里叫它"运动副"，指同一个东西——转动副 = 旋转关节，移动副 = 移动关节

• 按接触形式，运动副分两类：

类别	接触方式	例子
低副	面接触（接触面积大，耐磨）	转动副、移动副
高副	点/线接触（能实现复杂运动）	齿轮啮合、凸轮

• 机械臂几乎全用低副——结构简单、承载大、控制也容易
  

• 实际机械臂中绝大多数关节都只提供 一个 自由度（详见1-4），主要分两类：

• 旋转关节（Revolute Joint）

  • 让相邻连杆绕着一个固定轴线旋转，就像门轴
  • 只有一个旋转自由度，用关节角 $q_i$ 描述
  • 绝大多数工业机械臂（UR、KUKA、ABB）的主关节都是旋转关节

• 移动关节（Prismatic Joint）

  • 让相邻连杆沿一个固定轴线做直线滑动，就像抽屉
  • 只有一个平移自由度，用关节位移 $d_i$ 描述
  • 常见于直角坐标机械臂或某些并联机构的支腿

• 除了这两类主关节，还有球关节（Spherical Joint，3 个自由度）、圆柱关节（Cylindrical Joint，2 个自由度） 等，但在机器人领域出于结构可靠、控制简单的考虑，几乎都拆成多个旋转/移动关节的组合

说人话：旋转关节就是"转"，移动关节就是"滑"。一台 6 自由度机械臂，绝大多数情况下就是 6 个旋转关节——每个关节只负责转一个角度，6 个角度一配合，末端就能在空间里自由翱翔

1-3 连杆 Link

• 连杆是机械臂的"骨头"——连接相邻两个关节的刚性部件
• 我们通常用连杆 i 表示连接关节 i 和关节 i+1 的刚体，它本身不产生运动，只负责把运动"传递"下去
• 描述连杆要用两个核心尺寸：
  • 连杆长度 $a_{i-1}$：沿公法线方向，关节轴 $z_{i-1}$ 到关节轴 $z_i$ 的距离
  • 连杆偏距 $d_i$：沿关节轴 $z_i$ 方向，相邻两连杆公垂线之间的距离
  • （这两个量正是 DH 参数的核心，详见 3-4 节）

说人话：关节负责"转"，连杆负责"撑"。两个关节之间必须有根杆子连起来，杆子的长度和朝向直接决定了末端能摸到哪里

1-4 自由度 DOF

• 自由度（Degree of Freedom，DOF）指确定机械臂位形所需的最少独立变量个数

• 一个自由物体在三维空间有 6 个自由度：3 个平动（$x,y,z$）+ 3 个转动（roll, pitch, yaw）

• 要让末端能到达工作空间内任意位置和姿态，机械臂至少需要 6 个自由度

• 少于 6 个叫欠驱动（underactuated），多于 6 个叫冗余机械臂（redundant）——比如 7 自由度的 KUKA LBR iiwa，多出来的自由度让机械臂能在保持末端位姿不变的情况下调整自身姿态，方便避障

• 

• 机械臂总自由度是各关节自由度之和：

$$n_{DOF}=\sum _{i=1}^m{f}_i$$

• 其中 $f_i$ 是第 $i$ 个关节的自由度数。对 6 个旋转关节的机械臂，$n_{DOF}=6$

说人话：自由度就是"这台机器能独立变几个参数"。6 自由度 = 末端位置 3 个 + 姿态 3 个，正好凑齐；7 自由度 = 多了一个"手臂摆造型"的自由，避障时特别好用

1-5 末端执行器 End Effector

• 末端执行器是安装在机械臂最前端、真正与物体/环境接触的装置
• 常见类型如下：

末端执行器	典型用途
二指/多指夹爪	抓取、搬运
吸盘	抓取平整表面物体（纸箱、玻璃）
焊枪/喷枪	焊接、喷涂
电动/气动螺丝刀	装配
夹爪 + 力传感器	力控、柔性抓取

• 在 VLA 具身智能里，末端执行器就是大模型输出动作的"执行端"——模型预测夹爪开合和位置，机械臂把它变成物理上的抓取
• 最简单的二指夹爪就两个状态：开 = 放松，合 = 夹紧，配合机械臂运动就能完成"抓取 - 搬运 - 放置"的完整闭环

说人话：末端执行器就是机械臂的"手"。运动学把"手"送到位，夹爪决定"手"抓不抓。VLA 模型输出的最终指令，就是"手该去哪 + 手该抓还是放"

---

2 坐标系与位姿

• 机械臂运动学要做的事，本质上是在坐标系之间做变换
• 描述一个刚体的位置和朝向，合起来叫位姿（Pose） = 位置（Position）+ 姿态（Orientation）
• 我们在 SLAM 里天天打交道的那套东西——TF、外参、SE(3)——在这里全部原样复用

2-1 位置 Position

• 位置描述刚体上某点在参考系中的坐标，是一个 3 维向量：

$$p=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]\in {\mathbb{R}}^3$$

• 描述机械臂时，每个连杆都有一个自己的坐标系，位置就是这些坐标系原点之间的平移关系

2-2 姿态 Orientation

• 位置说完了，还差朝向。姿态（Orientation） 描述刚体相对参考系的朝向，本质是一次旋转
• 位置是"在哪"，姿态是"朝哪"。描述姿态不像位置那样直接给 3 个数就行，它有多种表示方式
• 三种主流表示，各有优劣：

表示	个数	优点	缺点	典型用途
旋转矩阵	9	数学标准、便于连乘	冗余（9 个数 3 个独立量）	运动学、动力学推导
欧拉角	3	直观，人一眼看懂	万向锁、插值不平滑	人机交互、调试显示
四元数	4	无万向锁、插值平滑	不直观	ROS / MoveIt2 内部存储

• 下面依次介绍：先讲数学标准表示旋转矩阵，再讲直观的欧拉角和工程常用的四元数
• 三种表示之间可以互相转换：欧拉角 ↔ 旋转矩阵、四元数 ↔ 旋转矩阵，工程代码里拿到哪种就转哪种

说人话：姿态有三种"写法"，旋转矩阵像正规档案、欧拉角像口头描述、四元数像机器编码。内容都一样，看场合选一种。下面一个个讲

2-3 旋转矩阵 Rotation Matrix

• 姿态的标准数学表示是旋转矩阵 $R$

2-3-1 定义

• 旋转矩阵是一个正交矩阵，属于三维旋转群 $SO(3)$：

R ∈ S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I ,   det ⁡ ( R ) = + 1 } R \in SO(3) = \{ R \in \mathbb{R}^{3\times3} \mid R^T R = I,\ \det(R) = +1 \} R∈SO(3)={R∈R3×3∣RTR=I, det(R)=+1}

• 旋转矩阵的列向量，就是旋转后坐标系各轴在参考系中的方向余弦

$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$$

• 9 个元素只有 3 个独立自由度，因为要满足 $R^{T}R=I$（6 个约束）加上 $\det (R)=1$

2-3-2 基本性质

• 旋转矩阵的几个基本性质，后面全程都会用到：

  • 正交性：$R^{T}R=I$，于是 $R^{-1}=R^T$

  • 绕单轴旋转（欧拉角里用到的 $R_x/R_y/R_z$ 就是它们）：$$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right],R_y(\beta )=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right],R_z(\gamma )=\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]$$

  • 组合：多个旋转连乘，$R=R_1{R}_2\cdots R_n$，连乘时注意两点：

    • 不满足交换律：$R_1{R}_2\ne R_2{R}_1$，先绕哪根轴、后绕哪根轴，结果完全不同
    • 乘序看坐标系：矩阵连乘从右往左读，最右边的矩阵先执行。绕固定坐标系旋转时新旋转左乘，绕物体自身坐标系旋转时新旋转右乘——机械臂的坐标系逐级嵌套在连杆上，相邻连杆变换连乘正是右乘（见 2-6 节 $T_{AC}=T_{AB}\cdot T_{BC}$）

  • 反对称矩阵：向量 $a=[a_1,a_2,a_3{]}^T$ 的反对称矩阵 $[a{]}_{\times }$，满足 $[a{]}_{\times }b=a\times b$：$$[a{]}_{\times }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

  • 指数映射：绕单位轴 $\omega$ 旋转角 $\theta$，$R=\exp ([\omega {]}_{\times }\theta )$（罗德里格斯公式）——这正是 ESKF 里 $⊞$ 操作的基础

• 旋转矩阵是"标准表示"，但 9 个数不直观，工程上常用下面两种更省的表示——欧拉角来读、四元数来算

说人话：旋转矩阵就是"把坐标系转到哪去"的三行三列数表。写出来有 9 个数，实际只有 3 个独立量。它是姿态的"标准表示"，但人眼看不直观，所以工程上常用欧拉角来读、四元数来算

2-4 欧拉角 Euler Angles

• 姿态最直观的表示是欧拉角：把任意旋转拆成依次绕固定轴旋转的三个角度：Roll（横滚）、Pitch（俯仰）、Yaw（偏航）
• 三个角度各自对应 2-3 节里的单轴旋转矩阵：

$$R_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right],R_y(\beta )=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right],R_z(\gamma )=\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]$$

• 最常见的是 ZYX 顺序。这里绕的是固定坐标系，所以是左乘——每转一步，新的旋转矩阵都乘在最左边（对应 2-3 节"绕固定坐标系 → 左乘"那条规则）：$$R=R_z(\gamma )\cdot R_y(\beta )\cdot R_x(\alpha )$$

  • 连乘从右往左读、依次执行：最右边 $R_x$ 先转 $\alpha$（roll），再 $R_y$ 转 $\beta$（pitch），最后 $R_z$ 转 $\gamma$（yaw）——即先 roll、再 pitch、最后 yaw

• 欧拉角的优点：直观，只有 3 个数，人一眼能看懂"偏了多少度"

• 但它有两个致命缺点：

  • 万向锁（Gimbal Lock）：当 pitch 转到 $\pm 90^{\circ }$ 时，roll 和 yaw 的旋转轴重合，丢失一个自由度，出现奇异（万向锁是任意三参数欧拉角的通病，不"因为是绕固定坐标系"才出现：绕固定坐标系时，pitch = $\pm 90^{\circ }$ 会让固定 $x$ 轴和固定 $z$ 轴映射到物体的同一根轴上，roll、yaw 退化成同一个自由度；绕物体自身坐标系时同理，先转的 yaw 轴（物体 $z$ 轴）会被 pitch 转到 roll 轴（物体 $x$ 轴）方向，两根轴在物体上重合，roll、yaw 同样退化成同一个自由度）
  • 插值不平滑：两个欧拉角直接线性插值，中间姿态会"扭"得很奇怪

• 所以工程上的约定是：人看用欧拉角，机器算用旋转矩阵/四元数

• 理解万向锁节死锁：【无伤理解欧拉角中的"万向死锁"现象-哔哩哔哩】 https://b23.tv/0hBxmJr

说人话：欧拉角就是"先向左转多少度，再抬头多少度，再歪头多少度"。很好懂，但在 pitch = ±90° 时会"卡死"一个轴——这就是万向锁，做姿态插值和路径规划时必须避开

2-5 四元数 Quaternion

• 欧拉角直观但会万向锁，工程上更常用四元数，用 4 个数表示一个旋转：

$$q=w+xi+yj+zk=\left[\begin{array}{c}w\\ x\\ y\\ z\end{array}\right],\Vert q\Vert =1$$

• 其中 $w$ 是实部（标量），$(x,y,z)$ 是虚部（向量），单位四元数满足 $w^2+x^2+y^2+z^2=1$
• 绕单位轴 $\omega =[{\omega }_x,{\omega }_y,{\omega }_z{]}^T$ 旋转角 $\theta$ 的四元数为：

$$q=\left[\begin{array}{c}\cos (\theta /2)\\ {\omega }_x\sin (\theta /2)\\ {\omega }_y\sin (\theta /2)\\ {\omega }_z\sin (\theta /2)\end{array}\right]$$

• 四元数的核心优势：
  • 没有万向锁：任意旋转都能用单位四元数表示（除差一个正负号）
  • 组合高效：两个旋转的复合就是四元数乘法 $q_1\otimes q_2$
  • 插值平滑：用球面线性插值 Slerp，$q(t)=\frac{\sin ((1-t)\Omega )}{\sin \Omega }{q}_1+\frac{\sin (t\Omega )}{\sin \Omega }{q}_2$，姿态过渡自然
• 四元数也可以转回 2-3 节的旋转矩阵：

$$R=\left[\begin{array}{ccc}1-2(y^2+z^2)& 2(xy-wz)& 2(xz+wy)\\ 2(xy+wz)& 1-2(x^2+z^2)& 2(yz-wx)\\ 2(xz-wy)& 2(yz+wx)& 1-2(x^2+y^2)\end{array}\right]$$

说人话：四元数是姿态的"4 个数版本"，看着不如欧拉角直观，但它不会万向锁、插值又顺滑，所以 ROS / MoveIt2 / 游戏引擎里全用它存姿态

2-6 齐次变换矩阵 Homogeneous Transformation Matrix

• 这是机械臂运动学里最重要的概念——把旋转和位移打包成一个 $4\times 4$ 矩阵
• 刚体坐标系相对于参考系的位姿：

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& t_x\\ r_{21}& r_{22}& r_{23}& t_y\\ r_{31}& r_{32}& r_{33}& t_z\\ 0& 0& 0& 1\end{array}\right]\in SE(3)$$

• 其中 $R$ 是旋转矩阵（姿态），$t$ 是平移向量（位置）
• 齐次坐标下，点 $\tilde{p}=[x,y,z,1{]}^T$ 经变换 $T$ 得到新坐标：

$${\tilde{p}}^{\prime }=T\cdot \tilde{p}=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]\left[\begin{array}{c}p\\ 1\end{array}\right]$$

• 为什么重要？因为变换可以连乘：

$$T_{AC}=T_{AB}\cdot T_{BC}$$

• 乘法顺序从右往左读、依次执行：最右边 $T_{BC}$ 先把 C 系坐标换算到 B 系，$T_{AB}$ 再把 B 系换算到 A 系——右乘的变换先作用
• 机械臂的连杆坐标系逐级嵌套在连杆上，每一级都相对"上一级"定义，所以整条臂的变换连乘正是这种右乘链（对应 2-3 节"绕物体自身坐标系 → 右乘"的规则）——下一章的正运动学就是这么算的

说人话：齐次变换矩阵 = "位置 + 姿态"打包成一个 4×4 的盒子，好处是能直接连乘。从基座到手腕，每一节一个盒子，全部乘起来就是末端位姿——这就是正运动学的本质

---

3 运动学

• 运动学研究几何关系——关节怎么动、末端怎么走，不关心力
• 核心就两个方向：正运动学（FK）和逆运动学（IK），中间用 DH 参数把机械臂"数字化"
  

3-1 关节向量与两个空间

• 先建立"状态"的语言：机械臂的位形，用一个关节向量描述：

$$q=\left[\begin{array}{c}{q}_1\\ q_2\\ ⋮\\ q_n\end{array}\right]\in {\mathbb{R}}^n$$

• 其中 $q_i$ 是第 $i$ 个关节的关节变量，是一个标量，具体长什么样取决于关节类型（对应第 1 章讲的两种关节）：
  • 旋转关节：$q_i={\theta }_i$，一个角度，比如 $q_1=30^{\circ }$——就是第 1 个关节转了多少度（工程代码里通常用弧度 $\text{rad}$）
  • 移动关节：$q_i=d_i$，一个位移，比如 $q_2=0.1\text{m}$——就是第 2 个关节沿轴伸出多少米
• 所以关节向量 $q$ 是 $n$ 个标量拼成的列向量，$n$ 是关节数——比如最常见的 6 轴工业机械臂，$q=[q_1,\dots ,q_6{]}^T$，六个分量全是关节角
• 注意区分两个空间：
  • 关节空间（Joint Space）：以关节角为坐标的空间，维度 = 关节数 $n$
  • 操作空间 / 任务空间（Task Space）：以末端位姿为坐标的空间，维度 $\le 6$

3-2 正运动学 FK

• FK 和 IK 干的事，就是在这两个空间之间"翻译"——正运动学（Forward Kinematics，FK）就是已知关节角 $q$，求末端位姿 $T_{ee}$
• 思路：给每个连杆建立坐标系，把每个关节的"旋转 + 平移"用一个齐次变换矩阵 $T_i$ 描述，然后连乘：

$$T_{ee}=T_0^1(q_1)\cdot T_1^2(q_2)\cdots T_{n-1}^n(q_n)=\prod _{i=1}^n{T}_{i-1}^i(q_i)$$

• 其中每个 $T_{i-1}^i(q_i)$ 只依赖第 $i$ 个关节的关节角 $q_i$
• 举个最简单的例子：二连杆平面机械臂（两个旋转关节，在 $xy$ 平面内运动）
  • 末端位置（推导见 3-4-2 节）：

$$x=L_1\cos q_1+L_2\cos (q_1+q_2),y=L_1\sin q_1+L_2\sin (q_1+q_2)$$

* 末端姿态（相对基座的偏航角）：$\phi = q_1 + q_2$

• FK 的数学性质：解唯一、计算简单——给一个 $q$，必然有且只有一个末端位姿

说人话：正运动学就是"给我每个关节转多少度，我告诉你手到哪了"。二连杆就像你的手臂，知道肩关节、肘关节的角度，自然知道手在哪里

• 用 Python 把上面的 FK 公式画出来（完整代码见下篇附录一），左图是一个具体位形，红色星号是末端；右图是遍历所有关节角得到的可达工作空间——一个环形区域：

3-3 逆运动学 IK

• 逆运动学（Inverse Kinematics，IK）：已知末端期望位姿 $T_{ee}^{des}$，求关节角 $q$
• 数学上，IK 是解非线性方程 $T_{ee}(q)=T_{ee}^{des}$
• IK 的求解一般分两类：

求解方法	原理	优点	缺点
解析法	代数/几何推导出显式解	快、稳定、解可枚举	只适用于特定构型（如 6 轴球形手腕）
数值法	迭代优化（Newton、雅可比伪逆）	通用，任意机械臂可用	慢、依赖初值、可能局部收敛

• 数值 IK 的核心迭代式（雅可比伪逆法）：

$$q_{k+1}=q_k+J^{†}(q_k)\cdot \left(x^{des}-f(q_k)\right)$$

• 其中 $J^{†}=J^T(J{J}^T{)}^{-1}$ 是雅可比矩阵的伪逆，$f(q_k)$ 是当前关节角下的末端位姿（FK）
• IK 的三个重要特性：
  • 多解性：同一个末端位姿可能对应多组关节角（比如"肘上/肘下"）
  • 奇异问题：在奇异位形附近 $J$ 退化，$J^{†}$ 数值爆炸，迭代容易发散
  • 无解：目标点超出工作空间（可达范围），怎么解都到不了

说人话：逆运动学是"我想让手到这，每个关节该转多少"。这是抓取、轨迹规划里天天要算的东西——MoveIt2 里的 KDL / IKFast 求解器，干的就是这件事

• 用雅可比伪逆迭代求解上面的 IK（完整代码见下篇附录二），蓝色细线是迭代的中间位形，绿色粗线是收敛解——多迭代几次，末端就"吸"到目标点了：

3-4 DH 参数

3-4-1 定义

• 用齐次变换矩阵做 FK 时，最规范的方法就是 DH 参数（Denavit–Hartenberg）
• 它是 1955 年 Denavit 和 Hartenberg 提出的标准做法：给每个连杆坐标系定好建法，用 4 个参数 把相邻两连杆之间的相对位姿"编码"清楚，从此正运动学就变成填表 + 连乘的机械操作
• DH 约定：给每个连杆建立坐标系，用 4 个参数唯一描述相邻两个坐标系之间的变换：

参数	含义	说明
$a_{i-1}$	连杆长度	沿 $x_{i-1}$ 轴，从 $z_{i-1}$ 移到 $z_i$ 的距离
${\alpha }_{i-1}$	连杆扭转角	绕 $x_{i-1}$ 轴，从 $z_{i-1}$ 转到 $z_i$ 的角度
$d_i$	连杆偏距	沿 $z_i$ 轴，从 $x_{i-1}$ 移到 $x_i$ 的距离
${\theta }_i$	关节角	绕 $z_i$ 轴，从 $x_{i-1}$ 转到 $x_i$ 的角度

• 相邻两连杆坐标系间的变换矩阵：

$$T_{i-1}^i=R_x({\alpha }_{i-1})\cdot T_x(a_{i-1})\cdot R_z({\theta }_i)\cdot T_z(d_i)$$

$$=\left[\begin{array}{cccc}\cos {\theta }_i& -\sin {\theta }_i\cos {\alpha }_{i-1}& \sin {\theta }_i\sin {\alpha }_{i-1}& a_{i-1}\cos {\theta }_i\\ \sin {\theta }_i& \cos {\theta }_i\cos {\alpha }_{i-1}& -\cos {\theta }_i\sin {\alpha }_{i-1}& a_{i-1}\sin {\theta }_i\\ 0& \sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& d_i\\ 0& 0& 0& 1\end{array}\right]$$

• 对旋转关节，${\theta }_i$ 是变量；对移动关节，$d_i$ 是变量
• 一台机械臂的 DH 参数表就是一张 $n\times 4$ 的表格——建好这张表，FK 就只是连乘

3-4-2 二连杆平面机械臂的 DH 参数

• 二连杆平面机械臂的 DH 参数表（两个旋转关节）：

连杆	$a_{i-1}$	${\alpha }_{i-1}$	$d_i$	${\theta }_i$
1	$L_1$	0	0	$q_1$
2	$L_2$	0	0	$q_2$

• 有了这张表格以后，代入 3-4-1 的变换矩阵公式，就能写出每个连杆的变换矩阵（平面臂 ${\alpha }_{i-1}=0$、$d_i=0$，矩阵退化成"绕 $z$ 轴转 + 沿 $x$ 轴移"）：

$$T_0^1(q_1)=\left[\begin{array}{cccc}\cos q_1& -\sin q_1& 0& L_1\cos q_1\\ \sin q_1& \cos q_1& 0& L_1\sin q_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],T_1^2(q_2)=\left[\begin{array}{cccc}\cos q_2& -\sin q_2& 0& L_2\cos q_2\\ \sin q_2& \cos q_2& 0& L_2\sin q_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

• 按 2-6 节的右乘链连乘 $T_0^2=T_0^1\cdot T_1^2$，平移部分正好还原 3-2 节的末端位置：
  • $x=L_1\cos q_1+L_2\cos (q_1+q_2)$，$y=L_1\sin q_1+L_2\sin (q_1+q_2)$
  • DH 表 + 公式 + 连乘，FK 一次到位

说人话：DH 参数就是给机械臂每个关节量"四围"——长度、扭转、偏移、转角，量完填一张表，正运动学就自动出来了。URDF 里每个 joint 的 origin + axis，本质上就是在描述这套参数

• 用 Python 把上面的 DH 表画在二连杆机械臂上（完整代码见下篇附录三），就能直观看到 4 个参数各自"管"什么：红绿箭头是连杆坐标系，$x_0$ 到 $x_1$ 的转角就是 ${\theta }_1=q_1$，$x_1$ 到 $x_2$ 的转角就是 ${\theta }_2=q_2$；连杆长度分别是 $a_0=L_1$、$a_1=L_2$；因为是平面臂，三个 $z$ 轴全部垂直纸面向外（$\alpha =0$），三个 $x$ 轴共面（$d=0$）：

---

4 速度运动学

• 第 3 章的运动学只关心"位置关系"，速度运动学关心"位置怎么随时间变"——也就是关节速度与末端速度的关系

4-1 雅可比矩阵

• 雅可比矩阵（Jacobian）$J$ 是末端速度对关节速度的线性映射矩阵
• 以防你忘记雅可比矩阵（Jacobian）的数学定义：输出对输入的一阶偏导数矩阵

$$J=\frac{\partial f}{\partial \mathbf{x}}$$

• 对 $n$ 关节机械臂，末端有 6 维速度（3 线速度 + 3 角速度），雅可比是一个 $6\times n$ 矩阵：

$$\dot{\mathcal{X}}=\left[\begin{array}{c}\dot{p}\\ \omega \end{array}\right]=J(q)\cdot \dot{q}=\left[\begin{array}{c}{J}_v(q)\\ J_{\omega }(q)\end{array}\right]\dot{q}$$

• 其中：
  • $\dot{p}\in {\mathbb{R}}^3$：末端线速度
  • $\omega \in {\mathbb{R}}^3$：末端角速度
  • $J_v(q)\in {\mathbb{R}}^{3\times n}$：线速度雅可比
  • $J_{\omega }(q)\in {\mathbb{R}}^{3\times n}$：角速度雅可比
• 对旋转关节，雅可比每列的计算：

$$J_{vi}=z_i\times (p_{ee}-p_i),J_{\omega i}=z_i$$

• 对移动关节，只有平移、不贡献角速度：$$J_{vi}=z_i,J_{\omega i}=0$$

  • 其中 $z_i$ 是关节 $i$ 的旋转/移动轴方向，$p_i$ 是关节 $i$ 的位置，$p_{ee}$ 是末端位置

• 雅可比把"关节空间"和"操作空间"连接起来，是运动学、动力学、控制的枢纽

说人话：雅可比就是"关节转 1°/s，末端能跑多快、往哪个方向跑"的换算表。它是速度层的运动学，后面控制里要用的"力 → 力矩"转换也靠它

4-2 关节速度到末端速度

4-2-1 定义

• 正运动学给出位置映射 $x=f(q)$，对时间求导就是：

$$\dot{x}=\frac{\partial f}{\partial q}\cdot \dot{q}=J(q)\dot{q}$$

• 反向呢？已知末端速度求关节速度，$J$ 是方阵且可逆时：$$\dot{q}=J^{-1}(q)\dot{x}$$

• $J$ 不是方阵（冗余机械臂 $n>6$）或不可逆时，用伪逆：$$\dot{q}=J^{†}(q)\dot{x},J^{†}=J^T(J{J}^T{)}^{-1}$$

  • 为什么叫"伪逆"？因为这时 $J^{-1}$ 根本不存在，直接求逆会失败。伪逆是"逆"的推广：只要 $J$ 行满秩（6 自由度机械臂在非奇异位形就是），$J{J}^T$ 可逆，$J^{†}$ 就存在
  • 它解的是一个最小范数问题：在所有满足 $\dot{x}=J\dot{q}$ 的关节速度里，挑范数 $\Vert \dot{q}\Vert$ 最小的那个——用"最省的关节运动"达到同样的末端速度

• 还可以利用雅可比的零空间，在完成主任务的同时做冗余优化（避障、关节限位）：

$$\dot{q}=J^{†}\dot{x}+(I-J^{†}J){\dot{q}}_0$$

• 其中 $(I-J^{†}J)$ 把次任务速度 ${\dot{q}}_0$ 投影到零空间——它在不改变末端速度的前提下调整关节位形

4-2-2 二连杆的雅可比推导

• 以防你忘记，用解析法再把二连杆的雅可比推一遍（平面臂只需 $x,y$ 两行，是 $2\times 2$）：
• 从 3-2 节的 FK 公式出发：

$$x=L_1\cos q_1+L_2\cos (q_1+q_2),y=L_1\sin q_1+L_2\sin (q_1+q_2)$$

• 对 $q_1$、$q_2$ 分别求偏导：

$$\frac{\partial x}{\partial q_1}=-L_1\sin q_1-L_2\sin (q_1+q_2),\frac{\partial x}{\partial q_2}=-L_2\sin (q_1+q_2)$$

$$\frac{\partial y}{\partial q_1}=L_1\cos q_1+L_2\cos (q_1+q_2),\frac{\partial y}{\partial q_2}=L_2\cos (q_1+q_2)$$

• 把这 4 个偏导按行列排好，就是雅可比矩阵：

$$J(q)=\left[\begin{array}{cc}\frac{\partial x}{\partial q_1}& \frac{\partial x}{\partial q_2}\\ \frac{\partial y}{\partial q_1}& \frac{\partial y}{\partial q_2}\end{array}\right]=\left[\begin{array}{cc}-L_1\sin q_1-L_2\sin (q_1+q_2)& -L_2\sin (q_1+q_2)\\ L_1\cos q_1+L_2\cos (q_1+q_2)& L_2\cos (q_1+q_2)\end{array}\right]$$

• 有了上面的矩阵，我们就能直接算末端速度：给定任意一组关节速度 $\dot{q}=[{\dot{q}}_1,{\dot{q}}_2{]}^T$，代进 $\dot{x}=J(q)\cdot \dot{q}$，末端线速度立即出来——这就是本节的"关节速度 → 末端速度"
• 这就是"FK 求导 → J"的解析法，和 4-1 的几何法殊途同归；它的行列式：$\det (J)=L_1{L}_2\sin q_2$——这直接引出下面的奇异位形

4-3 奇异位形

• 奇异位形（Singularity）：雅可比矩阵降秩的位形
• 对 6 关节方阵雅可比，就是：

$$\text{rank}(J)<6\iff \det (J)=0$$

• 也就是说，这个矩阵不满秩：有某些方向上的速度"映射不出来"。于是奇异位形下会出现三件"坏事"：
  • 末端失去某个方向的运动能力——不管关节怎么转，末端在某方向都"动不了"
  • 关节速度爆炸：要完成某个末端速度，$\dot{q}=J^{-1}\dot{x}$ 中的 $J^{-1}$ 趋于无穷大，关节速度飙到天上去
  • 力控失效：末端在奇异方向上的力映射到关节力矩会很小，机械臂在该方向"无力"
• 二连杆的例子：$\det (J)=L_1{L}_2\sin q_2=0$ 当且仅当 $q_2=0$ 或 $q_2=\pm \pi$
  • $q_2=0$：手臂完全伸直——末端只能沿径向（手臂方向）移动，无法沿切向移动
  • $q_2=\pm \pi$：手臂完全折叠——末端只能沿切向移动，无法沿径向移动
• 处理奇异的方法：

  • 规划时避开奇异位形（代价函数里惩罚接近奇异的位形）

  • 奇异附近用阻尼最小二乘（DLS）：$$J^{†}=J^T(J{J}^T+{\lambda }^{2}I{)}^{-1}$$

  • 多出来的 ${\lambda }^{2}I$ 是阻尼项，牺牲一点精度换数值稳定——下篇附录二我们把它的几何效果画出来

说人话：奇异位形就是机械臂的"死锁姿势"——比如手臂完全伸直时，你只能推着它向外动，没法让它"横着走"。这时候关节转速会飙到天上去，是实际控制中必须躲开的坑

• 下面左图画出 $\det (J)=L_1{L}_2\sin q_2$ 随 $q_2$ 的变化——在 $q_2=0,\pm \pi$ 处归零，正是奇异位形；右图是 $q_2=0$（手臂完全伸直）时的情形，末端只能沿绿色箭头（径向）移动，红色箭头（切向）方向"锁死"（完整代码见下篇附录二）：

---

5 动力学

• 动力学研究力与运动的关系——机械臂在重力、外力、电机力矩作用下怎么运动
• 如果说运动学是"几何学"，动力学就是"物理学"

5-1 机械臂动力学方程

• 关节空间中，机械臂的动力学方程（标准形式）：

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau$$

• 各符号含义（这张表后面推导会反复用，以防你忘记）：

符号	含义	单位
$M(q)$	惯性矩阵（质量/惯量分布）	$\text{kg}\cdot {\text{m}}^2$
$C(q,\dot{q})$	科氏力 + 离心力矩阵	—
$g(q)$	重力项	$\text{N}\cdot \text{m}$
$\tau$	关节驱动力矩	$\text{N}\cdot \text{m}$
$q,\dot{q},\ddot{q}$	关节位置、速度、加速度	rad, rad/s, rad/s²

• 每一项的物理含义：

  • $M(q)\ddot{q}$：克服惯性——机械臂本身"懒"，要加速就得用力
  • $C(q,\dot{q})\dot{q}$：科氏力/离心力——关节转动产生的"耦合"和"甩出去"的力
  • $g(q)$：重力——把机械臂"往上抬"要克服的力

• 这些符号不是凭空来的，它们由每个连杆的物理量组合而成——先说清楚动力学的"原料"：

  • 质量 $m_i$：连杆 i 的质量
  • 质心 $c_i$：连杆 i 质量中心的位置
  • 转动惯量 $I_i\in {\mathbb{R}}^{3\times 3}$：连杆 i 绕质心的惯性张量，衡量"转起来有多费力"

• 惯性矩阵 $M(q)$ 由所有连杆的质量、质心、惯量组合而成，且：

  • $M(q)$ 是正定对称矩阵：对任意 $q$，$M(q)=M(q{)}^T\succ 0$
  • $M(q)$ 随 $q$ 变化：手臂姿势不同，等效惯量不同——伸出去时惯性大，收回来时惯性小

• 重力项来自重力势能：

$$g(q)=\frac{\partial V(q)}{\partial q}$$

• 其中 $V(q)={\sum }_i{m}_{i}g{h}_i(q)$ 是系统的重力势能，$h_i$ 是连杆 i 质心高度
• 方程右边的驱动力矩 $\tau$ 还和末端力 $f$ 通过雅可比联系起来（力 - 力矩对偶）：

$$\tau =J^T(q)\cdot f$$

• 这表示：末端要产生力 $f$，各关节需要提供力矩 $J^{T}f$——这正是阻抗控制、力控的基础，第 7 章控制会用到

说人话：这个方程就是机械臂版的牛顿第二定律 $F=ma$——左边是"动起来需要的力"（惯性 + 科氏离心 + 重力），右边是"电机给的力"。两边平衡了，机械臂就按你想要的加速度运动。方程里的"原料"——质量管"多重"、惯量管"多难转"、重力管"要扛多大分量"，其中重力最坑：手臂伸平 vs 垂下，重力矩能差好几倍，后面逆动力学一节会看到怎么对付它

5-2 拉格朗日法（能量法）

• 那这个方程是哪来的？它来自拉格朗日力学（能量法）——机械臂动力学的两大经典来源之一
• 拉格朗日法不逐连杆列力平衡，而是只看系统的能量，只需要写一个标量函数（拉格朗日函数）：
  $$L(q,\dot{q})=T-V$$
• 其中 $T$ 是动能，$V$ 是势能。对旋转关节机械臂，动能是关节速度的二次型，势能只依赖 $q$：
  $$T=\frac{1}{2}{\dot{q}}^{T}M(q)\dot{q},V=V(q)$$
• 代入欧拉-拉格朗日方程，动力学方程就"机械地"长出来了：
  $$\frac{d}{dt}\frac{\partial L}{\partial \dot{q}}-\frac{\partial L}{\partial q}=\tau$$
• 逐项求导（以防你忘记：$M(q)$ 只依赖 $q$，所以对时间求导会出现 $\dot{M}$）：
  $$\frac{d}{dt}\frac{\partial T}{\partial \dot{q}}=M\ddot{q}+\dot{M}\dot{q},\frac{\partial T}{\partial q}=\frac{1}{2}\frac{\partial ({\dot{q}}^{T}M\dot{q})}{\partial q},\frac{\partial V}{\partial q}=g(q)$$
• 整理一下，三个项的"出身"就一目了然了：
  • $M(q)\ddot{q}$：动能对 $\dot{q}$ 求导、再对时间求导——惯性项
  • $C(q,\dot{q})\dot{q}=\dot{M}\dot{q}-\frac{1}{2}\frac{\partial ({\dot{q}}^{T}M\dot{q})}{\partial q}$：$M(q)$ 对 $q$ 求导的组合——科氏/离心项不是凭空加的，它是惯性项的"派生物"
  • $g(q)=\partial V/\partial q$：势能对 $q$ 求导——重力项

说人话：拉格朗日法只要喂一个标量 $L=T-V$（一个数！），方程自己就长出来了，所以叫"能量法"。它的强项是分析——闭式公式、每项物理意义清清楚楚；代价是推导繁琐，大规模机械臂手推很累

5-3 牛顿-欧拉法（递推法）

• 拉格朗日法从能量出发，适合推导闭式；牛顿-欧拉法对每个连杆老老实实列力/力矩平衡，是逆动力学的经典算法（复杂度 $O(n)$，实时性好，Pinocchio 这类刚体动力学库底层就是它）。全程只靠两条物理定律：
  • 牛顿方程（力平衡）：$F=ma$
  • 欧拉方程（力矩平衡）：$N=I\dot{\omega }+\omega \times (I\omega )$
• 整条算法就是两条递推，像链式反应一样一正一反把整条链扫两遍：
  • 前向递推（基座 → 末端）：由 $q,\dot{q},\ddot{q}$ 沿链算出每个连杆的角速度 ${\omega }_i$、角加速度 ${\dot{\omega }}_i$、质心加速度 ${\dot{v}}_{C_i}$，再用牛顿/欧拉方程得到该连杆的惯性力 $F_i$ 和惯性力矩 $N_i$
  • 后向递推（末端 → 基座）：从末端外力 $f_{n+1}=0$ 出发，倒着解每个连杆的力/力矩平衡，得到每个关节要输出的力 $f_i$ 和力矩 ${\tau }_i$
• 前向递推的核心公式（以旋转关节为例）：
  $${\omega }_{i+1}={\omega }_i+{\dot{\theta }}_{i+1}{\hat{z}}_{i+1}$$
  $${\dot{\omega }}_{i+1}={\dot{\omega }}_i+{\ddot{\theta }}_{i+1}{\hat{z}}_{i+1}+{\omega }_i\times ({\dot{\theta }}_{i+1}{\hat{z}}_{i+1})$$
  $${\dot{v}}_{C_{i+1}}={\dot{v}}_{i+1}+{\dot{\omega }}_{i+1}\times r_{C_{i+1}}+{\omega }_{i+1}\times ({\omega }_{i+1}\times r_{C_{i+1}})$$
  $$F_{i+1}=m_{i+1}{\dot{v}}_{C_{i+1}},N_{i+1}=I_{C_{i+1}}{\dot{\omega }}_{i+1}+{\omega }_{i+1}\times (I_{C_{i+1}}{\omega }_{i+1})$$
• 后向递推的力/力矩平衡（从末端往基座收）：
  $$f_i=f_{i+1}+F_i,n_i=n_{i+1}+N_i+r_{C_i}\times F_i+r_{i+1}\times f_{i+1},{\tau }_i={\hat{z}}_i^T{n}_i$$

5-4 正动力学与逆动力学

• 5-2、5-3 给了两种求解动力学的方法，先对比一下它们的区别：

对比维度	拉格朗日法（5-2 节）	牛顿-欧拉法（5-3 节）
出发点	系统总能量 $L=T-V$	每个连杆的力/力矩平衡
手段	欧拉-拉格朗日方程求偏导	前向 + 后向两条递推
产物	闭式方程 $M(q),C(q,\dot{q}),g(q)$	关节力矩 $\tau$
计算复杂度	符号推导繁琐、计算量大	$O(n)$ 递推、实时性好
擅长	理论分析、建闭式模型	实时控制、逆动力学
典型场景	正动力学（仿真推运动）	逆动力学（控制算力矩）

• 两种方法殊途同归，最后都落回 5-1 节的动力学方程。方程到手了，接下来按方向分正、逆两种用法（和运动学一样）：

方向	已知	求解	用途
正动力学	力矩 $\tau$	运动 $q,\dot{q},\ddot{q}$	仿真（Gazebo 用的就是它）
逆动力学	期望运动 $q,\dot{q},\ddot{q}$	力矩 $\tau$	控制（计算力矩控制）

• 正动力学：数值积分 $\ddot{q}=M^{-1}(q)(\tau -C\dot{q}-g)$，一步一步推出运动——仿真器（Gazebo、Mujoco）的物理引擎就是干这个的
• 逆动力学：把期望加速度代进去算力矩——做控制时要算"让机械臂按期望运动需要多大扭矩"
• 逆动力学最常用的一个例子就是重力补偿（Gravity Compensation）：机械臂悬空时电机断电会被重力拽下去，所以要让机械臂"停住"，电机必须一直出力对抗重力。重力补偿 = 用逆动力学算出当前位形下的重力项 $g(q)$，把它加进控制力矩里抵消重力：

$${\tau }_{comp}=g(q)$$

• 补偿后机械臂像在"失重"环境里工作，悬停时电机力矩刚好等于重力项、稳稳停住；MoveIt2/UR 的很多 demo 默认开着重力补偿，目的就是让手动拖拽（拖动示教）“轻如无物”——这是逆动力学在实物上最有体验感的应用
• 两条路线殊途同归——用二连杆做个数值对账（完整代码见下篇附录四）：随机抽 20 组 $(q,\dot{q},\ddot{q})$，拉格朗日闭式（5-2 的 $M,C,g$）和牛顿-欧拉递推（5-3 节）各算一遍关节力矩，结果全部落在 $y=x$ 直线上，最大误差 $10^{-15}$ 量级（浮点精度）：

说人话：正动力学是"给力，看它怎么动"（仿真）；逆动力学是"想要它这么动，得给多少力"（控制）。Pinocchio 就是干这个的 C++ 刚体动力学库，后面讲 VLA 之前我们会专门用一期来玩它

---

---

下篇

• 运动规划（关节空间三次多项式/梯形/S 曲线、笛卡尔空间）、控制（串级 PID、计算力矩、阻抗/导纳）、全链路串联（第 8 章），以及全部可视化代码附录，详见下篇：【玩转VLA具身智能机械臂】（一）：机械臂基础（下篇）——规划、控制与全链路
• 上篇建好了模型，下篇就看机械臂怎么"动"起来