当前位置： 首页 > news >正文

rl_sar实现sim2real的整体思路

news 来源：原创 2025/6/8 14:37:39

文章目录

  • RL_Real_Go2 真实机器人控制系统详细解析
    • 概述
    • 系统架构图
    • 详细流程分析
      • 1. 系统初始化阶段
        • 1.1 参数配置加载
        • 1.2 坐标系适配
        • 1.3 机器人服务初始化
      • 2. 通信系统建立
        • 2.1 DDS 发布器创建
        • 2.2 DDS 订阅器创建
      • 3. 强化学习系统初始化
        • 3.1 PyTorch 环境配置
        • 3.2 历史观测缓冲区
        • 3.3 神经网络模型加载
      • 4. 多线程控制循环
        • 4.1 线程架构设计
        • 4.2 线程同步机制
      • 5. 状态获取系统 (GetState)
        • 5.1 手柄控制状态检测
        • 5.2 IMU 数据处理
        • 5.3 关节状态映射
      • 6. 强化学习推理系统 (RunModel)
        • 6.1 观测数据准备
        • 6.2 神经网络前向推理
        • 6.3 安全保护机制
      • 7. 命令执行系统 (SetCommand)
        • 7.1 命令映射和发送
        • 7.2 CRC 校验和发布
      • 8. 低级命令初始化 (InitLowCmd)
      • 9. 数据流向图
      • 10. 关键技术特点
        • 10.1 实时性保证
        • 10.2 安全机制
        • 10.3 适应性设计
      • 11. 主要数据结构
        • 11.1 机器人状态
        • 11.2 机器人命令
      • 12. 系统优势
      • 13. 应用场景

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]; // wstate->imu.quaternion[0] = this->unitree_low_state.imu_state().quaternion()[1]; // xstate->imu.quaternion[1] = this->unitree_low_state.imu_state().quaternion()[2]; // ystate->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. 应用场景

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

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

相关文章：

  • 按字典序排列最小的等效字符串
  • 第三章支线二 ·函数幻阶：语法召唤与逻辑封印
  • C#提取CAN ASC文件时间戳：实现与性能优化
  • ＜ 自用文 OS有关 新的JD云主机＞ 国内 京东云主机 2C4G 60G 5Mb 498/36月 Ubuntu22
  • 618来了，推荐京东云服务器
  • 如何构建船舵舵角和船的航向之间的动力学方程？它是一个一阶惯性环节吗？
  • python打卡第47天
  • ArcPy扩展模块的使用
  • 数控滑台技术革新：实现高效精密加工的全面探索
  • 【读论文】U-Net: Convolutional Networks for Biomedical Image Segmentation 卷积神经网络
  • 第二十七课：手搓梯度提升树
  • FreeRTOS任务调度过程vTaskStartScheduler()任务设计和划分
  • 推客系统小程序开发：告别低效推广，开启精准获客新时代
  • 本地windows服务器部署私有云网盘Nextcloud并无公网IP实现外部访问
  • go语言学习 第9章：映射（Map）
  • 数论总结，（模版与题解）
  • csrf攻击学习
  • PL0语法，分析器实现！
  • AIGC行业发展演进：从技术萌芽到智能革命
  • 0基础破解Typora，使用正版已激活Typora
  • 网站建设出现乱码是怎么回事/百度通用网址
  • 天津网站建设推广/武汉百度推广电话
  • 网站建设空间申请/网站免费搭建平台
  • 网站建设应注意什么/做竞价托管的公司
  • 动态网站做登录界面/南京seo推广
  • 做网站方案怎么写/短视频推广渠道