rl_sar实现sim2real的整体思路

发布于：2025-06-08 ⋅ 阅读:(21) ⋅ 点赞:(0)

文章目录

RL_Real_Go2 真实机器人控制系统详细解析

概述

rl_real_go2.cpp 是一个基于强化学习的真实 Unitree Go2 四足机器人控制系统。该系统通过 DDS 通信协议与真实机器人进行交互，运行训练好的神经网络模型来实现机器人的自主运动控制。

系统架构图

┌─────────────────┐    ┌─────────────────┐    ┌─────────────────┐
│   手柄输入      │    │   IMU传感器     │    │   关节状态      │
│  (Joystick)     │    │   (姿态数据)    │    │  (位置/速度)    │
└─────────┬───────┘    └─────────┬───────┘    └─────────┬───────┘
          │                      │                      │
          └──────────────────────┼──────────────────────┘
                                 │
                    ┌─────────────▼─────────────┐
                    │      状态获取模块         │
                    │    (GetState)            │
                    └─────────────┬─────────────┘
                                 │
                    ┌─────────────▼─────────────┐
                    │    强化学习推理模块       │
                    │   (RunModel/Forward)     │
                    └─────────────┬─────────────┘
                                 │
                    ┌─────────────▼─────────────┐
                    │     命令生成模块          │
                    │   (SetCommand)           │
                    └─────────────┬─────────────┘
                                 │
          ┌──────────────────────┼──────────────────────┐
          │                      │                      │
┌─────────▼───────┐    ┌─────────▼───────┐    ┌─────────▼───────┐
│   FL关节控制    │    │   FR关节控制    │    │   RL/RR关节控制 │
│  (前左腿)       │    │  (前右腿)       │    │  (后腿)         │
└─────────────────┘    └─────────────────┘    └─────────────────┘

详细流程分析

1. 系统初始化阶段

1.1 参数配置加载
// 硬编码机器人配置
this->robot_name = "go2";
this->config_name = "robot_lab";
std::string robot_path = this->robot_name + "/" + this->config_name;
this->ReadYaml(robot_path);

功能说明：

  • 设置机器人类型为 Go2
  • 配置文件名为 “robot_lab”
  • 从 YAML 文件加载训练参数和网络配置
1.2 坐标系适配
for (std::string &observation : this->params.observations)
{
    // 真实 Go2 机器人的角速度在机体坐标系中
    if (observation == "ang_vel")
    {
        observation = "ang_vel_body";
    }
}

重要区别：

  • 仿真环境：角速度在世界坐标系 (ang_vel_world)
  • 真实机器人：角速度在机体坐标系 (ang_vel_body)
1.3 机器人服务初始化
this->InitRobotStateClient();
while (this->QueryServiceStatus("sport_mode"))
{
    std::cout << "Try to deactivate the service: " << "sport_mode" << std::endl;
    this->rsc.ServiceSwitch("sport_mode", 0);
    sleep(1);
}

功能说明：

  • 初始化机器人状态客户端
  • 关闭 Unitree 自带的运动模式服务
  • 确保机器人处于可控状态

2. 通信系统建立

2.1 DDS 发布器创建
// 创建低级命令发布器
this->lowcmd_publisher.reset(new ChannelPublisher<unitree_go::msg::dds_::LowCmd_>(TOPIC_LOWCMD));
this->lowcmd_publisher->InitChannel();
2.2 DDS 订阅器创建
// 机器人状态订阅器
this->lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
this->lowstate_subscriber->InitChannel(std::bind(&RL_Real::LowStateMessageHandler, this, std::placeholders::_1), 1);

// 手柄输入订阅器
this->joystick_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::WirelessController_>(TOPIC_JOYSTICK));
this->joystick_subscriber->InitChannel(std::bind(&RL_Real::JoystickHandler, this, std::placeholders::_1), 1);

通信架构：

应用程序 ←→ DDS中间件 ←→ 机器人硬件
    ↑           ↑           ↑
发布命令    数据传输    执行动作
订阅状态    实时通信    传感器反馈

3. 强化学习系统初始化

3.1 PyTorch 环境配置
torch::autograd::GradMode::set_enabled(false);  // 禁用梯度计算
torch::set_num_threads(4);                      // 设置线程数
3.2 历史观测缓冲区
if (!this->params.observations_history.empty())
{
    this->history_obs_buf = ObservationBuffer(1, this->params.num_observations, this->params.observations_history.size());
}
3.3 神经网络模型加载
std::string model_path = std::string(CMAKE_CURRENT_SOURCE_DIR) + "/models/" + robot_path + "/" + this->params.model_name;
this->model = torch::jit::load(model_path);

4. 多线程控制循环

4.1 线程架构设计
// 键盘输入线程 (20Hz)
this->loop_keyboard = std::make_shared<LoopFunc>("loop_keyboard", 0.05, std::bind(&RL_Real::KeyboardInterface, this));

// 控制循环线程 (高频率，通常1000Hz)
this->loop_control = std::make_shared<LoopFunc>("loop_control", this->params.dt, std::bind(&RL_Real::RobotControl, this));

// 强化学习推理线程 (低频率，通常50Hz)
this->loop_rl = std::make_shared<LoopFunc>("loop_rl", this->params.dt * this->params.decimation, std::bind(&RL_Real::RunModel, this));

线程频率分析：

  • 键盘线程：20Hz - 用户交互响应
  • 控制线程：1000Hz - 实时控制保证
  • RL推理线程：50Hz - 神经网络计算
4.2 线程同步机制
┌─────────────┐    ┌─────────────┐    ┌─────────────┐
│ 键盘线程    │    │ 控制线程    │    │ RL推理线程  │
│   20Hz      │    │  1000Hz     │    │   50Hz      │
└──────┬──────┘    └──────┬──────┘    └──────┬──────┘
       │                  │                  │
       └──────────────────┼──────────────────┘
                          │
                ┌─────────▼─────────┐
                │   共享数据结构    │
                │ robot_state      │
                │ robot_command    │
                └──────────────────┘

5. 状态获取系统 (GetState)

5.1 手柄控制状态检测
if ((int)this->unitree_joy.components.R2 == 1)
{
    this->control.control_state = STATE_POS_GETUP;      // 起立
}
else if ((int)this->unitree_joy.components.R1 == 1)
{
    this->control.control_state = STATE_RL_INIT;        // RL初始化
}
else if ((int)this->unitree_joy.components.L2 == 1)
{
    this->control.control_state = STATE_POS_GETDOWN;    // 趴下
}
5.2 IMU 数据处理
// 根据不同框架处理四元数顺序
if (this->params.framework == "isaacgym")
{
    state->imu.quaternion[3] = this->unitree_low_state.imu_state().quaternion()[0]; // w
    state->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[1]; // x
    state->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[2]; // y
    state->imu.quaternion[2] = this->unitree_low_state.imu_state().quaternion()[3]; // z
}

坐标系转换：

  • IsaacGym: [x, y, z, w] 格式
  • IsaacSim: [w, x, y, z] 格式
5.3 关节状态映射
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
    state->motor_state.q[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].q();
    state->motor_state.dq[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].dq();
    state->motor_state.tau_est[i] = this->unitree_low_state.motor_state()[this->params.state_mapping[i]].tau_est();
}

6. 强化学习推理系统 (RunModel)

6.1 观测数据准备
this->obs.ang_vel = torch::tensor(this->robot_state.imu.gyroscope).unsqueeze(0);
this->obs.commands = torch::tensor({{this->joystick.ly(), -this->joystick.rx(), -this->joystick.lx()}});
this->obs.base_quat = torch::tensor(this->robot_state.imu.quaternion).unsqueeze(0);
this->obs.dof_pos = torch::tensor(this->robot_state.motor_state.q).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);
this->obs.dof_vel = torch::tensor(this->robot_state.motor_state.dq).narrow(0, 0, this->params.num_of_dofs).unsqueeze(0);

观测空间组成：

  • 角速度 (ang_vel): 3维陀螺仪数据
  • 命令 (commands): 3维手柄输入 [前进, 转向, 侧移]
  • 姿态 (base_quat): 4维四元数
  • 关节位置 (dof_pos): 12维关节角度
  • 关节速度 (dof_vel): 12维关节角速度
6.2 神经网络前向推理
torch::Tensor RL_Real::Forward()
{
    torch::autograd::GradMode::set_enabled(false);
    torch::Tensor clamped_obs = this->ComputeObservation();
    
    torch::Tensor actions;
    if (!this->params.observations_history.empty())
    {
        // 使用历史观测
        this->history_obs_buf.insert(clamped_obs);
        this->history_obs = this->history_obs_buf.get_obs_vec(this->params.observations_history);
        actions = this->model.forward({this->history_obs}).toTensor();
    }
    else
    {
        // 仅使用当前观测
        actions = this->model.forward({clamped_obs}).toTensor();
    }
    
    // 动作限幅
    if (this->params.clip_actions_upper.numel() != 0 && this->params.clip_actions_lower.numel() != 0)
    {
        return torch::clamp(actions, this->params.clip_actions_lower, this->params.clip_actions_upper);
    }
    
    return actions;
}
6.3 安全保护机制
this->TorqueProtect(this->output_dof_tau);                                    // 力矩保护
this->AttitudeProtect(this->robot_state.imu.quaternion, 75.0f, 75.0f);      // 姿态保护

7. 命令执行系统 (SetCommand)

7.1 命令映射和发送
for (int i = 0; i < this->params.num_of_dofs; ++i)
{
    this->unitree_low_command.motor_cmd()[i].mode() = 0x01;  // 伺服模式
    this->unitree_low_command.motor_cmd()[i].q() = command->motor_command.q[this->params.command_mapping[i]];
    this->unitree_low_command.motor_cmd()[i].dq() = command->motor_command.dq[this->params.command_mapping[i]];
    this->unitree_low_command.motor_cmd()[i].kp() = command->motor_command.kp[this->params.command_mapping[i]];
    this->unitree_low_command.motor_cmd()[i].kd() = command->motor_command.kd[this->params.command_mapping[i]];
    this->unitree_low_command.motor_cmd()[i].tau() = command->motor_command.tau[this->params.command_mapping[i]];
}
7.2 CRC 校验和发布
this->unitree_low_command.crc() = Crc32Core((uint32_t *)&unitree_low_command, (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
lowcmd_publisher->Write(unitree_low_command);

8. 低级命令初始化 (InitLowCmd)

void RL_Real::InitLowCmd()
{
    this->unitree_low_command.head()[0] = 0xFE;
    this->unitree_low_command.head()[1] = 0xEF;
    this->unitree_low_command.level_flag() = 0xFF;
    this->unitree_low_command.gpio() = 0;

    for (int i = 0; i < 20; ++i)
    {
        this->unitree_low_command.motor_cmd()[i].mode() = (0x01);    // 伺服模式
        this->unitree_low_command.motor_cmd()[i].q() = (PosStopF);   // 位置停止
        this->unitree_low_command.motor_cmd()[i].kp() = (0);         // 位置增益
        this->unitree_low_command.motor_cmd()[i].dq() = (VelStopF);  // 速度停止
        this->unitree_low_command.motor_cmd()[i].kd() = (0);         // 速度增益
        this->unitree_low_command.motor_cmd()[i].tau() = (0);        // 力矩
    }
}

9. 数据流向图

手柄输入 ──┐
          │
IMU数据 ──┼──→ GetState() ──→ 状态向量 ──→ RunModel() ──→ 动作向量
          │                                    │
关节状态 ──┘                                    │
                                               ▼
                                         ComputeOutput()
                                               │
                                               ▼
                                         SetCommand() ──→ 机器人执行

10. 关键技术特点

10.1 实时性保证
  • 多线程架构：分离控制和推理逻辑
  • 非阻塞通信：DDS 异步消息传递
  • 优化推理：禁用梯度计算，固定线程数
10.2 安全机制
  • 力矩限制：防止关节过载
  • 姿态保护：防止机器人翻倒
  • 服务管理：确保独占控制权
10.3 适应性设计
  • 坐标系适配：支持不同仿真框架
  • 参数化配置：通过 YAML 文件灵活配置
  • 映射机制：处理仿真与真实机器人的差异

11. 主要数据结构

11.1 机器人状态
struct RobotState {
    struct {
        double quaternion[4];    // 四元数姿态
        double gyroscope[3];     // 角速度
    } imu;
    
    struct {
        double q[12];            // 关节位置
        double dq[12];           // 关节速度
        double tau_est[12];      // 估计力矩
    } motor_state;
};
11.2 机器人命令
struct RobotCommand {
    struct {
        double q[12];            // 目标位置
        double dq[12];           // 目标速度
        double kp[12];           // 位置增益
        double kd[12];           // 速度增益
        double tau[12];          // 前馈力矩
    } motor_command;
};

12. 系统优势

  1. 高实时性：1000Hz 控制频率保证精确控制
  2. 强鲁棒性：多重安全保护机制
  3. 易扩展性：模块化设计便于功能扩展
  4. 高效率：优化的神经网络推理
  5. 通用性：支持多种仿真到真实的迁移

13. 应用场景

  • 四足机器人运动控制
  • 强化学习算法验证
  • 仿真到真实的迁移学习
  • 机器人行为研究
  • 自主导航系统

这个系统代表了现代机器人控制技术的前沿水平，将深度强化学习与实时控制系统完美结合，为四足机器人的智能控制提供了完整的解决方案。

网站公告

今日签到

点亮在社区的每一天
去签到

热门文章

最新发布