接触估计 Contact Estimation

这一课在人形机器 人里非常关键，因为从这里开始，你会发现：

光知道身体姿态还不够，机器人还必须知道自己现在“靠什么支撑”。

机器人并没有“脚底感觉”，机器人必须自己判断：

我哪只脚现在真的接触地面？

传感器
   ↓
┌─────────────────┐
│   状态估计       │
│                 │
│ 身体姿态         │
│ 身体速度         │
│ 身体位置         │
│ 关节状态         │
│ 接触状态  ← 新的 │
└────────┬────────┘
         ↓
       MPC/WBC

所以：

“脚有没有踩地”本身就是机器人状态的一部分。

---

机器人怎么判断脚踩没踩地？

足底力传感器

比如脚底测到：

Fz = 400 N

很明显：

这只脚正在承受机器人重量。

于是：

contact = true

---

滞回 Hysteresis

不用一个阈值：

30 N

而是使用两个阈值：

接触阈值：40 N

离地阈值：20 N

于是：

从离地变成接触

必须：

Fz > 40 N

才宣布：

踩地了。

已经踩地以后

只有：

Fz < 20 N

才宣布：

离地了。

中间：

20 ~ 40 N

怎么办？

保持之前状态。

---

这样刚才：

35
28
40
32

就不会疯狂：

0 1 0 1

来回切。

这是工程控制中特别常见的思想：

不要因为传感器一点点抖动，就不断改变系统状态。

---

但是如果没有脚底力传感器怎么办？有些机器人可以结合：

关节编码器
+
IMU
+
电机力矩
+
运动学

去推测。例如机器人认为：

脚正在往下运动

突然发现：

脚的位置不再继续下降

同时：

关节力矩发生变化

那么：

很可能脚碰地了。

所以接触状态不一定必须靠一个传感器，也可以：多信息融合

现在把编码器、IMU、接触一起放进去

你之前学的东西终于可以合起来了：

               IMU
          ┌─────┴─────┐
          ↓           ↓
      角速度        加速度
          │           │
          └─────┬─────┘
                ↓
           身体运动预测
                │
                │
编码器 ─────────┼────→ 腿部运动学
                │
                ↓
            状态估计器
                ↑
                │
          脚接触状态
                │
     足底力 / 力矩 / 运动学
                │
                ↓
        ┌──────────────┐
        │ Base姿态      │
        │ Base速度      │
        │ Base位置      │
        │ 关节状态      │
        │ 接触状态      │
        └───────┬──────┘
                ↓
             WBC/MPC

这已经非常接近真正人形机器人的系统结构了。

---

Base

以后你会经常看到：

base
base_pos
base_vel
base_quat
base_omega

这里的 base 可以粗略理解：

机器人身体主体的参考坐标系。

在人形机器人里通常会选择类似：

pelvis
torso
base link

作为主体，于是状态估计很重要的任务就是估：

Base position
Base orientation
Base linear velocity
Base angular velocity

翻译：

身体在哪里
身体朝哪
身体移动多快
身体旋转多快

---

“接触”和“无滑动接触”

必须区分：

Contact

和：

Stable Contact

也就是：

接触地面

不等于：

牢牢固定在地面

所以高级一点的状态估计还要判断：

有没有打滑？

例如：

脚底力：
说我踩地

但是运动学估出来：

脚相对于世界似乎在横向快速移动

同时 IMU 也发现：

身体运动和预期对不上

那么：

这只脚可能正在滑。

这时候状态估计器应该：

降低这只脚的信息可信度

而不是死信它。你会发现：

状态估计真正核心不是“算一个公式”，而是不断判断“现在该相信谁”。

刚开始可能觉得状态估计就是：

IMU滤一滤

其实远远不是。

人形机器人真正需要的状态可能包括：

关节位置 q
关节速度 dq

身体姿态 R / quaternion
身体角速度 ω

身体位置 p
身体线速度 v

左脚是否接触
右脚是否接触

脚是否打滑

甚至还可能包括：

IMU bias
外力
地面状态

所以：

State Estimation

真正可以理解成：

把一堆不完美的传感器信息拼起来，尽可能还原机器人此时此刻真实的物理状态。

                     真实机器人
                         │
       ┌─────────────────┼─────────────────┐
       ↓                 ↓                 ↓
     编码器              IMU            足部信息
       │                 │                 │
       └─────────────────┼─────────────────┘
                         ↓
                  State Estimator
                         │
          ┌──────────────┼───────────────┐
          ↓              ↓               ↓
        q、dq        Base状态        Contact状态
          │              │               │
          └──────────────┼───────────────┘
                         ↓
                    MPC / WBC
                         ↓
                    控制命令
                         ↓
                      电机
                         ↓
                     机器人

这一张图，你后面会反复用。

---

Base Velocity Estimation

编码器
+
IMU
+
支撑脚接触
+
机器人运动学
        ↓
Base Velocity

也就是：

机器人怎么知道自己的身体正在往哪个方向移动、移动多快。

编码器能测：

髋关节角
膝关节角
踝关节角
肩关节角
……

还能估：

各个关节转得多快

也就是：

q
dq

但是编码器不知道：

整个机器人相对于房间到底有没有移动。

关节运动 ≠ 整个机器人在世界中的运动。

IMU主要给你：

陀螺仪
→ 角速度

加速度计
→ 比力 / 与重力相关的加速度测量

它没有一个传感器直接说：

“现在机器人向前 0.43 m/s”

没有。所以 Base Velocity 必须：估出来，这就是为什么它属于：

State Estimation

---

加速度积分

我们先假装世界很完美，如果机器人向前加速：

a = 1 m/s²

那么1秒后大概：

v = 1 m/s

所以理论上：

加速度
  ↓ 积分
速度

---

重力

加速度计受到姿态影响特别大，因为我们真正想知道：

机器人平移加速度

但传感器读数里会混进：

重力方向

所以需要先知道：

机器人当前姿态

再把重力分量处理掉。链路大概是：

Accelerometer
      ↓
原始测量
      ↓
利用姿态估计
      ↓
处理重力影响
      ↓
得到线性加速度估计
      ↓
积分
      ↓
速度

---

Leg Odometry 腿式里程计

它的核心思想之一就是：

利用腿部编码器 + 运动学 + 支撑脚约束，反推出身体的运动。

大致：

编码器 q、dq
     ↓
腿部运动学
     ↓
算脚相对 Base 的运动
     ↓
知道支撑脚世界速度 ≈ 0
     ↓
反推出 Base velocity

             IMU
          /       \
         /         \
    加速度         角速度
       │             │
       │             │
       └──────┬──────┘
              │
              ▼
           Base预测
              ▲
              │
        编码器 q、dq
              │
              ▼
          腿部运动学
              │
              ▼
          支撑脚约束
              │
              ▼
       Base速度修正

这才是真正的 Sensor Fusion 传感器融合

---

IMU的优点

IMU更新很快。对：

突然运动
身体快速转动
短时间动态变化

非常敏感，所以：

短时间变化通常很好。

---

IMU的问题

主要是：

bias
噪声
重力处理误差
积分漂移

所以：

时间长了容易漂。

---

腿部运动学的优点

如果：

支撑脚真的牢牢踩地

那么：

脚速度≈0

是一个很强的物理约束，可以帮助纠正：

IMU速度漂移

---

腿部运动学的问题

假如：

脚打滑

那：

脚速度≈0

这条假设就错了，于是 Leg Odometry 也会骗人。

---

具体的走路过程理解

假设机器人正在向前走，当前：

左脚 = 支撑
右脚 = 摆动

状态估计开始工作。

---

第一步：IMU

IMU告诉我们：

身体角速度
身体加速度

比如：

身体正在稍微向前加速

---

第二步：编码器

编码器告诉我们：

左腿髋关节速度
左膝速度
左踝速度

也就是：

dq_left

---

第三步：运动学

根据：

q_left
+
dq_left

计算：

左脚相对于Base怎么运动

比如：

向后0.45 m/s

---

第四步：接触估计

系统判断：

左脚 = 稳定接触

因此：

左脚世界速度 ≈ 0

---

第五步：反推

于是：

Base大约向前0.45m/s

---

第六步：和IMU融合

IMU可能预测：

0.48 m/s

腿运动学估计：

0.45 m/s

状态估计器综合后可能输出：

0.46 m/s

注意，这里的数字只是为了理解。

真正系统不会简单地：

(0.48+0.45)/2

完事，而会根据：

传感器噪声
可信度
接触状态
历史状态
模型

做融合。

---

EKF 和 KF

因为机器人运动通常是：

旋转
三维姿态
非线性运动学

不是简单的：

直线匀速小车

很多关系不是简单线性的。

---

“浮动基座机器人” Floating-base Robot

浮动基座机器人。

为什么？

机械臂通常：

底座
████████████
固定在桌子上

所以 Base：

位置固定

你根本不用估：

base velocity

---

但人形机器人：

      O
     /|\
      |
     / \

整个身体可以：

前后移动
左右移动
上下移动
翻滚
俯仰
转向

所以 Base 并没有固定在世界上。

这就叫：

Floating Base。

因此状态里除了关节：

q_joint

还必须考虑：

Base position
Base orientation
Base velocity
Base angular velocity

---

状态估计的输出

你以后看人形机器人代码，可能会看到类似：

state.base_position
state.base_velocity
state.base_orientation
state.base_angular_velocity

state.joint_position
state.joint_velocity

state.left_contact
state.right_contact

也可能名称完全不同。

但意思差不多：

身体位置
身体速度
身体姿态
身体角速度

关节位置
关节速度

左右脚接触状态

然后：

State Estimator
      ↓
MPC
      ↓
WBC

编码器
↓
q、dq
↓
知道机器人关节怎么动

陀螺仪
↓
身体角速度
↓
帮助估计姿态变化

加速度计
↓
身体受到的惯性/重力相关信息
↓
帮助预测运动

姿态估计
↓
知道身体朝哪
↓
正确处理重力方向

Contact Estimation
↓
知道哪只脚踩地

Leg Kinematics
↓
利用支撑脚约束
↓
估计Base Velocity

Kalman / EKF
↓
融合所有信息

最终：

State Estimate
│
├─ q
├─ dq
├─ base position
├─ base velocity
├─ base orientation
├─ base angular velocity
└─ contact state

再交给：

MPC / WBC

这已经非常接近真正的人形机器人运控架构了。

                    ┌──────────────┐
                    │   Encoder    │
                    │    q / dq    │
                    └──────┬───────┘
                           │
                           ↓
                      Leg Kinematics
                           │
                           │
┌──────────────┐           │
│     IMU      │           │
│ gyro / acc   │           │
└──────┬───────┘           │
       │                   │
       ↓                   ↓
 Orientation          Foot Velocity
 Estimation                 │
       │                    │
       │             ┌──────┴──────┐
       │             │   Contact   │
       │             │ Estimation  │
       │             └──────┬──────┘
       │                    │
       └─────────┬──────────┘
                 ↓
        ┌──────────────────┐
        │  State Estimator │
        │    EKF / etc.    │
        └────────┬─────────┘
                 ↓
        ┌──────────────────┐
        │ Base position    │
        │ Base velocity    │
        │ Base orientation │
        │ Base omega       │
        │ q / dq           │
        │ Contact          │
        └────────┬─────────┘
                 ↓
             MPC / WBC
                 ↓
             Robot

加速度积分能得到速度，但：

积分会放大 bias、噪声和姿态误差，长期会漂。

利用：

编码器
+
腿部运动学
+
支撑脚约束

可以反推出：

Base velocity

这就是腿式里程计的重要思想。

---

IMU负责：

短时间快速预测。

支撑脚/腿部运动学负责：

提供物理约束、纠正漂移。

滤波器负责：

把它们融合起来。

IMU
     ↘
编码器 → State Estimator → Base State → MPC/WBC
     ↗
接触

而不是：

某一个传感器
↓
直接得到机器人真实状态

机器人永远没有这么幸运。

Odometry —— 里程计

最直觉的理解：

速度
↓
不断累积
↓
位置

比如机器人向前以：

1 m/s

走1秒。那大概前进：

1 m

再走1秒：

2 m

所以：

当前位置
=
上一时刻的位置
+
这一小段时间走过的距离

程序思维大概就是：

position = position + velocity * dt;

例如控制周期：

dt = 0.01 s

当前速度：

vx = 0.5 m/s

这一帧就大概前进：

0.5 × 0.01
=
0.005 m

也就是5毫米。然后下一帧继续加。

机器人领域里的 odometry ：

根据自身传感器，估计“我相对于刚开始的位置，移动了多少”。

它通常告诉你的是相对运动。

而不是：

“我现在在东京某实验室的精确经纬度和绝对坐标。”

---

Leg Odometry

上一课我们讲过：

编码器 q、dq
+
支撑脚
+
腿部运动学
       ↓
Base Velocity

现在只要继续：

Base Velocity
       ↓
积分
       ↓
Base Position

于是：

Encoder
   ↓
Leg Kinematics
   ↓
支撑脚约束
   ↓
Base Velocity
   ↓
时间积分
   ↓
Base Position

这就是腿式机器人 odometry 的基本思路之一。甚至可以不用先算速度，直接利用支撑脚位置

还有另外一种直觉。假设机器人左脚落地以后，我们说：

左脚在世界的位置暂时固定

例如：

left_foot_world =
(1.0, 0.1, 0)

编码器告诉我们：

Base相对于左脚在哪里

那么反过来：

已知左脚世界位置
+
腿部正运动学
↓
推算Base世界位置

例如：

地面
────────────● 左脚
             \
              \
             [Base]

脚作为锚点，这时候机器人身体怎么移动，可以通过腿的几何关系推出来，但机器人走一步之后，锚点变了，比如：

阶段1
左脚支撑
右脚向前迈

此时：

左脚 = 世界锚点

然后右脚落地：

阶段2
左右脚都接触

接下来左脚抬起来：

阶段3
右脚支撑

现在：

右脚 = 新的世界锚点

于是整个过程中：

左脚锚定
   ↓
右脚落地
   ↓
锚点切换
   ↓
右脚锚定
   ↓
左脚落地
   ↓
再次切换

这就是腿式机器人走路时很典型的过程。

---

“外部纠偏”

假设机器人 odometry 认为：

我在 x = 10.4 m

但另一个外部传感器告诉它：

实际上你应该在 x = 9.9 m

那状态估计器就可以发现：

我漂了0.5 m

然后进行修正。

所以完整思路逐渐变成：

IMU
+
Encoder
+
Leg Odometry
       ↓
高频运动估计
       ↓
但是会漂
       ↓
外部定位信息
       ↓
修正漂移

---

谁可以提供外部位置参考？

• GPS / GNSS：室外可以直接给全球位置参考，但室内通常不好用。

• VIO：Visual-Inertial Odometry，视觉+IMU，根据摄像头看到的环境特征估运动。

• LiDAR Odometry / SLAM：用激光雷达扫描环境，根据前后扫描的匹配估计机器人移动。

• Motion Capture：实验室里的光学动作捕捉系统可以非常准确地给机器人世界位置。

• UWB / Beacon：通过外部基站提供位置约束。

这些东西看起来五花八门，其实共同目的就是：

给机器人一个来自“外部世界”的参考，不让自己的内部 odometry 无限漂下去。

---

Localization

Odometry：我从刚才到现在动了多少。

Localization：我现在到底在地图的哪里。

例如：

Odometry：
我向前走了5米。

而：

Localization：
我现在位于实验室地图坐标
x = 12.3
y = 4.7

所以可以粗略理解：

Odometry
↓
关注相对运动

Localization
↓
关注世界/地图中的位置

当然实际工程里两者会深度融合，不一定严格分开。

---

State Estimation 和 Localization

State Estimation 更大。

它问：

机器人现在整个状态是什么？

可能包括：

Base Position
Base Velocity
Base Orientation
Base Angular Velocity
q
dq
Contact State
IMU Bias
...

而 Localization 主要关心：

我在哪里
+
我朝哪个方向

所以你可以把关系暂时理解成：

               State Estimation
                     │
        ┌────────────┼────────────┐
        ↓            ↓            ↓
   Orientation    Velocity    Position
                                │
                                ↓
                          Localization

不是严格的数学分类，但非常适合你现在建立框架。

odom 

机器人根据自己的传感器“推算出来的局部运动坐标系”。

它不是某个传感器，也不是某个关节，而通常是一个坐标系 / 里程计参考系，在 ROS / ROS2 里，你以后经常会看到：

map
 ↓
odom
 ↓
base_link

这里分别可以先这样理解：

• map：全局世界坐标系，强调“我在整个地图哪里”

• odom：局部连续坐标系，强调“我从启动以后走了多少”

• base_link：机器人身体自身的坐标系

例如机器人启动时：

odom 原点
   O
   |
   |
 Robot

假设机器人向前走了 1 m。

那么可能有：

base_link 相对 odom：

x = 1.0 m
y = 0
yaw = 0

也就是说：

机器人根据编码器、IMU等信息估计：
“我相对于刚开始的位置，往前走了1米。”

这就是 odometry，中文叫：里程计 / 里程估计，所以常见 TF 树：

map
  │
  └── odom
        │
        └── base_link
              │
              ├── torso
              ├── left_foot
              ├── right_foot
              └── ...

---

对人形机器人来说怎么理解？

例如你的机器人刚启动：

odom原点
   ↓
机器人骨盆附近

机器人往前走：

odom
 O────────────────→
                    Robot

你可以问：

机器人 base_link
相对于 odom
走了多远？

比如：

x = 2.3 m
y = 0.1 m
yaw = 5°

这表示：

从里程计参考系看，机器人已经向前移动约2.3m，横向偏了0.1m，朝向转了5°。

---

odom 和我们刚才讲的 World 有什么区别？

这个地方很关键。

我们前面为了讲运动学，经常用：

World

泛指“外部固定参考系”。

但在真正 ROS 系统里，可能会进一步细分成：

map
odom
base_link

所以你可以暂时理解：

World

是一个泛称。而：

odom

是实际工程中一种具体的世界参考系——由里程计积分得到的局部世界坐标系。

---

odom 和 base_link 一定不要混

base_link：

跟着机器人身体一起走。

odom：

通常固定在机器人启动附近，不跟着机器人移动。

例如机器人往前走：

odom
 O
 |
 |-----------> base_link

机器人继续走：

odom
 O
 |
 |---------------------> base_link

所以：

odom → base_link

这个 Transform 一直在变化。

---

一句话记住

odom
=
机器人通过编码器、IMU等状态估计得到的
“从启动位置开始，我现在移动到哪里了”
的参考坐标系

而它最大的特点：

连续、平滑，但会累计漂移。

你现在顺便把这三个记住就够了：

map       = 全局定位参考系
odom      = 局部里程计参考系
base_link = 机器人本体参考系

等我们后面讲 ROS2 TF 时，我会专门把 map → odom → base_link → pelvis/foot/hand 这整棵树给你彻底讲通。

---

控制状态和导航状态

这是一个非常工程化的概念，导航系统关心：

我在整个地图里准确在哪

控制系统更在意：

我最近这10毫秒到底怎么动了

控制需要：

高频
低延迟
连续
平滑

而全局定位可能：

频率比较低
偶尔修正
甚至发生跳变

所以真正机器人系统里，可能不会把某个全局定位结果：

原封不动

直接塞给底层控制器，而会进行融合和坐标系管理。

---

EKF

现在 EKF 又多了一层含义，状态可能是：

x =

Base position
Base velocity
Base orientation
IMU bias
...

IMU负责：

快速预测

腿部 odometry：

提供支撑约束

视觉或者 LiDAR：

提供环境参考

于是：

              IMU
               │
               ↓
             预测
               │
               ▼
        ┌─────────────┐
Encoder →│             │← Leg Odometry
         │ EKF / State │
Vision  →│  Estimator  │← LiDAR
         │             │
        └──────┬──────┘
               ↓
        Base State Estimate

你现在再看“多传感器融合”这句话，应该不再只是一个抽象词了。

---

状态估计 —— 可信度

IMU：

短期很可信
长期位置不可信

支撑脚：

稳定接触时很可信
打滑时不可信

视觉：

看到丰富纹理时可信
黑暗/快速运动时可能变差

LiDAR：

结构丰富环境很好
某些退化环境可能不好

于是状态估计器真正不断做的是：

根据当前情况决定“这一刻更应该相信谁”。

这其实就是你前面学 Kalman Filter 时那个思想的机器人版本：

预测有多可信？
测量有多可信？
↓
决定各相信多少

                 IMU
            ┌─────┴─────┐
            ↓           ↓
        角速度         加速度
            │           │
            └─────┬─────┘
                  ↓
             短期运动预测
                  │
                  │
Encoder q,dq ─────┤
      │           │
      ↓           │
Leg Kinematics    │
      │           │
      ↓           │
Contact / Foot    │
Constraint        │
      │           │
      └──────┬────┘
             ↓
      Local Odometry
             │
       ┌─────┴─────┐
       ↓           ↓
 Base Velocity  Base Position
       │           │
       └─────┬─────┘
             ↓
       State Estimator
             ↑
      外部定位信息
    Vision/LiDAR/GNSS
             │
             ↓
       长期漂移修正

Odometry
=
“我移动了多少？”

Localization
=
“我现在在世界哪里？”

State Estimation
=
“我现在整个机器人是什么状态？”

其中：

State Estimation

范围最大。