第4章仿真环境搭建

4. 仿真环境搭建：基于PyTorch的机器人控制仿真平台

说实话，做机器人控制算法最怕什么？不是算法本身难，而是你写好了代码，却不敢往真机上跑。一跑就是几千块的维修费，搞不好还把电机烧了。所以我个人习惯，先搭一个靠谱的仿真环境，把算法跑通了再说。

这一章，我们就来搭建一个基于PyTorch的机器人控制仿真平台。别担心，不是那种动不动就上ROS、Gazebo的重型方案。我们要的是一个轻量、灵活、能跟PyTorch无缝对接的仿真环境。

4.1 为什么选择PyTorch做仿真？

你可能会问：仿真不是该用物理引擎吗？用PyTorch能行？

嗯，这里要说明一下。传统的仿真器（比如MuJoCo、PyBullet）确实擅长物理模拟，但它们跟深度学习框架的交互往往很别扭。你得把数据从仿真器里导出来，再喂给PyTorch，训练完再把参数传回去。一来一回，效率低不说，还容易出bug。

我在项目中遇到过这种情况：用PyBullet做仿真，训练一个强化学习控制器，结果仿真器里跑得挺好，一部署到真机上就抖得跟筛子似的。后来排查了半天，发现是仿真器的时间步长和PyTorch的更新频率没对齐。从那以后，我就开始用PyTorch自己搭仿真环境了。

说白了，用PyTorch做仿真有这几个好处：

原生GPU加速：批量仿真、并行计算，直接利用PyTorch的自动微分
端到端可微：整个仿真流程可以求导，方便做基于梯度的优化
灵活可控：想加什么物理模型就加什么，不受第三方库限制
调试方便：所有变量都在PyTorch的Tensor里，随时可以打印、可视化

4.2 仿真平台的整体架构

我们先来看一下这个仿真平台长什么样。下面这张图是我自己画的架构图，你看一眼就能明白整个流程。

这个架构其实不复杂。核心就三个模块：控制器、机器人模型和状态更新。控制器输出控制量，机器人模型根据控制量计算加速度，状态更新模块做积分得到新的状态。然后这个新状态又反馈给控制器，形成闭环。

你想想看，这不就是真实机器人的工作方式吗？只不过我们把物理世界换成了PyTorch里的张量运算。

4.3 核心代码实现

好，理论说完了，我们直接上代码。我习惯把仿真环境封装成一个类，这样用起来方便。

import torch
import torch.nn as nn
import numpy as np
import matplotlib.pyplot as plt
from typing import Tuple, Optional

class RobotSimulator:
    """
    基于PyTorch的机器人控制仿真平台
    支持批量仿真、GPU加速、自动微分
    """
    
    def __init__(
        self,
        dt: float = 0.01,           # 仿真步长 (秒)
        mass: float = 1.0,           # 质量 (kg)
        damping: float = 0.1,        # 阻尼系数
        device: str = 'cpu'          # 计算设备
    ):
        self.dt = dt
        self.mass = mass
        self.damping = damping
        self.device = device
        
        # 状态变量：位置、速度
        self.pos = torch.tensor(0.0, device=device)
        self.vel = torch.tensor(0.0, device=device)
        
        # 记录历史数据
        self.history = {
            'time': [],
            'pos': [],
            'vel': [],
            'force': []
        }
        
        self._time = 0.0
    
    def reset(
        self,
        pos: float = 0.0,
        vel: float = 0.0
    ) -> torch.Tensor:
        """重置仿真环境到初始状态"""
        self.pos = torch.tensor(pos, device=self.device)
        self.vel = torch.tensor(vel, device=self.device)
        self._time = 0.0
        
        # 清空历史记录
        for key in self.history:
            self.history[key].clear()
        
        return self._get_observation()
    
    def _get_observation(self) -> torch.Tensor:
        """获取当前观测状态"""
        return torch.stack([self.pos, self.vel])
    
    def _dynamics(
        self,
        pos: torch.Tensor,
        vel: torch.Tensor,
        force: torch.Tensor
    ) -> Tuple[torch.Tensor, torch.Tensor]:
        """
        机器人动力学模型
        这里用最简单的弹簧-阻尼系统做演示
        实际项目中可以替换成更复杂的模型
        """
        # 弹簧力 (假设弹簧系数 k=1.0)
        spring_force = -1.0 * pos
        
        # 阻尼力
        damping_force = -self.damping * vel
        
        # 总合力
        total_force = force + spring_force + damping_force
        
        # 加速度 (F = ma)
        acc = total_force / self.mass
        
        return vel, acc
    
    def step(
        self,
        force: torch.Tensor
    ) -> Tuple[torch.Tensor, float, bool]:
        """
        执行一步仿真
        返回: (观测, 奖励, 是否终止)
        """
        # 计算动力学
        vel_dot, acc = self._dynamics(self.pos, self.vel, force)
        
        # 半隐式欧拉积分 (比显式欧拉更稳定)
        self.vel = self.vel + acc * self.dt
        self.pos = self.pos + self.vel * self.dt
        
        # 更新时间
        self._time += self.dt
        
        # 记录历史
        self.history['time'].append(self._time)
        self.history['pos'].append(self.pos.item())
        self.history['vel'].append(self.vel.item())
        self.history['force'].append(force.item())
        
        # 计算奖励 (这里用位置误差的负值)
        reward = -torch.abs(self.pos).item()
        
        # 判断是否终止 (位置超出边界)
        done = torch.abs(self.pos) > 5.0
        
        return self._get_observation(), reward, done.item()
    
    def render(self):
        """可视化仿真结果"""
        fig, axes = plt.subplots(3, 1, figsize=(10, 8))
        
        time = self.history['time']
        
        axes[0].plot(time, self.history['pos'], 'b-', linewidth=2)
        axes[0].set_ylabel('位置 (m)')
        axes[0].grid(True, alpha=0.3)
        
        axes[1].plot(time, self.history['vel'], 'r-', linewidth=2)
        axes[1].set_ylabel('速度 (m/s)')
        axes[1].grid(True, alpha=0.3)
        
        axes[2].plot(time, self.history['force'], 'g-', linewidth=2)
        axes[2].set_ylabel('控制力 (N)')
        axes[2].set_xlabel('时间 (s)')
        axes[2].grid(True, alpha=0.3)
        
        plt.tight_layout()
        plt.show()

核心设计思路：

所有状态变量都是PyTorch的Tensor，天然支持GPU和自动微分
动力学模型单独抽出来，方便替换成更复杂的模型
积分器用了半隐式欧拉，比显式欧拉稳定得多
历史记录功能方便后续分析和可视化

4.4 实战：用PID控制器测试仿真环境

仿真环境搭好了，总得跑个例子验证一下。我选最简单的PID控制器，看看能不能让机器人稳定在目标位置。

class PIDController:
    """简单的PID控制器"""
    
    def __init__(self, kp=10.0, ki=0.0, kd=1.0):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral = 0.0
        self.prev_error = 0.0
    
    def compute(self, error, dt):
        self.integral += error * dt
        derivative = (error - self.prev_error) / dt
        self.prev_error = error
        
        return self.kp * error + self.ki * self.integral + self.kd * derivative


# 创建仿真环境
sim = RobotSimulator(dt=0.01, mass=1.0, damping=0.5)

# 创建PID控制器
pid = PIDController(kp=10.0, ki=0.5, kd=2.0)

# 目标位置
target_pos = 2.0

# 重置环境
obs = sim.reset()

# 运行仿真
for i in range(1000):
    # 计算误差
    error = target_pos - obs[0].item()
    
    # PID计算控制力
    force = pid.compute(error, sim.dt)
    
    # 执行一步仿真
    obs, reward, done = sim.step(torch.tensor(force))
    
    if done:
        print(f"仿真在第{i}步终止")
        break

# 可视化结果
sim.render()

跑完这个例子，你会看到位置曲线慢慢靠近目标值2.0，速度曲线先增大后减小，控制力也逐渐收敛到零。这说明我们的仿真环境是正常的，PID控制器也能正常工作。

我的经验：

刚开始做仿真时，我总喜欢把步长设得很小，觉得这样更精确。后来发现，步长太小反而容易出问题——数值误差累积，仿真跑着跑着就飘了。一般取0.01秒就够用，除非你仿真的是高频系统。

4.5 批量仿真与GPU加速

做强化学习或者参数优化的时候，经常需要同时跑很多个仿真。PyTorch的向量化操作正好派上用场。

class BatchRobotSimulator:
    """
    批量仿真版本
    同时仿真多个机器人，共享计算资源
    """
    
    def __init__(self, batch_size=64, dt=0.01, device='cuda'):
        self.batch_size = batch_size
        self.dt = dt
        self.device = device
        
        # 批量状态
        self.pos = torch.zeros(batch_size, device=device)
        self.vel = torch.zeros(batch_size, device=device)
        
        # 物理参数 (每个机器人可以不同)
        self.mass = torch.ones(batch_size, device=device)
        self.damping = 0.1 * torch.ones(batch_size, device=device)
    
    def step_batch(self, forces):
        """批量执行一步仿真"""
        # 动力学计算 (全部向量化)
        spring_force = -1.0 * self.pos
        damping_force = -self.damping * self.vel
        total_force = forces + spring_force + damping_force
        acc = total_force / self.mass
        
        # 半隐式欧拉积分
        self.vel = self.vel + acc * self.dt
        self.pos = self.pos + self.vel * self.dt
        
        return torch.stack([self.pos, self.vel], dim=1)


# 使用示例
batch_sim = BatchRobotSimulator(batch_size=256, device='cuda')

# 随机生成控制力
forces = torch.randn(256, device='cuda') * 5.0

# 批量仿真一步
observations = batch_sim.step_batch(forces)
print(f"批量观测形状: {observations.shape}")  # [256, 2]

注意：

批量仿真时，如果每个机器人的物理参数不一样，一定要确保参数是Tensor格式，并且跟状态在同一个设备上。我曾经犯过一个低级错误：参数用numpy数组，状态用PyTorch Tensor，结果类型转换浪费了大量时间。

4.6 仿真平台的扩展性

我们这个仿真平台虽然简单，但扩展性很强。你可以往里面加各种东西：

扩展方向	实现方式	典型应用
多关节机器人	用矩阵运算代替标量运算	机械臂控制、人形机器人
接触力模型	添加弹簧-阻尼接触模型	抓取、行走、跳跃
传感器噪声	在观测上叠加高斯噪声	鲁棒控制、状态估计
时变参数	让质量、阻尼随时间变化	自适应控制、系统辨识
多智能体	扩展成多个机器人交互	编队控制、协同操作

我个人建议，刚开始不要搞太复杂。先把单关节的仿真跑通，理解整个流程，然后再逐步扩展。我在项目里就是这么干的——先在一个关节上验证算法，没问题了再推广到全身。

4.7 避坑指南

最后，分享几个我踩过的坑：

积分器选择：显式欧拉虽然简单，但遇到刚性系统容易发散。我一般用半隐式欧拉或者RK4，稳定性好很多。
数值精度：PyTorch默认是float32，对于长时间仿真可能精度不够。如果发现仿真结果异常，试试换成float64。
梯度流：如果要用自动微分做优化，记得在不需要梯度的操作上加上torch.no_grad()，否则计算图会越来越大，内存爆炸。
随机种子：做对比实验时，一定要固定随机种子。否则同样的算法跑两次结果不一样，你都不知道是算法问题还是随机性导致的。

嗯，这一章的内容就到这里。仿真环境搭好了，下一章我们就可以在这个平台上跑各种控制算法了。记住，仿真只是手段，不是目的。最终我们还是要回到真机上，但有了这个仿真平台，你在真机上翻车的概率会小很多。

公众号:蓝海资料掘金营，微信deep3321