机器人仿真与碰撞检测

1. PyBullet物理仿真入门

import pybullet as p, pybullet_data, time
p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
plane = p.loadURDF("plane.urdf")
robot = p.loadURDF("franka_panda/panda.urdf", useFixedBase=True)
for t in range(1000):
    targets = [0.5 * np.sin(t * 0.02) for _ in range(7)]
    for i, tgt in enumerate(targets):
        p.setJointMotorControl2(robot, i, p.POSITION_CONTROL, targetPosition=tgt)
    p.stepSimulation()
    time.sleep(1/240)

2. 碰撞检测与RRT路径规划

RRT快速扩展随机树

从起点随机采样，逐步构建探索树，每次向随机方向生长，直到触及目标区域。

class RRTPlanner:
    def plan(self, start, goal, max_iters=5000, step=0.1):
        tree = [(start, None)]
        for _ in range(max_iters):
            q_rand = self.sample_free()
            nearest_idx = self.nearest(tree, q_rand)
            q_near = tree[nearest_idx][0]
            q_new = q_near + step * (q_rand - q_near) / np.linalg.norm(q_rand - q_near)
            if self.collision_free(q_near, q_new):
                tree.append((q_new, nearest_idx))
                if np.linalg.norm(q_new - goal) < step:
                    return self.extract_path(tree, len(tree)-1)
        return None

3. Domain Randomization 域随机化

为什么需要域随机化

仿真中训练的策略放到真机上往往表现差。解决思路：在仿真中故意添加随机扰动——让策略学会适应不确定性，而不是记忆精确的仿真参数。这与你做公差分析时的"最坏情况分析"完全一样。

可随机的参数

def randomize_env(env):
    """每次episode开始时随机化环境参数"""
    # 视觉域随机化
    env.lighting_intensity = np.random.uniform(0.6, 1.4)
    env.camera_pos += np.random.normal(0, 0.005, 3)
    # 动力学域随机化
    env.arm_mass = 3.5 * np.random.uniform(0.9, 1.1)
    env.joint_friction = 0.01 * np.random.uniform(0.8, 1.2)
    # 几何域随机化
    env.link_length *= np.random.uniform(0.995, 1.005)  # ±0.5% = ±2mm on 400mm link
    return env

4. Gazebo仿真集成

Gazebo vs PyBullet

# 启动Gazebo + URDF机械臂
# ros2 launch my_robot gazebo.launch.py
from ros_gz_bridge import RosGzBridge

# ROS 2 ↔ Gazebo 话题桥接
bridge = RosGzBridge()
bridge.create_bridge('/joint_states', 'sensor_msgs/msg/JointState',
                    '/world/default/model/robot/joint_state',
                    'gz.msgs.Model')

5. 仿真引擎深度对比——MuJoCo vs PyBullet vs Gazebo vs Isaac Sim

四款主流仿真引擎的核心差异

为什么RL社区偏爱MuJoCo

MuJoCo的接触模型基于凸优化而非传统LCP（线性互补问题），这使得：

• 速度快：单个模拟步长比PyBullet快3-5倍（批量化时差距更大）
• 梯度平滑：接触力对状态的偏导数存在且连续——这对基于梯度的RL算法（PPO、SAC）至关重要
• 确定性：相同输入严格产生相同输出——可复现的实验结果

MuJoCo上手：从URDF到训练环境

import mujoco
import mujoco.viewer
import numpy as np

# 加载URDF模型（MuJoCo 3.0+原生支持URDF）
model = mujoco.MjModel.from_xml_path("robot.urdf")
data = mujoco.MjData(model)

# 仿真循环
with mujoco.viewer.launch_passive(model, data) as viewer:
    for step in range(1000):
        # PD控制器示例
        target_pos = np.sin(step * 0.01) * 0.5
        data.ctrl[:] = 100 * (target_pos - data.qpos) - 10 * data.qvel
        
        mujoco.mj_step(model, data)
        viewer.sync()

Gymnasium环境封装——标准RL接口

import gymnasium as gym
from gymnasium import spaces

class MuJoCoRobotEnv(gym.Env):
    """标准Gymnasium环境，兼容所有RL算法库（Stable-Baselines3, RLlib等）"""
    
    def __init__(self, xml_path="robot.xml", render_mode="rgb_array"):
        super().__init__()
        self.model = mujoco.MjModel.from_xml_path(xml_path)
        self.data = mujoco.MjData(self.model)
        self.render_mode = render_mode
        
        # 定义动作空间：关节力矩（连续）
        self.action_space = spaces.Box(
            low=-1.0, high=1.0,
            shape=(self.model.nu,), dtype=np.float32)
        
        # 定义观测空间：关节位置+速度+末端位姿
        self.observation_space = spaces.Box(
            low=-np.inf, high=np.inf,
            shape=(self.model.nq + self.model.nv + 6,), dtype=np.float32)
    
    def step(self, action):
        self.data.ctrl[:] = action
        mujoco.mj_step(self.model, self.data)
        
        obs = self._get_obs()
        reward = self._compute_reward()
        terminated = self._is_terminated()
        
        return obs, reward, terminated, False, {}
    
    def _get_obs(self):
        # 关节位置 + 速度 + 末端位姿
        ee_pos = self.data.body("end_effector").xpos
        ee_quat = self.data.body("end_effector").xquat
        return np.concatenate([
            self.data.qpos, self.data.qvel, ee_pos, ee_quat
        ]).astype(np.float32)

6. 仿真中的传感器建模

深度相机仿真

仿真中的深度相机不是简单的射线投射——必须建模传感器噪声、遮挡、材质反射率。

def simulate_depth_camera(physics_client, camera_pos, camera_orn, width=640, height=480, fov=60):
    """PyBullet中模拟RGB-D相机"""
    import pybullet as p
    
    view_matrix = p.computeViewMatrix(camera_pos, camera_orn)
    proj_matrix = p.computeProjectionMatrixFOV(
        fov=fov, aspect=width/height, nearVal=0.05, farVal=5.0)
    
    # 获取RGB + 深度 + 分割
    w, h, rgb, depth, seg = p.getCameraImage(
        width, height, view_matrix, proj_matrix,
        renderer=p.ER_BULLET_HARDWARE_OPENGL)
    
    # 深度值从OpenGL深度缓冲转换到实际距离（米）
    far, near = 5.0, 0.05
    depth_meters = far * near / (far - (far - near) * depth)
    
    # 添加仿真传感器噪声（高斯噪声）
    noise_scale = 0.001  # 1mm标准差（模拟RealSense精度）
    depth_meters += np.random.normal(0, noise_scale, depth_meters.shape)
    
    # 模拟无效深度（超出量程返回0）
    depth_meters[depth_meters > far] = 0.0
    depth_meters[depth_meters < near] = 0.0
    
    return rgb, depth_meters, seg

LiDAR仿真

def simulate_lidar(physics_client, robot_pos, num_rays=360, range_max=10.0):
    """模拟2D激光雷达——射线投射"""
    angles = np.linspace(0, 2*np.pi, num_rays, endpoint=False)
    ranges = np.zeros(num_rays)
    
    for i, angle in enumerate(angles):
        ray_from = robot_pos[:2]
        ray_to = ray_from + range_max * np.array([np.cos(angle), np.sin(angle)])
        
        results = p.rayTest(ray_from, ray_to)
        if results[0][0] != -1:  # 有命中
            hit_fraction = results[0][2]
            ranges[i] = hit_fraction * range_max
        else:
            ranges[i] = range_max
    
    return angles, ranges

7. 接触动力学——仿真中最难的部分

接触模型的核心参数

# PyBullet中配置接触参数
p.changeDynamics(
    robot_id, link_index,
    lateralFriction=0.5,            # μ_d (动摩擦)
    spinningFriction=0.01,           # 旋转摩擦
    rollingFriction=0.001,          # 滚动摩擦
    restitution=0.1,                # ε (恢复系数)
    contactStiffness=50000,         # k_p (N/m)
    contactDamping=1000,           # k_d (Ns/m)
)

# 物体间独立的摩擦系数对
p.setCollisionFilterPair(robot_id, object_id, link_a, link_b, enableCollision=1)
# 对robot-link_a与object-link_b这对使用自定义接触参数

为什么仿真中的接触力会"爆炸"

接触刚度过高→小穿透产生大接触力→数值不稳定→物体飞出去。你的FEA经验知道：单元太硬→矩阵病态→不收敛。完全一样的问题。

• 解法1：降低接触刚度+提高仿真频率（1/Δt > 接触频率的10倍）
• 解法2：使用隐式积分器（Gazebo的DART/MuJoCo）替代显式欧拉法
• 解法3：在接触对之间添加"软约束"（弹簧-阻尼模型）

8. Domain Randomization消融实验设计与Sim2Real

系统性消融实验——找到哪个随机化真正有用

不是所有随机化都对Sim2Real有帮助。有些可能引入过多噪声反而降低性能。你需要系统地测试每种随机化：

class AblationStudy:
    def __init__(self, env_factory, policy, n_eval=100):
        self.env_factory = env_factory
        self.policy = policy
        self.n_eval = n_eval
    
    def run_ablation(self):
        results = {}
        
        # A: 无随机化（仿真上界）
        env_sim = self.env_factory(randomization='none')
        results['A_sim_upper_bound'] = self.eval(env_sim)
        
        # B: 仅视觉随机化
        env_visual = self.env_factory(randomization='visual')
        results['B_visual_only'] = self.eval(env_visual)
        
        # C: 仅动力学随机化
        env_dynamics = self.env_factory(randomization='dynamics')
        results['C_dynamics_only'] = self.eval(env_dynamics)
        
        # ... D, E, F, G ...
        
        return results
    
    def eval(self, env):
        successes = 0
        for _ in range(self.n_eval):
            obs = env.reset()
            done = False
            while not done:
                action = self.policy(obs)
                obs, reward, done, info = env.step(action)
                if info.get('success'):
                    successes += 1
                    break
        return successes / self.n_eval

真实世界的Sim2Real迁移——你的机械经验优势

作为机械设计工程师，你做Sim2Real有天然优势：

• 极限/间隙分析 → 仿真几何参数的扰动范围（名义值±公差）
• FEA材料参数 → 仿真中的质量/摩擦/刚度不确定性
• 振动模态分析 → 理解刚体仿真的局限性（没有柔性效应）