项目九：PyBullet+RRT避障规划

1. 项目背景与学习目标

RRT的核心思想

在高维空间中随机采样→向随机方向生长→碰撞检测→重复直到到达目标。优点：不需要显式建模障碍物、概率完备（给定足够时间必能找到解）、可处理高维度。

学习目标

• 在PyBullet中加载Franka Panda机械臂URDF
• 实现RRT和RRT*路径规划器（关节空间）
• 实现基于PyBullet/FCL的碰撞检测
• 对比RRT vs RRT*的路径质量和规划时间
• 理解仿真碰撞检测与真机安全距离的关系

2. 项目结构与环境

目录结构

rrt_collision_avoidance/
├── src/
│   ├── rrt_planner.py        # RRT和RRT*核心算法
│   ├── collision_checker.py  # PyBullet/FCL碰撞检测
│   ├── robot_env.py          # 机器人和障碍物环境
│   └── visualize.py          # 关节空间轨迹可视化
├── urdf/
│   └── franka_panda.urdf     # Franka Panda模型
├── tests/
│   ├── test_rrt.py
│   └── test_collision.py
├── results/
│   └── benchmark.json        # RRT vs RRT*性能对比
└── README.md

环境准备

pip install pybullet numpy matplotlib scipy

3. RRT*核心实现

import numpy as np
from collections import namedtuple
import pybullet as p

Node = namedtuple('Node', ['q', 'parent', 'cost'])

class RRTStar:
    """RRT* — 渐进最优的路径规划"""
    
    def __init__(self, start, goal, joint_limits, collision_fn,
                 step_size=0.05, max_iter=5000, search_radius=0.5, gamma=1.0):
        self.start = Node(np.array(start), None, 0.0)
        self.goal = np.array(goal)
        self.limits = np.array(joint_limits)
        self.collision_fn = collision_fn
        self.step_size = step_size
        self.max_iter = max_iter
        self.search_radius = search_radius
        self.n_dims = len(start)
        self.gamma = gamma
        
        self.nodes = [self.start]
        self.goal_node = None
    
    def plan(self):
        """主规划循环"""
        for iteration in range(self.max_iter):
            # 1. 随机采样（10%概率直接采样goal——加速收敛）
            if np.random.random() < 0.1:
                q_rand = self.goal
            else:
                q_rand = np.random.uniform(self.limits[:, 0], self.limits[:, 1])
            
            # 2. 找最近节点
            nearest_idx, nearest_node = self._nearest(q_rand)
            
            # 3. 向随机方向生长一步
            direction = q_rand - nearest_node.q
            dist = np.linalg.norm(direction)
            if dist < 1e-6:
                continue
            direction = direction / dist * min(dist, self.step_size)
            q_new = nearest_node.q + direction
            
            # 4. 碰撞检测
            if not self.collision_fn(nearest_node.q, q_new):
                continue
            
            # 5. 找邻域内最优父节点（RRT*核心）
            near_indices = self._near(q_new)
            best_parent = nearest_idx
            best_cost = nearest_node.cost + dist
            
            for idx in near_indices:
                potential_parent = self.nodes[idx]
                pot_dist = np.linalg.norm(q_new - potential_parent.q)
                pot_cost = potential_parent.cost + pot_dist
                if pot_cost < best_cost and self.collision_fn(potential_parent.q, q_new):
                    best_parent = idx
                    best_cost = pot_cost
            
            new_node = Node(q_new, best_parent, best_cost)
            self.nodes.append(new_node)
            new_idx = len(self.nodes) - 1
            
            # 6. 重新连接邻域节点（RRT* rewiring）
            for idx in near_indices:
                near_node = self.nodes[idx]
                dist_to_new = np.linalg.norm(near_node.q - q_new)
                if new_node.cost + dist_to_new < near_node.cost:
                    if self.collision_fn(q_new, near_node.q):
                        self.nodes[idx] = Node(near_node.q, new_idx, new_node.cost + dist_to_new)
            
            # 7. 检查是否到达目标
            dist_to_goal = np.linalg.norm(q_new - self.goal)
            if dist_to_goal < self.step_size:
                if self.collision_fn(q_new, self.goal):
                    self.goal_node = Node(self.goal, new_idx, new_node.cost + dist_to_goal)
                    return self._extract_path()
        
        return None  # 未找到路径
    
    def _nearest(self, q):
        """kd-tree最近邻搜索（加速版用scipy.spatial.cKDTree）"""
        distances = [np.linalg.norm(node.q - q) for node in self.nodes]
        idx = np.argmin(distances)
        return idx, self.nodes[idx]
    
    def _near(self, q):
        """找搜索半径内的所有节点"""
        # 动态搜索半径（RRT*理论）
        n = len(self.nodes)
        radius = min(self.search_radius,
                       self.gamma * (np.log(n) / n) ** (1 / self.n_dims))
        
        near = []
        for i, node in enumerate(self.nodes):
            if np.linalg.norm(node.q - q) < radius:
                near.append(i)
        return near
    
    def _extract_path(self):
        """回溯构建最终路径"""
        path = []
        node = self.goal_node
        while node is not None:
            path.append(node.q)
            node = self.nodes[node.parent] if node.parent is not None else None
        path.reverse()
        return np.array(path)

4. 碰撞检测实现

class PyBulletCollisionChecker:
    """基于PyBullet的机器人自碰撞+环境碰撞检测"""
    
    def __init__(self, robot_id, obstacle_ids, 
                 self_collision_pairs=None, safety_margin=0.01):
        self.robot_id = robot_id
        self.obstacle_ids = obstacle_ids
        self.safety_margin = safety_margin
        
        # 自碰撞对：排除相邻连杆
        self.self_collision_pairs = self_collision_pairs or []
    
    def is_collision_free(self, q1, q2, n_interpolate=10):
        """检查q1→q2路径上是否有碰撞（插值检测）"""
        for alpha in np.linspace(0, 1, n_interpolate):
            q = q1 + alpha * (q2 - q1)
            if not self._check_single(q):
                return False
        return True
    
    def _check_single(self, q):
        """检查单个构型是否无碰撞"""
        # 设置机器人关节位置
        for i, angle in enumerate(q):
            p.resetJointState(self.robot_id, i, angle)
        
        # 检查自碰撞
        for link_a, link_b in self.self_collision_pairs:
            contacts = p.getClosestPoints(
                bodyA=self.robot_id, bodyB=self.robot_id,
                linkIndexA=link_a, linkIndexB=link_b,
                distance=self.safety_margin)
            if len(contacts) > 0:
                return False
        
        # 检查环境碰撞
        for obs_id in self.obstacle_ids:
            contacts = p.getClosestPoints(
                bodyA=self.robot_id, bodyB=obs_id,
                distance=self.safety_margin)
            if len(contacts) > 0:
                return False
        
        return True

5. 性能对比与可视化

def benchmark(planner_class, start, goal, n_trials=20):
    """RRT vs RRT* 性能对比"""
    results = {'planning_time': [], 'path_length': [], 'success_rate': 0}
    
    for _ in range(n_trials):
        t0 = time.time()
        planner = planner_class(start, goal, joint_limits, collision_fn)
        path = planner.plan()
        t1 = time.time()
        
        if path is not None:
            results['planning_time'].append(t1 - t0)
            results['path_length'].append(compute_path_length(path))
            results['success_rate'] += 1
    
    results['success_rate'] /= n_trials
    print(f"成功率: {results['success_rate']*100:.0f}%")
    print(f"平均规划时间: {np.mean(results['planning_time'])*1000:.0f}ms")
    print(f"平均路径长度: {np.mean(results['path_length']):.2f} rad")
    return results

6. 关节空间轨迹平滑化与时间参数化

从RRT路径到可执行轨迹

RRT输出的路径是关节空间中的一系列路标点。但机器人不能瞬间从路标点跳到下一个——需要时间参数化的平滑轨迹（位置、速度、加速度都连续）。

三次样条插值

from scipy.interpolate import CubicSpline

def smooth_trajectory(waypoints, n_interp=100, v_max=3.14, a_max=10.0):
    """将路标点平滑为时间参数化轨迹，含速度/加速度约束"""
    n_waypoints = len(waypoints)
    n_joints = waypoints.shape[1]
    
    # 1. 用弧长参数化（而非等时间间隔）
    segment_lengths = np.linalg.norm(np.diff(waypoints, axis=0), axis=1)
    cumulative_s = np.concatenate([[0], np.cumsum(segment_lengths)])
    
    # 2. 计算每个segment的最小时间（受速度限制）
    segment_times = segment_lengths / v_max
    time_points = np.concatenate([[0], np.cumsum(segment_times)])
    
    # 3. 三次样条插值生成密集轨迹
    t_dense = np.linspace(0, time_points[-1], n_interp)
    trajectory = np.zeros((n_interp, n_joints))
    
    for j in range(n_joints):
        cs = CubicSpline(time_points, waypoints[:, j], bc_type='natural')
        trajectory[:, j] = cs(t_dense)
    
    # 4. 验证速度/加速度约束
    dt = t_dense[1] - t_dense[0]
    velocities = np.diff(trajectory, axis=0) / dt
    accelerations = np.diff(velocities, axis=0) / dt
    
    if np.any(np.abs(velocities) > v_max):
        print(f"⚠️ 速度超限: scale factor needed")
    if np.any(np.abs(accelerations) > a_max):
        print(f"⚠️ 加速度超限: 考虑延长轨迹时间")
    
    return t_dense, trajectory

仿真执行——Trajectory Following

def execute_trajectory(robot_id, trajectory, joint_indices, dt=0.01):
    """在PyBullet中执行平滑轨迹"""
    for q_target in trajectory:
        for i, joint_idx in enumerate(joint_indices):
            p.setJointMotorControl2(
                bodyUniqueId=robot_id,
                jointIndex=joint_idx,
                controlMode=p.POSITION_CONTROL,
                targetPosition=q_target[i],
                maxVelocity=5.0,
                force=100
            )
        p.stepSimulation()
        time.sleep(dt)

7. Sim2Real考量——从仿真碰撞到真机安全

真机安全距离模型

safety_distance = 几何误差 + 控制误差 + 感知误差 + 人工裕量
                 = 2mm     + 1mm      + 3mm       + 5mm
                 = 11mm  (小于机器人有效载荷下推荐值)