上篇文章学习总结：

1. Base Position 是机器人主体在某个世界/参考坐标系中的位置。

2. Base Velocity 积分可以得到 Base Position，但误差也会被一起积累。

3. Odometry 更关注“从起点到现在移动了多少”，本质上主要是相对运动估计。

4. Leg Odometry 可以利用编码器、腿部运动学和支撑脚约束来估计 Base 的运动。

5. 即使脚不明显打滑，模型误差、关节误差、接触误差等仍然会让位置逐渐漂移。

6. Localization 解决“我在地图/世界哪里”，通常需要视觉、LiDAR、GNSS、动捕等外部环境参考来长期纠偏。

最终要形成：

IMU + Encoder + Contact
          ↓
    Local Odometry
          ↓
   短期连续、但会漂
          +
Vision / LiDAR / GNSS
          ↓
     Global Correction
          ↓
    更完整的状态估计

这节课的学习思路：

Roll / Pitch 为什么能靠重力纠正
↓
Yaw 为什么重力帮不上忙
↓
陀螺仪积分为什么让 Yaw 越来越漂
↓
磁力计能不能救？
↓
为什么机器人上经常不敢太信磁力计
↓
视觉 / LiDAR 怎么帮助修正 Yaw
↓
Yaw 错一点为什么走远以后位置会偏很多
↓
这和 VIO / SLAM / Odometry 有什么关系

这一课讲完，你对 IMU姿态估计 → Odometry → Localization 这条线就会真正连起来。

Base Position
↓
Odometry 里程计
↓
为什么机器人走着走着地图位置会漂
↓
为什么脚没打滑也可能累计误差
↓
IMU + Leg Odometry 能不能得到绝对位置
↓
为什么 GPS / VIO / LiDAR 会进入机器人状态估计
↓
localization 和 state estimation 到底什么关系

link、joint、frame 

这三个词以后你每天都会见。

如果这三个没搞清楚，URDF、Pinocchio、TF、MuJoCo都会看得非常乱。

今天先不写公式。

---

1. 先把机器人想象成“骨头 + 关节”

比如你的胳膊：

身体
 │
肩膀
 │
上臂
 │
肘
 │
前臂
 │
手

机器人里可以理解成：

link
 │
joint
 │
link
 │
joint
 │
link

因此最基本的规律是：

Link 是刚体。Joint 负责连接两个 Link，并规定它们怎么相对运动。

---

2. Link 是什么？

Link 可以先理解成：

一块不会自己变形的刚体零件。

例如人形机器人：

pelvis
torso
left_thigh
left_shank
left_foot

right_upper_arm
right_forearm
right_hand

都可以是 Link。比如：

UpperArm Link

它本身的：

长度
质量
质心
惯量
外形

通常是固定的。

它不会突然：

30 cm → 40 cm

所以运动学里，我们把它当成刚体。

---

3. Joint 是什么？

Joint 是：

两个 Link 之间允许发生相对运动的连接。

比如：

upper_arm
    │
   elbow_joint
    │
forearm

肘关节允许：

前臂相对上臂旋转

因此：

Link = 东西本身

Joint = 东西之间怎么动

这个区别一定要记住。

Pose = 位置 + 姿态

---

4. Link、Joint、Frame 三者是什么关系？

来看一条机器人手臂：

          torso_link
              │
       shoulder_joint
              │
      upper_arm_link
              │
        elbow_joint
              │
       forearm_link
              │
         hand_link

但我们还可以在这些 Link 上定义 Frame：

          torso_link
          [torso_frame]
               │
        shoulder_joint
               │
       upper_arm_link
               │
         elbow_joint
               │
        forearm_link
               │
          hand_link
        [hand_frame]

所以：

Link
= 实际刚体

Joint
= 刚体之间的连接关系

Frame
= 我们放在某个位置用来描述空间关系的坐标系

---

5. URDF

以后你看到 URDF：

<link name="upper_arm"/>

<joint name="elbow_joint">
    ...
</joint>

<link name="forearm"/>

脑子里不要想着：

XML配置文件，好复杂。

直接翻译：

upper_arm
一块骨头

elbow_joint
肘关节

forearm
另一块骨头

URDF 本质上就是在告诉计算机：

机器人有哪些刚体
        ↓
这些刚体怎么连接
        ↓
关节能怎么运动
        ↓
尺寸是多少
        ↓
质量是多少
        ↓
惯量是多少

Pinocchio 再读取这个模型。

于是：

URDF
 ↓
Pinocchio Model
 ↓
输入 q
 ↓
FK
 ↓
得到所有 Frame 的 Pose

1. Link = 刚体

2. Joint = 两个刚体之间如何运动

3. Frame = 用来描述位置和方向的一套坐标轴

4. Position = 只有位置

5. Pose = 位置 + 姿态

然后再加一句非常关键的：

所有位置和姿态，都必须问一句：相对于哪个 Frame？

---

Translation 

Translation 就是：

两个坐标系的原点相差多少。

例如：

World O
       \
        \
         Robot O

Robot Base 相对于 World：

x = 1 m
y = 2 m
z = 0 m

那么这个：

(1,2,0)

就是一部分平移关系。

以后你会经常看到：

translation

或者：

Eigen::Vector3d translation;

本质就是：

dx
dy
dz

---

Rotation

假设有两个坐标系：

World：

       y
       ↑
       |
       O────→ x

机器人转了90°：

Robot：

       x
       ↑
       |
       O────→ ?

机器人自己的：

x轴

已经不再和 World 的 x 轴平行。

这时候一个点：

Robot frame中：
(1,0)

是什么意思？

意思是：

在机器人自己的 x 轴方向前方1米。

但由于机器人已经转了90°，

从 World 看：

它可能是：
(0,1)

也就是说：

Robot里：
(1,0)

World里：
(0,1)

同一个点，数字不同，原因就是：坐标轴旋转了

假设：

肩膀
  ↓
上臂
  ↓
肘
  ↓
前臂
  ↓
手

肩膀转：

30°

肘相对于肩膀又转：

60°

那么手的位置怎么算？

不能简单写：

上臂长度 + 前臂长度

因为前臂已经不是沿原来的方向伸出去。

而是：

        Shoulder
           O
          /
         /
        O Elbow
         \
          \
           O Hand

因此真正计算过程中会变成：

Base
 ↓
肩膀的平移 + 旋转
 ↓
UpperArm
 ↓
肘的平移 + 旋转
 ↓
Forearm
 ↓
Hand

每经过一个 Joint：

坐标系都可能跟着改变。

---

所以 FK 真正是在干什么？

上一课我们说：

FK = 已知关节角，求手脚在哪里。

现在可以再深入一级：

FK其实是在从机器人根节点开始，不断进行坐标变换。

比如：

World
  ↓
Base
  ↓
Torso
  ↓
Shoulder
  ↓
UpperArm
  ↓
Elbow
  ↓
Forearm
  ↓
Hand

程序实际上是在算：

World → Base

Base → Torso

Torso → Shoulder

Shoulder → UpperArm

UpperArm → Elbow

Elbow → Forearm

Forearm → Hand

最后把这些关系串起来：

World
   ↓
   ↓
   ↓
Hand

得到：

Hand相对于World的Pose

这就是 FK 的底层结构。

---

T_AB

以后运动学里你可能看到：

T_AB

或者：

^A T_B

不同教材符号可能不一样，现在不要死背，先建立一个意识：

这种东西通常是在描述“B坐标系相对于A坐标系是什么关系”。

比如：

World T Hand

可以理解为：

Hand相对于World的变换

它里面包含：

Hand在哪里
+
Hand朝哪里

也就是完整 Pose。

Noitom / Xsens
        ↓
Human Hand Pose
        ↓
Mocap坐标系
        ↓
坐标变换
        ↓
Robot Base坐标系
        ↓
缩放 / 身体比例映射
        ↓
Robot Target Hand Pose
        ↓
IK / QP
        ↓
q_target
        ↓
MuJoCo

这里：

Mocap frame
→
Robot frame

就是我们现在正在学的东西。

---

“动作镜像”

常见原因之一就是：

坐标系轴定义不同

甚至还有：

左手系 / 右手系

的问题。你现在只需要形成一个排错习惯：

当机器人方向明显不对时，不要第一反应就说：

IK坏了。

先检查：

目标数据到底在哪个Frame里？

一个空间点 / Pose
        ↓
在 Frame A 中描述
        ↓
坐标变换
        ↓
在 Frame B 中重新描述

而坐标变换本身：

Transform
   │
   ├── Translation
   │      平移
   │
   └── Rotation
          旋转

这就是今天最核心的东西。

---

旋转矩阵

旋转矩阵的本质一句话：

旋转矩阵负责把一个向量，从一个方向定义转换到另一个坐标系里。

比如：

Robot坐标系里的向量
        ↓
旋转矩阵 R
        ↓
World坐标系里的向量

可以写成：

p_world = R · p_robot

你现在不用害怕这个式子，它只是在说：

“把机器人眼里的坐标，翻译成世界眼里的坐标。”

---

二维旋转矩阵

如果机器人绕 z 轴旋转角度 θ：

R =
[ cosθ  -sinθ
  sinθ   cosθ ]

你现在不要背。

我们只拿：

θ = 90°

来看。

因为：

cos90° = 0
sin90° = 1

于是：

R =
[ 0  -1
  1   0 ]

现在机器人中的“前方1米”：

p_robot =
[1
 0]

乘起来：

p_world =
[0
 1]

也就是：

Robot：
前方1米

World：
y方向1米

和刚才直觉完全一致。

---

列

这是今天最值得你理解的地方，刚才：

R =
[ 0  -1
  1   0 ]

看第一列：

[0
 1]

它表示：

Robot 的 x 轴，在 World 坐标系里是什么方向。

结果是：

World：
(0,1)

说明 Robot x 朝 World y，再看第二列：

[-1
  0]

它表示：

Robot 的 y 轴，在 World 坐标系里是什么方向。

也就是 Robot y 朝 World 的负 x。

所以你以后看到旋转矩阵：

R =
[ |  |  |
  x  y  z
  |  |  | ]

可以理解为：

三列分别告诉你，新坐标系 x、y、z 三根轴，在旧坐标系里长什么样。

这是非常强的直觉。

现实机器人是三维的，一个 Frame 有：

x轴
y轴
z轴

所以旋转矩阵变成：

3 × 3

形式：

R =
[ r11 r12 r13
  r21 r22 r23
  r31 r32 r33 ]

你暂时不用理解9个数字各自怎么计算，先知道：

第一列
=
新坐标系x轴在旧坐标系中的方向

第二列
=
新坐标系y轴在旧坐标系中的方向

第三列
=
新坐标系z轴在旧坐标系中的方向

---

为什么旋转矩阵里每一列长度都是1？因为它们表示的是：

坐标轴方向

方向不关心长度。

例如 x 轴方向：

(1,0,0)

长度就是1。

旋转以后可能变成：

(0.707, 0.707, 0)

它的长度仍然是1。

所以旋转矩阵的每一列：

都是单位向量。

而且三根轴互相垂直。

所以：

x ⟂ y
y ⟂ z
x ⟂ z

这就是为什么旋转矩阵不是随便一个 3×3 矩阵。

---

放到机器人手臂上看，假设上臂长度：

0.3 m

在上臂自己的坐标系里，它可能沿 x 轴：

[0.3
 0
 0]

肩膀转了90°以后：

R ·
[0.3
 0
 0]

可能变成：

[0
 0.3
 0]

这就是说：

上臂长度没变，只是方向变了。

FK里干的事情，本质就是不断做这种变换。

---

一个关节接一个关节，假设：

肩膀旋转30°

再：

肘旋转60°

那手的方向不是只看其中一个。

而是：

肩膀旋转
   ↓
肘坐标系已经跟着变了
   ↓
再在肘坐标系里旋转

所以最终会出现：

R_total
=
R_shoulder · R_elbow

你现在不用管乘法顺序细节。

只要知道：

多个旋转可以通过矩阵乘法串起来。

这就是为什么机器人 FK 里面到处都是矩阵乘法。

---

旋转矩阵和欧拉角

你可能已经见过：

roll
pitch
yaw

也就是：

RPY

它们是一种“描述旋转的方法”，旋转矩阵也是一种，比如：

yaw = 90°

最终可以转换成一个：

3×3 Rotation Matrix

所以：

Euler Angle / RPY
      ↓
可以转换
      ↓
Rotation Matrix

但它们不是同一个东西。

---

四元数

你以后还会看到

quaternion

例如：

x
y
z
w

它也是描述旋转的方法，所以目前先建立：

旋转的表示方法

├─ Rotation Matrix
├─ Euler Angle / RPY
└─ Quaternion

它们都在描述：

一个坐标系相对于另一个坐标系转了多少。

只是表达方式不同。

---

齐次变换矩阵

因为现在我们已经有：

Rotation

但还缺：

Translation

下一步要把：

旋转
+
平移

合成一个统一的：

4×4 Transform Matrix

这一步之后，你就真正能开始看懂 FK 里面最常见的 T 矩阵了。

p_world = R · p_base + t

你可以直接翻译：

机器人眼里的点
      ↓
先根据机器人朝向旋转
      ↓
再加上机器人在世界中的位置
      ↓
得到世界眼里的点

所以：

R
=
方向关系

t
=
位置关系

两个加起来就能完整描述两个 Frame 的关系。

---

为什么还需要“齐次变换矩阵”？既然已经有：

R · p + t

为什么还要搞一个 4×4 矩阵？

因为我们希望：

旋转和平移都能统一写成一次矩阵乘法。

这样以后多个关节连起来就特别方便。

于是我们把：

R

和：

t

塞进一个矩阵里。

得到：

T =
[ R  t
  0  1 ]

展开就是：

T =
[ r11 r12 r13 tx
  r21 r22 r23 ty
  r31 r32 r33 tz
   0   0   0   1 ]

这就是：4×4 齐次变换矩阵，真实机器人一般同时有旋转和平移，比如肩关节坐标系相对于 torso：

位置：
右边 0.2m
上方 0.3m

方向：
绕某轴旋转

那么完整关系就由：

T_torso_shoulder

描述。

里面同时存着：

shoulder在哪里
+
shoulder坐标轴朝哪里

所以一个 T 实际上就是：

一个完整 Pose。

---

Pose 和 Transform 的关系

之前说：

Pose
=
Position + Orientation

而齐次变换矩阵：

T

也包含：

Translation + Rotation

所以在很多机器人库里：

Pose

和：

Transform

表达的是非常接近的空间关系，只是数据结构和语义可能略有差异。

例如你以后在 ROS2 里会见到：

geometry_msgs/Pose

也会见到：

geometry_msgs/Transform

Pinocchio里则可能看到：

SE3

本质都在处理：

三维位置 + 三维姿态。

假设一个两关节机械臂：

Base
 ↓
Joint1
 ↓
Link1
 ↓
Joint2
 ↓
Link2
 ↓
Hand

每一段都可以有一个变换矩阵：

T_base_1

T_1_2

T_2_hand

那么手相对于 Base：

T_base_hand
=
T_base_1
·
T_1_2
·
T_2_hand

这就是你以后天天会见到的：

变换矩阵连乘。

人形机器人可以看成：

pelvis
├── torso
│   ├── left_arm
│   └── right_arm
│
├── left_leg
│   └── left_foot
│
└── right_leg
    └── right_foot

如果你想算：

pelvis → right_hand

就沿右臂这条链：

pelvis
↓
torso
↓
shoulder
↓
upper_arm
↓
elbow
↓
forearm
↓
hand

把这些 T 一路乘下去。

如果你想算：

pelvis → left_foot

就走左腿链。

所以：

FK本质就是在机器人树上向下传播变换。

T_WB

表示：

Base 相对于 World 的变换。

T_HB

表示：

Hand 相对于 Base 的变换。

那么：

T_WH
=
T_WB · T_BH

就是：

Hand 相对于 World。

你可以把它看成：

World → Base → Hand

很自然。

---

TF 

你以后学 ROS2 时，TF/TF2其实就是在回答：

“任意两个坐标系之间，现在是什么关系？”

例如：

right_hand

相对于：

base_link

在哪里？

或者：

left_foot

相对于：

odom

在哪里？

TF会利用整棵树：

odom
 ↓
base_link
 ↓
pelvis
 ↓
...
 ↓
left_foot

把中间变换串起来。本质还是今天这件事。

---

Pinocchio里的 SE3 

以后你会看到：

pinocchio::SE3

你可以把它理解成：

一个三维刚体变换。

也就是：

Rotation
+
Translation

所以本质还是：

T

只是 Pinocchio 不一定真的每次都用一个裸 4×4 矩阵来存。

---

SE(3) 和 SO(3) 

SO(3)
=
纯三维旋转

SE(3)
=
三维旋转 + 三维平移

所以：

Rotation Matrix
属于 SO(3)

而：

Rigid Transform
属于 SE(3)

q
↓
每个Joint产生一个局部变换
↓
沿运动链不断相乘
↓
得到末端T

也就是：

Joint Angle
↓
Rotation / Translation
↓
Transform
↓
Transform
↓
Transform
↓
End-Effector Pose

这已经非常接近真正的机器人运动学实现了。