01 数学基础 3D 视觉视觉 SLAM 机器人机器人抓取

Mathematics of Rigid-Body Motion

从欧氏几何，
走到 SE(3) 上的优化

刚体位姿不是 4×4 数组的十六个独立元素，而是六维流形上的状态。本章从点、向量、基和齐次坐标出发，建立 SO(3)、SE(3)、指数/对数映射、twist、Adjoint 与左右扰动的统一语言。

Affine Space Before Matrices

先识别几何对象，再选择正确的变换规则

空间中的点描述位置；向量描述两个点之差或自由方向；法向编码切平面约束；坐标基负责把几何对象变成数字。它们都可写成数组，但数组形状相同并不代表语义相同。

Point, direction and normal

\tilde{\mathbf p}=\begin{bmatrix}\mathbf p\\1\end{bmatrix},\quad \tilde{\mathbf v}=\begin{bmatrix}\mathbf v\\0\end{bmatrix},\quad \mathbf p'=A\mathbf p+\mathbf t,\quad \mathbf v'=A\mathbf v,\quad \mathbf n'\propto A^{-\top}\mathbf n

w=1 让点接收平移；w=0 让自由向量忽略平移。法向由 nᵀd=0 定义，因此要求 n′ᵀd′=0，推出 n′∝A⁻ᵀn。

几何对象	齐次 / 结构	刚体规则	一般仿射规则	保持量	常见陷阱
点 point	`[pᵀ, 1]ᵀ`	p′ = Rp + t	p′ = Ap + t	刚体下点间距离保持；点会受平移影响	点属于仿射空间；两个点可以相减，但“原点改变”会改变点坐标
自由向量 / 方向	`[vᵀ, 0]ᵀ`	v′ = Rv	v′ = Av	刚体下长度、夹角和点积保持；不受平移影响	速度若在旋转 frame 间转换还需区分表达 frame；不能给方向补 w=1
法向 normal	`协向量 / 平面约束`	n′ = Rn	n′ ∝ A⁻ᵀn	保持与变换后切向垂直，再按需要单位化	只有正交或一致尺度变换下，把法向当普通方向变换才“碰巧”可用
坐标基 basis	`B = [b₁ b₂ b₃]`	B′ = RB	B′ = AB	R 的列向量构成正交单位基；一般 A 的列不必正交	矩阵的列究竟是“新基在旧系中的表达”还是线性映射，取决于主动/被动语义
位姿 pose	`T = [[R,t],[0ᵀ,1]]`	T ∈ SE(3)	T ∈ Aff(3) 时左上块为一般 A	刚体保持距离、角度、体积和定向	4×4 外形相同不保证是刚体；必须检查 RᵀR 与 det(R)

DOT PRODUCT

点积定义长度与夹角

‖v‖²=vᵀv，cosθ=(aᵀb)/(‖a‖‖b‖)。正交 R 满足 (Ra)ᵀ(Rb)=aᵀb。

CROSS PRODUCT

叉乘可以写成反对称矩阵

[a]×b=a×b，且 [Ra]×=R[a]×Rᵀ。它把三维角速度、法向与 SE(3) Jacobian 连接起来。

AFFINE COMBINATION

点的加权和要求权重和为 1

Σᵢαᵢpᵢ 在 Σᵢαᵢ=1 时与原点选择无关；质心、重心坐标和插值都依赖这一性质。

What Is Preserved?

刚体、相似、仿射与反射的差别在保持量

不要只看矩阵尺寸。一个变换属于哪个族，取决于左上线性块的约束、行列式与它保持的几何结构；这也决定自由度、可用求解器和法向处理方式。

变换族	矩阵形式	DoF	约束	保持量	6D 相关用途
SO(3)	`R ∈ ℝ³ˣ³`	3	RᵀR = I，det(R)=+1	长度、角度、体积与定向；原点固定	朝向、角速度积分、旋转误差
SE(3)	`[[R,t],[0ᵀ,1]]`	6	R ∈ SO(3)，t ∈ ℝ³	欧氏距离、角度、体积与定向	6D 刚体位姿、相机轨迹、机器人运动学
Sim(3)	`[[sR,t],[0ᵀ,1]]`	7	R ∈ SO(3)，s > 0	角度与长度比例；所有方向统一缩放	单目 SLAM 尺度对齐、未知统一尺度
Aff(3)	`[[A,t],[0ᵀ,1]]`	12	A ∈ GL(3)，det(A) ≠ 0	直线、平行关系与仿射组合	图像/三维数据变换、非刚性前处理；不是 6D pose
O(3) \ SO(3)	`Q，det(Q)=−1`	3 + 离散分支	QᵀQ = I，det(Q)=−1	距离与角度，但翻转定向	SVD 配准中的反射失败、轴约定镜像排错

projective⊃affine⊃similarity⊃rigid · SE(3)⊃rotation · SO(3) 集合越向右，约束越强、保持的度量结构越多；不是“矩阵越大越高级”。

SO(3) and SE(3)

合法刚体集合对乘法和求逆封闭

群结构让变换链可以可靠复合：单位元表示不运动，逆元反向翻译坐标，结合律允许重新加括号。矩阵乘法通常不交换，因此动作顺序不能随意调换。

Special orthogonal group

SO(3)=\left\{R\in\mathbb R^{3\times3}\mid R^\top R=I,\ \det R=+1\right\}

R 的 9 个元素受 6 个独立约束，只剩 3 个物理自由度。det=−1 的正交矩阵是反射或旋转加反射，不属于 SO(3)。

Special Euclidean group

SE(3)=\left\{T=\begin{bmatrix}R&\mathbf t\\\mathbf0^\top&1\end{bmatrix}\ \middle|\ R\in SO(3),\ \mathbf t\in\mathbb R^3\right\}

SE(3) 包含由旋转作用于平移形成的半直积结构：旋转会改变后续平移的表达，因此 6D 刚体不是两个互不相关的三维向量简单拼接。

Composition

T_1T_2=\begin{bmatrix}R_1R_2&R_1\mathbf t_2+\mathbf t_1\\\mathbf0^\top&1\end{bmatrix}

右侧变换先作用。第二段平移 t₂ 必须先由 R₁ 旋转到最终 target frame，再与 t₁ 相加。

Inverse

T^{-1}=\begin{bmatrix}R^\top&-R^\top\mathbf t\\\mathbf0^\top&1\end{bmatrix}

逆平移不是简单 −t；它还要从原 target frame 重新表达回 source frame。

MINIMUM NUMERICAL EXAMPLE

绕 z 旋转 90°，再平移 [1,2,3]ᵀ

R = [[0,−1,0],[1,0,0],[0,0,1]]
p = [1,0,0]ᵀ
p′ = Rp+t = [1,3,3]ᵀ

用 T⁻¹ 恢复时先减去 t，再乘 Rᵀ：Rᵀ(p′−t)=[1,0,0]ᵀ。若直接对 t 取负并按错误顺序计算，就无法回到原点。

Interactive Rigid Geometry Lab

让保持量、Exp / Log 与扰动 frame 变成可计算现象

第一个视图切换刚体、相似、仿射和反射；第二个视图真正执行 SE(3) 指数/对数映射；第三个视图比较左右乘更新，并用 Adjoint 证明两套局部坐标如何表达同一物理增量。

点 p · w=1 方向 d · w=0 法向 n · A⁻ᵀn 二维切片的规律直接推广到三维

Classification

Rigid transform

保距离 · 保角度 · 保定向

det(A)

体积与定向

+1.000orientation preserving

Length ratio

‖A d‖ / ‖d‖

1.000方向长度保持

Normal check

(A d) · (A⁻ᵀ n)

0.00e+0正确法向仍垂直于切向

Wrong rule

(A d) · (A n)

0.00e+0刚体下碰巧仍正确

齐次坐标不是把所有三维量都补一个 1。 点使用 [p;1]，因此受平移影响；自由向量与方向使用 [d;0]。法向是约束切平面的协向量，一般仿射变换下应使用 A⁻ᵀ，而不是 A。

Lie Group and Lie Algebra

在非线性姿态流形旁边，建立一个可线性计算的切空间

SO(3) 和 SE(3) 可以连续变化，因此同时具有群与光滑流形结构。Lie algebra 是单位元处的切空间；它是普通向量空间，适合描述速度、小增量、优化步和局部高斯不确定性。

GROUP

T ∈ SE(3)

全局、非线性、可复合的有限刚体运动。

Log ↓
↑ Exp

ALGEBRA

ξ̂ ∈ se(3)

4×4 矩阵形式的单位元切向量。

vee ⇄ hat

COORDINATES

ξ = [ρ;φ] ∈ R⁶

本站固定先平移切向量 ρ，后旋转向量 φ。

Hat operator

[\boldsymbol\phi]_\times=\begin{bmatrix}0&-\phi_z&\phi_y\\\phi_z&0&-\phi_x\\-\phi_y&\phi_x&0\end{bmatrix},\qquad \boldsymbol\xi^\wedge=\begin{bmatrix}[\boldsymbol\phi]_\times&\boldsymbol\rho\\\mathbf0^\top&0\end{bmatrix}

[φ]×v=φ×v。不同库可能把 ξ 排成 [φ;ρ]；这不是符号外观问题，Jacobian、Adjoint 和协方差块都随顺序改变。

SO(3) exponential · Rodrigues

\operatorname{Exp}(\boldsymbol\phi)=I+\frac{\sin\theta}{\theta}\Phi+\frac{1-\cos\theta}{\theta^2}\Phi^2,\quad \Phi=[\boldsymbol\phi]_\times,\ \theta=\|\boldsymbol\phi\|

θ→0 时不能直接除以 θ，应使用 sinc / cosc 或 Taylor 级数。Log 返回主值时，旋转角通常落在 [0,π]。

SE(3) exponential

\operatorname{Exp}(\boldsymbol\xi)=\exp(\boldsymbol\xi^\wedge)=\begin{bmatrix}\operatorname{Exp}(\boldsymbol\phi)&J_l(\boldsymbol\phi)\boldsymbol\rho\\\mathbf0^\top&1\end{bmatrix}

旋转和平移不是各自独立 exp；半直积结构通过 SO(3) 左 Jacobian 把 ρ 映射成最终 t。

SO(3) left Jacobian

J_l(\boldsymbol\phi)=I+\frac{1-\cos\theta}{\theta^2}\Phi+\frac{\theta-\sin\theta}{\theta^3}\Phi^2,\qquad \boldsymbol\rho=J_l^{-1}(\boldsymbol\phi)\mathbf t

小角度下 Jₗ≈I+½Φ+⅙Φ²；纯平移时 φ=0、Jₗ=I，因此此时才有 ρ=t。

VELOCITY

twist 是瞬时刚体速度

若 ξ 随时间恒定，T(s)=Exp(sξ^) 描述一条螺旋运动。φ 给角运动，ρ 与旋转耦合后形成有限平移。

OPTIMIZATION

在 R⁶ 解线性增量

Gauss–Newton / LM 求 δξ，再用 Exp 把更新送回 SE(3)，保证每次迭代仍是合法刚体。

UNCERTAINTY

协方差属于某个局部切空间

6×6 Σ 必须说明左/右扰动、参考 pose、frame、分量顺序与平移单位，否则无法正确传播或比较。

Retraction, Jacobian and Adjoint

“在 pose 上加一点”必须说明乘在哪一侧

同一个六维数组放在 T 左边或右边，会在不同坐标系中解释。二者都可建立正确优化器，但 residual、Jacobian、协方差和更新规则必须成套一致。

约定	更新	对 y=Tp 的一阶作用	点 Jacobian	几何语义
左扰动 / target-frame	`T ← Exp(δξₗ^) T`	y = Tp 时，δy ≈ δρₗ − [y]×δφₗ	`[I, −[y]×]`	增量在输出 / 世界 / 相机 target frame 中表达
右扰动 / source-frame	`T ← T Exp(δξᵣ^)`	y = Tp 时，δy ≈ Rδρᵣ − R[p]×δφᵣ	`[R, −R[p]×]`	增量在输入 / 物体 / 机体 local frame 中表达
Adjoint 等价转换	`δξₗ = AdT δξᵣ`	Exp(δξₗ^)T = T Exp(δξᵣ^)	`Σₗ = AdT Σᵣ AdTᵀ`	同一物理微扰的两套六维坐标，而不是两种不同运动

Adjoint for ξ = [ρ; φ]

\operatorname{Ad}_T=\begin{bmatrix}R&[\mathbf t]_\times R\\0&R\end{bmatrix},\qquad \begin{bmatrix}\boldsymbol\rho_l\\\boldsymbol\phi_l\end{bmatrix}=\operatorname{Ad}_T\begin{bmatrix}\boldsymbol\rho_r\\\boldsymbol\phi_r\end{bmatrix}

该块结构依赖本站 [ρ;φ] 排序。若库使用 [φ;ρ]，同一个 Adjoint 的分块位置会改变。

Conjugation identity

T\operatorname{Exp}(\delta\boldsymbol\xi_r)T^{-1}=\operatorname{Exp}(\operatorname{Ad}_T\delta\boldsymbol\xi_r)\ \Longrightarrow\ \operatorname{Exp}(\delta\boldsymbol\xi_l)T=T\operatorname{Exp}(\delta\boldsymbol\xi_r)

Adjoint 不是额外的物理运动，而是同一 Lie algebra 元素在另一 frame / 扰动侧中的坐标。

OPTIMIZER CONTRACT

实现前冻结六项约定

pose 方向：target-from-source
ξ 顺序：[ρ;φ] 或 [φ;ρ]
左乘还是右乘 retraction
φ 单位：优化内部必须 rad
Jacobian 对哪个局部坐标求导
Σ 表达在哪个切空间与 frame

Errors Are Also Geometric Objects

姿态误差要在流形上比较，旋转和平移不要无尺度混成一个数

矩阵逐元素差、Euler 角逐分量差和四元数 L2 都受表示选择影响。更稳定的做法是先构造相对变换，再用 Log 得到局部误差；之后根据任务尺度或协方差选择如何加权。

Right-invariant local pose error

\mathbf e_r=\operatorname{Log}\!\left(T_{\mathrm{est}}^{-1}T_{\mathrm{gt}}\right)^\vee\in\mathbb R^6

误差在 est 的 local / source 侧表达。交换乘法顺序或使用左 invariant error 会得到另一 frame 中的六维坐标，但在一致实现下都可以成立。

Rotation geodesic distance

e_R=\left\|\operatorname{Log}(R_{\mathrm{est}}^\top R_{\mathrm{gt}})^\vee\right\|=\cos^{-1}\!\left(\operatorname{clip}\frac{\operatorname{tr}(R_{\mathrm{est}}^\top R_{\mathrm{gt}})-1}{2}\right)

clip 防止浮点误差把 acos 输入推到 [−1,1] 外。对称物体还需在等价旋转集合上取最小值。

TRANSLATION

米与毫米会改变数值

eₜ=‖t_est−t_gt‖ 有长度单位；任何综合阈值必须写单位与对象尺度。

ROTATION

角度应采用最短测地线

SO(3) 主值角位于 [0,π]；Euler 分量差可能在 ±180° 处不连续。

WHITENING

用协方差给残差定标

若 Σ 合法且 frame 一致，可用 eᵀΣ⁻¹e；这比随意设“1 m = 1 rad”更有意义。

TASK LOSS

几何等距不代表任务等价

抓取、装配和投影对误差方向的敏感度不同，最终还需任务 Jacobian 或成功率。

NUMERICAL STABILITY

SO(3) / SE(3) 核心函数的最小防护

小角度：对 sinθ/θ、(1−cosθ)/θ² 与 (θ−sinθ)/θ³ 使用级数或稳定 special functions。
接近 π：Log 的旋转轴对微小噪声敏感，使用稳定 quaternion / diagonal 分支，并接受轴符号与主值不唯一。
旋转投影：对噪声矩阵 M=UΣVᵀ，使用 R=U diag(1,1,det(UVᵀ))Vᵀ，禁止输出反射。
正交检查：持续记录 ‖RᵀR−I‖、det(R)、有限值与矩阵条件数；不要等渲染异常才发现。
有限差分：解析 Jacobian 必须用同一左/右 retraction 做 central difference 验证，并扫描零角、一般角与接近 π。
矩阵复合：长链可周期性归一化 quaternion 或投影 R，但先定位漂移来源，不能用投影掩盖方向/单位 bug。

A Shared Mathematical Interface

同一套刚体几何贯穿视觉、SLAM、机器人与抓取

领域之间主要差在状态命名、观测残差和任务约束；变换复合、局部更新、Adjoint 与误差传播仍是同一套机制。

领域	典型状态	典型残差	本章数学的作用
6D 物体位姿	`TCO ∈ SE(3)`	重投影、轮廓、深度、表面或特征残差	网络可预测任意表示，但求解、精修和误差最终必须回到刚体流形
视觉 SLAM / BA	`TWC、landmarks、尺度或速度偏置`	跨帧重投影、IMU 预积分、回环和相对 pose	每次迭代在切空间求 δξ，再用左/右 retraction 更新 pose，不能直接相加 4×4 矩阵
机器人运动学	`TBE(q)、TET、手眼外参`	末端位置/姿态、闭环约束、标定方程	twist、Adjoint 和 Jacobian 把关节速度、工具速度和不同 frame 的扰动连接起来
机器人抓取	`TBO、TOG、TBG`	抓取位姿误差、接触、碰撞、可达性	object-to-grasp 与视觉 object pose 复合；误差必须转换到任务真正敏感的 frame

02 · NEXT

坐标系与变换约定

把抽象 target/source 具体化为 object、camera、world、robot 与 gripper frame。

03

旋转与姿态表示

深入 Euler、矩阵、轴角、四元数、连续表示与 Egocentric / Allocentric。

07

3D–3D 对齐与配准

把刚体约束落到 Kabsch、Umeyama、RANSAC 与 ICP 的实际估计。

Failure Modes

大多数几何 bug 不是公式不会，而是语义没有冻结

把 frame、变换方向、分量顺序、更新侧、角度和长度单位写进 API 类型、日志和测试。只有这样，数值正确的函数才能组成语义正确的系统。

高频错误	表现	最低成本修复
把点和方向都写成 w=1	平移错误地加到方向、轴或速度上	对点用 [p;1]，对自由向量用 [v;0]；分别写单元测试
用 A 直接变换一般仿射法向	非均匀缩放或 shear 后法向不再垂直表面	使用 A⁻ᵀn 并重新单位化，检查 n′ᵀd′≈0
对 4×4 pose 逐元素做 SGD 更新	R 离开 SO(3)，det 与正交性漂移	在切空间优化 δξ，再通过 Exp / retraction 更新；必要时用 SVD 投影
把 twist 的 ρ 当成最终 t	有限角度时忽略 Jₗ(φ)，轨迹与 Jacobian 错误	明确 ξ 顺序和 Exp 公式；纯平移之外计算 t=Jₗρ
左 Jacobian 配右扰动	梯度方向和协方差 frame 不一致，收敛慢或直接发散	冻结 boxplus 定义，解析 Jacobian 与有限差分按同一侧对照
用同一六维数字比较左右更新	把 local 与 target frame 的坐标当作同一物理增量	通过 δξₗ=AdTδξᵣ 转换后再比较
Exp / Log 在零角和 π 附近直接套通式	0/0、灾难性消减、轴符号不稳定	小角度使用级数；接近 π 使用稳定分支并接受主值/轴符号不唯一
把米和毫米直接塞进同一 se(3) 范数	旋转与平移权重无物理意义，阈值随单位改变	分开报告或用任务尺度/协方差白化后组合

GEOMETRY UNIT TESTS

不要只测试一个“看起来合理”的姿态

inverse(T) · T ≈ I Exp(Log(T)) ≈ T Log(Exp(ξ)) ≈ ξ · principal branch det(R) ≈ +1 RᵀR ≈ I Exp(AdT ξ)T ≈ T Exp(ξ) (Ad A·B) ≈ Ad A · Ad B analytic J ≈ central difference J

Primary & Technical Sources

刚体运动与 Lie 理论的代表性原始资料

不同资料的 twist 顺序、左/右 Jacobian 与 perturbation 约定可能不同；使用公式前必须先读其 notation contract。

01

Solà, Deray & Atchuthan · A micro Lie theory for state estimation in roboticsSO(3) / SE(3)、Exp / Log、Jacobian、Adjoint、左/右扰动和不确定性的系统公式参考

↗ 02

Murray, Li & Sastry · A Mathematical Introduction to Robotic Manipulation刚体运动、指数坐标、twist、运动学与机器人操作的经典数学基础

↗ 03

Ethan Eade · Lie Groups for 2D and 3D TransformationsSO(3)、SE(3)、Sim(3) 的矩阵表示、指数映射、Adjoint 与不确定性推导

↗ 04

GTSAM · Pose3 documentationpose manifold 与 transform group 的区别、SE(3) 复合、逆、Exp / Log、retraction 和实现约定

↗ 05

Modern Robotics · Exponential Coordinates of Rigid-Body Motion从 se(3) twist 到 SE(3) 有限刚体运动，以及矩阵指数/对数的机器人学解释

↗

← 上一专题 · 00

6D 位姿入门导览

下一专题 · 02

坐标系与变换约定

把刚体几何放进 object、camera、world 与 robot 的实际 frame 链。

继续阅读 →

从欧氏几何，走到 SE(3) 上的优化

先识别几何对象，再选择正确的变换规则

点积定义长度与夹角

叉乘可以写成反对称矩阵

点的加权和要求权重和为 1

刚体、相似、仿射与反射的差别在保持量

合法刚体集合对乘法和求逆封闭

绕 z 旋转 90°，再平移 [1,2,3]ᵀ

让保持量、Exp / Log 与扰动 frame 变成可计算现象

Rigid transform

体积与定向

‖A d‖ / ‖d‖

(A d) · (A⁻ᵀ n)

(A d) · (A n)

T = Exp(ξ̂)

t = Jₗ(φ)ρ

Log(Exp(ξ̂))∨

δξₗ

δξᵣ

Tₗ versus Tᵣ

在非线性姿态流形旁边，建立一个可线性计算的切空间

T ∈ SE(3)

ξ̂ ∈ se(3)

ξ = [ρ;φ] ∈ R⁶

twist 是瞬时刚体速度

在 R⁶ 解线性增量

协方差属于某个局部切空间

“在 pose 上加一点”必须说明乘在哪一侧

实现前冻结六项约定

姿态误差要在流形上比较，旋转和平移不要无尺度混成一个数

米与毫米会改变数值

角度应采用最短测地线

用协方差给残差定标

几何等距不代表任务等价

SO(3) / SE(3) 核心函数的最小防护

同一套刚体几何贯穿视觉、SLAM、机器人与抓取

坐标系与变换约定

旋转与姿态表示

3D–3D 对齐与配准

大多数几何 bug 不是公式不会，而是语义没有冻结

不要只测试一个“看起来合理”的姿态

刚体运动与 Lie 理论的代表性原始资料

6D 位姿入门导览

坐标系与变换约定

进入完整专题

从欧氏几何，
走到 SE(3) 上的优化