Part I · 第 5 章 刚体运动与李群 · 5.5Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.5

李代数与 hat / veeLie Algebras, hat & vee

群是弯的，群在单位元处的切空间却是平的。对 SO(3)，这个平地就是三维的角速度向量；对 SE(3)，是六维的速度旋量 twist。hat 把向量变成矩阵，vee 把矩阵变回向量；李括号记录了"先动 A 再动 B"和"先动 B 再动 A"的差别，它在小量上就是叉积。 The group is curved, but its tangent space at the identity is flat. For SO(3) this flat ground is the 3D space of angular velocities; for SE(3) it is the 6D space of twists. The hat operator turns a vector into a matrix and vee turns it back; the Lie bracket records the difference between "move A then B" and "B then A", and for small motions it is the cross product.

约 25 分钟~25 min 前置：1.8 叉积 · 5.3 齐次变换与 SE(3) · 5.4 为什么需要李群Prereqs: 1.8 Cross product · 5.3 SE(3) · 5.4 Why Lie groups? 5 个交互5 interactives

    1为什么机器人需要它Why robots need this

    陀螺仪读出的是三个数 \((\omega_x, \omega_y, \omega_z)\)，不是旋转矩阵。一个机械臂关节的速度、一个轮式机器人的"速度和转弯率"，也都是几个数。它们有个共同点：可以相加、可以乘以时间、可以放进滤波器的状态里。上一节说过，姿态 \(R\) 和位姿 \(T\) 做不到这些，所以我们需要一个专门放"速度"的向量空间，并且知道它怎么和群对应。这个向量空间就是李代数。

    它在机器人学里无处不在：陀螺积分（5.6 节）、EKF 里的姿态误差状态和协方差（5.7、5.9 节）、机械臂的运动学和雅可比（3.4 节、5.10 节）、因子图优化里的"增量" \(\delta\)（6.9、6.10 节），统统都是李代数里的向量。本节介绍它的两个成员：\(\so\)（三维旋转的速度）和 \(\se\)（刚体的速度，即 twist），以及在向量和矩阵之间来回转换的 hat 和 vee。

    A gyroscope reads three numbers \((\omega_x, \omega_y, \omega_z)\), not a rotation matrix. The velocity of a joint, or the "speed and turn rate" of a wheeled robot, are a few numbers too. What they share: they can be added, multiplied by time, and placed in a filter's state. The previous lesson said that orientations \(R\) and poses \(T\) cannot do any of this, so we need a dedicated vector space for "velocities", together with a map between it and the group. That vector space is the Lie algebra.

    It shows up all over robotics: gyro integration (Lesson 5.6), orientation error states and covariances in an EKF (Lessons 5.7 and 5.9), arm kinematics and Jacobians (Lessons 3.4 and 5.10), the "increment" \(\delta\) in factor-graph optimisation (Lessons 6.9 and 6.10): all of them are vectors in a Lie algebra. This lesson introduces two of its members, \(\so\) (velocities of 3D rotations) and \(\se\) (velocities of rigid bodies, i.e. twists), and the hat and vee operators that convert between vectors and matrices.

    2直觉：旋转的"速度"是一个叉积Intuition: a rotation's "velocity" is a cross product

    刚体绕过原点的一根轴以角速度 \(\boldsymbol\omega\) 转动：转轴方向是 \(\boldsymbol\omega\) 的方向，转得多快是 \(\|\boldsymbol\omega\|\)（弧度每秒），方向由右手定则确定。物体上位置为 \(\mathbf p\) 的点，速度是 \(\boldsymbol\omega \times \mathbf p\)（1.8 节）：垂直于转轴和 \(\mathbf p\)，离转轴越远越快，转轴上的点不动。

    叉积是线性的，所以每个 \(\boldsymbol\omega\) 对应一个 \(3\times3\) 矩阵 \(\skew{\boldsymbol\omega}\)，使得 \(\skew{\boldsymbol\omega}\,\mathbf p = \boldsymbol\omega\times\mathbf p\)。这个把向量变成矩阵的操作叫 hat，写作 \(\boldsymbol\omega^\wedge\)；反过来读出向量叫 vee，写作 \(\Omega^\vee\)。下面拖动 \(\boldsymbol\omega\)，看每个格点的速度箭头，和矩阵的每个元素是怎么随 \(\boldsymbol\omega\) 变的。

    A rigid body spins about an axis through the origin at angular velocity \(\boldsymbol\omega\): the direction of \(\boldsymbol\omega\) is the axis, its length \(\|\boldsymbol\omega\|\) is how fast (rad/s), and the sense follows the right-hand rule. A body point at position \(\mathbf p\) moves with velocity \(\boldsymbol\omega \times \mathbf p\) (Lesson 1.8): perpendicular to both the axis and \(\mathbf p\), faster the farther from the axis, and not at all on the axis.

    The cross product is linear, so every \(\boldsymbol\omega\) gives a \(3\times3\) matrix \(\skew{\boldsymbol\omega}\) with \(\skew{\boldsymbol\omega}\,\mathbf p = \boldsymbol\omega\times\mathbf p\). This vector-to-matrix operation is called hat, written \(\boldsymbol\omega^\wedge\); reading the vector back is vee, written \(\Omega^\vee\). Drag \(\boldsymbol\omega\) below and watch the velocity arrow at every lattice point, and how each entry of the matrix follows \(\boldsymbol\omega\).

    FIG 1so(3)：角速度向量 ω 与矩阵 ω^so(3): angular velocity ω and matrix ω^
    拖动画面旋转视角drag to orbit
    紫色粗箭头是 \(\boldsymbol\omega\)（转轴，长度正比于转速）。青色小箭头是格点处的速度 \(\boldsymbol\omega\times\mathbf p\)；不带颜色的圆点和箭头是你用 \(\mathbf p\) 滑块选的测试点及其速度。矩阵 \(\skew{\boldsymbol\omega}\) 里，红、绿、蓝依次来自 \(\omega_1, \omega_2, \omega_3\)，正负号和位置固定：对角线是 0，且关于对角线反对称。 The thick purple arrow is \(\boldsymbol\omega\) (the axis, with length proportional to the rate). The small cyan arrows are the velocities \(\boldsymbol\omega\times\mathbf p\) at lattice points; the plain (uncoloured) dot and arrow are the test point chosen with the \(\mathbf p\) sliders and its velocity. In the matrix \(\skew{\boldsymbol\omega}\), red, green and blue come from \(\omega_1, \omega_2, \omega_3\) respectively, in fixed positions with fixed signs: the diagonal is 0 and the matrix is antisymmetric about it.
    试一试Try this
    • 令 \(\boldsymbol\omega = (0, 0, 1)\)。所有箭头在 \(xy\) 平面里绕 \(z\) 轴逆时针转圈。读一读矩阵：只剩哪两个元素非零？Set \(\boldsymbol\omega = (0, 0, 1)\). All arrows circulate counter-clockwise about \(z\) in the \(xy\) plane. Read the matrix: which two entries remain nonzero?
    • 令 \(\boldsymbol\omega\) 非零，再用 \(\mathbf p\) 滑块把测试点放到转轴上（\(\mathbf p\parallel\boldsymbol\omega\)，\(\|\mathbf p\| > 0.5\)）。它的速度是 0：转轴上的点不动。With \(\boldsymbol\omega\) nonzero, use the \(\mathbf p\) sliders to put the test point on the axis (\(\mathbf p\parallel\boldsymbol\omega\), \(\|\mathbf p\| > 0.5\)). Its velocity is 0: points on the axis do not move.
    核心洞见Key insight

    \(\so\) 里的元素有两种等价的写法：3 维向量 \(\boldsymbol\omega\)（方便存储、相加、放进状态向量），和3×3 反对称矩阵 \(\skew{\boldsymbol\omega}\)（方便和群里的矩阵相乘）。hat 和 vee 在二者之间翻译。上一节从 \(R\T R = I\) 推出的"\(\dot R(0)\) 是反对称矩阵"，现在有了名字：它就是 \(\boldsymbol\omega^\wedge\)。

    Elements of \(\so\) have two equivalent spellings: a 3-vector \(\boldsymbol\omega\) (handy for storing, adding, and putting into a state vector) and a 3×3 skew-symmetric matrix \(\skew{\boldsymbol\omega}\) (handy for multiplying with matrices of the group). Hat and vee translate between them. The statement "\(\dot R(0)\) is skew-symmetric" derived from \(R\T R = I\) in the last lesson now has a name: it is \(\boldsymbol\omega^\wedge\).

    3动手玩：从曲线读出速度，与李括号Play: reading velocity off a curve, and the Lie bracket

    从旋转曲线 R(t) 读出角速度Reading angular velocity off a rotation curve R(t)

    给你一条旋转曲线 \(R(t)\)，怎样得到角速度？对它求导得 \(\dot R\)，它不是反对称矩阵，但 \(R\T\dot R\) 和 \(\dot R R\T\) 是：

    Given a rotation curve \(R(t)\), how do you get the angular velocity? Differentiate to get \(\dot R\), which is not skew-symmetric; but \(R\T\dot R\) and \(\dot R R\T\) are:

    $$ \skew{\boldsymbol\omega_b} = R\T\dot R \quad(\text{body frame}), \qquad \skew{\boldsymbol\omega_s} = \dot R R\T \quad(\text{space / world frame}), \qquad \boldsymbol\omega_s = R\,\boldsymbol\omega_b. $$

    它们读出的是同一个角速度矢量的两套坐标：世界系的 \(\boldsymbol\omega_s\) 和机体系的 \(\boldsymbol\omega_b\)，正如 5.1 节同一个点在 W 和 B 里有不同读数。陀螺仪是装在机体上的，它读出的是 \(\boldsymbol\omega_b\)。下面的实验里，机体沿一条随时间变化的欧拉角曲线运动，数值求导得 \(\dot R\)，再读出两个角速度。

    They read off the same angular-velocity vector in two coordinate systems: \(\boldsymbol\omega_s\) in the world frame and \(\boldsymbol\omega_b\) in the body frame, just as in Lesson 5.1 the same point has different readings in W and B. A gyro is bolted to the body, so it reads \(\boldsymbol\omega_b\). In the lab below, the body follows a time-varying Euler-angle curve; we differentiate numerically to get \(\dot R\) and then read off both angular velocities.

    FIG 2从 Ṙ 读出 ω（机体系与世界系）Reading ω off Ṙ (body vs. world frame)
    拖动画面旋转视角drag to orbit
    紫色箭头是物理上的角速度矢量（只有一个）。浅色细线是它在世界轴（红绿蓝虚线）和在机体轴（实线）上的分量，分别对应 \(\boldsymbol\omega_s\) 和 \(\boldsymbol\omega_b\) 的三个读数。青色细线是机体的 \(x\) 轴尖端走过的轨迹。右侧第三行检查两件事：\(R\T\dot R\) 的对称部分确实是 0（数值误差内），以及 \(\boldsymbol\omega_s = R\boldsymbol\omega_b\)。 The purple arrow is the physical angular-velocity vector (there is only one). The thin pale lines are its components along the world axes (dashed) and along the body axes (solid), i.e. the three readings of \(\boldsymbol\omega_s\) and of \(\boldsymbol\omega_b\). The cyan line is the path traced by the tip of the body's \(x\) axis. The third readout row checks two things: the symmetric part of \(R\T\dot R\) really is 0 (up to numerical error), and \(\boldsymbol\omega_s = R\boldsymbol\omega_b\).
    试一试Try this
    • 拖动时间，找一个时刻使 \(\|\boldsymbol\omega_s - \boldsymbol\omega_b\| > 0.8\)：同一个矢量，两套读数差得很远。Scrub time to find an instant with \(\|\boldsymbol\omega_s - \boldsymbol\omega_b\| > 0.8\): the same vector, two readings very far apart.
    • 切换到"纯绕 z"曲线：任何时候 \(\boldsymbol\omega_s = \boldsymbol\omega_b\)，为什么？Switch to the "pure z-spin" curve: \(\boldsymbol\omega_s = \boldsymbol\omega_b\) at all times. Why?

    李括号：小运动的"不可交换度"The Lie bracket: how much small motions fail to commute

    5.4 节的 FIG 2 里，先绕 \(x\) 转 \(a\) 再绕 \(y\) 转 \(b\)，和反过来，夹角约是 \(ab\)。把这个观察精确化：取两个小的角速度 \(\mathbf a = \varepsilon\mathbf A\)、\(\mathbf b = \varepsilon\mathbf B\)，做群的换位"先 \(\mathbf a\)，再 \(\mathbf b\)，再倒着撤销 \(\mathbf a\)，再撤销 \(\mathbf b\)"。如果它们可交换，结果就是单位旋转。实际上结果是一个小的旋转，它的转轴-转角向量近似等于 \(\mathbf a\times\mathbf b\)，也就是 hat 之后矩阵的换位子 \([\mathbf a^\wedge, \mathbf b^\wedge] = \mathbf a^\wedge\mathbf b^\wedge - \mathbf b^\wedge\mathbf a^\wedge\) 的 vee。这个 \(\mathbf a\times\mathbf b\) 就是 \(\so\) 的李括号。

    In FIG 2 of Lesson 5.4, turning about \(x\) by \(a\) then \(y\) by \(b\), versus the reverse, differs by an angle of about \(ab\). Make this precise: take two small angular velocities \(\mathbf a = \varepsilon\mathbf A\), \(\mathbf b = \varepsilon\mathbf B\) and form the group commutator "do \(\mathbf a\), then \(\mathbf b\), then undo \(\mathbf a\), then undo \(\mathbf b\)". If they commuted, the result would be the identity rotation. In fact the result is a small rotation whose axis-angle vector is approximately \(\mathbf a\times\mathbf b\), i.e. the vee of the matrix commutator \([\mathbf a^\wedge, \mathbf b^\wedge] = \mathbf a^\wedge\mathbf b^\wedge - \mathbf b^\wedge\mathbf a^\wedge\). This \(\mathbf a\times\mathbf b\) is the Lie bracket of \(\so\).

    FIG 3群的换位子 ≈ 李括号 a × bThe group commutator ≈ the Lie bracket a × b
    拖动画面旋转视角drag to orbit
    琥珀 = \(\mathbf a = \varepsilon\mathbf A\)，紫 = \(\mathbf b = \varepsilon\mathbf B\)，青 = \(\mathbf a\times\mathbf b\)，绿 = 群换位子的 Log：\(\mathbf c = \Log\big(\Exp(\mathbf a)\Exp(\mathbf b)\Exp(-\mathbf a)\Exp(-\mathbf b)\big)\)（这里先用 5.6 节的 Exp 和 Log，把它们当作"旋转向量 ↔ 旋转"的转换即可）。右侧表格列出 \(\varepsilon\) 减半时相对误差 \(\|\mathbf c - \mathbf a\times\mathbf b\|/\|\mathbf a\times\mathbf b\|\) 的变化：每次大约减半，说明误差比 \(\varepsilon^2\) 高一阶。 Amber = \(\mathbf a = \varepsilon\mathbf A\), purple = \(\mathbf b = \varepsilon\mathbf B\), cyan = \(\mathbf a\times\mathbf b\), green = the Log of the group commutator \(\mathbf c = \Log\big(\Exp(\mathbf a)\Exp(\mathbf b)\Exp(-\mathbf a)\Exp(-\mathbf b)\big)\) (Exp and Log are those of Lesson 5.6; for now read them as the conversion "rotation vector ↔ rotation"). The table lists the relative error \(\|\mathbf c - \mathbf a\times\mathbf b\|/\|\mathbf a\times\mathbf b\|\) as \(\varepsilon\) is halved: it roughly halves each time, so the error is one order higher than \(\varepsilon^2\).
    试一试Try this
    • 令 \(\mathbf A = (1,0,0)\)、\(\mathbf B = (0,1,0)\)，把 \(\varepsilon\) 减到 0.1 以下：绿色箭头几乎盖住青色箭头，相对误差小于 10%。Take \(\mathbf A = (1,0,0)\), \(\mathbf B = (0,1,0)\) and reduce \(\varepsilon\) below 0.1: the green arrow nearly covers the cyan one, with a relative error under 10%.
    • 让 \(\mathbf B\) 与 \(\mathbf A\) 平行（例如 \(\mathbf B = 2\mathbf A\)）。叉积为 0，换位子也精确为 0：绕同一根轴的旋转总是可交换的。Make \(\mathbf B\) parallel to \(\mathbf A\) (say \(\mathbf B = 2\mathbf A\)). The cross product is 0 and so is the commutator, exactly: rotations about the same axis always commute.

    se(3)：加上平移，得到 twistse(3): add translation to get the twist

    刚体的速度有 6 个数：角速度 \(\boldsymbol\omega\) 和线速度 \(\mathbf v\)，合起来是 twist \(\boldsymbol\xi = (\boldsymbol\omega, \mathbf v) \in \R^6\)（旋转在前，与 5.6 节、Modern Robotics 一致）。它的 hat 是一个 \(4\times4\) 矩阵 \(\boldsymbol\xi^\wedge = \begin{bmatrix}\skew{\boldsymbol\omega} & \mathbf v\\ \mathbf 0\T & 0\end{bmatrix}\)，作用在齐次点 \((\mathbf p, 1)\) 上，得到 \((\boldsymbol\omega\times\mathbf p + \mathbf v,\ 0)\)：点 \(\mathbf p\) 的速度。注意 \(\mathbf v\) 是"此刻位于原点的那个物体点"的速度，不一定是物体原点（一般位置）的速度。

    A rigid body's velocity has six numbers: angular velocity \(\boldsymbol\omega\) and linear velocity \(\mathbf v\), together a twist \(\boldsymbol\xi = (\boldsymbol\omega, \mathbf v) \in \R^6\) (rotation first, as in Lesson 5.6 and Modern Robotics). Its hat is a \(4\times4\) matrix \(\boldsymbol\xi^\wedge = \begin{bmatrix}\skew{\boldsymbol\omega} & \mathbf v\\ \mathbf 0\T & 0\end{bmatrix}\), which acts on the homogeneous point \((\mathbf p, 1)\) to give \((\boldsymbol\omega\times\mathbf p + \mathbf v,\ 0)\): the velocity of the point \(\mathbf p\). Note that \(\mathbf v\) is the velocity of whichever body point currently sits at the origin, which in general is not the velocity of the body's reference point.

    FIG 4se(3)：twist ξ = (ω, v) 的速度场se(3): the velocity field of a twist ξ = (ω, v)
    拖动画面旋转视角drag to orbit
    青色小箭头是各个格点的速度 \(\boldsymbol\omega\times\mathbf p + \mathbf v\)。紫色箭头是 \(\boldsymbol\omega\)，紫色虚线是"螺旋轴"：当 \(\boldsymbol\omega\ne0\) 时，轴上最近原点的点是 \(\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2\)，轴上的点只沿轴方向运动。琥珀色小箭头从原点指出 \(\mathbf v\)。 The small cyan arrows are the velocities \(\boldsymbol\omega\times\mathbf p + \mathbf v\) at lattice points. The purple arrow is \(\boldsymbol\omega\) and the dashed purple line is the "screw axis": for \(\boldsymbol\omega\ne0\), the axis point nearest the origin is \(\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2\), and points on the axis only move along it. The small amber arrow from the origin is \(\mathbf v\).
    试一试Try this
    • 保持 \(\boldsymbol\omega\ne0\)，调 \(\mathbf v\)，让测试点 \(\mathbf p\) 的速度恰好为 0。这时 \(\mathbf v\) 应等于什么？（答案：\(-\boldsymbol\omega\times\mathbf p\)。）Keep \(\boldsymbol\omega\ne0\) and tune \(\mathbf v\) until the test point \(\mathbf p\) has exactly zero velocity. What must \(\mathbf v\) equal? (Answer: \(-\boldsymbol\omega\times\mathbf p\).)
    • 令 \(\boldsymbol\omega = 0\)：每个格点的箭头一模一样，这是纯平移，螺旋轴在无穷远。Set \(\boldsymbol\omega = 0\): every lattice arrow is identical. This is a pure translation, with the screw axis at infinity.

    4再看数学The math

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 或 FIG 4 中对应的量会被高亮。Tip: hover the highlighted symbols in the formulas; the matching quantity in FIG 1 or FIG 4 lights up.

    hat 与 vee（\(\so\)）hat and vee (\(\so\))

    $$ \hl{w}{\cw{\boldsymbol\omega}} = \begin{bmatrix}\omega_1\\\omega_2\\\omega_3\end{bmatrix}\ \xrightarrow{\ (\cdot)^\wedge\ }\ \boldsymbol\omega^\wedge = \skew{\boldsymbol\omega} = \begin{bmatrix} 0 & -\omega_3 & \omega_2 \\ \omega_3 & 0 & -\omega_1 \\ -\omega_2 & \omega_1 & 0 \end{bmatrix}, \qquad \Omega^\vee = (\Omega_{32},\ \Omega_{13},\ \Omega_{21}), \qquad \skew{\boldsymbol\omega}\,\hl{p}{\mathbf p} = \boldsymbol\omega\times\mathbf p. $$

    \(\so = \{\Omega \in \R^{3\times3} : \Omega\T = -\Omega\}\) 是所有反对称矩阵的集合，是一个 3 维的向量空间，hat 与 vee 是它与 \(\R^3\) 之间的线性同构。（本课程、Modern Robotics、Barfoot 都用这个符号，Solà 的 micro Lie theory 写作 \([\boldsymbol\omega]_\times\)；约定一致。）

    \(\so = \{\Omega \in \R^{3\times3} : \Omega\T = -\Omega\}\) is the set of all skew-symmetric matrices, a 3-dimensional vector space, and hat and vee are the linear isomorphism between it and \(\R^3\). (This course, Modern Robotics and Barfoot use this notation; Solà's micro Lie theory writes \([\boldsymbol\omega]_\times\); the convention is the same.)

    生成元（基）Generators (a basis)

    取三根坐标轴方向的单位角速度，得到 \(\so\) 的三个生成元，它们正是基本旋转 \(R_x(\theta), R_y(\theta), R_z(\theta)\) 在 \(\theta=0\) 处的导数：

    The unit angular velocities along the three coordinate axes give the three generators of \(\so\); they are exactly the derivatives of the elementary rotations \(R_x(\theta), R_y(\theta), R_z(\theta)\) at \(\theta=0\):

    $$ G_1 = \ci{\skew{\hat{\mathbf e}_1}} = \begin{bmatrix}0&0&0\\0&0&-1\\0&1&0\end{bmatrix},\quad G_2 = \cj{\skew{\hat{\mathbf e}_2}} = \begin{bmatrix}0&0&1\\0&0&0\\-1&0&0\end{bmatrix},\quad G_3 = \ck{\skew{\hat{\mathbf e}_3}} = \begin{bmatrix}0&-1&0\\1&0&0\\0&0&0\end{bmatrix}, \qquad \boldsymbol\omega^\wedge = \omega_1G_1 + \omega_2G_2 + \omega_3G_3. $$

    例如 \(\frac{d}{d\theta}R_z(\theta)\big|_{0} = \begin{bmatrix}-\sin\theta&-\cos\theta&0\\\cos\theta&-\sin\theta&0\\0&0&0\end{bmatrix}_{\theta=0} = G_3\)。李代数中的任何元素都是生成元的线性组合，所以"3 个自由度"一目了然。

    For example, \(\frac{d}{d\theta}R_z(\theta)\big|_{0} = \begin{bmatrix}-\sin\theta&-\cos\theta&0\\\cos\theta&-\sin\theta&0\\0&0&0\end{bmatrix}_{\theta=0} = G_3\). Every element of the algebra is a linear combination of generators, which makes "three degrees of freedom" plain.

    曲线读出的速度Velocity read off a curve

    对 \(R\T R = I\) 求导（5.4 节）：\(R\T\dot R\) 反对称，所以 \(\dot R = R\,\boldsymbol\omega_b^\wedge\)；同理 \(\dot R R\T\) 反对称，\(\dot R = \boldsymbol\omega_s^\wedge R\)。比较两式：\(\boldsymbol\omega_s^\wedge = R\,\boldsymbol\omega_b^\wedge R\T = (R\boldsymbol\omega_b)^\wedge\)，即 \(\boldsymbol\omega_s = R\boldsymbol\omega_b\)（FIG 2）。\(\dot R = R\boldsymbol\omega_b^\wedge\) 是 5.6 节陀螺积分 \(R_{k+1} = R_k\Exp(\boldsymbol\omega\,dt)\) 的出发点：群元素乘上李代数元素，得到切向量。

    Differentiate \(R\T R = I\) (Lesson 5.4): \(R\T\dot R\) is skew-symmetric, so \(\dot R = R\,\boldsymbol\omega_b^\wedge\); likewise \(\dot R R\T\) is skew-symmetric and \(\dot R = \boldsymbol\omega_s^\wedge R\). Comparing: \(\boldsymbol\omega_s^\wedge = R\,\boldsymbol\omega_b^\wedge R\T = (R\boldsymbol\omega_b)^\wedge\), i.e. \(\boldsymbol\omega_s = R\boldsymbol\omega_b\) (FIG 2). The equation \(\dot R = R\boldsymbol\omega_b^\wedge\) is the starting point of the gyro integration \(R_{k+1} = R_k\Exp(\boldsymbol\omega\,dt)\) in Lesson 5.6: a group element times a Lie-algebra element gives a tangent vector.

    李括号The Lie bracket

    李代数除了加法、数乘，还带一个运算：李括号，对矩阵李代数就是换位子 \([A, B] = AB - BA\)。它衡量"\(A\)、\(B\) 不可交换的程度"。对 \(\so\)，换位子仍然是反对称的，并且

    Besides addition and scaling a Lie algebra carries one more operation, the Lie bracket, which for a matrix Lie algebra is the commutator \([A, B] = AB - BA\). It measures "how much \(A\) and \(B\) fail to commute". On \(\so\) the commutator is again skew-symmetric, and

    $$ \big[\boldsymbol\omega_1^\wedge,\ \boldsymbol\omega_2^\wedge\big] = (\boldsymbol\omega_1\times\boldsymbol\omega_2)^\wedge, \qquad [G_1, G_2] = G_3,\ \ [G_2, G_3] = G_1,\ \ [G_3, G_1] = G_2. $$

    证明用雅可比恒等式 \(\mathbf a\times(\mathbf b\times\mathbf x) - \mathbf b\times(\mathbf a\times\mathbf x) = (\mathbf a\times\mathbf b)\times\mathbf x\)（对任意 \(\mathbf x\) 成立，把它写成矩阵形式就是上式）。李括号还有一个更几何的含义，就是 FIG 3：群的换位子 \(e^{\mathbf a}e^{\mathbf b}e^{-\mathbf a}e^{-\mathbf b} = \exp\big([\mathbf a, \mathbf b] + O(\varepsilon^3)\big)\)。\(\se\) 的李括号为

    The proof uses the Jacobi identity \(\mathbf a\times(\mathbf b\times\mathbf x) - \mathbf b\times(\mathbf a\times\mathbf x) = (\mathbf a\times\mathbf b)\times\mathbf x\) (true for any \(\mathbf x\); written in matrix form it is the formula above). The bracket also has a geometric meaning, FIG 3: the group commutator is \(e^{\mathbf a}e^{\mathbf b}e^{-\mathbf a}e^{-\mathbf b} = \exp\big([\mathbf a, \mathbf b] + O(\varepsilon^3)\big)\). The bracket of \(\se\) is

    $$ \big[\boldsymbol\xi_1^\wedge,\ \boldsymbol\xi_2^\wedge\big]^\vee = \begin{bmatrix}\boldsymbol\omega_1\times\boldsymbol\omega_2\\ \boldsymbol\omega_1\times\mathbf v_2 - \boldsymbol\omega_2\times\mathbf v_1\end{bmatrix} = \underbrace{\begin{bmatrix}\skew{\boldsymbol\omega_1} & \mathbf 0\\ \skew{\mathbf v_1} & \skew{\boldsymbol\omega_1}\end{bmatrix}}_{\operatorname{ad}_{\boldsymbol\xi_1}}\boldsymbol\xi_2. $$

    这个 \(6\times6\) 矩阵 \(\operatorname{ad}_{\boldsymbol\xi}\) 叫李括号的矩阵形式（"小伴随"），5.8 节的伴随 Ad 是它的指数版本。这里的分块顺序对应 \((\boldsymbol\omega, \mathbf v)\)；若换成平移在前，分块的行和列都要对调，得到 \(\begin{bmatrix}\skew{\boldsymbol\omega_1} & \skew{\mathbf v_1}\\ \mathbf 0 & \skew{\boldsymbol\omega_1}\end{bmatrix}\)。

    The \(6\times6\) matrix \(\operatorname{ad}_{\boldsymbol\xi}\) is the matrix form of the bracket (the "little adjoint"); the adjoint Ad of Lesson 5.8 is its exponentiated version. The block order here follows \((\boldsymbol\omega, \mathbf v)\); with translation first, both the block rows and the block columns swap, giving \(\begin{bmatrix}\skew{\boldsymbol\omega_1} & \skew{\mathbf v_1}\\ \mathbf 0 & \skew{\boldsymbol\omega_1}\end{bmatrix}\).

    \(\se\) 的 hat、生成元与 se(2)The hat of \(\se\), its generators, and se(2)

    $$ \boldsymbol\xi = \begin{bmatrix}\boldsymbol\omega\\ \hl{v}{\mathbf v}\end{bmatrix}\xrightarrow{\ (\cdot)^\wedge\ }\boldsymbol\xi^\wedge = \begin{bmatrix}\skew{\boldsymbol\omega} & \mathbf v\\ \mathbf 0\T & 0\end{bmatrix},\qquad \boldsymbol\xi^\wedge\begin{bmatrix}\mathbf p\\1\end{bmatrix} = \begin{bmatrix}\boldsymbol\omega\times\mathbf p + \mathbf v\\ 0\end{bmatrix}, \qquad \mathrm{se}(2):\ \boldsymbol\xi = \begin{bmatrix}\omega\\ v_x\\ v_y\end{bmatrix}\mapsto\begin{bmatrix}0&-\omega&v_x\\ \omega&0&v_y\\0&0&0\end{bmatrix}. $$

    \(\se\) 有 6 个生成元：三个转动 \(\begin{bmatrix}G_i&\mathbf 0\\\mathbf 0\T&0\end{bmatrix}\)，三个平移 \(\begin{bmatrix}0&\hat{\mathbf e}_i\\\mathbf 0\T&0\end{bmatrix}\)，所以它是 6 维的，与 \(\SE\) 的维数相同（5.4 节）。群的切向量和速度的关系是 \(\dot T = T\,\boldsymbol\xi^\wedge\)（\(\boldsymbol\xi\) 在机体系里表示的 twist，\(T^{-1}\dot T = \boldsymbol\xi^\wedge\)），5.8 节会讲怎样在机体系 twist 和世界系 twist 之间转换。

    \(\se\) has six generators: three rotations \(\begin{bmatrix}G_i&\mathbf 0\\\mathbf 0\T&0\end{bmatrix}\) and three translations \(\begin{bmatrix}0&\hat{\mathbf e}_i\\\mathbf 0\T&0\end{bmatrix}\), so it is 6-dimensional, matching the dimension of \(\SE\) (Lesson 5.4). The relation between a tangent vector of the group and a velocity is \(\dot T = T\,\boldsymbol\xi^\wedge\) (the twist \(\boldsymbol\xi\) expressed in the body frame, \(T^{-1}\dot T = \boldsymbol\xi^\wedge\)); Lesson 5.8 shows how to convert between body-frame and world-frame twists.

    5机器人中的它：差速小车的 twistIn robotics: the twist of a differential-drive robot

    两个驱动轮的小车，轮距 \(b\)，左右轮线速度 \(v_l, v_r\)。车体中心的前进速度 \(v = (v_r + v_l)/2\)，转弯角速度 \(\omega = (v_r - v_l)/b\)。在机体系里，这辆车的运动就是一个 \(\mathrm{se}(2)\) 的 twist \(\boldsymbol\xi = (\omega, v, 0)\)：不会侧滑，所以 \(v_y = 0\)。位姿满足 \(\dot T = T\,\boldsymbol\xi^\wedge\)。

    常数 twist 的运动是什么样？如果 \(\omega\ne0\)，是一个圆：圆心在机体系里是 \((0, v/\omega)\)（瞬时转动中心），半径 \(|v/\omega|\)。\(\omega = 0\) 时是直线（圆心在无穷远），\(v = 0\) 时是原地转（圆心就是车体中心）。这正是 5.6 节的"螺旋运动"在平面里的版本。

    A two-wheeled cart with track width \(b\) and wheel speeds \(v_l, v_r\). The forward speed of the body centre is \(v = (v_r + v_l)/2\), and the turn rate is \(\omega = (v_r - v_l)/b\). In the body frame the motion is an \(\mathrm{se}(2)\) twist \(\boldsymbol\xi = (\omega, v, 0)\): no sideways slip, so \(v_y = 0\). The pose obeys \(\dot T = T\,\boldsymbol\xi^\wedge\).

    What does a constant twist do? If \(\omega\ne0\) the cart drives a circle whose centre, in the body frame, is \((0, v/\omega)\) (the instantaneous centre of rotation) and whose radius is \(|v/\omega|\). With \(\omega = 0\) it is a straight line (centre at infinity), and with \(v = 0\) it spins in place (the centre is the body centre). This is the planar version of the "screw motion" of Lesson 5.6.

    FIG 5差速小车：轮速 → twist → 轨迹Differential drive: wheel speeds → twist → path
    拖动小车可以重新放置drag the cart to relocate it
    琥珀色曲线是小车走过的轨迹，紫色虚圆是瞬时转动中心画出的圆，紫点是圆心 \((0, v/\omega)\) 在世界里的位置。轨迹由恒定 twist 下 \(\dot T = T\boldsymbol\xi^\wedge\) 的精确解（平面里的指数映射，下一节展开）逐步算出。把滑块拖到 \(v_l = v_r\) 时圆心跑到无穷远。 The amber curve is the path driven so far, the dashed purple circle is drawn around the instantaneous centre of rotation, and the purple dot is the centre \((0, v/\omega)\) placed in the world. The path is advanced step by step with the exact solution of \(\dot T = T\boldsymbol\xi^\wedge\) for a constant twist (the planar exponential map, expanded in the next lesson). When \(v_l = v_r\) the centre runs off to infinity.
    试一试Try this
    • 让两个轮速相等：\(\omega = 0\)，twist 是纯平移，路径是直线。Make the wheel speeds equal: \(\omega = 0\), the twist is a pure translation, and the path is a straight line.
    • 让左右轮速大小相等、方向相反：\(v = 0\)，原地转，圆心就在车体中心。Make the wheel speeds equal and opposite: \(v = 0\), the cart spins in place and the centre is the body centre.
    • 调出半径正好为 1 m 的圆周运动（误差 ±5 cm）。提示：\(R = \tfrac{b}{2}\tfrac{v_r+v_l}{v_r-v_l}\)。Tune the wheel speeds for a circle of radius exactly 1 m (±5 cm). Hint: \(R = \tfrac{b}{2}\tfrac{v_r+v_l}{v_r-v_l}\).
    真实系统里In real systems

    IMU 的陀螺仪直接给出 \(\boldsymbol\omega_b\)，是李代数 \(\so\) 里的向量；轮式里程计从编码器算出 \(\mathrm{se}(2)\) 的 twist，再积分得到位姿（5.6 节）；机械臂的末端 twist 是关节速度的线性函数 \(\boldsymbol\xi = J(\mathbf q)\dot{\mathbf q}\)（3.4 节机器人雅可比）。状态估计里，姿态的误差状态 \(\delta\boldsymbol\theta\) 和位姿的误差状态 \(\delta\boldsymbol\xi\in\R^6\) 就是李代数元素，它们的协方差是 \(3\times3\) 或 \(6\times6\) 的普通矩阵（5.9 节）。库的符号：Sophus 的 SO3::hat / vee、GTSAM 的 Pose3::Logmap / Expmap、Eigen 里自己写的 skew()，都是同一个东西。

    An IMU's gyro gives \(\boldsymbol\omega_b\) directly, a vector of the algebra \(\so\); wheel odometry turns encoder readings into an \(\mathrm{se}(2)\) twist and integrates it into a pose (Lesson 5.6); a robot arm's end-effector twist is a linear function of the joint rates, \(\boldsymbol\xi = J(\mathbf q)\dot{\mathbf q}\) (the robot Jacobian, Lesson 3.4). In state estimation, the orientation error state \(\delta\boldsymbol\theta\) and the pose error state \(\delta\boldsymbol\xi\in\R^6\) are Lie-algebra elements, and their covariances are ordinary \(3\times3\) or \(6\times6\) matrices (Lesson 5.9). Library names: Sophus's SO3::hat / vee, GTSAM's Pose3::Logmap / Expmap and a hand-written skew() in Eigen code are the same objects.

    6常见误区Common pitfalls

    误区 1：把 \(\dot R\) 当成角速度，或忘了它不是反对称的Pitfall 1: treating \(\dot R\) as the angular velocity, or forgetting it is not skew-symmetric

    \(\dot R\) 是切向量，位置依赖于 \(R\)；反对称的是 \(R\T\dot R\) 或 \(\dot R R\T\)。所以 \(\dot R\) 本身不能 vee。读出角速度时一定要乘回 \(R\T\)（机体系）或 \(R\T\) 的另一侧（世界系）。

    \(\dot R\) is a tangent vector whose position depends on \(R\); the skew-symmetric objects are \(R\T\dot R\) and \(\dot R R\T\). So you cannot vee \(\dot R\) itself. To read off the angular velocity, always multiply by \(R\T\) on the appropriate side (body frame: left, space frame: right).

    误区 2：分不清机体系与世界系的角速度Pitfall 2: mixing up body-frame and world-frame angular velocities

    \(\boldsymbol\omega_s = R\boldsymbol\omega_b\)。陀螺仪读的是 \(\boldsymbol\omega_b\)；控制器里用的角速度，有的在机体系（姿态环），有的在世界系（某些任务空间控制），弄错就会导致"小车倾斜后才出现的"方向错误。变量名里写清楚（omega_body、omega_world），和 5.1 节的坐标系命名纪律一样。

    \(\boldsymbol\omega_s = R\boldsymbol\omega_b\). A gyro reads \(\boldsymbol\omega_b\); controllers use angular velocities in the body frame (attitude loops) or the world frame (some task-space controllers), and mixing them gives errors that only appear after the robot tilts. Spell out the frame in variable names (omega_body, omega_world), the same discipline as in Lesson 5.1.

    误区 3：twist 的分量顺序、以及 v 的含义Pitfall 3: the ordering of twist components, and what v means

    本课程和 Modern Robotics 用 \((\boldsymbol\omega, \mathbf v)\)；Barfoot、Solà、Sophus 用 \((\mathbf v, \boldsymbol\omega)\)（5.6 节误区 5）。另外，twist 里的 \(\mathbf v\) 不一般是"物体原点的速度"：机体系 twist 里它恰好是机体原点的速度（机体系读数），世界系 twist 里却是"此刻位于世界原点的那个物体点"的速度，两者相差 \(\boldsymbol\omega\times\mathbf t\)。

    This course and Modern Robotics use \((\boldsymbol\omega, \mathbf v)\); Barfoot, Solà and Sophus use \((\mathbf v, \boldsymbol\omega)\) (Pitfall 5 of Lesson 5.6). Also, the \(\mathbf v\) of a twist is not in general "the velocity of the body's origin": in a body-frame twist it happens to be the body origin's velocity (read in body axes), but in a world-frame twist it is the velocity of whichever body point currently sits at the world origin; the two differ by \(\boldsymbol\omega\times\mathbf t\).

    误区 4：以为换位子精确等于叉积Pitfall 4: thinking the commutator exactly equals the cross product

    矩阵换位子 \([\mathbf a^\wedge, \mathbf b^\wedge]\) 精确等于 \((\mathbf a\times\mathbf b)^\wedge\)，这是李代数里的恒等式。但群的换位子 \(e^{\mathbf a}e^{\mathbf b}e^{-\mathbf a}e^{-\mathbf b}\) 只在小量下近似 \(\exp(\mathbf a\times\mathbf b)\)，误差是 \(\varepsilon^3\) 阶（FIG 3）。大旋转的"可交换程度"要用完整的 BCH 公式。

    The matrix commutator \([\mathbf a^\wedge, \mathbf b^\wedge]\) equals \((\mathbf a\times\mathbf b)^\wedge\) exactly; that is an identity in the algebra. But the group commutator \(e^{\mathbf a}e^{\mathbf b}e^{-\mathbf a}e^{-\mathbf b}\) only approximates \(\exp(\mathbf a\times\mathbf b)\) for small quantities, with an error of order \(\varepsilon^3\) (FIG 3). For large rotations, how much they fail to commute needs the full BCH formula.

    7小练习Check yourself

    1. \(\boldsymbol\omega = (1, 2, 3)\) 的 hat \(\boldsymbol\omega^\wedge\) 是？1. What is the hat \(\boldsymbol\omega^\wedge\) of \(\boldsymbol\omega = (1, 2, 3)\)?
    hat 的固定模式：第 1 行 \((0, -\omega_3, \omega_2)\)，第 2 行 \((\omega_3, 0, -\omega_1)\)，第 3 行 \((-\omega_2, \omega_1, 0)\)。选项 A 是它的转置（符号反了，相当于 \(-\boldsymbol\omega\) 的 hat）；选项 D 是对称矩阵，不是 hat。The fixed pattern: row 1 \((0, -\omega_3, \omega_2)\), row 2 \((\omega_3, 0, -\omega_1)\), row 3 \((-\omega_2, \omega_1, 0)\). Option A is its transpose (the hat of \(-\boldsymbol\omega\)); option D is symmetric, not a hat.
    2. \(\boldsymbol\omega = (0,0,1)\)，一个点在 \(\mathbf p = (1, 0, 0)\)。它的瞬时速度 \(\boldsymbol\omega\times\mathbf p\) 是？2. With \(\boldsymbol\omega = (0,0,1)\) and a point at \(\mathbf p = (1, 0, 0)\), what is its instantaneous velocity \(\boldsymbol\omega\times\mathbf p\)?
    \((0,0,1)\times(1,0,0) = (0\cdot0 - 1\cdot0,\ 1\cdot1 - 0\cdot0,\ 0) = (0, 1, 0)\)。右手定则：绕 \(z\) 轴逆时针转，\(x\) 轴上的点向 \(+y\) 方向运动。\((0,0,1)\times(1,0,0) = (0\cdot0 - 1\cdot0,\ 1\cdot1 - 0\cdot0,\ 0) = (0, 1, 0)\). Right-hand rule: counter-clockwise about \(z\), a point on the \(x\) axis moves in \(+y\).
    3. 生成元 \(G_1, G_2, G_3\) 的李括号 \([G_1, G_2]\) 等于？3. The Lie bracket \([G_1, G_2]\) of the generators equals
    \([\hat{\mathbf e}_1^\wedge, \hat{\mathbf e}_2^\wedge] = (\hat{\mathbf e}_1\times\hat{\mathbf e}_2)^\wedge = \hat{\mathbf e}_3^\wedge = G_3\)。如果括号是 0，两个转动就可交换，而绕 \(x\) 与绕 \(y\) 转动显然不可交换。\([\hat{\mathbf e}_1^\wedge, \hat{\mathbf e}_2^\wedge] = (\hat{\mathbf e}_1\times\hat{\mathbf e}_2)^\wedge = \hat{\mathbf e}_3^\wedge = G_3\). A zero bracket would mean the rotations commute, and rotations about \(x\) and \(y\) clearly do not.
    4. twist \(\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)\)，\(\boldsymbol\omega = (0,0,2)\)，\(\mathbf v = (0,-2,0)\)。哪个点的速度恰好为 0？4. A twist \(\boldsymbol\xi = (\boldsymbol\omega, \mathbf v)\) has \(\boldsymbol\omega = (0,0,2)\) and \(\mathbf v = (0,-2,0)\). Which point has exactly zero velocity?
    点 \(\mathbf p\) 的速度是 \(\boldsymbol\omega\times\mathbf p + \mathbf v\)。\(\mathbf p = (1,0,0)\)：\((0,0,2)\times(1,0,0) = (0,2,0)\)，加上 \(\mathbf v\) 得 0。这正是螺旋轴上最近原点的点 \(\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2 = (1,0,0)\)。原点的速度是 \(\mathbf v \ne 0\)。The velocity of the point \(\mathbf p\) is \(\boldsymbol\omega\times\mathbf p + \mathbf v\). For \(\mathbf p = (1,0,0)\): \((0,0,2)\times(1,0,0) = (0,2,0)\), and adding \(\mathbf v\) gives 0. This is exactly the screw-axis point nearest the origin, \(\mathbf q = \boldsymbol\omega\times\mathbf v/\|\boldsymbol\omega\|^2 = (1,0,0)\). The origin moves with velocity \(\mathbf v \ne 0\).
    5. \(\so\) 和 \(\se\) 的维数分别是？5. What are the dimensions of \(\so\) and \(\se\)?
    李代数的维数等于对应李群作为流形的维数（5.4 节）：\(\SO\) 是 3，\(\SE\) 是 6。矩阵虽然是 \(3\times3\)、\(4\times4\)，但反对称（以及最后一行为 0）的约束把独立元素个数压到了 3 和 6。The dimension of a Lie algebra equals that of the Lie group as a manifold (Lesson 5.4): 3 for \(\SO\), 6 for \(\SE\). Although the matrices are \(3\times3\) and \(4\times4\), the skew-symmetry (and the zero last row) squeeze the independent entries down to 3 and 6.

    8延伸阅读Further reading

    • K. Lynch & F. Park, Modern Robotics, Ch. 3.2–3.3 — 角速度、\(\so\)、twist 与 \(\se\)（本课记号与它一致）angular velocity, \(\so\), twists and \(\se\) (this lesson's notation matches it)
    • J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537, §2 — 李群、李代数、hat/vee 与李括号的简明总结（平移在前）a compact summary of Lie groups, algebras, hat/vee and the bracket (translation first)
    • T. Barfoot, State Estimation for Robotics, Ch. 7 — \(\so\)、\(\se\) 的矩阵李代数与换位子the matrix Lie algebras \(\so\), \(\se\) and their commutators
    • R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 2 — 刚体运动与 twist 的经典处理the classic treatment of rigid motion and twists
    • 3Blue1Brown, "Cross products in the light of linear transformations" — 叉积为什么是一个线性变换（hat 的几何来源）why the cross product is a linear map (the geometric origin of hat)