从仿真到实机：LCM Agent、State Estimator、Deployment Runner 与 Actuator Net

训练一个四足机器人策略，难点从来不只是“在仿真里学会走路”，而是如何把仿真中的观测、动作和动力学假设，尽可能无缝地搬到真实机器人上。这个项目的部署链条，正是围绕这个目标展开的。

这一篇我们把部署侧和 actuator network 放在同一条主线上看，重点回答三个问题：

1. 真实传感器数据是如何被整理成和训练时同构的 observation 的
2. 策略输出是如何变成真正发往电机的控制命令的
3. 为什么还要额外拟合一个 actuator network，而不是直接把策略动作当作理想 PD 目标使用

主读文件包括：

• deploy_policy.py
• lcm_agent.py
• cheetah_state_estimator.py
• command_profile.py
• deployment_runner.py
• scripts/actuator_net/utils.py
• scripts/actuator_net/train.py

---

一、部署入口：deploy_policy.py 其实在做一条最小推理闭环

部署入口在 deploy_policy.py。

它的主流程可以概括成：

1. 读取训练保存下来的 parameters.pkl
2. 创建状态估计器 StateEstimator
3. 创建命令源 RCControllerProfile
4. 创建硬件环境 LCMAgent
5. 再套一层部署侧 HistoryWrapper
6. 加载 adaptation_module_latest.jit 和 body_latest.jit
7. 用 DeploymentRunner 进入控制循环

核心代码如下：

se = StateEstimator(lc)  
# 订阅实机 LCM 数据，负责把 IMU、关节、遥控器消息整理成状态估计结果

command_profile = RCControllerProfile(dt=control_dt, state_estimator=se, ...)
# 把遥控器输入映射成速度、步态、机身高度等命令

hardware_agent = LCMAgent(cfg, se, command_profile)
hardware_agent = HistoryWrapper(hardware_agent)
# LCMAgent 负责构造单帧 observation，HistoryWrapper 再把它拼成 obs_history

policy = load_policy(logdir)
# 加载部署所需的两个 TorchScript：adaptation_module 和 actor_body

而 load_policy() 里最重要的一句，就是这条 student 推理路径：

latent = adaptation_module.forward(obs["obs_history"].to('cpu'))
action = body.forward(torch.cat((obs["obs_history"].to('cpu'), latent), dim=-1))
# 先用历史观测估计环境因子，再和 obs_history 拼接，最后送入 actor_body 输出动作

这说明真实部署时并不需要 critic，也不需要完整 PPO，只需要：

obs_history -> adaptation_module -> latent/privileged_hat -> actor_body -> action

这和训练侧导出的 .jit 文件完全一致。

LCM 是通信总线，Python 通过它和机器人/底层程序收发消息。
StateEstimator 负责从 LCM 订阅机器人状态和遥控器消息，并整理成可读取状态。
RCControllerProfile 负责把遥控器状态映射成策略命令 command。
LCMAgent 负责从 StateEstimator + command_profile 取数据，拼成单帧 obs，并把策略输出转换成 PD target 再通过 LCM 发出去。
HistoryWrapper 负责把单帧 obs 滑窗累积成 obs_history。
policy 负责前向推理，输出 action。
DeploymentRunner 负责把这整条链稳定地跑起来。

StateEstimator 从 LCM 订阅机器人状态和遥控器消息，并整理成可读取状态。
RCControllerProfile 从 StateEstimator 读取遥控器输入，生成策略命令 command。
LCMAgent 从 StateEstimator + RCControllerProfile 读取数据，拼成单帧 obs。
HistoryWrapper 把连续单帧 obs 维护成 obs_history。
obs_history 输入 adaptation_module 得到 latent。
obs_history 与 latent 拼接后输入 actor(body)，输出 action。
action 被解释为关节目标位置偏移，经过缩放并加上默认关节角，变成 joint_pos_target。
LCMAgent 再把它封装成 q_des / qd_des / kp / kd 的 PD target，通过 LCM 发布给机器人。

---

二、State Estimator：真实传感器数据是怎么变成“可训练同构状态”的

部署链里最底层的数据入口，是 cheetah_state_estimator.py。

它的职责不是控制，而是“听 LCM 总线上的消息，然后把原始机器人状态整理成高层可用信号”。

初始化时，它会订阅三类 LCM 消息：

self.imu_subscription = self.lc.subscribe("state_estimator_data", self._imu_cb)
self.legdata_state_subscription = self.lc.subscribe("leg_control_data", self._legdata_cb)
self.rc_command_subscription = self.lc.subscribe("rc_command", self._rc_command_cb)
# IMU / 状态估计消息：姿态、接触估计等
# 关节控制消息：关节角、关节速度、估计力矩
# 遥控器消息：摇杆与按键

它内部维护的关键状态包括：

self.joint_pos = np.zeros(12)          # 12 个关节角
self.joint_vel = np.zeros(12)          # 12 个关节角速度
self.tau_est = np.zeros(12)            # 电机估计输出力矩
self.world_lin_vel = np.zeros(3)       # 世界系线速度
self.world_ang_vel = np.zeros(3)       # 世界系角速度
self.euler = np.zeros(3)               # 机身 roll/pitch/yaw
self.R = np.eye(3)                     # 姿态旋转矩阵
self.contact_state = np.ones(4)        # 四足接触状态

然后对外提供一组和训练环境非常对应的接口：

def get_body_linear_vel(self):
    self.body_lin_vel = np.dot(self.R.T, self.world_lin_vel)
    return self.body_lin_vel
# 把世界系速度旋转到机身坐标系
# 这和训练环境里 base_lin_vel 的定义是一致的

def get_gravity_vector(self):
    grav = np.dot(self.R.T, np.array([0, 0, -1]))
    return grav
# 计算“机身坐标系下的重力方向”
# 这和训练里的 projected_gravity 在语义上是同构的

所以可以这样理解：

StateEstimator 的任务，就是尽可能把真实机器人传感器流，整理成和仿真环境 compute_observations() 中相同语义的状态量。

这一步是 sim-to-real 能否成功的前提。因为如果真实 observation 的定义和训练 observation 不一致，策略即使在仿真里学得再好，部署时也会“看错世界”。

---

三、遥控器命令不是直接给速度，而是先被整理成训练同构的 command vector

训练里，策略看到的不只是机器人状态，还会看到 commands。部署侧同样要构造这部分输入。

这一层由 command_profile.py 和 StateEstimator.get_command() 配合完成。

在 StateEstimator.get_command() 里，遥控器被映射成一个高维 command 向量：

return np.array([
    cmd_x, cmd_y, cmd_yaw, cmd_height, cmd_freq,
    self.cmd_phase, self.cmd_offset, self.cmd_bound, self.cmd_duration,
    cmd_footswing, cmd_ori_pitch, cmd_ori_roll, cmd_stance_width,
    cmd_stance_length, 0, 0, 0, 0, 0
])
# 不只是前进速度和转向速度
# 还包括机身高度、步频、相位、offset、bound、步摆高度、姿态命令、步宽和步长

这和训练里的 gait-conditioned command space 是一一对应的。

然后 RCControllerProfile 再对这些命令做缩放与触发逻辑：

command = self.state_estimator.get_command()
command[0] = command[0] * self.x_scale
command[1] = command[1] * self.y_scale
command[2] = command[2] * self.yaw_scale
# 把遥控器原始输入缩放到真正允许的速度/角速度范围

如果某些按键绑定了预设动作轨迹，还可以切换成 profile 驱动的命令，而不只是手动摇杆。

所以从部署系统角度看，遥控器不是直接操控关节，而是在操控：

“策略任务目标”

也就是和仿真里完全同构的 command vector。

---

四、LCMAgent：真实状态如何被拼成训练同构 observation

真正把“状态估计 + 命令”拼成 observation 的，是 lcm_agent.py 中的 LCMAgent。

它的初始化里，会从训练配置恢复这些关键量：

self.num_obs = self.cfg["env"]["num_observations"]
self.num_privileged_obs = self.cfg["env"]["num_privileged_obs"]
self.num_actions = self.cfg["env"]["num_actions"]
self.num_commands = self.cfg["commands"]["num_commands"]
# 部署侧并不自己定义观测维度，而是直接沿用训练配置

同时还会恢复训练中使用的默认关节角、PD 参数和 observation scale：

self.default_dof_pos = np.array([...])     # 训练中的 default joint angles
self.p_gains[i] = self.cfg["control"]["stiffness"][...]
self.d_gains[i] = self.cfg["control"]["damping"][...]
self.commands_scale = np.array([...])      # 和训练中 commands_scale 对齐
# 这样部署侧构造出来的 observation 才会和训练时的归一化尺度一致

最关键的是 get_obs()：

ob = np.concatenate((
    self.gravity_vector.reshape(1, -1),
    self.commands * self.commands_scale,
    (self.dof_pos - self.default_dof_pos).reshape(1, -1) * self.obs_scales["dof_pos"],
    self.dof_vel.reshape(1, -1) * self.obs_scales["dof_vel"],
    torch.clip(self.actions, -clip_actions, clip_actions).cpu().detach().numpy().reshape(1, -1)
), axis=1)
# 重力方向 + 命令 + 关节位置偏差 + 关节速度 + 当前动作
# 这和训练环境 compute_observations() 的主干结构是一致的

如果训练时打开了这些开关，部署侧也会同步拼上：

if self.cfg["env"]["observe_two_prev_actions"]:
    ob = np.concatenate((ob, self.last_actions.cpu().detach().numpy().reshape(1, -1)), axis=1)
# 两步动作历史，用于帮助策略感知控制时滞和系统惯性

if self.cfg["env"]["observe_clock_inputs"]:
    ob = np.concatenate((ob, self.clock_inputs), axis=1)
# 步态时钟输入，帮助策略维持周期性 gait

以及速度项：

if self.cfg["env"]["observe_vel"]:
    ob = np.concatenate((
        self.body_linear_vel.reshape(1, -1) * self.obs_scales["lin_vel"],
        self.body_angular_vel.reshape(1, -1) * self.obs_scales["ang_vel"],
        ob
    ), axis=1)
# 机身线速度和角速度也用训练时同样的 scale 喂给策略

这一步的意义非常大：

部署侧 observation 不是“差不多长得像训练输入”就行，而是要在拼接顺序、尺度、语义上严格对齐训练环境。

否则 student policy 看见的就不是它训练过的输入分布。

---

五、部署侧 HistoryWrapper：真实机器人也要维护 obs_history

训练时 adaptation_module 依赖的是 obs_history，不是单帧 obs。所以部署侧也必须维护同样的滑动窗口。

这一层在 go2_gym_deploy/envs/history_wrapper.py：

self.obs_history_length = self.env.cfg["env"]["num_observation_history"]
self.num_obs_history = self.obs_history_length * self.env.num_obs
self.obs_history = torch.zeros(self.env.num_envs, self.num_obs_history, ...)
# 历史窗口总维度 = 历史长度 × 单帧 observation 维度

每一步更新方式和训练环境保持一致：

self.obs_history = torch.cat((self.obs_history[:, self.env.num_obs:], obs), dim=-1)
# 丢掉最旧一帧，再拼上最新一帧
# 形成一个固定长度的滑动时间窗口

所以在部署时，策略真正吃到的输入不是实时单帧观测，而是：

最近若干步的观测拼接结果。

这正是 adaptation module 能在实机上工作的前提。因为它要根据时间序列去推断隐含动力学和环境状态。

---

六、策略输出如何变成电机命令

策略输出之后，真正把它发送给机器人的是 LCMAgent.publish_action()。

看核心逻辑：

self.joint_pos_target = (
    action[0, :12].detach().cpu().numpy() * self.cfg["control"]["action_scale"]
).flatten()
self.joint_pos_target[[0, 3, 6, 9]] *= self.cfg["control"]["hip_scale_reduction"]
self.joint_pos_target += self.default_dof_pos
# 策略输出先被缩放成“目标关节偏移”
# 再加上 default joint angles，变成真正的目标关节角

然后封装成 LCM 控制消息：

command_for_robot.q_des = joint_pos_target
command_for_robot.qd_des = self.joint_vel_target
command_for_robot.kp = self.p_gains
command_for_robot.kd = self.d_gains
command_for_robot.tau_ff = np.zeros(12)
# 这说明部署侧发送给底层控制器的不是 torque，而是 PD 目标：
# 目标关节角 + 目标关节速度 + Kp/Kd + 前馈力矩

最后通过 LCM 发布：

lc.publish("pd_plustau_targets", command_for_robot.encode())
# 由 C++/SDK 侧真正接收并下发到底层控制板

所以部署链里，策略输出并不是直接等于电机扭矩，而是：

policy action -> target joint angles -> PD target message -> 机器人底层执行

这和仿真中“动作先转 joint_pos_target，再由控制器产生 torque”的结构是一致的。

---

七、为什么还要额外拟合 actuator network

这里就进入最关键的问题了：
既然部署时最终还是发 PD 目标，那为什么训练侧还要单独搞一个 actuator network？

答案是：

因为真实机器人并不是理想 PD 系统。

即使你给相同的目标关节角、相同的 Kp/Kd，真实电机输出的 torque 也会受到很多因素影响：

• 电机内部控制器的真实动力学
• 驱动器延迟
• 摩擦、背隙和机械顺应性
• 电流限幅
• 编码器噪声和零偏
• 关节速度历史和误差历史

因此，训练时如果直接用理想 PD torque 去近似实机执行器，很容易出现 sim-to-real gap。

这个项目的思路是：

1. 先在真实硬件上跑控制与记录日志
2. 从日志中提取“关节误差/速度历史 -> 实际力矩估计”的映射
3. 训练一个 actuator network 去拟合这条映射
4. 再把它放回仿真控制链中，替代理想 PD，使训练里的 torque 更像真实机器人

所以 actuator network 的存在，本质上是在做：

执行器系统辨识（actuator system identification）

---

八、Actuator Net 是怎么从实机日志里训练出来的

这一部分在 scripts/actuator_net/utils.py。

先看数据从哪里来：

with open(log_path, 'rb') as file:
    data = pkl.load(file)

datas = data['hardware_closed_loop'][1]
# 训练 actuator net 的数据来源不是仿真 rollout，而是真实部署日志

从日志里提取的关键量包括：

tau_ests[i, :] = datas[i]["tau_est"]                 # 实机估计出的关节力矩
joint_positions[i, :] = datas[i]["joint_pos"]        # 实际关节角
joint_position_targets[i, :] = datas[i]["joint_pos_target"]  # 目标关节角
joint_velocities[i, :] = datas[i]["joint_vel"]       # 实际关节速度

然后构造训练样本：

joint_position_errors = joint_positions - joint_position_targets
# 输入不是关节角本身，而是位置误差
# 因为执行器输出主要取决于“目标和当前差了多少”

xs_joint = [
    joint_position_errors[2:-step+1, i:i+1],
    joint_position_errors[1:-step, i:i+1],
    joint_position_errors[:-step-1, i:i+1],
    joint_velocities[2:-step+1, i:i+1],
    joint_velocities[1:-step, i:i+1],
    joint_velocities[:-step-1, i:i+1]
]
# 每个关节输入 6 维：
# 3 帧位置误差历史 + 3 帧速度历史

监督目标就是实际力矩：

ys = tau_ests[step:-1, i:i+1]
# 目标输出是该关节当前真实力矩估计

然后训练一个小 MLP：

model = build_mlp(in_dim=6, units=32, layers=2, out_dim=1, act='softsign')
# 每个关节共享同一个小网络结构
# 输入 6 维历史特征，输出 1 个 torque

loss 很简单，就是 MSE：

tau_est_loss = ((y_pred - y_label) ** 2).mean()
# 学的就是“给定误差历史和速度历史，当前电机实际会输出多少力矩”

而 scripts/actuator_net/train.py 和 scripts/actuator_net/eval.py 只是分别负责：

• 训练新 actuator net
• 评估已有 actuator net

---

九、Actuator Net 在整个 sim-to-real 链中的位置

现在把训练侧和部署侧串起来，actuator net 的位置就清楚了。

部署侧

真实机器人执行链是：

policy action -> joint target -> 底层 PD/驱动器 -> 真实 torque -> 机械运动

仿真侧

如果直接用理想 PD，则链路会变成：

policy action -> joint target -> 理想 PD -> 仿真 torque -> 机械运动

这两条链最大的 gap，就在“理想 PD”和“真实执行器”之间。

而 actuator net 的作用，就是把仿真侧替换成：

policy action -> joint target -> actuator net -> 更真实的 torque -> 仿真运动

这样一来，训练中的控制响应就更接近真实机器人。

所以它不是部署时额外运行的网络，而是：

为了让仿真更像实机，而额外拟合的一层执行器动力学模型。

---

十、Deployment Runner：如何把所有模块接成一个实机控制闭环

最后看 deployment_runner.py。

它的角色可以理解成一个“控制实验 orchestrator”。
它负责把：

• control agent（这里是 HistoryWrapper(LCMAgent)）
• policy
• command profile
• logger

全部接在一起。

主循环很简单也很清楚：

action = self.policy(control_obs, policy_info)
# 先根据当前 obs_history 跑 student policy

obs, ret, done, info = self.agents[agent_name].step(action)
# 再把动作发给硬件 agent，让机器人执行一步

if logging:
    self.logger.log(agent_name, info)
# 同时把关节、姿态、命令、动作、torque 等信息记下来

它还负责：

• 开始前校准到 nominal pose
• 异常姿态时 emergency stop / 再校准
• 根据按键开始或停止日志记录
• 保存最终 log.pkl

因此，从工程角度看，DeploymentRunner 并不关心策略内部结构，它关心的是：

怎样稳定、安全、可记录地把策略真正跑到机器人身上。

---

结语

如果用一句话总结这条“从仿真到实机”的链路，那就是：

真实传感器先被 StateEstimator 整理成训练同构状态，再由 LCMAgent 按训练时相同的方式拼成 observation，HistoryWrapper 维护 obs_history，student policy 输出动作，DeploymentRunner 把动作发到实机，而 actuator net 则在训练侧负责把仿真中的执行器动力学尽量拉近真实机器人。

所以这个项目的 sim-to-real 并不是一句抽象口号，而是靠下面这几层一起完成的：

• 观测同构
• 命令同构
• 控制链同构
• 执行器动力学近似
• 部署闭环日志与校准机制

补充：C++ 桥接层与三线程通信结构

上面主体讲的，主要是 Python 侧策略部署流程。
但整条链真正能跑起来，还依赖底层的 C++ 桥接层，把 Unitree SDK2 / DDS 和 Python / LCM 接起来。

这一层主要在 lcm_position_go2.cpp。

1. 为什么还需要一层 C++ bridge

Python 侧负责的是：

• 整理 observation
• 跑 student policy
• 输出 PD target

但机器人本体并不是直接听 Python 的，它真正使用的是 Unitree SDK2 / DDS 体系下的低层接口。

所以需要一个桥接程序来做两件事：

• 把机器人底层状态转成 LCM 消息，发给 Python
• 把 Python 发回来的 PD target 转成 LowCmd，写给机器人

于是整套部署并不是一个单进程，而是：

• 一个 C++ 桥接进程
• 一个 Python 策略进程

两者通过 LCM 交换信息。

---

2. 三线程分别是什么

在 C++ 桥接程序里，会启动三个循环线程：

• lcm_send_thread
• lcm_recev_thread
• dds_write_thread

这就是常说的“三线程通信结构”。

线程 1：lcm_send_thread

这个线程负责：

从 Unitree SDK2 / DDS 读取底层状态和遥控器输入，再发布到 LCM。

它会把底层读取到的数据整理成三个 LCM 主题：

• leg_control_data
• state_estimator_data
• rc_command

其中包括：

• 12 个关节角 q
• 12 个关节速度 qd
• 12 个估计力矩 tau_est
• IMU 姿态与角速度
• 足端接触力
• 遥控器摇杆和按键状态

也就是说，Python 侧 StateEstimator 能拿到的数据，实际上就是这个线程不断发布出来的。

线程 2：lcm_recev_thread

这个线程负责：

从 LCM 读取 Python 发回来的神经网络控制目标。

它订阅的主题是：

• pd_plustau_targets

也就是 Python 侧 LCMAgent.publish_action() 发布出来的 PD target。

收到之后，C++ 会把这些目标缓存到 joint_command_simple 里，包括：

• q_des
• qd_des
• kp
• kd
• tau_ff

这个线程本身不直接写电机，而是先把来自 Python 的控制目标接住。

线程 3：dds_write_thread

这个线程负责：

把上一步收到的控制目标真正封装成 Unitree 的 LowCmd，再通过 DDS 写给底层电机控制器。

它做的是最后一步执行工作：

• 安全检查
• 状态机判断
• 将 joint_command_simple 写入 low_cmd
• 计算 CRC
• 调用 lowcmd_publisher->Write(low_cmd)

真正让机器人电机动起来的，是这条线程，而不是 Python 进程本身。

---

3. 从线程视角重写整条部署链

如果按线程来描述整个部署流程，可以写成下面这条完整链路。

线程视角下的 LCM 部署流程

• 在线程 1 lcm_send_thread 中，C++ 桥接程序持续从 Unitree SDK2 读取机器人底层状态与遥控器输入，并通过 LCM 发布到 leg_control_data、state_estimator_data 和 rc_command 三个主题。
• 在 Python 侧，StateEstimator 的监听线程持续订阅这三个 LCM 主题，把关节、姿态、接触与遥控器状态整理为可读取的本地状态。
• 在 Python 主控制线程中，RCControllerProfile 根据 StateEstimator 缓存的遥控器状态生成 command，LCMAgent 再将机器人状态与 command 拼接成单帧 obs，HistoryWrapper 进一步维护为 obs_history。
• 随后策略前向运行：obs_history 先进入 adaptation_module 得到 latent，再与 obs_history 拼接后输入 actor body，输出动作 action。
• LCMAgent 将 action 解释为目标关节位置偏移，恢复成 q_des / qd_des / kp / kd / tau_ff 形式的 PD target，并通过 LCM 发布到 pd_plustau_targets。
• 在线程 2 lcm_recev_thread 中，C++ 桥接程序从 LCM 订阅 pd_plustau_targets，把神经网络输出写入 joint_command_simple。
• 最后在线程 3 dds_write_thread 中，桥接程序将这些目标命令封装成 Unitree 的 LowCmd，通过 DDS 下发到底层电机控制器，机器人开始执行动作。

这样看就很清楚了：

• Python 侧负责“理解世界并做决策”
• C++ 侧负责“搬运状态、接收命令并真正执行”

---

4. 为什么三线程结构很重要

这三个线程并不是简单地“拆开写”，而是有明确的工程意义。

第一，读状态和写命令分离。
机器人状态上行和控制命令下行是两条不同的信息流，拆开之后时序更稳定。

第二，LCM 与 DDS 解耦。
Python 不需要理解 Unitree 的底层 DDS 接口，只需要理解 LCM 消息；C++ bridge 负责做协议转换。

第三，策略和执行分层。
即使未来换掉策略网络，只要输入输出消息保持一致，底层桥接结构仍然可以复用。

所以这层 C++ bridge 的意义，不只是“多写了一层代码”，而是让部署系统具备了：

• 清晰的模块边界
• 更好的调试性
• 更强的可替换性
• 更可靠的实时性

---

结语

如果用一句话总结这条从仿真到实机的链路，那就是：

真实传感器先经由 C++ bridge 和 LCM 总线送到 Python，再由 StateEstimator 整理成可读取状态，LCMAgent 按训练时的定义构造 observation，HistoryWrapper 维护 obs_history，student policy 输出动作，动作再被还原成 PD target，通过 LCM 回传给 C++ bridge，最后由底层控制器真正下发给机器人；而 actuator net 则在训练侧负责把仿真中的执行器动力学尽量拉近真实机器人。

因此，这个项目的 sim-to-real 不是一句抽象口号，而是靠下面几层一起完成的：

• 观测同构
• 命令同构
• 控制链同构
• 执行器动力学近似
• Python 策略层与 C++ 通信桥接层的协同
• 部署闭环日志与校准机制