机器人运动控制学习3——动力学

现在正式进入动力学

CoM 质心

Center of Mass，CoM，质心。

如果：

手臂向前伸

质心会向前一点。

如果：

抬起右腿

质心也会发生变化。

所以人形控制器会一直关心：

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 Control，WBC。

WBC 会协调：

全身所有关节 + 所有任务 + 各种约束

最终给底层：

关节加速度 关节力矩 关节目标

具体输出取决于控制架构。

QP

WBC 里经常会出现：

QP，Quadratic 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，时间抖动。

所以：

平均频率正确

不代表：

实时性就好

这点特别重要。

State

RobotState 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_index

Control 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 ↓ 不断记录所有关键数据

这已经非常接近以后会看到的真实架构。

现在你的“看代码思维”应该升级成这样

第一步 数据从哪里来？ ↓ 第二步 State 长什么样？ ↓ 第三步 控制循环在哪？ ↓ 第四步 目标从哪里来？ ↓ 第五步 Controller怎么算？ ↓ 第六步 Command从哪里出去？ ↓ 第七步 运行频率多少？ ↓ 第八步 异常怎么办？ ↓ 第九步 日志记录了什么？