4、Reeds-Shepp曲线（下）：代码实现、路径采样、碰撞检测、与A星搜索的结合方式

好，咱们接着上回聊。上节课我们把Reeds-Shepp曲线的数学原理和48种模式掰开揉碎讲了一遍，说实话，那堆公式看着确实有点头大。但别怕，今天咱们直接上代码，把理论落地。

我个人习惯是，先跑通一个最简单的demo，再慢慢往工程化方向靠。你想想看，如果连一条曲线都生成不出来，后面谈碰撞检测和A*融合都是空中楼阁。所以今天咱们就从最核心的代码实现开始。

4.1 核心代码实现：从公式到函数

Reeds-Shepp曲线的核心，说白了就是根据起点和终点的位姿（x, y, yaw），算出一系列路径点。我当年第一次实现的时候，照着论文抄公式，结果跑出来的路径车直接横着走……后来才发现是坐标系定义搞反了。

这里我给出一个精简但可用的Python实现。注意，为了教学清晰，我刻意省略了部分优化，保留了最核心的逻辑。

import numpy as np
import math

class ReedsSheppPath:
    """Reeds-Shepp路径生成器"""
    
    def __init__(self, turning_radius=1.0):
        self.radius = turning_radius
        self.step_size = 0.1  # 插值步长
        
    def generate_path(self, start, goal):
        """
        生成RS路径
        :param start: (x, y, yaw) 起点位姿
        :param goal: (x, y, yaw) 终点位姿
        :return: 路径点列表 [(x,y,yaw), ...]
        """
        # 1. 将起点平移到原点，朝向x轴正方向
        dx = goal[0] - start[0]
        dy = goal[1] - start[1]
        theta = math.atan2(dy, dx) - start[2]
        
        # 2. 计算相对坐标下的终点
        x = math.hypot(dx, dy) * math.cos(theta)
        y = math.hypot(dx, dy) * math.sin(theta)
        phi = goal[2] - start[2]
        
        # 3. 归一化到单位转弯半径
        x /= self.radius
        y /= self.radius
        
        # 4. 尝试所有48种模式，找到最短路径
        best_path = None
        best_length = float('inf')
        
        # 这里以最简单的LSL模式为例
        path = self._lsl(x, y, phi)
        if path and path['length'] < best_length:
            best_path = path
            best_length = path['length']
            
        # 实际工程中需要遍历所有模式
        # ... (省略其他模式)
        
        # 5. 将路径点转换回原始坐标系
        if best_path:
            return self._interpolate(best_path, start)
        return None
    
    def _lsl(self, x, y, phi):
        """LSL模式：左转-直行-左转"""
        # 几何计算（简化版）
        u, t = self._solve_lsl(x, y, phi)
        if u is None:
            return None
        return {
            'segments': [('L', t), ('S', u), ('L', t)],
            'length': 2*t + u
        }
    
    def _interpolate(self, path, start):
        """对路径段进行插值，生成密集点"""
        points = []
        x, y, yaw = start
        
        for seg_type, length in path['segments']:
            if seg_type == 'S':  # 直行
                for _ in np.arange(0, length, self.step_size):
                    x += self.step_size * math.cos(yaw)
                    y += self.step_size * math.sin(yaw)
                    points.append((x, y, yaw))
            elif seg_type == 'L':  # 左转
                for _ in np.arange(0, length, self.step_size):
                    yaw += self.step_size / self.radius
                    x += self.step_size * math.cos(yaw)
                    y += self.step_size * math.sin(yaw)
                    points.append((x, y, yaw))
            # 'R' 右转同理
        return points

4.2 路径采样：不是点越多越好

路径生成完了，接下来要采样。这里有个常见的误区——很多人觉得采样越密越好。其实不然。

为什么？你想想看，采样点太密，碰撞检测的计算量会爆炸。我见过一个项目，采样步长设了0.01米，结果一条10米的路径生成了1000个点，每次碰撞检测都要遍历一遍，帧率直接掉到个位数。

我个人经验是：步长取转弯半径的1/10到1/5。比如转弯半径2米，步长0.2~0.4米就够用了。泊车场景下，车身本身就有4-5米长，你采样再密也改变不了碰撞结果。

def sample_path(path, step_size=None):
    """智能采样路径点"""
    if step_size is None:
        step_size = path.radius * 0.2  # 默认取半径的1/5
    
    # 只在关键位置采样：转弯起始点、终点、以及直线段的中点
    key_points = []
    for i, seg in enumerate(path.segments):
        seg_type, length = seg
        # 每个段至少采3个点：起点、中点、终点
        num_points = max(3, int(length / step_size))
        for j in range(num_points):
            t = j / num_points
            point = path.interpolate_at(i, t)
            key_points.append(point)
    
    return key_points

4.3 碰撞检测：把车当成矩形来算

碰撞检测是路径规划里最绕不开的一环。说白了，就是检查车身的四个角有没有碰到障碍物。

我常用的方法是矩形包围盒法。把车辆近似成一个矩形，然后检查矩形与障碍物（通常也是矩形或圆形）是否有交集。这个方法简单粗暴，但够用。

def check_collision(vehicle_pose, obstacles, vehicle_length=4.5, vehicle_width=1.8):
    """
    碰撞检测
    :param vehicle_pose: (x, y, yaw) 车辆位姿
    :param obstacles: 障碍物列表，每个障碍物为 (x, y, width, height)
    :return: True表示碰撞，False表示安全
    """
    x, y, yaw = vehicle_pose
    
    # 计算车辆四个角点（相对于车辆中心）
    half_len = vehicle_length / 2
    half_wid = vehicle_width / 2
    
    corners_local = [
        (-half_len, -half_wid),  # 左后
        ( half_len, -half_wid),  # 右后
        ( half_len,  half_wid),  # 右前
        (-half_len,  half_wid)   # 左前
    ]
    
    # 旋转并平移到全局坐标
    corners_global = []
    for lx, ly in corners_local:
        gx = x + lx * math.cos(yaw) - ly * math.sin(yaw)
        gy = y + lx * math.sin(yaw) + ly * math.cos(yaw)
        corners_global.append((gx, gy))
    
    # 检查每个障碍物
    for obs in obstacles:
        ox, oy, ow, oh = obs
        # 简单的AABB碰撞检测
        for cx, cy in corners_global:
            if (ox - ow/2 <= cx <= ox + ow/2 and
                oy - oh/2 <= cy <= oy + oh/2):
                return True  # 碰撞！
    
    return False

4.4 与A*搜索的结合方式：RS曲线当“高速公路”

好，终于到了最核心的部分——怎么把Reeds-Shepp曲线和A*搜索结合起来？

我打个比方你就明白了。A*搜索就像是在城市里开车，一格一格地走网格，虽然能找到路径，但走出来的都是折线，车根本没法开。而RS曲线就像是高速公路，能让你从A点到B点一把方向打过去。

所以结合方式很自然：用A*做全局搜索，用RS曲线做局部连接。

具体做法是这样的：

1. A*搜索时，每隔N个节点，尝试用RS曲线直接连接当前节点和目标节点。
2. 如果RS曲线无碰撞，且长度小于A*剩余路径的估计值，就直接用RS曲线。
3. 否则，继续A*的网格扩展。

我当年做这个的时候，一开始是每扩展一个节点都尝试RS连接，结果A*几乎退化成了暴力搜索。后来改成每扩展5-10个节点尝试一次，效果好了很多。

def astar_with_rs(start, goal, grid, rs_generator):
    """结合RS曲线的A*搜索"""
    
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from = {}
    g_score = {start: 0}
    
    expansion_count = 0
    
    while not open_set.empty():
        current = open_set.get()[1]
        
        # 每隔5次扩展，尝试RS连接
        expansion_count += 1
        if expansion_count % 5 == 0:
            rs_path = rs_generator.generate_path(current, goal)
            if rs_path:
                # 检查RS路径是否无碰撞
                if is_path_collision_free(rs_path, grid):
                    # 找到路径！回溯并拼接
                    path = reconstruct_path(came_from, current)
                    path.extend(rs_path.points)
                    return path
        
        # 常规A*扩展
        for neighbor in get_neighbors(current, grid):
            tentative_g = g_score[current] + distance(current, neighbor)
            if tentative_g < g_score.get(neighbor, float('inf')):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score = tentative_g + heuristic(neighbor, goal)
                open_set.put((f_score, neighbor))
    
    return None  # 无解

4.5 实际工程中的几个关键参数

最后，我整理了一份参数配置表，这些都是我在实际项目中反复调出来的经验值，你可以直接拿去用。

参数名	推荐值	说明
转弯半径	车辆最小转弯半径 × 1.2	留20%余量，防止打满方向
RS连接间隔	5-10个A*节点	太频繁计算量大，太少容易错过最优解
碰撞检测步长	转弯半径的1/5	平衡精度和速度
RS模式优先级	LSL > RSR > LRL > 其他	优先尝试最简单的模式

嗯，到这里Reeds-Shepp曲线的代码实现部分就讲完了。说实话，RS曲线本身并不复杂，难的是怎么把它用好、用对。下一节课我们会讲如何把今天的内容整合到一个完整的泊车规划系统中，到时候你会看到这些代码是如何协同工作的。

如果你在实现过程中遇到问题，欢迎随时交流。记住，代码跑不通是常态，跑通了才是意外。别灰心，多调试几次就好了。

---

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