现在正式进入动力学CoM 质心Center of MassCoM质心。如果手臂向前伸质心会向前一点。如果抬起右腿质心也会发生变化。所以人形控制器会一直关心CoM在哪里 CoM速度多少 CoM接下来会怎么动支撑区域 Support Polygon假设双脚站地┌─────┐ ┌─────┐ │左脚 │ │右脚 │ └─────┘ └─────┘你可以把脚和脚之间形成的大致区域想成支撑区域。如果机器人整体平衡状态比较正常● CoM投影 ↓ ┌──────────────────┐ │ 支撑区域 │ └──────────────────┘比较容易站住。如果身体倾得特别厉害● ↓ ┌──────────────────┐ │ 支撑区域 │ └──────────────────┘就越来越危险。这是理解人形静态平衡的第一层直觉。Zero Moment Point 零力矩点机器人和地面的受力综合起来在地面上等效作用在哪里当前支撑是否还比较合理你可以粗略想象脚底┌────────────┐ │ ● │ │ ZMP │ └────────────┘如果 ZMP 在合理支撑区域里通常意味着当前接触状态在动态上比较可控。如果跑出去很多机器人可能需要调整或迈步。注意这是直觉理解不是完整数学定义。IMU你之前可能把 IMU 理解成测姿态。现在可以更具体。IMU 常提供角速度 加速度 姿态相关信息人形控制器通过它知道身体是不是前倾 是不是侧倒 转得多快 受到什么加速度所以关节编码器 ↓ 告诉我四肢怎么弯 IMU ↓ 告诉我身体整体怎么动它们结合才能更好估计机器人现在到底是什么状态。State Estimation 状态估计这个词以后非常重要。控制器需要q dq 身体姿态 身体速度 CoM 接触状态但传感器不一定直接完美告诉你所有东西。于是需要State Estimator状态估计器。它会把Encoder IMU Foot Contact 其他传感器融合起来传感器数据 ↓ 状态估计 ↓ 机器人现在的状态 ↓ 控制器所以高级控制器很依赖状态估计垃圾状态进去再高级的 MPC/WBC 也可能控制不好。目标动作 ↓ Trajectory / Planner ↓ IK / WBC ↓ q_des / dq_des / tau ↓ 关节控制 ↓ 电机 ↓ 人形机器人 ↓ ┌──────────┼──────────┐ ↓ ↓ ↓ Encoder IMU 足底/接触 └──────────┼──────────┘ ↓ State Estimator ↓ 当前状态 └────────────↺WBC假设机器人同时需要左脚固定 右脚抬起来 身体保持竖直 CoM往左移动 右手伸出去 所有关节别超限 电机力矩别太大你会发现一个简单 IK 已经不够用了。因为这些任务互相影响于是需要一个“大管家”Whole-Body ControlWBC。WBC 会协调全身所有关节 所有任务 各种约束最终给底层关节加速度 关节力矩 关节目标具体输出取决于控制架构。QPWBC 里经常会出现QPQuadratic Programming二次规划。在很多要求和限制同时存在时帮我找一个“综合最好”的解。比如我要右手尽量接近目标 同时 脚不能滑 关节不能超限 力矩不能超限 身体不能倒QP好我在这些约束里找一个最合适的关节命令。所以先记WBC 问题框架 QP 经常用来求这个问题的一种优化工具不是完全等号但这种理解对小白很有用。传感器 ↓ 状态估计 ↓ 当前机器人状态 ↓ 目标 / 轨迹 ↓ 运动学 ↓ 动力学 ↓ WBC / MPC ↓ 关节目标与力矩 ↓ 底层PD ↓ 电机 ↓ 机器人 ↓ 传感器反馈机器人程序 │ ├── 通信部门 │ └── 收发机器人数据 │ ├── 状态部门 │ └── 整理 q / dq / IMU │ ├── 规划部门 │ └── 决定机器人想怎么动 │ ├── 控制部门 │ └── 算 q_des / tau │ ├── 安全部门 │ └── 检查限位和异常 │ └── 日志部门 └── 记录发生了什么这些东西可能同时运行。于是就涉及线程 Thread线程 Thread比如线程1 不断接收传感器数据 线程2 不断运行控制器 线程3 不断保存日志不需要收一次数据 ↓ 写一次硬盘 ↓ 再控制 ↓ 再收数据全部串行。控制线程以后你进运控代码最先找Control Loop 在哪你可能看到while (running) { read_state(); compute_control(); send_command(); sleep_until_next_cycle(); }这几乎就是程序心脏你应该先问这个循环多少 Hz 每轮输入什么 每轮输出什么Nominal dt 和 Actual dt这是一个很实用的区别你希望控制周期dt 1 ms这叫理想 / 目标周期。但是实际可能第1次 1.01 ms 第2次 0.99 ms 第3次 1.04 ms 第4次 2.50 ms真实的actual_dt并不永远完美。所以程序可能会统计平均周期 最大周期 最小周期 抖动 超时次数这在运控调试里很重要。Jitter 抖动假设目标每1ms一次实际1.0 1.0 1.0 1.0 1.0很稳定。但另一台0.4 1.6 0.7 1.3 0.3 1.7平均可能还是≈1 ms可是节奏乱。这就是Jitter时间抖动。所以平均频率正确不代表实时性就好这点特别重要。StateRobotState state;state机器人现在是什么状态。可能包含机器人状态 state │ ├── joint position q ├── joint velocity dq ├── joint torque tau ├── IMU │ ├── orientation │ ├── gyro │ └── acceleration │ ├── foot contact ├── timestamp └── error flags所以state不是一个数字。它通常是一大包当前机器人信息。Command对应地RobotCommand cmd;就是我要让机器人接下来怎么做。可能包含command │ ├── q_des ├── dq_des ├── kp ├── kd ├── tau_ff └── control_mode所以整个控制系统极简化就是State ↓ Controller ↓ Command一定把这三个词刻住。通信线程如果控制程序在电脑上机器人本体在另外一个控制器上就必须PC ↔ 机器人传数据。例如Robot → PC q / dq / IMU PC → Robot q_des / kp / kd / tau中间可能通过Ethernet UDP CAN EtherCAT 其他实时总线所以程序里会出现communication thread专门处理接收 解析 发送Timestamp 时间戳于是每一帧数据最好有timestamp例如frame 1000 timestamp 10.000 frame 1001 timestamp 10.002 frame 1002 timestamp 10.004这样你才能判断这帧什么时候产生 间隔是不是正常 有没有卡顿 有没有乱序所以你以后看到timestamp seq frame_id这些都不要当“附属信息”它们对实时系统非常重要。Sequence Number 帧号例如100 101 102 103突然105那你马上知道104可能缺了。如果100 101 103 105你能统计掉了多少帧 连续掉几帧 在哪个位置掉的所以运控数据经常有seq / frame_indexControl Thread 和 Planning Thread这是一个非常常见的架构。例如Planning 100 Hz Control 1000 Hz为什么规划不也 1000 Hz因为规划通常更重。比如MPC 轨迹优化 IK WBC部分计算可能没必要每 1 ms 全部重算。于是低频规划器 ↓ 给出未来一段目标 高频控制器 ↓ 每1ms跟踪这个目标很好理解导航员 ↓ 偶尔告诉司机 “前面500米右转” 司机 ↓ 每时每刻控制方向盘和油门规划器和底层控制器就是类似这种关系。Race Condition假设线程A 正在更新 q_des 线程B 正在读取 q_des而 q_des 有 20 个关节。线程A只更新到前10个关节线程B突然来读。结果得到前10个 新数据 后10个 旧数据这种混乱就是并发程序里典型的Race Condition竞争条件。所以运控程序不能只考虑数学算法对不对还得考虑数据有没有被安全地交换Mutex 锁某个线程进去修改数据 ↓ 把门锁上其他线程先别进修改完开锁下一个线程再访问这就是Mutex互斥锁。但是实时控制里还有一个麻烦如果控制线程等锁等太久就会影响控制周期。所以真正高实时程序里经常会特别慎重地设计数据交换尽量减少不可控阻塞。日志至少应该想办法记录timestamp q_des q dq_des dq tau IMU control_dt frame_id error因为机器人一旦抖了一下 摔了一下 突然停了你不能只说我刚才肉眼感觉它好像抖了一下。应该看日志 ↓ 找发生时间 ↓ 看 q ↓ 看 dq ↓ 看 torque ↓ 看 IMU ↓ 看 dt ↓ 看通信帧这才是工程调试。时间轴对齐假设机器人在 12.3 秒摔倒你要问12.25s IMU发生了什么 12.26s 关节速度发生了什么 12.27s tau是否饱和 12.28s 通信有没有缺帧 12.30s 机器人倒下也就是把不同数据按时间对齐。这比单独看一个变量有价值得多。拿到一个陌生运控项目正确阅读顺序先画架构第一件事机器人状态从哪里进来找recv state subscribe read第二件事控制循环在哪找while loop control update step第三件事控制输入是什么找q dq imu contact第四件事控制输出是什么找q_des dq_des kp kd tau第五件事谁真正发送到机器人找send publish command write这五步抓住项目主链就出来了。Watchdog 看门狗逻辑如果超过一定时间 没收到新命令 ↓ 认为控制异常 ↓ 进入安全状态例如停止 阻尼模式 零力矩 其他平台安全策略具体行为看机器人系统。帧号 时间戳 Watchdog它们都在回答我的数据流还健康吗帧号 ↓ 有没有丢数据 时间戳 ↓ 数据是不是太旧 周期 ↓ 更新速度正常吗 Watchdog ↓ 太久没数据怎么办所以运控通信绝不是能收发 UDP 就完了。而是数据内容 数据顺序 数据时间 异常处理一起考虑。┌──────────────┐ │ Planner │ │ 低频规划线程 │ └──────┬───────┘ │ q_des / trajectory │ ↓ Robot ──state──→ ┌──────────────────────┐ │ State Estimator │ └─────────┬────────────┘ │ q / dq / IMU ↓ ┌──────────────────────┐ │ Control Thread │ │ 高频控制循环 │ └─────────┬────────────┘ │ q_des/dq_des/tau ↓ ┌──────────────────────┐ │ Safety / Limits │ └─────────┬────────────┘ ↓ Communication ↓ Robot 另外 Logger Thread ↓ 不断记录所有关键数据这已经非常接近以后会看到的真实架构。变量先理解成q当前关节位置dq当前关节速度ddq当前关节加速度tau当前/目标关节力矩q_des目标关节位置dq_des目标关节速度tau_ff前馈力矩kp位置刚度/比例增益kd阻尼/速度增益dt控制周期imu身体惯性传感器数据state当前机器人状态cmd要发给机器人的命令seq帧号timestamp时间戳contact接触状态base机器人基座/躯干相关状态现在你的“看代码思维”应该升级成这样第一步 数据从哪里来 ↓ 第二步 State 长什么样 ↓ 第三步 控制循环在哪 ↓ 第四步 目标从哪里来 ↓ 第五步 Controller怎么算 ↓ 第六步 Command从哪里出去 ↓ 第七步 运行频率多少 ↓ 第八步 异常怎么办 ↓ 第九步 日志记录了什么