机器人运动控制学习5——状态估计 State Estimation进阶

接触估计 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 velocityIMU / \ / \ 加速度 角速度 │ │ │ │ └──────┬──────┘ │ ▼ Base预测 ▲ │ 编码器 q、dq │ ▼ 腿部运动学 │ ▼ 支撑脚约束 │ ▼ Base速度修正这才是真正的 Sensor Fusion 传感器融合IMU的优点IMU更新很快。对突然运动 身体快速转动 短时间动态变化非常敏感所以短时间变化通常很好。IMU的问题主要是bias 噪声 重力处理误差 积分漂移所以时间长了容易漂。腿部运动学的优点如果支撑脚真的牢牢踩地那么脚速度≈0是一个很强的物理约束可以帮助纠正IMU速度漂移腿部运动学的问题假如脚打滑那脚速度≈0这条假设就错了于是 Leg Odometry 也会骗人。具体的走路过程理解假设机器人正在向前走当前左脚 支撑 右脚 摆动状态估计开始工作。第一步IMUIMU告诉我们身体角速度 身体加速度比如身体正在稍微向前加速第二步编码器编码器告诉我们左腿髋关节速度 左膝速度 左踝速度也就是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.480.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 VelocityKalman / 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室外可以直接给全球位置参考但室内通常不好用。VIOVisual-Inertial Odometry视觉IMU根据摄像头看到的环境特征估运动。LiDAR Odometry / SLAM用激光雷达扫描环境根据前后扫描的匹配估计机器人移动。Motion Capture实验室里的光学动作捕捉系统可以非常准确地给机器人世界位置。UWB / Beacon通过外部基站提供位置约束。这些东西看起来五花八门其实共同目的就是给机器人一个来自“外部世界”的参考不让自己的内部 odometry 无限漂下去。LocalizationOdometry我从刚才到现在动了多少。Localization我现在到底在地图的哪里。例如Odometry 我向前走了5米。而Localization 我现在位于实验室地图坐标 x 12.3 y 4.7所以可以粗略理解Odometry ↓ 关注相对运动 Localization ↓ 关注世界/地图中的位置当然实际工程里两者会深度融合不一定严格分开。State Estimation 和 LocalizationState 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范围最大。