Robotics - Modern Robotics
2026-08-25 · 随笔 · 371798712f24
Lie Group
SO(3)=所有三维旋转 SE(3)=所有三维旋转+平移 Lie group=既能组合，又能平滑变化的变换集合 Lie algebra=Lie group 在单位元附近的瞬时运动 exp=把瞬时运动积累成有限运动
SO(3)
好，先定义大写的 SO(3)。要注意：
- SO(3)：三维旋转矩阵组成的集合
- so(3)：与角速度有关的反对称矩阵空间
好，先定义大写的 (SO(3))。要注意：
- (SO(3))：三维旋转矩阵组成的集合
- (\mathfrak{so}(3))：与角速度有关的反对称矩阵空间
这是两个不同的对象。我们现在先讲 (SO(3))。
1. 直观定义
SO(3)=三维空间中所有合法旋转组成的集合\boxed{SO(3)=\text{三维空间中所有合法旋转组成的集合}}
一个三维旋转用 (3\times3) 矩阵 (R) 表示：
R=[r11r12r13r21r22r23r31r32r33]R=
\begin{bmatrix}
r_{11}&r_{12}&r_{13}
r_{21}&r_{22}&r_{23}
r_{31}&r_{32}&r_{33}
\end{bmatrix}
但不是任意 (3\times3) 矩阵都代表旋转。合法旋转必须满足：
RTR=I\boxed{R^TR=I}
以及：
detR=1\boxed{\det R=1}
因此正式定义是：
\boxed{
SO(3)=
\left{
R\in\mathbb R^{3\times3}
\mid
R^TR=I,\ \det R=1
\right}
}\boxed{
SO(3)=
\left{
R\in\mathbb R^{3\times3}
\mid
R^TR=I,\ \det R=1
\right}
}
2. 为什么要求 (R^TR=I)
由：
可以得到：
R−1=RTR^{-1}=R^T
这意味着 (R) 的三个列向量是相互正交的单位向量。
把 (R) 按列写成：
R=[r1r2r3]R=
\begin{bmatrix}
r_1&r_2&r_3
\end{bmatrix}
那么：
RTR=[r1Tr1r1Tr2r1Tr3r2Tr1r2Tr2r2Tr3r3Tr1r3Tr2r3Tr3]=IR^TR=
\begin{bmatrix}
r_1^Tr_1&r_1^Tr_2&r_1^Tr_3
r_2^Tr_1&r_2^Tr_2&r_2^Tr_3
r_3^Tr_1&r_3^Tr_2&r_3^Tr_3
\end{bmatrix}
=I
因此：
∣r1∣=∣r2∣=∣r3∣=1|r_1|=|r_2|=|r_3|=1
并且：
r1Tr2=r1Tr3=r2Tr3=0r_1^Tr_2=r_1^Tr_3=r_2^Tr_3=0
也就是三个列向量：
它们可以看成旋转后坐标系的三个坐标轴。
3. 为什么要求 (\det R=1)
满足 (R^TR=I) 的矩阵叫正交矩阵，它的行列式只能是：
detR=±1\det R=\pm1
其中：
- (\det R=1)：旋转
- (\det R=-1)：包含镜像反射
例如：
A=[−100010001]A=
\begin{bmatrix}
-1&0&0
0&1&0
0&0&1
\end{bmatrix}
满足：
但：
detA=−1\det A=-1
它把 (x) 轴翻转了，相当于镜像反射，而不是普通旋转。
所以 (SO(3)) 额外要求：
detR=1\det R=1
“Special”指的就是这个额外条件。
4. (SO(3)) 这个名字是什么意思
SO(3)=Special Orthogonal Group in 3 dimensionsSO(3)=\text{Special Orthogonal Group in 3 dimensions}
- (S)：Special，行列式为 (+1)
- (O)：Orthogonal，满足 (R^TR=I)
- (3)：作用于三维空间
- Group：在矩阵乘法下构成群
5. 一个旋转矩阵例子
绕 (z) 轴旋转角度 (\theta)：
Rz(θ)=[cosθ−sinθ0sinθcosθ0001]R_z(\theta)=
\begin{bmatrix}
\cos\theta&-\sin\theta&0
\sin\theta&\cos\theta&0
0&0&1
\end{bmatrix}
当：
θ=2π\theta=\frac{\pi}{2}
得到：
[
R_z\left(\frac{\pi}{2}\right)
\begin{bmatrix}
0&-1&0
1&0&0
0&0&1
\end{bmatrix}
]
把 (x) 轴单位向量：
ex=[1\0\0]e_x=
\begin{bmatrix}
1\0\0
\end{bmatrix}
旋转后：
Rzex=[0\1\0]=eyR_ze_x=
\begin{bmatrix}
0\1\0
\end{bmatrix}
=e_y
即 (x) 轴被旋转到了 (y) 轴。
6. 旋转矩阵为什么保持长度和角度
假设旋转前的向量是 (x)，旋转后：
[
x’=Rx
]
旋转后的长度：
[
|x’|^2
x’^Tx’
]
代入 (x’=Rx)：
[
|x’|^2
x^TR^TRx
]
因为 (R^TR=I)：
∣x′∣2=xTx=∣x∣2|x'|^2=x^Tx=|x|^2
所以：
∣Rx∣=∣x∣\boxed{|Rx|=|x|}
类似地，对于两个向量 (x,y)：
(Rx)T(Ry)=xTRTRy=xTy(Rx)^T(Ry)=x^TR^TRy=x^Ty
内积不变，因此夹角也不变。
所以旋转不会：
7. 为什么说 (SO(3)) 是一个群
单位元素
I=[100010001]∈SO(3)I=
\begin{bmatrix}
1&0&0
0&1&0
0&0&1
\end{bmatrix}
\in SO(3)
表示旋转 (0^\circ)。
旋转可以复合
如果：
R1,R2∈SO(3)R_1,R_2\in SO(3)
那么：
R1R2∈SO(3)R_1R_2\in SO(3)
(R_1R_2) 表示先执行 (R_2)，再执行 (R_1)。
每个旋转都有逆
R−1=RTR^{-1}=R^T
如果 (R) 表示旋转 (90^\circ)，(R^{-1}) 就表示反方向旋转 (90^\circ)。
满足结合律
(R1R2)R3=R1(R2R3)(R_1R_2)R_3=R_1(R_2R_3)
但旋转通常不满足交换律：
R1R2=R2R1\boxed{R_1R_2\neq R_2R_1}
先绕 (x) 轴转，再绕 (y) 轴转，一般不等于先绕 (y) 轴转，再绕 (x) 轴转。
8. 旋转矩阵的列表示什么
假设：
Rab=[x^bay^baz^ba]R_{ab}=
\begin{bmatrix}
\hat x_b^a&
\hat y_b^a&
\hat z_b^a
\end{bmatrix}
它的三列分别是：
- (b) 坐标系的 (x) 轴，在 (a) 坐标系中的表示
- (b) 坐标系的 (y) 轴，在 (a) 坐标系中的表示
- (b) 坐标系的 (z) 轴，在 (a) 坐标系中的表示
因此：
va=Rabvbv_a=R_{ab}v_b
表示把同一个几何向量从 (b) 坐标表达转换成 (a) 坐标表达。
例如：
Rab=[0−10100001]R_{ab}=
\begin{bmatrix}
0&-1&0
1&0&0
0&0&1
\end{bmatrix}
第一列：
[0\1\0]\begin{bmatrix}
0\1\0
\end{bmatrix}
说明 (b) 坐标系的 (x) 轴，在 (a) 坐标系看来指向 (a) 的 (y) 方向。
9. (SO(3)) 有几个自由度
旋转矩阵有 9 个元素，但受到正交约束，因此只有三个独立自由度：
dimSO(3)=3\dim SO(3)=3
可以直观理解为：
- 绕 (x) 轴旋转
- 绕 (y) 轴旋转
- 绕 (z) 轴旋转
但 (SO(3)) 不是普通的 (\mathbb R^3)，因为旋转具有周期性，并且旋转复合不满足交换律。
10. (SO(3)) 与 (SE(3)) 的关系
(SO(3)) 只描述朝向：
R∈SO(3)R\in SO(3)
(SE(3)) 同时描述朝向和位置：
T=[Rp01]∈SE(3)T=
\begin{bmatrix}
R&p
0&1
\end{bmatrix}
\in SE(3)
因此：
SO(3)=纯三维旋转\boxed{
SO(3)=\text{纯三维旋转}
}
SE(3)=三维旋转+三维平移\boxed{
SE(3)=\text{三维旋转}+\text{三维平移}
}
下一步的关键是小写的：
so(3)\mathfrak{so}(3)
它是所有 (3\times3) 反对称矩阵构成的空间，并且对应三维角速度和旋转轴：
ω∈R3⟷[ω]∈so(3)\omega\in\mathbb R^3
\longleftrightarrow
[\omega]\in\mathfrak{so}(3)
再通过矩阵指数连接到 (SO(3))：
R=e[ω]θ\boxed{
R=e^{[\omega]\theta}
}
SE(3)
SE(3)={[R0p1]R∈SO(3), p∈R3}
SE(3)=三维旋转+三维平移
刚体总共有六个自由度：
3 个平移自由度x,y,z+3 个旋转自由度roll,pitch,yaw
但要注意：SE(3) 是六维的，不代表我们一定用一个普通的六维向量表示它。旋转存在周期性和奇异性，不能在全局上简单地当成 R6
对，你已经基本抓住动机了。不过要把“连续组合”拆成两部分：
Group 负责组合与求逆\boxed{\text{Group 负责组合与求逆}}
Lie 结构负责连续变化、求导与局部线性化\boxed{\text{Lie 结构负责连续变化、求导与局部线性化}}
所以 (SO(3))、(SE(3)) 的价值不只是连续组合，而是为三维刚体运动提供一套同时支持：
- 表示
- 组合
- 求逆
- 求导
- 优化
- 状态估计
- 跨设备转换
的统一语言。
1. Group：把多个空间关系组合起来
例如：
[
T_{\text{world,object}}
T_{\text{world,camera}}
T_{\text{camera,object}}
]
或者机械臂：
[
T_{\text{base,gripper}}
T_{\text{base,1}}
T_{\text{1,2}}
\cdots
T_{\text{n,gripper}}
]
这里用到的是群结构：
- 两个位姿可以复合
- 复合结果仍然是合法位姿
- 位姿可以求逆
- 有单位位姿
例如：
[
T_{\text{camera,world}}
T_{\text{world,camera}}^{-1}
]
2. Lie：描述位姿怎样连续变化
当：
T(t)∈SE(3)T(t)\in SE(3)
随时间连续变化，我们关心：
- 当前速度
- 当前角速度
- 位姿的小变化
- Jacobian
- 如何在优化中更新位姿
于是使用：
so(3),se(3)\mathfrak{so}(3),\qquad\mathfrak{se}(3)
例如：
T(t+Δt)≈T(t)exp([V]Δt)T(t+\Delta t)
\approx
T(t)\exp([\mathcal V]\Delta t)
所以：
SE(3)=有限位姿\boxed{
SE(3)=\text{有限位姿}
}
se(3)=瞬时运动及局部小变化\boxed{
\mathfrak{se}(3)=\text{瞬时运动及局部小变化}
}
3. 机械臂参数只是特定机器人的内部语言
SO-ARM 的关节角：
[
q_{\mathrm{SOARM}}
(q_1,\dots,q_n)
]
只对这台特定结构的机械臂有意义。
换一台机械臂后：
相同的：
[
q
]
会产生完全不同的夹爪位姿。
但是“夹爪位于零件上方 5 cm，并垂直朝下”可以统一表示成：
Tgrasp∈SE(3)T_{\text{grasp}}\in SE(3)
不同机械臂分别计算：
qA=IKA(Tgrasp)q_A=IK_A(T_{\text{grasp}})
qB=IKB(Tgrasp)q_B=IK_B(T_{\text{grasp}})
所以：
q=机器人特定的内部表示\boxed{
q=\text{机器人特定的内部表示}
}
T∈SE(3)=相对通用的任务空间表示\boxed{
T\in SE(3)=\text{相对通用的任务空间表示}
}
这确实是跨机器人迁移的重要动机。
但 (SE(3)) 并不会自动保证 ML policy 能够泛化；它提供的是更统一、与具体机械结构较少绑定的表示，使泛化和迁移更容易。
4. State estimation 中为什么需要它
以 Aria 为例，系统需要估计：
Tworld,device(t)∈SE(3)T_{\text{world,device}}(t)\in SE(3)
但完整 state 可能还包括：
Xt=(Tt,,vt,,bgyro,,bacc)X_t=
\left(
T_t,,
v_t,,
b_{\text{gyro}},,
b_{\text{acc}}
\right)
其中：
- (T_t)：设备位姿
- (v_t)：线速度
- (b_{\text{gyro}})：陀螺仪偏置
- (b_{\text{acc}})：加速度计偏置
所以 (SE(3)) 不是 state estimation 算法本身，而是 state 中位姿部分的正确空间。
优化时不能简单写：
Tnew=T+ΔTT_{\text{new}}=T+\Delta T
因为两个合法旋转矩阵相加，通常不再是合法旋转矩阵。
正确更新是：
[
T_{\text{new}}
T\exp([\delta\xi])
]
其中：
δξ∈R6\delta\xi\in\mathbb R^6
这样更新后仍然满足：
Tnew∈SE(3)T_{\text{new}}\in SE(3)
这就是 Lie group 在 estimation 中真正重要的原因。
5. 如果没有 (SO(3))、(SE(3))，能不能做？
理论上能。
你可以使用：
- Euler angles
- quaternion
- 手写三角函数
- 每种机械臂单独推导公式
- 每对坐标系单独写转换逻辑
但这些其实都是在用不同方式表示同一个 (SO(3)/SE(3)) 几何结构。
例如 quaternion 是旋转的另一种参数化，但它表示的对象仍然是：
R∈SO(3)R\in SO(3)
Euler angles 也一样。它不是 (SO(3)) 的替代物，而是 (SO(3)) 的一种坐标表示。
如果完全不认识背后的结构，很容易遇到：
- 旋转顺序混乱
- Euler angle 奇异性
- quaternion 正负二义性
- 直接相减旋转矩阵
- 坐标系方向写反
- 插值得到非合法旋转
- 优化后矩阵不再正交
- 不同机器人和传感器代码无法复用
所以不是“没有 (SO(3)) 就完全算不了”，而是：
你会为每个问题重复发明一套局部方法，并且缺少保证这些运算始终符合刚体运动几何的统一框架。
应用例子:
SO(3)
它们最重要的地方在于“组合”
比如：
Tbase←cameraTcamera←objectT_{\text{base}\leftarrow\text{camera}}
T_{\text{camera}\leftarrow\text{object}}
可以把两段空间关系连续组合起来。而：
Tcamera←base=Tbase←camera−1T_{\text{camera}\leftarrow\text{base}}
=
T_{\text{base}\leftarrow\text{camera}}^{-1}
可以反过来描述关系。
这让机器人能够统一处理：
- 基座到末端执行器；
- 末端到腕部相机；
- 相机到物体；
- 物体到目标抓取姿态；
- 一台机械臂到另一台机械臂。
即使你只向 SO-ARM 输入六个舵机角度，内部的正运动学实际上仍然在做：
T(q)=T1(q1)T2(q2)⋯T6(q6)T(q)=T_1(q_1)T_2(q_2)\cdots T_6(q_6)其中每个 TiT_i 都是一个 SE(3)SE(3) 变换。
所以可以把它理解为：
关节角是机器人的内部语言，SE(3)SE(3) 位姿是机器人与三维世界交流的语言。
SO(3)/SE(3)SO(3)/SE(3) 不仅用于控制，也贯穿相机标定、点云融合、SLAM、状态估计、轨迹规划和模仿学习。它确实是现代机器人学最基础的一套数学工具。
假设我们在拍摄so arm 夹螺丝钉, 放到一个盒子里
那我们获得了: 相机视角的相片
下次, 怎么机器怎么来做这件事呢
通过相片 得到..
转换到机器臂底座坐标系上,
然后?
图像→识别物体→恢复3D位置→转换到基座坐标系→规划夹爪位姿→求关节角→电机执行\boxed{
\text{图像}
\rightarrow
\text{识别物体}
\rightarrow
\text{恢复3D位置}
\rightarrow
\text{转换到基座坐标系}
\rightarrow
\text{规划夹爪位姿}
\rightarrow
\text{求关节角}
\rightarrow
\text{电机执行}
}
- 相机看到螺丝和盒子
相机得到：
- RGB 图像；
- 如果是 RealSense，还可以得到深度图。
视觉算法首先确定：
- 螺丝在哪些像素；
- 盒子在哪些像素；
- 螺丝的大致方向；
- 哪一部分适合夹取。
例如检测到螺丝中心：
(u,v)=(420,260),Z=0.55 m(u,v)=(420,260),\qquad Z=0.55\text{ m}
2. 从像素恢复相机坐标
利用深度和相机内参：
Pcamera=ZK−1uv1P_{\text{camera}}
=
ZK^{-1}
\begin{bmatrix}
u\\v\\1
\end{bmatrix}
得到：
Pcamera=0.080.030.55P_{\text{camera}}
=
\begin{bmatrix}
0.08\\
0.03\\
0.55
\end{bmatrix}
意思是螺丝相对于相机的位置。
3. 转换到机械臂基座坐标系
利用事先标定好的相机外参：
Pbase=Tbase←cameraPcameraP_{\text{base}}
=
T_{\text{base}\leftarrow\text{camera}}
P_{\text{camera}}
例如得到：
Pbase=0.32−0.100.04P_{\text{base}}
=
\begin{bmatrix}
0.32\\
-0.10\\
0.04
\end{bmatrix}
现在机器人知道：螺丝相对于自己底座在哪里。
但这仍然不够，因为这只是一个点。机器人还要设计完整的夹爪抓取位姿。
4. 生成抓取位姿
机器人需要决定：
- 夹爪中心放在哪里；
- 夹爪开口朝哪个方向；
- 从上方还是侧面接近；
- 下降到什么高度；
- 夹爪什么时候闭合。
因此需要构造：
Tbase←grasp∈SE(3)T_{\text{base}\leftarrow\text{grasp}}\in SE(3)一般会规划几个连续目标：
- 螺丝上方的预抓取位姿；
- 下降到螺丝处；
- 闭合夹爪；
- 垂直抬起；
- 移动到盒子上方；
- 下降；
- 松开夹爪；
- 离开盒子。
5. 逆运动学得到关节角
抓取位姿描述的是“夹爪应该在哪里”，但舵机只能接收关节角，所以需要逆运动学：
q∗=IK(Tbase←grasp)q^*=IK(T_{\text{base}\leftarrow\text{grasp}})得到类似：
q∗=(q1,q2,q3,q4,q5,q6)q^*=
(q_1,q_2,q_3,q_4,q_5,q_6)
然后控制器把这些目标角度发送给 SO-ARM 的各个舵机。
如果使用 LeRobot / π₀ 这样的模仿学习
过程会有所不同。
你进行遥操作示范时，不能只保存相片，还要同步保存：
D={(imaget, qt, at)}t=1TD=
\left\{
(\text{image}_t,\ q_t,\ a_t)
\right\}_{t=1}^{T}
其中：
- imaget\text{image}_t：时刻 tt 的相机图像；
- qtq_t：机器人当前关节角；
- ata_t：人类在该时刻让机器人执行的动作；
- 还可以保存深度、夹爪状态等信息。
模型学习的是：
π(imaget,qt,instruction)→at\pi(\text{image}_t,q_t,\text{instruction})
\rightarrow a_t
例如：
π(当前图像,当前关节状态,“把螺丝放进盒子”)→下一段关节动作\pi(
\text{当前图像},
\text{当前关节状态},
\text{“把螺丝放进盒子”}
)
\rightarrow
\text{下一段关节动作}
执行时，它不是只看一次图片然后完整执行，而是不断循环：
拍摄→预测动作→执行一小段→重新拍摄→⋯\text{拍摄}
\rightarrow
\text{预测动作}
\rightarrow
\text{执行一小段}
\rightarrow
\text{重新拍摄}
\rightarrow\cdots
这叫闭环控制。如果夹取时螺丝发生移动，下一帧图像可以帮助策略调整。
你可以这样总结动机
三维刚体位姿不是普通向量，而是带有非交换组合规则的几何对象。(SO(3)) 和 (SE(3)) 为旋转和刚体位姿提供统一表示；群结构负责坐标变换的组合与求逆，Lie algebra 负责速度、小扰动、求导和优化。它使机械臂、相机、IMU、眼动、手部追踪和 SLAM 能用同一种空间语言交互，并使任务表示较少依赖某台机器人的具体关节结构。
最浓缩就是：
[
\boxed{
SO(3),SE(3)
\text{三维姿态和刚体运动的统一几何语言}
}
]
[
\boxed{
\text{Lie group}
\text{有限运动的组合}
+
\text{连续运动的微积分}
}
]
这就是它们被放在《Modern Robotics》第3章最前面的根本原因。