本篇笔记重点在于各种机械臂的原理推导.公式的数学书写和推导基本都是AI书写.
在此AI时代虽然不会亲自指出这种原理性的知识,但是想要成为真正的工程师,哪怕自己一行代码不写,哪怕全程vibe coding,但是仍然需要基本这种原理理解,算法推导的能力.这就是我写此篇笔记原因.
$$
q = [q_1,q_2,\ldots,q_n]^T.
$$
$$
\dot q = \frac{dq}{dt},\qquad \ddot q=\frac{d^2q}{dt^2}.
$$
那么根据这些假设要回答三件事：
- 位置问题（运动学）：已知 $q$，末端位姿 $x$ 是什么？
$$
x=f(q).
$$
- 速度问题（微分运动学）：已知 $\dot q$，末端速度 $\dot x$ 是什么？
$$
\dot x=J(q)\dot q.
$$
- 力学问题（动力学）：为获得给定的 $q,\dot q,\ddot q$，各关节所需力矩 $\tau$ 是什么？
$$
\tau=M(q)\ddot q+C(q,\dot q)\dot q+g(q)+\tau_f-\tau_{\rm ext}.
$$
其中 这里面定义的$M$ 是惯性矩阵，$C\dot q$ 是速度相关项（科氏项与离心项），$g$ 是重力项，$\tau_f$ 是摩擦项，$\tau_{\rm ext}$ 是外界施加在系统上的广义力矩。
0.2 从一个简单的二连杆模型推理开始
我们在这里建立平面 $xy$ 坐标系：$x$ 轴水平向右，$y$ 轴竖直向上；重力加速度方向为 $-y$。二连杆均为刚体，两个关节均为转动关节。
其中这里要说明一下,角度 $q_2$ 是相对角：第二根连杆相对第一根连杆旋转 $q_2$。因此第二根连杆相对于世界坐标系的绝对角为：
$$
\theta_2=q_1+q_2.
$$
但是绝大部分时候我们使用的都是相对角去计算
---
1. 涉及到的各种表达
1.1 点在不同坐标系中的表示
同一个几何点 $P$，相对坐标系 $\{A\}$ 的坐标记作 ${}^A\!p$，相对坐标系 $\{B\}$ 的坐标记作 ${}^B\!p$。若 $\{B\}$ 原点在 $\{A\}$ 中的位置是 ${}^A\!p_B$，且 $\{B\}$ 相对 $\{A\}$ 的旋转矩阵为 ${}^A\!R_B$，则：
$$
{}^A\!p = {}^A\!p_B+{}^A\!R_B\,{}^B\!p.
$$
这不是额外假设，而是向量首尾相接：从 $A$ 原点到点 $P$ 的向量，等于“从 $A$ 原点到 $B$ 原点”加上“从 $B$ 原点到 $P$”。后一个向量先要旋转到 $A$ 的坐标基下才能相加,也就是向量的基本表达,方向一致。
1.2 二维旋转矩阵的推导
为了防止后续容易蒙.这里做了一个简易的推导,可以理解连续旋转时，旋转矩阵进行乘法，而由于二维旋转矩阵的特殊结构，矩阵乘法等价于旋转角度相加
令单位向量 $e_x=[1,0]^T$ 绕原点逆时针旋转 $\theta$，得到：
$$
e_x' = [\cos\theta,\sin\theta]^T.
$$
同理，$e_y=[0,1]^T$ 旋转后为：
$$
e_y' = [-\sin\theta,\cos\theta]^T.
$$
把旋转后的两个新基向量作为矩阵的两列，得到：
$$
R(\theta)=
\begin{bmatrix}
\cos\theta & -\sin\theta\\
\sin\theta & \cos\theta
\end{bmatrix}.
$$
任意向量 $p=[x,y]^T$ 旋转后为：
$$
p'=R(\theta)p
=
\begin{bmatrix}
x\cos\theta-y\sin\theta\\
x\sin\theta+y\cos\theta
\end{bmatrix}.
$$
旋转不改变长度，因此：
$$
R^TR=I,\qquad R^{-1}=R^T=R(-\theta).
$$
连续旋转 $\alpha$ 和 $\beta$ 等价于旋转 $\alpha+\beta$：
$$
R(\alpha)R(\beta)=R(\alpha+\beta).
$$
矩阵是相乘，角度才是相加
1.3 齐次变换与变换连乘
我们前面分别讨论了旋转和平移，但这只是为了方便好理解一些.因为真正的机械臂的每一节连杆通常会同时发生这两件事：
- 坐标系的方向发生改变，这是旋转R；
- 坐标系的原点发生移动，这是平移p。
如果分别写，表达式会变得很长。齐次变换的做法是：增加一个坐标维度，把旋转和平移放进同一个矩阵里。
把旋转和平移合写为：
$$
T=
\begin{bmatrix}
R & p\\
0 & 1
\end{bmatrix}.
$$
这里的 $R$ 描述方向变化，$p$ 描述原点的平移。右下角的 $1$ 和左下角的 $0$ 是为了让平移可以和旋转一起参与矩阵乘法。因此，齐次变换不是一种新的物理运动，而是统一记录旋转和平移的数学工具。
二维点写成齐次坐标：
$$
\bar p=
\begin{bmatrix}x\\y\\1\end{bmatrix}.
$$
普通二维点只有 $(x,y)$ 两个数字。最后补上的 $1$ 不代表点真的多了一个空间方向，只是一个数学辅助量。它使下面的矩阵乘法能够同时完成“旋转后再平移”。
则刚体变换为：
$$
\bar p_A=
\begin{bmatrix}
R_{AB}&p_{AB}\\0&1
\end{bmatrix}
\bar p_B.
$$
把这个乘法展开，就得到：
$$
\begin{bmatrix}
R_{AB}&p_{AB}\\0&1
\end{bmatrix}
\begin{bmatrix}{}^B\!p\\1\end{bmatrix}
=
\begin{bmatrix}R_{AB}{}^B\!p+p_{AB}\\1\end{bmatrix}.
$$
因此它仍然只做两件事：先旋转点的坐标，再加上平移量。
这里 $T_A^B$ 的意思是：把 $\{B\}$ 坐标系中的坐标转换成 $\{A\}$ 坐标系中的坐标。上标表示结果使用哪个坐标系，下标表示原始坐标来自哪个坐标系。
若要把 $\{2\}$ 中的坐标转换到 $\{0\}$，并且中间经过 $\{1\}$，则：
$$
T_0^2=T_0^1T_1^2.
$$
就可以把它理解成两步：
- $T_1^2$：先把 $\{2\}$ 中的坐标转换到 $\{1\}$；
- $T_0^1$：再把 $\{1\}$ 中的坐标转换到 $\{0\}$。
所以，先执行的变换写在右边，后执行的变换写在左边。矩阵的顺序不能随意交换。
矩阵连乘来自代入：
$$
\bar p_0=T_0^1\bar p_1=T_0^1(T_1^2\bar p_2)=(T_0^1T_1^2)\bar p_2.
$$
这和普通代数中的代入完全一样。若 $a=bx$、$b=cy$，那么 $a=b(cy)=(bc)y$；机械臂中只是把普通数字换成了变换矩阵,前面将旋转和平移归一到同一个矩阵当中的时候目的就是为了可以在此进行表达.简易点来说就是旋转平移矩阵变换到另一个位姿态的时候,要先进行旋转的统一之后加上平移量。
对 $n$ 个串联关节：
$$
T_0^n=T_0^1T_1^2\cdots T_{n-1}^n.
$$
机械臂是一节连杆接着一节连杆。每个关节只描述相邻两个坐标系之间的相对运动，所以要逐级变换，最后才能得到末端相对基座的整体位姿。
1.4 三维刚体位姿的最小表示
前面所处于的位姿都是在二维空间进行变换的,在二维描述一个刚体的时候需要两个位置量和一个旋转角.但是在三位中想要描述一个刚体的位姿,则需要三维的位置空间XYZ.和三个姿态量pitch,yaw,roll
三维位置写成 $p\in\mathbb R^3$，姿态用旋转矩阵 $R\in SO(3)$ 表示：
$$
SO(3)=\{R\in\mathbb R^{3\times3}\mid R^TR=I,\det R=1\}.
$$
其中 $R^TR=I$ 表示旋转后坐标轴仍然互相垂直且长度为 $1$；$\det R=1$ 表示它是正常旋转，而不是镜像翻转。虽然矩阵有 $9$ 个元素，但元素之间受约束，真正独立的姿态自由度只有 $3$。
绕世界 $z$ 轴旋转：
$$
R_z(\theta)=
\begin{bmatrix}
\cos\theta&-\sin\theta&0\\
\sin\theta&\cos\theta&0\\
0&0&1
\end{bmatrix}.
$$
完整三维齐次变换为：
$$
T=
\begin{bmatrix}
R&p\\
0_{1\times3}&1
\end{bmatrix}.
$$
三维和二维的逻辑完全相同：左上角的 $R$ 负责旋转，右上角的 $p$ 负责平移。只是二维使用 $2\times2$ 的旋转矩阵和 $3\times3$ 的齐次矩阵，三维使用 $3\times3$ 的旋转矩阵和 $4\times4$ 的齐次矩阵。
综上，机械臂末端相对基座的三维位姿可以写成：
$$
T_0^e=
\begin{bmatrix}
R_0^e&p_0^e\\
0_{1\times3}&1
\end{bmatrix}.
$$
其中 $p_0^e$ 是末端原点在基座坐标系中的位置，$R_0^e$ 是末端坐标轴相对基座坐标轴的方向。
---
2. 正运动学（Forward Kinematics, FK）
正运动学要解决的问题就很简单了,就是在已知每个关节的关节角度的情况下,去求机械臂末端的位置和姿态
这里的正指的
$$
\text{关节变量 }q
\quad\Longrightarrow\quad
\text{末端位置与姿态 }x_e
$$
所以 FK 的本质是一个函数：
$$
x_e=f(q).
$$
我们先用一个很简单的二连杆模型进行理解,这个模型虽然很简单,但是其中的最重要的原理已经囊括其中：每一节连杆的位置，等于前一节末端位置加上本节连杆在世界坐标系中的位移。 其实结合上面的原理讲解,大致也能有理解这句话的含义了
2.1 二连杆末端位置的直接推导
假设第一关节位于坐标原点，第一根连杆长度为 $l_1$，第二根连杆长度为 $l_2$，关节角都逆时针为正。
这里要特别注意：$q_2$ 表示第二根连杆相对第一根连杆的转角，而不是第二根连杆相对世界坐标系的绝对角。因此第二根连杆相对世界的方向是：
$$
\theta_2=q_1+q_2.
$$
为什么是相加？因为第二根连杆先跟随第一根连杆整体转过 $q_1$，然后又相对第一根连杆转过 $q_2$。
以及这里的世界的方向不要简单理解成宏观的世界了,最好是还是以besa_link为参考系进行观察.
2.1.1 一根连杆的坐标怎么得到?
一根长度为 $l$、方向角为 $\theta$ 的杆，其水平投影和竖直投影分别是：
$$
\text{水平投影}=l\cos\theta,
\qquad
\text{竖直投影}=l\sin\theta.
$$
用向量写就是：
$$
\begin{bmatrix}x\\y\end{bmatrix}
=
\begin{bmatrix}l\cos\theta\\l\sin\theta\end{bmatrix}.
$$
所以，FK 中出现的 $\cos$ 和 $\sin$，本质上只是把一根有方向的连杆分解成水平和竖直两个分量。
也就是使用两个向量描述一个主向量,这个主向量就是我们要求的杆长.
第一根连杆末端的位置为：
$$
p_1=
\begin{bmatrix}
l_1\cos q_1\\l_1\sin q_1
\end{bmatrix}.
$$
这里的意思是:从基座出发，沿第一根连杆走 $l_1$。由于第一根连杆的方向角是 $q_1$，所以它在 $x$ 方向贡献 $l_1\cos q_1$，在 $y$ 方向贡献 $l_1\sin q_1$。
第二根连杆在世界坐标系中的方向角为 $q_1+q_2$，故从第一关节到末端的位移为：
$$
p_{2/1}=
\begin{bmatrix}
l_2\cos(q_1+q_2)\\l_2\sin(q_1+q_2)
\end{bmatrix}.
$$
这里的 $p_{2/1}$ 可以读作“从第一个关节指向第二个关节的位移”。它不是末端的绝对位置，而只是第二根连杆贡献的那一段位移。
由于第二根连杆相对世界的方向角是 $q_1+q_2$，它在两个坐标方向上的贡献分别是：
$$
\begin{aligned}
\text{$x$ 方向贡献}&=l_2\cos(q_1+q_2),\\
\text{$y$ 方向贡献}&=l_2\sin(q_1+q_2).
\end{aligned}
$$
向量相加：
$$
p_e=p_1+p_{2/1}.
$$
这一步就是向量的“首尾相接”：先从基座走到第一个关节，再从第一个关节沿第二根连杆走到末端。两段位移相加，就得到基座到末端的总位移。
于是末端位置：
$$
\boxed{
\begin{aligned}
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).
\end{aligned}}
$$
这两个式子就是二连杆机械臂的 FK 方程。给定 $q_1,q_2,l_1,l_2$ 后，代入公式即可得到末端的 $(x,y)$。
例如当 $q_1=0,q_2=0$ 时，两根连杆完全沿 $x$ 轴伸直：
$$
x=l_1+l_2,\qquad y=0.
$$
当 $q_1=\frac{\pi}{2},q_2=0$ 时，两根连杆完全沿 $y$ 轴伸直：
$$
x=0,\qquad y=l_1+l_2.
$$
这两个特殊情况可以用来检查 FK 公式的方向和正负号是否正确。
末端朝向为第二根连杆的绝对角：
$$
\phi=q_1+q_2.
$$
因此平面完整末端位姿可以记为：
$$
x_e=[x,y,\phi]^T=f(q).
$$
这里的 $x_e$ 不要和单独的坐标 $x$ 混淆。单独的 $x$ 是末端的水平位置；$x_e$ 是完整的末端状态，同时包含水平位置、竖直位置和末端方向：
$$
x_e=
\begin{bmatrix}
\text{末端水平位置}\\
\text{末端竖直位置}\\
\text{末端方向角}
\end{bmatrix}.
$$
2.2 用变换矩阵重写 FK
前面用三角函数直接写出了二连杆的坐标。现在换一种写法：不给每个坐标单独展开，而是给每一节连杆建立一个“局部变换矩阵”。
这个矩阵表示两个连续动作：先绕当前关节旋转 $q_i$，再沿旋转后的新 $x$ 轴前进 $l_i$。
定义“旋转 $q_i$，再沿新 $x$ 轴平移 $l_i$”的二维齐次变换：
$$
A_i(q_i)=
\begin{bmatrix}
\cos q_i&-\sin q_i&l_i\cos q_i\\
\sin q_i& \cos q_i&l_i\sin q_i\\
0&0&1
\end{bmatrix}.
$$
矩阵右上角的两项：
$$
\begin{bmatrix}
l_i\cos q_i\\l_i\sin q_i
\end{bmatrix}
$$
正好就是长度为 $l_i$、方向角为 $q_i$ 的连杆位移。因此这个矩阵没有引入新的物理内容，只是把“旋转后的方向”和“沿该方向移动一段长度”打包起来。
二连杆末端变换为：
$$
T_0^e=A_1(q_1)A_2(q_2).
$$
矩阵乘法的顺序仍然遵循前面的规则：先执行的局部变换在右边，后执行的局部变换在左边。这里先应用 $A_2$，再应用 $A_1$，所以结果是 $A_1A_2$。
矩阵相乘后，左上角给出末端姿态，右上角给出末端位置：
$$
T_0^e=
\begin{bmatrix}
R_0^e&p_0^e\\
0&1
\end{bmatrix}.
$$
其中 $p_0^e=[x,y]^T$，而 $R_0^e$ 描述末端相对基座的旋转。
将矩阵相乘后，右上角两项即为前节的 $x,y$ 公式。这给出多关节机械臂 FK 的一般结构：
$$
T_0^e(q)=\prod_{i=1}^{n}A_i(q_i).
$$
因此，直接三角函数法和齐次变换法是同一件事的两种写法：三角函数法适合看清楚每一项从哪里来，齐次变换法适合把多节连杆统一组织起来。
这里其实也并没有故弄玄虚,只要能理解这部分就是为了建立一个可以同时描述旋转和平移的矩阵就好,哪怕看不懂公式也要知道最基本的这里在做什么就好.
3. 逆运动学（Inverse Kinematics, IK）
正运动学是“已知关节角，求末端位置”。逆运动学正好反过来：
已知希望末端到达的位置，反过来求每个关节应该转到什么角度。
FK 写成：
$$
x=f(q).
$$
IK 要解决的是：已知目标位置 $x$，求满足下面方程的 $q$：
$$
f(q)=x.
$$
这里最容易产生一个误解：不能简单的理解成正解反过来进行解算就可以了.因为 FK 里通常有很多三角函数叠在一起，反解时可能出现无解、多个解，甚至无穷多个解。
我们还是继续用二维二连杆。目标末端位置记为：
$$
p_e=
\begin{bmatrix}
x\\y
\end{bmatrix}.
$$
这是此时的末端位姿,然后我们的目的就是在这个位姿态的基础上运动到另外一个位姿 $$q1,q2$$
3.1 先看目标点是否在工作范围内
先不要急着求角度。把基座原点和目标点连起来，设这条线的长度为 $\rho$。根据勾股定理：
$$
\rho=\sqrt{x^2+y^2}.
$$
现在把二连杆看成一个三角形：
- 第一根连杆是一条长度为 $l_1$ 的边；
- 第二根连杆是一条长度为 $l_2$ 的边；
- 基座到目标点的连线是一条长度为 $\rho$ 的边。
这个三角形能不能成立，决定 IK 有没有解。
如果目标点太远，连杆够不着；如果目标点太近，而两根连杆又不能完全折叠，同样够不着。这就是 IK 的第一个问题：目标点是否几何可达。
3.2 求第二个关节角 $q_2$
二连杆的三条边满足余弦定理：
$$
\rho^2=l_1^2+l_2^2+2l_1l_2\cos q_2.
$$
这里使用的是相对关节角 $q_2$。它描述两根连杆之间的夹角关系。
先移项：
$$
2l_1l_2\cos q_2
=
\rho^2-l_1^2-l_2^2.
$$
再除以 $2l_1l_2$：
$$
\cos q_2
=
\frac{\rho^2-l_1^2-l_2^2}{2l_1l_2}.
$$
因为 $\rho^2=x^2+y^2$，所以：
$$
\boxed{
\cos q_2=
\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}}.
$$
为了让公式短一点，记：
$$
c_2=
\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}.
$$
于是：
$$
\cos q_2=c_2.
$$
接下来由三角恒等式：
$$
\sin^2q_2+\cos^2q_2=1
$$
得到：
$$
\sin q_2=\pm\sqrt{1-c_2^2}.
$$
这里的正负号非常重要。因为对应两种不同的机械臂姿态：
- $\sin q_2>0$：肘部朝向一侧；
- $\sin q_2<0$：肘部朝向另一侧。
在图上看，就是同一个目标点，机械臂可以从目标点的上方绕过去，也可以从下方绕过去。两种姿态的末端位置相同，但关节角不同。
因此用同时考虑正弦和余弦的函数表示：
$$
\boxed{
q_2=\operatorname{atan2}
\left(\sin q_2,\cos q_2\right)}
$$
也就是：
$$
\boxed{
q_2=\operatorname{atan2}
\left(\pm\sqrt{1-c_2^2},c_2\right)}.
$$
之所以使用 $\operatorname{atan2}$，而不直接使用 $\arccos$，是因为：
$$
\arccos(c_2)
$$
只能根据余弦值给出一个主值，不能明确区分 $\sin q_2$ 的正负；而 $\operatorname{atan2}$ 同时接收正弦和余弦，因此可以保留角度所在的方向。
3.3 求第一个关节角 $q_1$
前面用三角形关系已经找到了第二关节角 $q_2$。这时两根杆之间“折成什么样”已经定下来了，剩下的问题是：整条手臂要朝哪个方向摆，才能让末端对准目标点？
先简单理解一下：把第一根杆想象成暂时沿着水平 $x$ 轴放好，也就是先令 $q_1=0$。这只是在纸上方便看几何关系，真实机械臂的姿态还没改变。
在这个临时姿态里：
- 第一根杆的位移是 $[l_1,0]^T$；
- 第二根杆相对第一根杆转过 $q_2$，位移是 $[l_2\cos q_2,l_2\sin q_2]^T$。
两段相加，末端向量为：
$$
u(q_2)=
\begin{bmatrix}
l_1+l_2\cos q_2\\
l_2\sin q_2
\end{bmatrix}.
$$
这个向量已经包含两根杆的长度和它们之间的夹角。它的方向角记为 $\beta$：
$$
\beta=
\operatorname{atan2}
\left(
l_2\sin q_2,\,
l_1+l_2\cos q_2
\right).
$$
目标点相对基座的向量是：
$$
p_d=
\begin{bmatrix}
x\\y
\end{bmatrix}.
$$
从基座指向目标点的角度记为 $\alpha$：
$$
\alpha=\operatorname{atan2}(y,x).
$$
现在把刚才那个“临时摆好的二连杆形状”整体绕基座转动 $q_1$。向量 $u$ 原本朝向 $\beta$，整体转过 $q_1$ 后就朝向 $\beta+q_1$。它要和目标方向 $\alpha$ 重合，所以：
$$
\beta+q_1=\alpha.
$$
移项：
$$
\boxed{
q_1=
\operatorname{atan2}(y,x)
-
\operatorname{atan2}
\left(
l_2\sin q_2,\,
l_1+l_2\cos q_2
\right)}.
$$
我会把这个式子读成两段角度相减：
- 先看“基座到目标”整体朝哪个方向；
- 再扣掉“二连杆自己折出来的偏角”；
- 剩下的角度就是第一关节需要转的角度。
为什么这里用 $\operatorname{atan2}$
向量 $[x,y]^T$ 的方向可以从横坐标和纵坐标一起确定：
$$
\alpha=\operatorname{atan2}(y,x).
$$
普通的 $\arctan(y/x)$ 只看斜率，容易把不同象限的向量弄混；当 $x=0$ 时还会遇到除零。记住 $\operatorname{atan2}$ 的输入顺序是“纵坐标、横坐标”：
$$
\operatorname{atan2}(y,x).
$$
把 $c_2=\cos q_2$、$s_2=\sin q_2$ 代进去，刚才的结果也可以写成：
$$
q_1=
\operatorname{atan2}(y,x)
-
\operatorname{atan2}
\left(
l_2s_2,\,
l_1+l_2c_2
\right).
$$
我拿 FK 回代检查一次
推逆解时，我习惯最后把结果放回正解里检查。二连杆 FK 是：
$$
\begin{aligned}
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).
\end{aligned}
$$
用和角公式展开：
$$
\begin{aligned}
\cos(q_1+q_2)
&=\cos q_1\cos q_2-\sin q_1\sin q_2,\\
\sin(q_1+q_2)
&=\sin q_1\cos q_2+\cos q_1\sin q_2.
\end{aligned}
$$
代回去之后：
$$
\begin{aligned}
x
&=(l_1+l_2\cos q_2)\cos q_1
-l_2\sin q_2\sin q_1,\\
y
&=(l_1+l_2\cos q_2)\sin q_1
+l_2\sin q_2\cos q_1.
\end{aligned}
$$
这就是把下面这个向量：
$$
\begin{bmatrix}
l_1+l_2\cos q_2\\
l_2\sin q_2
\end{bmatrix}
$$
整体旋转 $q_1$。和刚才先把第一根杆暂时放平、再把整个构型转向目标的想法完全对上。
3.4 二连杆 IK：我会按这个顺序算
给定目标点 $(x,y)$ 和两根杆的长度 $l_1,l_2$，先算第二关节，再算第一关节。
先算基座到目标点的距离：
$$
\rho=\sqrt{x^2+y^2}.
$$
再从余弦定理算 $q_2$ 的余弦：
$$
c_2=
\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2}.
$$
接着选一个肘部方向：
$$
s_2=
+\sqrt{1-c_2^2}
\qquad\text{或}\qquad
s_2=
-\sqrt{1-c_2^2}.
$$
正负号代表两种构型。可以把基座和目标点连成一条线，肘关节通常能落在这条线的两侧。选一个符号，就相当于选其中一侧。
有了正弦和余弦，就能用 $\operatorname{atan2}$ 找 $q_2$：
$$
q_2=\operatorname{atan2}(s_2,c_2).
$$
最后求 $q_1$：
$$
q_1=
\operatorname{atan2}(y,x)
-
\operatorname{atan2}
\left(
l_2s_2,\,
l_1+l_2c_2
\right).
$$
正负号各算一次，得到两组候选角度：
$$
q^{(\sigma)}=
\begin{bmatrix}
q_1^{(\sigma)}\\
q_2^{(\sigma)}
\end{bmatrix},
\qquad
\sigma\in\{+1,-1\}.
$$
我记这个顺序只用一句话：先定目标距离，用余弦定形状，选肘部，再求整体朝向。
3.5 手算一个目标点，看看两组解怎么来
令两根杆长度都是 1，目标点在 $(1,1)$：
$$
l_1=l_2=1,
\qquad
p_d=
\begin{bmatrix}
1\\1
\end{bmatrix}.
$$
目标距离：
$$
\rho=\sqrt{1^2+1^2}=\sqrt{2}.
$$
代入 $c_2$：
$$
c_2=
\frac{1^2+1^2-1^2-1^2}{2\cdot1\cdot1}=0.
$$
所以：
$$
s_2=\pm\sqrt{1-0^2}=\pm1.
$$
先取 $s_2=+1$
$$
q_2=\operatorname{atan2}(1,0)=\frac{\pi}{2}.
$$
再算 $q_1$：
$$
\begin{aligned}
q_1
&=\operatorname{atan2}(1,1)-\operatorname{atan2}(1,1)\\
&=0.
\end{aligned}
$$
所以第一组是：
$$
q_1=0,
\qquad
q_2=\frac{\pi}{2}.
$$
放回 FK：
$$
\begin{aligned}
x&=\cos 0+\cos\frac{\pi}{2}=1,\\
y&=\sin 0+\sin\frac{\pi}{2}=1.
\end{aligned}
$$
确实到达 $(1,1)$。
再取 $s_2=-1$
$$
q_2=\operatorname{atan2}(-1,0)=-\frac{\pi}{2}.
$$
再算 $q_1$：
$$
\begin{aligned}
q_1
&=\operatorname{atan2}(1,1)-\operatorname{atan2}(-1,1)\\
&=\frac{\pi}{4}-\left(-\frac{\pi}{4}\right)\\
&=\frac{\pi}{2}.
\end{aligned}
$$
所以第二组是：
$$
q_1=\frac{\pi}{2},
\qquad
q_2=-\frac{\pi}{2}.
$$
代回 FK：
$$
\begin{aligned}
x&=\cos\frac{\pi}{2}+\cos 0=1,\\
y&=\sin\frac{\pi}{2}+\sin 0=1.
\end{aligned}
$$
两组关节角都把末端带到 $(1,1)$，只是肘关节落在目标连线的不同侧。这个例子也顺手检查了前面推导的两个分支。
3.6 目标点能不能到：从 $c_2$ 的范围看
前面算出：
$$
s_2=\pm\sqrt{1-c_2^2}.
$$
根号里面不能小于零，所以：
$$
1-c_2^2\ge0,
$$
也就是：
$$
-1\le c_2\le1.
$$
把 $c_2$ 的表达式代入：
$$
-1
\le
\frac{\rho^2-l_1^2-l_2^2}{2l_1l_2}
\le
1.
$$
两根杆长度为正，所以可以乘以 $2l_1l_2$：
$$
-2l_1l_2
\le
\rho^2-l_1^2-l_2^2
\le
2l_1l_2.
$$
两边加上 $l_1^2+l_2^2$：
$$
l_1^2+l_2^2-2l_1l_2
\le
\rho^2
\le
l_1^2+l_2^2+2l_1l_2.
$$
左右分别配成平方：
$$
(l_1-l_2)^2
\le
\rho^2
\le
(l_1+l_2)^2.
$$
距离 $\rho$ 不会小于零，开平方后：
$$
\boxed{
|l_1-l_2|
\le
\sqrt{x^2+y^2}
\le
l_1+l_2}.
$$
其实这条不等式可以直接用手臂想出来：
- 两根杆拉成一条直线，够到最远处，距离是 $l_1+l_2$；
- 两根杆尽量往回折，最靠近基座的距离是 $|l_1-l_2|$。
所以如果目标距离大于两根杆总长，手臂够不到；如果两根杆长度不一样，目标也不能落在比长度差更小的圆里面。
在边界上：
$$
\rho=l_1+l_2
\quad\Longrightarrow\quad
q_2=0\pmod{2\pi},
$$
两根杆同向伸直。
$$
\rho=|l_1-l_2|
\quad\Longrightarrow\quad
q_2=\pi\pmod{2\pi},
$$
两根杆方向相反，处于最折叠的边界。
有个特殊点要单独记：如果 $l_1=l_2$，目标又在基座原点，两根杆可以首尾抵消。此时改变 $q_1$，末端仍然停在原点，所以位置本身无法告诉我 $q_1$ 是多少。公式里的 $\operatorname{atan2}(0,0)$ 没有确定方向，正好对应这个几何事实。
3.7 为什么一个位置会有多组关节角
工作空间内部通常有：
$$
-1<c_2<1.
$$
于是 $s_2$ 有正、负两个取值：
$$
s_2=+\sqrt{1-c_2^2}
\qquad\text{或}\qquad
s_2=-\sqrt{1-c_2^2}.
$$
把它们分别代回 $q_2$ 和 $q_1$，就得到两种肘部构型。想象基座和目标点之间拉一根线，肘关节可以在这条线的一边，也可以在另一边。
此外，转角还可以加整圈：
$$
q_i
\quad\text{和}\quad
q_i+2k\pi,
\qquad k\in\mathbb Z,
$$
从几何方向上看代表同一个朝向。真实机械臂的关节有限位，所以最后还要检查候选角度能不能落在允许范围。
还有一个我容易混淆的地方：上面解的是末端位置 $(x,y)$。如果任务还要求末端必须朝某个方向，就多了一个条件：
$$
\phi=q_1+q_2.
$$
平面二连杆只有两个关节变量，却要指定 $x,y,\phi$ 三个量。一般给定位置后，末端方向由选中的那组关节角一起决定，不能再随意指定一个独立方向。自由度更多的机械臂才可能同时满足位置、方向以及其他约束。
算出候选解后，我还要挨个检查：角度是否超限、连杆会不会碰撞、构型是否适合当前动作、从现在的位置能不能连续走过去。公式给出的候选值只是几何解，最后还要看机械臂本身的限制。
3.8 多关节时：在当前姿态附近一点点修
二连杆的几何解比较干净。关节数增加后，解析公式会很长，甚至不容易直接解出来。这时先不要求一口气走到目标，可以只看当前姿态附近的一小步。
设末端任务变量和关节变量的关系为：
$$
x=f(q),
\qquad
q\in\mathbb R^n,
\qquad
x\in\mathbb R^m.
$$
所以这里的雅可比尺寸是 $m\times n$：$m$ 行对应末端任务量，$n$ 列对应关节。先把这个尺寸记住，后面判断能不能求逆时会用到。
让关节从 $q$ 变成 $q+\Delta q$。在当前位置附近做一阶泰勒展开：
$$
f(q+\Delta q)
\approx
f(q)+J(q)\Delta q.
$$
如果把被省略的小量也写出来，就是：
$$
f(q+\Delta q)
=
f(q)+J(q)\Delta q+O(\|\Delta q\|^2).
$$
最后的 $O(\|\Delta q\|^2)$ 表示跟关节变化量的平方同阶的小量。步子越小，这部分相对越不显眼；步子很大时，局部直线近似就可能偏得比较多。
这里先把高阶小量放一边，就得到：
$$
\Delta x\approx J(q)\Delta q,
\qquad
J(q)=\frac{\partial f}{\partial q}.
$$
这句话可以理解成：在当前姿态附近，雅可比告诉我每个关节稍微动一点，末端会往哪边走、走多少。
如果 $J$ 是方阵而且可逆：
$$
\Delta q\approx J^{-1}(q)\Delta x.
$$
如果自由度多于任务维数，也就是 $n>m$，同一个末端小位移可能有多组关节变化。常用伪逆取一个关节变化量较小的解：
$$
\Delta q\approx J^+(q)\Delta x.
$$
当 $J$ 行满秩时，伪逆可以写成：
$$
J^+=J^T(JJ^T)^{-1}.
$$
这个逆矩阵要求 $JJ^T$ 可逆；如果姿态靠近奇异位形，它会变得不好求，算出来的关节变化也可能很大。
当 $J$ 行满秩时：
$$
J^+=J^T(JJ^T)^{-1}.
$$
冗余机械臂还可以在这个解上加一项零空间运动：
$$
\Delta q
=
J^+\Delta x+
(I-J^+J)z.
$$
看它对末端产生的影响：
$$
J(I-J^+J)z
=
(J-JJ^+J)z
=0.
$$
所以第二项在一阶近似下不会改变目标末端位移，可以用来调整肘部、避开限位等。记忆时我会说：第一项负责把末端往目标推，第二项在不动末端的前提下摆内部姿势。
如果目标是 $x_d$，当前关节状态是 $q^{(k)}$，当前误差为：
$$
e^{(k)}=x_d-f(q^{(k)}).
$$
就可以按小步更新：
$$
q^{(k+1)}
=
q^{(k)}
+
\eta J^+(q^{(k)})e^{(k)},
\qquad
0<\eta\le1.
$$
$\eta$ 是步长。太大时可能跨过合适区域，太小时则要走很多步。走完一步以后姿态变了，雅可比也变了，所以再用新姿态重新算。
这套局部方法还有两个需要留意的地方：
- 靠近奇异位形时，伪逆可能给出很大的关节变化；
- 如果任务里有末端方向，方向误差要用合适的角度差表示，不能直接把两个跨过 $\pi$ 的角度做普通相减。
---
第三部分：微分运动学——速度、力与奇异性
4. 雅可比矩阵：关节的小动作怎样传到末端
我把雅可比先当成一张“当前姿态下的换算表”：
$$
\text{关节小变化}
\quad\longrightarrow\quad
\text{末端小变化}.
$$
它只描述当前位置附近的关系。机械臂姿态换了，这张表通常也要跟着换。
4.1 从一个转动杆开始
长度为 $l$ 的杆，关节角为 $q$，末端位置：
$$
p(q)=
\begin{bmatrix}
l\cos q\\
l\sin q
\end{bmatrix}.
$$
角度稍微变化 $\mathrm dq$，位置的变化近似为：
$$
\mathrm dp=\frac{\partial p}{\partial q}\mathrm dq.
$$
对 $q$ 求导：
$$
\frac{\partial p}{\partial q}
=
\begin{bmatrix}
-l\sin q\\
l\cos q
\end{bmatrix}.
$$
这个向量跟杆垂直，正好是圆周切线方向。也就是说，转动关节的小幅运动会让末端沿着圆弧切线走。
如果把 $\mathrm dq$ 除以时间，就得到：
$$
\dot p=
\frac{\partial p}{\partial q}\dot q.
$$
二连杆只是多了一个关节：每个关节各自对末端速度贡献一项，再把这些贡献相加。
4.2 从 FK 对时间求导
二连杆位置写成：
$$
p=
\begin{bmatrix}
x\\y
\end{bmatrix}
=f(q).
$$
链式法则告诉我：
$$
\dot x
=
\frac{\partial x}{\partial q_1}\dot q_1
+
\frac{\partial x}{\partial q_2}\dot q_2,
$$
$$
\dot y
=
\frac{\partial y}{\partial q_1}\dot q_1
+
\frac{\partial y}{\partial q_2}\dot q_2.
$$
把偏导数排成矩阵，就定义出位置雅可比：
$$
J_p(q)=
\begin{bmatrix}
\dfrac{\partial x}{\partial q_1}&
\dfrac{\partial x}{\partial q_2}\\[6pt]
\dfrac{\partial y}{\partial q_1}&
\dfrac{\partial y}{\partial q_2}
\end{bmatrix}.
$$
所以：
$$
\boxed{
\begin{bmatrix}
\dot x\\
\dot y
\end{bmatrix}
=
J_p(q)
\begin{bmatrix}
\dot q_1\\
\dot q_2
\end{bmatrix}}.
$$
我看雅可比的一列时，会想象只动一个关节：
- 第一列：先让 $q_1$ 动一点，其他关节停住，看末端怎么走；
- 第二列：让 $q_2$ 动一点，看末端怎么走。
列向量记录的就是这个关节单独运动时，末端瞬时速度的方向和大小。
4.3 二连杆位置雅可比：逐项求偏导
二连杆 FK：
$$
\begin{aligned}
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).
\end{aligned}
$$
为了不反复抄很长的角度，记：
$$
s_1=\sin q_1,\qquad c_1=\cos q_1,
$$
$$
s_{12}=\sin(q_1+q_2),\qquad
c_{12}=\cos(q_1+q_2).
$$
先对水平位置 $x$ 求偏导：
$$
\frac{\partial x}{\partial q_1}
=
-l_1s_1-l_2s_{12},
$$
$$
\frac{\partial x}{\partial q_2}
=
-l_2s_{12}.
$$
为什么两项不一样？$q_1$ 一动，两根杆的绝对方向都跟着变；$q_2$ 一动，第一根杆方向不变，只有第二根杆跟着变。
再对竖直位置 $y$ 求偏导：
$$
\frac{\partial y}{\partial q_1}
=
l_1c_1+l_2c_{12},
$$
$$
\frac{\partial y}{\partial q_2}
=
l_2c_{12}.
$$
把四个偏导按“第一行对应 $x$、第二行对应 $y$”放进去：
$$
\boxed{
J_p(q)=
\begin{bmatrix}
-l_1s_1-l_2s_{12}&-l_2s_{12}\\
l_1c_1+l_2c_{12}&l_2c_{12}
\end{bmatrix}}.
$$
把它乘以关节速度，展开后就是：
$$
\begin{bmatrix}
\dot x\\
\dot y
\end{bmatrix}
=
\begin{bmatrix}
-l_1s_1-l_2s_{12}&-l_2s_{12}\\
l_1c_1+l_2c_{12}&l_2c_{12}
\end{bmatrix}
\begin{bmatrix}
\dot q_1\\
\dot q_2
\end{bmatrix}.
$$
我用两条来记这个矩阵：
- 第一列能看到两根杆，因为第一关节带着后面整条手臂一起转；
- 第二列只跟第二根杆有关，因为它只直接改变第二根杆的方向。
4.3.1 把机械臂伸直检查一下
令：
$$
q_1=0,\qquad q_2=0.
$$
两根杆沿 $x$ 轴伸直，所以：
$$
s_1=s_{12}=0,
\qquad
c_1=c_{12}=1.
$$
代入：
$$
J_p=
\begin{bmatrix}
0&0\\
l_1+l_2&l_2
\end{bmatrix}.
$$
于是：
$$
\dot x=0,
\qquad
\dot y=(l_1+l_2)\dot q_1+l_2\dot q_2.
$$
末端的一阶速度只有竖直分量。想象一根水平伸直的手臂，关节刚开始转时，末端先沿竖直切线方向动；水平方向要到更高阶的变化才显出来。这个姿态刚好也会在后面看到雅可比退化。
4.4 如果任务还要控制末端方向
第二根杆的绝对方向：
$$
\phi=q_1+q_2.
$$
因此角速度：
$$
\dot\phi=\dot q_1+\dot q_2.
$$
把位置和方向一起写：
$$
x_e=
\begin{bmatrix}
x\\y\\\phi
\end{bmatrix},
\qquad
\dot x_e=
\begin{bmatrix}
\dot x\\\dot y\\\dot\phi
\end{bmatrix}.
$$
对应的平面位姿雅可比：
$$
\boxed{
\dot x_e=J_e(q)\dot q,
\qquad
J_e(q)=
\begin{bmatrix}
-l_1s_1-l_2s_{12}&-l_2s_{12}\\
l_1c_1+l_2c_{12}&l_2c_{12}\\
1&1
\end{bmatrix}}.
$$
这里矩阵是 $3\times2$：末端任务里有 $x,y,\phi$ 三个量，关节只有两个。三个末端速度不能任意独立指定，机械臂能实现的速度必须符合自身的几何约束。
4.5 推到三维：一列一列看关节的作用
三维末端速度通常把线速度和角速度堆在一起：
$$
\begin{bmatrix}
v_e\\
\omega_e
\end{bmatrix}
=
J(q)\dot q,
\qquad
J(q)\in\mathbb R^{6\times n}.
$$
上面三行是线速度，下面三行是角速度。这里约定所有向量都用同一个坐标系表达，下面先用基座坐标系。
后面若把任务量 $x$ 写成“位置加欧拉角”，要记得欧拉角变化率和这里的 $\omega_e$ 需要通过姿态相关的映射转换。算雅可比时，我要先确认自己用的是角速度向量，还是某种姿态参数的导数，不能把两种量直接当成同一个东西。
这里有个我容易混的地方：下半部分的 $\omega_e$ 是角速度向量。如果把末端姿态写成欧拉角，欧拉角的变化率通常不能直接当成这个角速度向量。使用哪种姿态表示，雅可比就要和那种表示配套。
设第 $i$ 个转动关节：
- 转轴方向的单位向量是 $z_i$；
- 转轴上任取一点是 $o_i$；
- 末端位置是 $p_e$。
从转轴上的点到末端的向量为：
$$
r_i=p_e-o_i.
$$
关节转过很小的角度 $\mathrm dq_i$，可以把这次小转动看作角位移向量 $z_i\mathrm dq_i$。刚体小转动引起的点位移为“角位移向量叉乘位置向量”：
$$
\mathrm dp_e
=
(z_i\mathrm dq_i)\times r_i.
$$
把 $\mathrm dq_i$ 提出来：
$$
\mathrm dp_e
=
\left[z_i\times(p_e-o_i)\right]\mathrm dq_i.
$$
所以这个关节对末端线速度的贡献是：
$$
v_i=
\left[z_i\times(p_e-o_i)\right]\dot q_i.
$$
同时，关节自身的角速度贡献沿着转轴：
$$
\omega_i=z_i\dot q_i.
$$
把线速度和角速度放成雅可比的第 $i$ 列：
$$
\boxed{
J_i=
\begin{bmatrix}
z_i\times(p_e-o_i)\\
z_i
\end{bmatrix}}
\qquad\text{（转动关节）}.
$$
如果第 $i$ 个关节是移动关节，它沿 $z_i$ 方向平移，末端线速度也沿这个方向变化，姿态不因此转动：
$$
\boxed{
J_i=
\begin{bmatrix}
z_i\\
0
\end{bmatrix}}
\qquad\text{（移动关节）}.
$$
把每个关节的列排在一起：
$$
J=
\begin{bmatrix}
J_1&J_2&\cdots&J_n
\end{bmatrix}.
$$
我会把三维雅可比的构造记成：一列对应一个关节，列里上半写它带来的线速度，下半写它带来的角速度。
---
5. 由雅可比得到加速度
上一节有：
$$
\dot x=J(q)\dot q.
$$
这句我会读成：当前姿态下，关节速度经过 $J$ 换算成末端速度。要算加速度，就对两边再求一次时间导数。
5.1 乘积法则带出两项
右边有两个会随时间变化的量：$J(q)$ 和 $\dot q$。所以：
$$
\begin{aligned}
\ddot x
&=
\frac{d}{dt}\left[J(q)\dot q\right]\\
&=
\frac{dJ}{dt}\dot q
+
J(q)\frac{d\dot q}{dt}.
\end{aligned}
$$
记住：
$$
\dot J=\frac{dJ}{dt},
\qquad
\ddot q=\frac{d\dot q}{dt},
$$
就得到：
$$
\boxed{
\ddot x
=
J(q)\ddot q+
\dot J(q,\dot q)\dot q}.
$$
我把两项分开记：
$$
\underbrace{J(q)\ddot q}_{\text{关节自己在加速}}
+
\underbrace{\dot J(q,\dot q)\dot q}_{\text{姿态变化，速度映射也在变化}}.
$$
第二项其实可以这样想：手臂在转，末端速度的方向也跟着转。哪怕速度大小不变，只要方向持续改变，末端就有加速度。这个方向变化的影响记在 $\dot J\dot q$ 里。
5.2 $\dot J$ 为什么会出现
雅可比依赖关节角，而关节角随时间变化。因此用链式法则：
$$
\dot J
=
\sum_{i=1}^{n}
\frac{\partial J}{\partial q_i}\dot q_i.
$$
每一项表示：第 $i$ 个关节转动时，雅可比跟着改变多少。把所有关节造成的变化加起来，就得到总的 $\dot J$。
如果机械臂保持一个姿态不动，$J$ 不随时间变，$\dot J=0$。机械臂正在运动时，通常要把这一项一起算上。
5.3 用一根杆检查这个结果
一根长度为 $l$ 的杆，末端位置：
$$
p(q)=
\begin{bmatrix}
l\cos q\\
l\sin q
\end{bmatrix}.
$$
它的位置雅可比：
$$
J(q)=
\frac{\partial p}{\partial q}
=
\begin{bmatrix}
-l\sin q\\
l\cos q
\end{bmatrix}.
$$
速度：
$$
\dot p=J(q)\dot q
=
\begin{bmatrix}
-l\sin q\\
l\cos q
\end{bmatrix}\dot q.
$$
现在对 $J$ 本身求导：
$$
\dot J
=
\begin{bmatrix}
-l\cos q\\
-l\sin q
\end{bmatrix}\dot q.
$$
所以加速度：
$$
\begin{aligned}
\ddot p
&=J(q)\ddot q+\dot J\dot q\\
&=
\begin{bmatrix}
-l\sin q\\
l\cos q
\end{bmatrix}\ddot q
+
\begin{bmatrix}
-l\cos q\\
-l\sin q
\end{bmatrix}\dot q^2.
\end{aligned}
$$
第一项跟角加速度有关；第二项跟角速度平方有关，方向指向圆心。就算 $\ddot q=0$，只要杆还在转，末端仍有向心加速度。
我记这一节的重点只要一句：末端加速度要同时考虑关节加速度，以及关节运动导致雅可比变化的影响。
---
6. 末端的力怎样变成关节力矩
上一节的雅可比把关节速度换成末端速度。现在反过来问：末端受到一个力，各个关节要承担多少力矩？
我用“功”来推这件事，因为功可以把末端和关节两边放进同一条等式里。
末端受到力 $F$，发生一个很小的位移 $\delta x$，外力做功：
$$
\delta W_{\mathrm{ext}}=F^T\delta x.
$$
关节力矩为 $\tau$，关节发生很小的角度变化 $\delta q$，关节力矩做功：
$$
\delta W_{\mathrm{joint}}=\tau^T\delta q.
$$
这里的 $\delta$ 可以先理解成“任意一个足够小的变化”。根据雅可比的微分关系：
$$
\delta x=J\delta q.
$$
把这个关系塞进末端做功的式子：
$$
\delta W_{\mathrm{ext}}
=
F^TJ\delta q.
$$
对于理想刚性机构，同一个微小运动从末端看、从关节看，功要对得上：
$$
\tau^T\delta q=F^TJ\delta q.
$$
两边都有 $\delta q$。由于关节微小变化可以任意选，前面的系数必须相同：
$$
\tau^T=F^TJ.
$$
转置后得到：
$$
\boxed{\tau=J^TF}.
$$
我怎么记这个转置
速度方向是：
$$
\dot q
\overset{J}{\longrightarrow}
\dot x.
$$
力的方向则是：
$$
F
\overset{J^T}{\longrightarrow}
\tau.
$$
更稳妥的记忆方式是从功率检查：
$$
F^T\dot x
=
F^TJ\dot q
=
(J^TF)^T\dot q.
$$
末端功率和关节功率相等时，关节力矩只能是 $J^TF$。
这里的正负号取决于 $F$ 的定义。如果 $F$ 表示环境施加在机械臂末端的力，那么它对机械臂的等效广义力为 $J^TF$；如果我们要求执行器去抵抗这个外力，静态平衡时执行器力矩方向相反。
---
7. 奇异位形：机械臂有些方向突然“不好动”
我先把奇异位形记成一句话：机械臂的几何姿态让某些关节运动方向变得重复，末端可实现的瞬时运动方向少了。
拿二连杆位置雅可比来检查：
$$
J_p=
\begin{bmatrix}
-l_1s_1-l_2s_{12}&-l_2s_{12}\\
l_1c_1+l_2c_{12}&l_2c_{12}
\end{bmatrix},
$$
其中：
$$
s_1=\sin q_1,\quad c_1=\cos q_1,
$$
$$
s_{12}=\sin(q_1+q_2),\quad
c_{12}=\cos(q_1+q_2).
$$
对这个 $2\times2$ 矩阵求行列式：
$$
\begin{aligned}
\det J_p
={}&
(-l_1s_1-l_2s_{12})(l_2c_{12})\\
&-
(-l_2s_{12})(l_1c_1+l_2c_{12})\\
={}&
l_1l_2(s_{12}c_1-s_1c_{12})\\
={}&
l_1l_2\sin\left[(q_1+q_2)-q_1\right]\\
={}&
l_1l_2\sin q_2.
\end{aligned}
$$
所以：
$$
\boxed{\det J_p=l_1l_2\sin q_2}.
$$
当：
$$
\sin q_2=0,
$$
也就是：
$$
q_2=k\pi,\qquad k\in\mathbb Z,
$$
雅可比行列式为零。几何上，两根连杆共线：$q_2=0$ 时伸直，$q_2=\pi$ 时折叠。
这个时候具体发生了什么
一般姿态下，两个关节各自转动，会让末端产生不同方向的瞬时速度。共线时，这两个方向变得线性相关。雅可比的两列没法张成整个平面，末端瞬时速度只能落在一个较小的方向集合里。
速度关系仍然是：
$$
\dot x=J_p\dot q.
$$
但给定一个任意的末端速度，可能找不到对应的有限关节速度。靠近奇异姿态时，求逆会变得很敏感：末端只要求一点点变化，计算出来的关节速度可能已经很大。
这和工作空间边界能对上。二连杆伸到最远处：
$$
\rho=l_1+l_2,
$$
或者折到最里面：
$$
\rho=|l_1-l_2|,
$$
两根杆都处于共线构型，正好落在上面算出来的奇异条件里。
我记这部分时会把三件事连起来：共线 → 雅可比降秩 → 某些末端方向失去瞬时运动能力。
---
第四部分：动力学——力矩从哪里来
8. 先从最简单的牛顿定律开始
平动中：
$$
F=ma.
$$
这句大家都熟悉：要让质量为 $m$ 的物体产生加速度 $a$，就得有相应的力 $F$。
转动也有对应关系。转动时：
- 力对应力矩 $\tau$；
- 质量分布对应转动惯量 $I$；
- 线加速度对应角加速度 $\alpha$。
固定轴转动的角动量：
$$
L=I\omega.
$$
力矩等于角动量变化率：
$$
\tau=\frac{dL}{dt}.
$$
当 $I$ 不变时：
$$
\boxed{\tau=I\alpha}.
$$
转动惯量由物体上的质量分布决定：
$$
I=\int r^2\,dm.
$$
我会把它简单记成：质量离转轴越远，转起来越费劲。 因为每一小块质量的贡献都乘了距离的平方。
如果转轴不经过质心，还会用到平行轴定理。质心处转动惯量为 $I_C$，新轴和质心轴相距 $d$：
$$
I_O=I_C+md^2.
$$
机械臂连杆一边平移、一边转动，所以一根连杆的动能既有质心平动部分，也有绕质心转动部分。
---
9. 先把二连杆的质心找出来
动力学算的是质量怎么动，所以先要知道每根连杆的质心在哪里。
第一根连杆
质心距第一关节 $r_1$，方向角是 $q_1$：
$$
p_{c1}=
\begin{bmatrix}
r_1\cos q_1\\
r_1\sin q_1
\end{bmatrix}.
$$
第二根连杆
第二根连杆的质心要先走过第一根连杆，再沿第二根连杆方向走 $r_2$：
$$
p_{c2}=
\begin{bmatrix}
l_1\cos q_1+r_2\cos(q_1+q_2)\\
l_1\sin q_1+r_2\sin(q_1+q_2)
\end{bmatrix}.
$$
这个写法和 FK 很像。区别是这里要找的是质心位置，第二根连杆只走到距离关节 $r_2$ 的地方，末端 FK 则走完整个 $l_2$。
---
10. 质心速度：把位置对时间求导
10.1 第一根连杆
对 $p_{c1}$ 求导：
$$
v_{c1}
=
\begin{bmatrix}
-r_1\sin q_1\,\dot q_1\\
r_1\cos q_1\,\dot q_1
\end{bmatrix}.
$$
两个分量平方相加：
$$
\begin{aligned}
\|v_{c1}\|^2
&=
r_1^2\sin^2q_1\,\dot q_1^2
+
r_1^2\cos^2q_1\,\dot q_1^2\\
&=
r_1^2\dot q_1^2.
\end{aligned}
$$
最后一步用的是 $\sin^2q_1+\cos^2q_1=1$。这个结果也很直观：质心绕第一关节走圆，线速度大小就是半径乘角速度。
10.2 第二根连杆
第二根连杆质心的位置有两段，所以求导后也有两段速度：
$$
v_{c2}
=
\begin{bmatrix}
-l_1\sin q_1\,\dot q_1
-r_2\sin(q_1+q_2)(\dot q_1+\dot q_2)\\
l_1\cos q_1\,\dot q_1
+r_2\cos(q_1+q_2)(\dot q_1+\dot q_2)
\end{bmatrix}.
$$
角度 $q_1+q_2$ 对时间求导时，链式法则会给出 $\dot q_1+\dot q_2$。
把两个速度分量平方再相加，展开交叉项时会碰到恒等式：
$$
\cos a\cos b+\sin a\sin b=\cos(a-b).
$$
两个方向角之差正好是 $q_2$，所以：
$$
\boxed{
\begin{aligned}
\|v_{c2}\|^2
={}&
l_1^2\dot q_1^2
+
r_2^2(\dot q_1+\dot q_2)^2\\
&+
2l_1r_2\cos q_2\,
\dot q_1(\dot q_1+\dot q_2).
\end{aligned}}
$$
这里的交叉项值得留意：第二根连杆的质心运动同时受两个关节影响，所以速度平方里自然会出现 $\dot q_1\dot q_2$ 这样的乘积。
两根连杆的角速度分别是：
$$
\omega_1=\dot q_1,
\qquad
\omega_2=\dot q_1+\dot q_2.
$$
---
11. 从动能收集出惯性矩阵
刚体在平面内运动的动能：
$$
T=
\frac12m\|v_C\|^2+
\frac12I_C\omega^2.
$$
第一根连杆：
$$
\begin{aligned}
T_1
&=
\frac12m_1\|v_{c1}\|^2+
\frac12I_1\omega_1^2\\
&=
\frac12(m_1r_1^2+I_1)\dot q_1^2.
\end{aligned}
$$
第二根连杆：
$$
T_2
=
\frac12m_2\|v_{c2}\|^2
+
\frac12I_2(\dot q_1+\dot q_2)^2.
$$
总动能就是两根连杆的动能相加：
$$
T=T_1+T_2.
$$
把 $T$ 展开后，按 $\dot q_1^2$、$\dot q_1\dot q_2$、$\dot q_2^2$ 收集。任何关于关节速度的二次式，都可以写成矩阵二次型：
$$
\boxed{
T=\frac12\dot q^TM(q)\dot q}.
$$
其中：
$$
q=
\begin{bmatrix}
q_1\\q_2
\end{bmatrix},
\qquad
\dot q=
\begin{bmatrix}
\dot q_1\\\dot q_2
\end{bmatrix}.
$$
对二连杆，惯性矩阵为：
$$
M(q)=
\begin{bmatrix}
M_{11}&M_{12}\\
M_{12}&M_{22}
\end{bmatrix},
$$
$$
\begin{aligned}
M_{11}
={}&
I_1+I_2+m_1r_1^2\\
&+
m_2
\left(
l_1^2+r_2^2+2l_1r_2\cos q_2
\right),
\end{aligned}
$$
$$
M_{12}
=
I_2+
m_2\left(r_2^2+l_1r_2\cos q_2\right),
$$
$$
M_{22}=I_2+m_2r_2^2.
$$
我怎么读这个矩阵
先看对角线：
- $M_{11}$：第一关节加速时，系统表现出来的惯性；
- $M_{22}$：第二关节加速时，系统表现出来的惯性。
再看非对角线 $M_{12}$：两个关节的运动彼此带着对方。比如第二关节加速，第一关节也可能需要额外力矩。
矩阵是对称的：
$$
M(q)=M^T(q).
$$
原因在于动能是一个标量二次型，交叉项 $\dot q_1\dot q_2$ 的两个矩阵位置必须配成一样。
只要系统有质量且不是退化模型，非零关节速度应当对应正动能：
$$
\dot q^TM(q)\dot q=2T>0.
$$
所以 $M(q)$ 是正定矩阵。记忆时可以把 $M$ 当作“机械臂姿态相关的转动惯量表”。
---
12. 势能对关节角求导，得到重力项
先约定 $y$ 轴向上。质量 $m$ 在高度 $y$ 的势能：
$$
U=m g_0 y.
$$
第一根连杆质心高度：
$$
y_{c1}=r_1\sin q_1.
$$
第二根连杆质心高度：
$$
y_{c2}
=
l_1\sin q_1+r_2\sin(q_1+q_2).
$$
两根连杆势能相加：
$$
\boxed{
U(q)
=
m_1g_0r_1\sin q_1
+
m_2g_0
\left[
l_1\sin q_1+
r_2\sin(q_1+q_2)
\right]}.
$$
重力项是势能对关节变量的梯度：
$$
g(q)=\frac{\partial U}{\partial q}.
$$
把它拆成两个偏导：
$$
g_1(q)=\frac{\partial U}{\partial q_1},
\qquad
g_2(q)=\frac{\partial U}{\partial q_2}.
$$
逐项求导：
$$
\boxed{
g(q)=
\begin{bmatrix}
(m_1r_1+m_2l_1)g_0\cos q_1
+
m_2r_2g_0\cos(q_1+q_2)\\
m_2r_2g_0\cos(q_1+q_2)
\end{bmatrix}}.
$$
符号怎么检查
本笔记里 $y$ 轴向上，$q_1=0$ 时第一根杆水平向右。把杆抬起来需要逆时针力矩，也就是正力矩。此时重力补偿项为正，方向符合这个约定。
静止时：
$$
\dot q=0,\qquad \ddot q=0.
$$
先忽略摩擦和末端负载，维持姿态的力矩就是：
$$
\boxed{\tau=g(q)}.
$$
我会把重力补偿记成：把当前位置的势能斜率换成关节力矩，再由电机提供相反于重力趋势的作用。
---
13. 拉格朗日方程：把动能和势能合在一起
13.1 先组成拉格朗日量
$$
L(q,\dot q)=T(q,\dot q)-U(q).
$$
$T$ 描述运动带来的能量，$U$ 描述高度带来的能量。将它们放进 $L$ 后，对每个关节变量写：
$$
\boxed{
\tau_i
=
\frac{d}{dt}
\left(
\frac{\partial L}{\partial\dot q_i}
\right)
-
\frac{\partial L}{\partial q_i}}.
$$
这里可以按顺序理解：
- 先看动能对关节速度有多敏感；
- 再看这个量随时间怎样变化；
- 再减掉拉格朗日量对关节位置的偏导；
- 得到这个关节需要提供的广义力矩。
把二连杆的 $T$ 和 $U$ 代进去，整理所有关节方程，得到熟悉的结构：
$$
\boxed{
\tau
=
M(q)\ddot q+
C(q,\dot q)\dot q+
g(q)}.
$$
我会把这条总式子记成三类账单：
$$
\underbrace{M(q)\ddot q}_{\text{想加速，要付惯性力矩}}
+
\underbrace{C(q,\dot q)\dot q}_{\text{关节在运动时，彼此会耦合}}
+
\underbrace{g(q)}_{\text{想停在这个高度，要抵抗重力}}
=
\underbrace{\tau}_{\text{电机给出的力矩}}.
$$
13.2 速度二次项从哪来
惯性矩阵 $M(q)$ 会随姿态变化。关节在运动时，$M$ 的元素也在变，对动能求导自然会产生速度乘积项。
可以用 Christoffel 符号把这些项写清楚：
$$
c_{ijk}
=
\frac12
\left(
\frac{\partial M_{ij}}{\partial q_k}
+
\frac{\partial M_{ik}}{\partial q_j}
-
\frac{\partial M_{jk}}{\partial q_i}
\right).
$$
速度项的第 $i$ 个分量为：
$$
\left[C(q,\dot q)\dot q\right]_i
=
\sum_{j=1}^{n}
\sum_{k=1}^{n}
c_{ijk}\dot q_j\dot q_k.
$$
记住结构就够了：这里每一项都包含两个关节速度相乘，所以速度相关力矩会随速度平方或速度乘积变化。
13.3 二连杆的具体结果
令：
$$
h=m_2l_1r_2\sin q_2.
$$
把二连杆的 $M(q)$ 代进上面的速度项，整理后得到：
$$
\boxed{
C(q,\dot q)\dot q
=
\begin{bmatrix}
-h\left(2\dot q_1\dot q_2+\dot q_2^2\right)\\
h\dot q_1^2
\end{bmatrix}}.
$$
于是完整的二连杆方程：
$$
\boxed{
\begin{aligned}
\tau_1
={}&
M_{11}\ddot q_1+
M_{12}\ddot q_2\\
&-
h\left(2\dot q_1\dot q_2+\dot q_2^2\right)
+
g_1(q),\\
\tau_2
={}&
M_{12}\ddot q_1+
M_{22}\ddot q_2\\
&+
h\dot q_1^2+
g_2(q).
\end{aligned}}
$$
观察一下：即使 $\ddot q_1=\ddot q_2=0$，只要关节还在转，速度项也可能需要力矩来抵消。
---
第五部分：我怎么区分惯性、离心和科氏项
14. 惯性项
动力学方程中的惯性部分：
$$
\tau_{\mathrm{inertia}}=M(q)\ddot q.
$$
单关节情况退化成：
$$
\tau=I\ddot q.
$$
我会这样记：想改变运动状态就要对抗惯性。 关节加速越大、连杆和负载越重，所需力矩通常越大。
二连杆里：
$$
\begin{bmatrix}
\tau_1\\
\tau_2
\end{bmatrix}_{\mathrm{inertia}}
=
\begin{bmatrix}
M_{11}&M_{12}\\
M_{12}&M_{22}
\end{bmatrix}
\begin{bmatrix}
\ddot q_1\\
\ddot q_2
\end{bmatrix}.
$$
非对角项 $M_{12}$ 说明一个关节的加速度会影响另一个关节的力矩。机械臂的连杆连接在一起，运动自然会相互牵连。
15. 离心项和科氏项
二连杆速度相关力矩是：
$$
\tau_v
=
\begin{bmatrix}
-h\left(2\dot q_1\dot q_2+\dot q_2^2\right)\\
h\dot q_1^2
\end{bmatrix}.
$$
先按速度的形式分组：
- 只有一个速度平方 $\dot q_i^2$ 的，通常称为离心项；
- 两个不同关节速度相乘 $\dot q_i\dot q_j$ 的，通常称为科氏项。
按这个约定，二连杆的离心项可以写成：
$$
\tau_{\mathrm{centrifugal}}
=
\begin{bmatrix}
-h\dot q_2^2\\
h\dot q_1^2
\end{bmatrix},
$$
科氏项为：
$$
\tau_{\mathrm{Coriolis}}
=
\begin{bmatrix}
-2h\dot q_1\dot q_2\\
0
\end{bmatrix}.
$$
两部分相加，就回到 $\tau_v$。
在转动参考系中，常见的科氏力、离心力形式为：
$$
F_{\mathrm{Cor}}
=
-2m\,\omega\times v_{\mathrm{rel}},
$$
$$
F_{\mathrm{cen}}
=
-m\,\omega\times(\omega\times r).
$$
我先记住它们对速度的依赖：
- 离心项通常跟某个速度平方有关；
- 科氏项通常跟两个运动速度的乘积有关；
- 在机械臂总方程里，它们放在 $C(q,\dot q)\dot q$ 这一组里。
不同教材对“科氏项”和“离心项”的拆分记法可能略有差别，做推导时以完整的速度二次项为准。
---
16. 重力补偿和动力学前馈
忽略摩擦和外力时：
$$
\tau=
M(q)\ddot q+
C(q,\dot q)\dot q+
g(q).
$$
这条式子告诉我，电机要提供的力矩可以按来源拆开。
只想让机械臂静止悬停
令：
$$
\dot q=0,\qquad \ddot q=0.
$$
剩下：
$$
\tau_{\mathrm{gravity}}=g(q).
$$
想补上期望加速度带来的惯性
$$
\tau_{\mathrm{inertia}}=M(q)\ddot q.
$$
想补上关节同时运动带来的速度耦合
$$
\tau_{\mathrm{velocity}}=C(q,\dot q)\dot q.
$$
三类模型项一起计算
$$
\boxed{
\tau_{\mathrm{ff}}
=
M(q)\ddot q+
C(q,\dot q)\dot q+
g(q)}.
$$
这里的前馈就是：根据当前姿态和希望的运动状态，先用模型算出一个理论力矩，再交给执行器。
我会记成三问：
- 想加速吗？看 $M\ddot q$。
- 关节正在一起运动吗？看 $C\dot q$。
- 想在这个姿态停住吗？看 $g(q)$。
模型参数不准、负载变化、摩擦和柔性都会让实际力矩与理想计算值有差异，所以这条式子是理想刚体模型的力矩需求。
17. 末端外力如何进入关节方程
如果末端碰到物体，环境会对末端施加外力 $F_{\mathrm{ext}}$。通过虚功关系，它对应的关节广义力为：
$$
\tau_{\mathrm{ext}}=J^T(q)F_{\mathrm{ext}}.
$$
这里约定 $F_{\mathrm{ext}}$ 是“环境施加在机械臂上的力”。按这个符号约定，动力学方程写为：
$$
\boxed{
M(q)\ddot q+
C(q,\dot q)\dot q+
g(q)+\tau_f
=
\tau+J^T(q)F_{\mathrm{ext}}}.
$$
如果把力定义成“机械臂施加给环境的力”，方向会相反，公式右侧相应带负号。写公式时要先说清楚力是谁对谁施加，符号才不会乱。
---
把二连杆的思路搬到一般机械臂
自由度串联机械臂
末端位置和姿态
每个相邻坐标系之间都有一个变换。把这些变换从基座一路乘到末端：
$$
T_0^n
=
T_0^1T_1^2\cdots T_{n-1}^n.
$$
如果末端还安装了工具，工具坐标系相对最后一节连杆的变换记作 $T_n^e$，则：
$$
T_0^e=T_0^nT_n^e.
$$
把完整变换拆成旋转和平移：
$$
T_0^e=
\begin{bmatrix}
R_0^e&p_0^e\\
0_{1\times3}&1
\end{bmatrix}.
$$
这里：
- $p_0^e$ 是末端在基座坐标系中的位置；
- $R_0^e$ 是末端方向相对基座方向的旋转。
这就是多关节 FK 的统一写法。关节数量变多，连乘的矩阵变多，计算逻辑没有换一套。
速度和加速度
一般机械臂的速度关系：
$$
\dot x=J(q)\dot q.
$$
继续对时间求导：
$$
\ddot x
=
J(q)\ddot q+
\dot J(q,\dot q)\dot q.
$$
三维任务里，末端线速度和角速度可以堆在一起：
$$
\begin{bmatrix}
v_e\\
\omega_e
\end{bmatrix}
=
J(q)\dot q,
\qquad
J(q)\in\mathbb R^{6\times n}.
$$
如果关节数少于六，机械臂不能在任意时刻独立控制六个末端速度分量；如果关节数多于六，通常会有多余的关节自由度可以安排。
能量和动力学
把所有连杆动能加起来：
$$
T=\frac12\dot q^TM(q)\dot q.
$$
把所有连杆重力势能加起来：
$$
U=U(q).
$$
形成拉格朗日量：
$$
L=T-U.
$$
对每个关节使用拉格朗日方程：
$$
\tau_i
=
\frac{d}{dt}
\left(
\frac{\partial L}{\partial\dot q_i}
\right)
-
\frac{\partial L}{\partial q_i}.
$$
最后整理成统一动力学式：
$$
\boxed{
M(q)\ddot q+
C(q,\dot q)\dot q+
g(q)+\tau_f
=
\tau+J^T(q)F_{\mathrm{ext}}}.
$$
我把从二连杆推广到一般机械臂的路线记成：
$$
\boxed{
\text{坐标变换}
\longrightarrow
\text{FK}
\longrightarrow
\text{雅可比}
\longrightarrow
\text{动能和势能}
\longrightarrow
\text{动力学}}.
$$
几何部分告诉我末端在哪里；微分运动学告诉我关节动起来后末端怎么动；能量和拉格朗日方程告诉我需要多大力矩。
---
简单记忆且一定要熟悉的公式
位置和姿态
二连杆 FK：
$$
\begin{aligned}
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.
\end{aligned}
$$
记法：每根杆拆成横向和竖向投影，向量首尾相加。
逆运动学
$$
c_2=
\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2},
\qquad
s_2=\pm\sqrt{1-c_2^2},
$$
$$
q_2=\operatorname{atan2}(s_2,c_2),
$$
$$
q_1
=
\operatorname{atan2}(y,x)
-
\operatorname{atan2}
\left(
l_2s_2,
l_1+l_2c_2
\right).
$$
记法：先用三角形定肘部，再把整个构型转向目标。
速度、加速度和力
$$
\dot x=J\dot q,
$$
$$
\ddot x=J\ddot q+\dot J\dot q,
$$
$$
\tau=J^TF.
$$
记法：速度正向乘 $J$，力矩映射乘 $J^T$；加速度还要记上 $\dot J\dot q$。
动力学和补偿
$$
\tau=M(q)\ddot q+C(q,\dot q)\dot q+g(q).
$$
加速看 $M$，同时运动看 $C$，悬停看 $g$。
$$
\tau_{\mathrm{gravity}}=g(q).
$$
机械臂想停在当前位置，电机要顶住当前位置对应的重力矩。
结语
真的要理解全部的理论公式才行吗? 其实我觉得不一定,反而我是真的调过整个机械臂之后,做出成果之后才来学习了解推导这些理论公式的. 本文大多数公式和推导过程都是由AI主导写出来的,本猫很多时候直接一边学习AI的推导过程一边加上一点自己的理解,拖拖拉拉的接近一个月才真正看完.虽然也不一定真的能够一步步推导出来,但是至少现在真的看到机械臂的我能从原理开始理解了,很多现象也能通过原理公式进行反推,更多起到了理解和贯穿的作用,