00.7｜机械臂运动学：从坐标链到雅可比与逆解

逐步建立坐标变换链，比较 DH 与 POE，推导二连杆雅可比、奇异性和阻尼逆解，并明确几何速度与空间旋量的区别。

这节先回答三个不同的问题

机械臂的正运动学回答“关节处在这些位置时，工具在哪里”；速度运动学回答“关节以这些速度运动时，工具向哪里运动”；动力学才回答“要产生这种运动，需要多大力矩”。不能把三个问题都叫作“算出控制量”。

本节使用右手坐标系。 $R_{AB}$ 把在 $B$ 系表达的向量变成在 $A$ 系表达的向量； $p_{AB}^{A}$ 是 $B$ 系原点相对 $A$ 系原点的位置，在 $A$ 系表达，单位 m。关节角单位 rad，角速度单位 rad/s。虽然弧度在量纲分析中可视为无量纲，代码接口仍应明确区分角度和长度。

先修：旋转与四元数、李群与李代数、旋转求导。

1. 一个点经过多个坐标系，究竟怎么算？

1.1 平移的不是向量，而是坐标原点

相机在机械臂工具上，检测到一个物体的相机坐标 $p_C$ 。相机原点在工具系的位置是 $p_{EC}^{E}$ ，姿态是 $R_{EC}$ 。先转方向，再加原点偏移：

p_E=R_{EC}p_C+p_{EC}^{E}.

旋转项处理“同一段箭头在另一个坐标系如何表达”；平移项处理“两个坐标原点不重合”。若变换的是速度方向、单位轴或两个点的差，不能直接加这项平移。

把点增加最后一个分量 $1$ ，可以将两个操作写成一次矩阵乘法：

\bar p_A=T_{AB}\bar p_B,\qquad T_{AB}=\begin{bmatrix}R_{AB}&p_{AB}^{A}\\0_{1\times3}&1\end{bmatrix},\qquad \bar p_B=\begin{bmatrix}p_B\\1\end{bmatrix}.

$T_{AB}$ 为 $4\times4$ 齐次变换。方向的齐次最后一维是 $0$ ，这使它不受平移影响。不要把任意四维物理状态都当作齐次坐标。

1.2 变换连乘不是按名字随意排列

假设机械臂基座是 $B$ ，工具是 $E$ ，相机是 $C$ ，则

T_{BC}=T_{BE}T_{EC},\qquad p_B=R_{BE}(R_{EC}p_C+p_{EC}^{E})+p_{BE}^{B}.

第二式直接展开后，第一式的乘法顺序就得到解释：先应用右边的相机到工具变换，再应用左边的工具到基座变换。中间下标 $E$ 相接不是装饰，而是在检查接口能否衔接。三维旋转一般不交换，交换两个矩阵会表示另一种运动。

例：工具相对基座绕 $z$ 轴转 $90^\circ$ ，工具原点在 $[1,0,0]^T$ m；相机与工具同向，在工具前方 $[0.2,0,0]^T$ m。相机测得点 $[0.3,0,0]^T$ m，则点在工具系为 $[0.5,0,0]^T$ m，在基座系为 $[1,0.5,0]^T$ m。直接把所有 $x$ 坐标相加得到 $1.5$ m，忽略了工具的朝向。

1.3 逆变换为何不是只把平移取负？

从 $p_A=R_{AB}p_B+p_{AB}^{A}$ 解出 $p_B$ ：先减平移，再乘旋转的逆。

p_B=R_{AB}^{T}p_A-R_{AB}^{T}p_{AB}^{A},\qquad T_{AB}^{-1}=\begin{bmatrix}R_{AB}^{T}&-R_{AB}^{T}p_{AB}^{A}\\0&1\end{bmatrix}.

负平移还要转到新的表达坐标系。实际接口的第一组单元测试应是 $T^{-1}(Tp)=p$ ，而不是仅检查矩阵尺寸。

2. 从二连杆的几何图开始建立正运动学

考虑平面二转动关节机械臂，杆长 $l_1,l_2>0$ ，两关节角为 $q_1,q_2$ 。 $q_1$ 相对基座 $x$ 轴， $q_2$ 相对第一杆，不是第二杆相对世界的绝对角。

第一杆端点为 $[l_1\cos q_1,l_1\sin q_1]^T$ ；第二杆绝对方向为 $q_1+q_2$ ，因此末端为

p(q)=\begin{bmatrix}x\\y\end{bmatrix} =\begin{bmatrix} l_1\cos q_1+l_2\cos(q_1+q_2)\\ l_1\sin q_1+l_2\sin(q_1+q_2) \end{bmatrix},\qquad \psi=q_1+q_2.

两个位置分量单位 m，姿态角 $\psi$ 单位 rad。注意只有两个关节，不可能任意独立指定二维位置和末端朝向共三个目标。多写一条目标方程不会增加执行器自由度。

取 $l_1=l_2=1$ m、 $q_1=0$ 、 $q_2=\pi/2$ ，第一杆朝右、第二杆朝上，所以 $p=[1,1]^T$ m、 $\psi=\pi/2$ 。这种可画出来的例子是检查轴向和相对角定义最有效的方法。

3. DH 参数到底把什么压缩成了四个数？

一条空间链有很多变换。DH 方法通过约定每节坐标轴的放置，让相邻连杆变换只需四个参数。本节仅使用标准 DH，明确规定

{}^{i-1}T_i=R_z(\theta_i)T_z(d_i)T_x(a_i)R_x(\alpha_i).

$\theta_i$ 是绕前一节 $z$ 轴的转角， $d_i$ 是沿该轴的位移； $a_i$ 是沿选定公共法线的长度， $\alpha_i$ 是两关节轴的扭转角。旋转关节通常以 $\theta_i=q_i+\theta_{i0}$ 为变量；移动关节通常以 $d_i=q_i+d_{i0}$ 为变量。其他参数是几何常量。

按这个精确乘法次序相乘，设 $c_\theta=\cos\theta_i$ 、 $s_\theta=\sin\theta_i$ ， $c_\alpha=\cos\alpha_i$ 、 $s_\alpha=\sin\alpha_i$ ，得到

{}^{i-1}T_i= \begin{bmatrix} c_\theta&-s_\theta c_\alpha&s_\theta s_\alpha&a_i c_\theta\\ s_\theta&c_\theta c_\alpha&-c_\theta s_\alpha&a_i s_\theta\\ 0&s_\alpha&c_\alpha&d_i\\ 0&0&0&1 \end{bmatrix}.

第一、第二行的平移来自 $T_x(a_i)$ 被前面的 $R_z(\theta_i)$ 转过，因而不是固定的 $[a_i,0,0]^T$ 。沿 $z$ 的平移不受同轴 $R_z$ 影响，所以第三行仍是 $d_i$ 。这比背整张矩阵更不容易写反。

整条链为

{}^0T_n={}^0T_1{}^1T_2\cdots{}^{n-1}T_n.

二连杆的标准 DH 可取 $d_1=d_2=0$ 、 $\alpha_1=\alpha_2=0$ 、 $a_1=l_1$ 、 $a_2=l_2$ 、 $\theta_i=q_i$ ，连乘应恢复上一节的几何结果。

修改 DH 采用另一种连乘/坐标配置约定，参数表不能直接塞进这张标准 DH 矩阵。 从 CAD 或 URDF 导出时，必须记录固定关节变换、轴向、零点和正方向，不能仅因都有四个参数就认为兼容。URDF 的 joint origin 也不是自动等于某一行 DH。

4. POE：直接描述每根关节轴如何产生刚体运动

DH 管理相邻坐标系；指数积 POE 从“关节轴产生的运动”出发。二者描述同一物理链，建模信息一致时正运动学必须一致。Modern Robotics 第四章给出了空间系和末端系两种指数积表示。

4.1 旋转轴和移动轴怎样成为六维列向量？

本小节按 Modern Robotics 的顺序使用 $S=[\omega^T,v^T]^T$ ，角分量在前，不要与水动力学常用 $[v^T,\omega^T]^T$ 混用。

一根转动轴的单位方向是 $\omega$ ，轴上一点是 $a$ 。任意点 $p$ 因单位关节速度产生的速度为

\dot p=\omega\times(p-a)=\omega\times p-\omega\times a.

因此取 $v=-\omega\times a$ 。它不是末端点的线速度，而是空间旋量中的平移分量。若轴通过原点， $v=0$ ，但远离轴的末端仍有速度 $\omega\times p$ 。

纯移动关节的单位运动方向为 $u$ 时， $\omega=0$ 、 $v=u$ ；关节变量单位为 m，而转动关节变量单位为 rad。把旋量写成矩阵：

[S]^\wedge=\begin{bmatrix}[\omega]_\times&v\\0&0\end{bmatrix}.

这里 $[\omega]_\times p=\omega\times p$ 。右上角 $v$ 单位取决于关节类型和关节变量归一化，不能把混合关节的六个数字当作相同物理单位。

4.2 为什么连乘指数后还要乘零位姿？

设零关节位置下末端位姿为 $M$ ，各关节零位时在固定基座系表达的轴为 $S_i$ ：

T(q)=e^{[S_1]^\wedge q_1}e^{[S_2]^\wedge q_2}\cdots e^{[S_n]^\wedge q_n}M.

当全部 $q_i=0$ ，指数都是单位矩阵，必须得到真实零位姿 $M$ ，而不是强行让工具与基座重合。前面关节的指数左乘后，会一并带动后面的轴；这体现串联机构结构。

二连杆零位沿基座 $x$ 轴伸直，有

S_1=\begin{bmatrix}0&0&1&0&0&0\end{bmatrix}^T,\quad S_2=\begin{bmatrix}0&0&1&0&-l_1&0\end{bmatrix}^T,\quad M=\begin{bmatrix}I&[l_1+l_2,0,0]^T\\0&1\end{bmatrix}.

第二轴穿过 $a=[l_1,0,0]^T$ ，故 $v=-[0,0,1]^T\times a=[0,-l_1,0]^T$ 。把该轴的平移分量错填为零，就把第二关节错放到了基座原点。

5. 雅可比不是“一个现成矩阵”，而是一阶变化率

5.1 先对二连杆逐项求导

当关节发生小变化 $\delta q$ ，末端变化一阶近似为 $\delta p\approx J_p\delta q$ ，其中 $J_p=\partial p/\partial q$ 。对正运动学每一项求导：

J_p= \begin{bmatrix} -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{bmatrix},\qquad \dot p=J_p\dot q.

第一列是只动第一关节时末端的速度方向，两根杆都会动；第二列是只动第二关节时的贡献。线速度由两个贡献相加，解释了矩阵乘法。

在 $l_1=l_2=1$ 、 $q=[0,\pi/2]^T$ 时， $J_p=[[-1,-1],[1,0]]$ m/rad。若 $\dot q=[0.2,-0.1]^T$ rad/s，则 $\dot p=[-0.1,0.2]^T$ m/s。

5.2 三维关节为什么出现叉乘？

设第 $i$ 个转轴方向 $z_i$ 、轴上一点 $o_i$ 、工具原点 $p_E$ ，全部在同一基座系表达。由绕轴圆周运动，转动关节的几何雅可比列是

J_{g,i}=\begin{bmatrix}z_i\times(p_E-o_i)\\z_i\end{bmatrix}.

这里顺序改为 $[\dot p_E;\omega]$ ，线速度在前。若是移动关节，该列为 $[z_i;0]$ 。因此 $J_g$ 的前后两块输出单位不同；需要用位置/角度任务权重或特征长度归一化，不能直接把“1 m 误差”和“1 rad 误差”等价相加。

5.3 空间雅可比与工具点几何雅可比不是只交换行

空间旋量定义为 $\dot T T^{-1}=[V_s]^\wedge$ ，若 $V_s=[\omega_s;v_s]$ ，展开得到

v_s=\dot p_E-\omega_s\times p_E,\qquad \dot p_E=v_s+\omega_s\times p_E.

因此空间雅可比下方三行对应 $v_s$ ，不是直接对应工具原点的 $\dot p_E$ 。要得到点的线速度还需补上叉乘项。

在 POE 中，空间雅可比第 $i$ 列为

J_{s,i}=\operatorname{Ad}_{T_{i-1}}S_i,\qquad T_{i-1}=e^{[S_1]^\wedge q_1}\cdots e^{[S_{i-1}]^\wedge q_{i-1}},

其中对 $T=[R,p;0,1]$ 、角在前顺序， $\operatorname{Ad}_T=[R,0;[p]_\times R,R]$ 。其作用是把一个刚体速度的表达坐标系转换，而非仅旋转六个数字。第一列为 $S_1$ ，后续列只依赖上游关节。空间雅可比与机体雅可比官方讲解可用于交叉核对约定。

解析雅可比则把关节速度映射到某套姿态参数的导数，例如欧拉角速度。一般 $\omega=E(\eta)\dot\eta$ ，不是 $\omega=\dot\eta$ 。机械结构没到奇异位形时，欧拉角参数化本身也可能奇异；这是两类不同问题。

6. 奇异性：为什么末端还在，某方向却动不了？

二连杆位置雅可比的行列式为

\det J_p=l_1l_2\sin q_2.

可由展开两项乘积，再用 $\sin(a-b)=\sin a\cos b-\cos a\sin b$ 得到。当 $q_2=0$ 或 $\pi$ ，两杆共线，秩从二降到一。伸直时两个关节的小转动都只能先产生垂直于杆的速度，没有沿杆的瞬时一阶速度。

这不等于机器人永远不能向该方向移动：先弯曲再运动可以产生有限位移。但要求它在该位形瞬时以任意速度移动是不可能的。逆矩阵数值爆炸反映物理不可达方向，不是单纯浮点精度差。

7. 从小变化方程推到阻尼逆解

末端目标与当前位置的误差为 $e=p_d-p(q)$ 。线性化得到 $J_p\delta q\approx e$ 。当 $J_p$ 方且非奇异时可解线性方程；一般情况最小化

\mathcal L(\delta q)=\frac12\|J_p\delta q-e\|^2+\frac{\lambda^2}{2}\|\delta q\|^2.

第一项要求末端误差变小，第二项惩罚过大的关节变化。对于本例米制位置和弧度关节， $\lambda$ 的尺度应与 $J_p$ 相容，不能把相同数值无脑迁移到毫米模型。求导并令零：

J_p^T(J_p\delta q-e)+\lambda^2\delta q=0, \qquad (J_p^TJ_p+\lambda^2I)\delta q=J_p^Te.

$\lambda>0$ 让该矩阵正定，因此可稳定求解；不代表原来的任务变得可达。增大阻尼会减小关节步幅，也留下更大的任务残差。每次更新 $q\leftarrow q+\delta q$ 后重算正运动学，配合步长上限、关节限位和停止条件。数值逆运动学官方资料解释了线性化迭代及初值影响；这里的正则项是对奇异附近大步长的显式处理。

对于速度控制，把 $e$ 换成期望末端速度 $v_d$ ，解出 $\dot q$ 。两者公式外形相似，但单位与采样处理不同：位置迭代的 $\delta q$ 不是未经乘时间就直接发给电机的速度。

8. C++ 手撕练习：两连杆、解析雅可比与有限差分

实现 fk、jac 和一次阻尼步 dampedStep。测试必须包含伸直奇异位形，不要求奇异时错误地精确追踪任意方向。有限差分是检查符号的工具，不是证明解析公式普遍正确。

展开参考实现与测试（C++17）

#include <array>
#include <cassert>
#include <cmath>
#include <iostream>
using V2 = std::array<double, 2>;
using M2 = std::array<V2, 2>;
V2 fk(V2 q, double a=1, double b=1) {
    return {a*std::cos(q[0])+b*std::cos(q[0]+q[1]),
            a*std::sin(q[0])+b*std::sin(q[0]+q[1])};
}
M2 jac(V2 q, double a=1, double b=1) {
    double s=std::sin(q[0]+q[1]), c=std::cos(q[0]+q[1]);
    return {{{-a*std::sin(q[0])-b*s, -b*s},
             {a*std::cos(q[0])+b*c, b*c}}};
}
V2 dampedStep(M2 J, V2 e, double lambda) {
    assert(lambda > 0);
    double a=J[0][0]*J[0][0]+J[1][0]*J[1][0]+lambda*lambda;
    double b=J[0][0]*J[0][1]+J[1][0]*J[1][1];
    double d=J[0][1]*J[0][1]+J[1][1]*J[1][1]+lambda*lambda;
    double u=J[0][0]*e[0]+J[1][0]*e[1];
    double v=J[0][1]*e[0]+J[1][1]*e[1];
    double det=a*d-b*b;
    assert(det > 0); // 小型教学系统；大型系统用分解求解，不手写逆。
    return {(d*u-b*v)/det, (a*v-b*u)/det};
}
int main() {
    const double pi=std::acos(-1.0), eps=1e-6;
    auto p=fk({0,pi/2});
    assert(std::abs(p[0]-1)<1e-12 && std::abs(p[1]-1)<1e-12);
    for (V2 q : {V2{0,0},V2{0,pi/2},V2{0.7,-0.4},V2{1,pi}}) {
        auto J=jac(q);
        for(int c=0;c<2;++c) {
            V2 qp=q,qm=q;qp[c]+=eps;qm[c]-=eps;
            auto pp=fk(qp),pm=fk(qm);
            for(int r=0;r<2;++r)
                assert(std::abs((pp[r]-pm[r])/(2*eps)-J[r][c])<1e-8);
        }
    }
    auto dq=dampedStep(jac({0,0}),{0.1,0},0.1);
    assert(std::abs(dq[0])+std::abs(dq[1])<1e-12); // 不可达一阶方向
    dq=dampedStep(jac({0,pi/2}),{0.01,-0.02},0.01);
    auto J=jac({0,pi/2});
    assert(std::abs(J[0][0]*dq[0]+J[0][1]*dq[1]-0.01)<1e-5);
    assert(std::abs(J[1][0]*dq[0]+J[1][1]*dq[1]+0.02)<1e-5);
    std::cout << "kinematics PASS\n";
}

保存为 manipulator.cpp，执行 g++ -std=c++17 -O2 manipulator.cpp -o manipulator，再运行程序。不要把本段 main 和其他课的 main 直接拼进同一个文件。

9. 怎样接到自己的机器人工程？

从 CAD、URDF 或标定结果记录每根轴、零点、固定变换和单位，先做零位与单关节运动检查。
选择标准 DH 或 POE，并用同一组实测关节状态计算工具位姿；若两套实现并存，用随机姿态交叉检验。
用编码器速度进入雅可比，预测工具速度，与有限差分或外部测量比较，注意时刻一致。
逆运动学输出的是关节目标/速度，后面仍需轨迹规划、限位、关节控制器与动力学补偿。
若工具装在 ROV 上，令 $W$ 为世界、 $B$ 为机体、 $A$ 为机械臂基座、 $E$ 为工具，则 $T_{WE}=T_{WB}T_{BA}T_{AE}$ ；每一项把右下标坐标变到左下标坐标。只算 $T_{AE}$ 会漏掉载体六自由度运动。

**自检：**能否不查资料解释为什么转动关节雅可比出现叉乘？为什么空间旋量下三维不是工具点速度？为什么阻尼逆解能稳定计算，却不能消除物理奇异性？

机械臂运动学：从坐标链到雅可比与逆解