企业官网建设流程全解析

接触估计 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

范围最大。