4、复杂路径生成：多段路径拼接、动态障碍物避让、路径跟随与预判、速度曲线规划

好，咱们进入第四章。说实话，这一章才是真正考验功力的地方。

前面几章我们聊了基础路径、贝塞尔曲线、样条插值，那些都是「单段」路径。但真实项目里，哪有这么简单？你想想看，一个机器人要从A点走到B点，中间可能要先直走、再拐弯、绕过一张桌子、最后精准停在充电桩前——这就是多段路径拼接。再加上动态障碍物突然出现，你还得实时避让。嗯，这一章我把压箱底的经验都掏出来。

4.1 多段路径拼接：别让衔接处「卡顿」

多段路径拼接，说白了就是把几段不同性质的路径粘在一起。比如一段直线接一段圆弧，再接一段贝塞尔曲线。

但问题来了——衔接处如果不做处理，速度和方向会突变。我在项目中遇到过一台AGV，直线转圆弧时突然「点头」，把货架上的零件晃下来一半。后来排查发现，就是拼接点处的切向量不连续。

具体怎么做？我习惯用「重叠过渡法」：

1. 让前一段路径的终点和后一段路径的起点有 5%-10% 的重叠区间
2. 在重叠区间内做加权平均，权重从 1→0 和 0→1 渐变
3. 这样拼接出来的路径，肉眼几乎看不出接缝

def smooth_join(path1, path2, overlap_ratio=0.08):
    """
    多段路径平滑拼接
    path1, path2: 每段路径的点列表 [(x,y), ...]
    """
    n1 = len(path1)
    n2 = len(path2)
    overlap_len = int(min(n1, n2) * overlap_ratio)
    
    # 取重叠区间
    tail = path1[-overlap_len:]
    head = path2[:overlap_len]
    
    # 加权融合
    joined = path1[:-overlap_len]  # 前段非重叠部分
    for i in range(overlap_len):
        w = i / overlap_len  # 权重从0到1
        blended_x = tail[i][0] * (1-w) + head[i][0] * w
        blended_y = tail[i][1] * (1-w) + head[i][1] * w
        joined.append((blended_x, blended_y))
    joined.extend(path2[overlap_len:])  # 后段非重叠部分
    return joined

4.2 动态障碍物避让：别撞上，也别绕远路

动态障碍物避让，这是做路径规划最头疼的部分。静态障碍物好办，画个地图标出来就行。但动态的——比如突然走过来的人、另一台移动的机器人——你得实时响应。

我曾经在仓库项目里吃过亏。当时用的纯反应式避让，就是检测到障碍物就原地停下，等障碍物走了再继续。结果呢？两台机器人在走廊里「互相让路」，卡了整整三分钟。

后来我改用「预测+局部重规划」的策略：

• 预测阶段：用卡尔曼滤波或简单的线性外推，估算障碍物未来2-3秒的位置
• 决策阶段：如果预测路径会碰撞，才触发避让；否则保持原路径
• 重规划阶段：只在障碍物周围局部区域重新生成路径，不动全局路径

def dynamic_avoid(current_pos, target_pos, obstacles, horizon=2.0):
    """
    动态障碍物避让
    obstacles: 每个障碍物包含位置、速度、预测轨迹
    """
    # 1. 预测障碍物未来位置
    predicted_collisions = []
    for obs in obstacles:
        future_pos = obs['pos'] + obs['vel'] * horizon
        if distance(current_pos, future_pos) < SAFE_DIST:
            predicted_collisions.append(obs)
    
    # 2. 无碰撞风险，直接走原路径
    if not predicted_collisions:
        return plan_direct_path(current_pos, target_pos)
    
    # 3. 有碰撞风险，局部绕行
    # 在障碍物周围生成一个「安全走廊」
    safe_corridor = generate_safe_corridor(current_pos, target_pos, predicted_collisions)
    local_path = plan_within_corridor(current_pos, target_pos, safe_corridor)
    return local_path

4.3 路径跟随与预判：别让机器人「蛇形走位」

路径规划好了，但机器人能不能「跟住」这条路径，是另一回事。

你想想看，如果机器人只是盯着当前目标点走，到了再换下一个点——那走出来的轨迹就是折线，而不是平滑曲线。这就是为什么很多入门级的机器人走起来像喝醉了酒。

我常用的方法是「纯追踪法（Pure Pursuit）」加上预判：

• 不是看当前最近的点，而是看路径前方一个「预瞄距离」处的点
• 根据当前速度和转向角，计算到达预瞄点需要的转向指令
• 预瞄距离随速度动态调整——速度快时看远点，速度慢时看近点

def pure_pursuit_control(robot_pos, robot_yaw, path, lookahead_dist):
    """
    纯追踪法路径跟随
    """
    # 找到路径上距离预瞄点最近的点
    target = find_lookahead_point(path, robot_pos, lookahead_dist)
    
    # 计算目标转向角
    dx = target[0] - robot_pos[0]
    dy = target[1] - robot_pos[1]
    target_yaw = atan2(dy, dx)
    
    # 转向误差
    yaw_error = normalize_angle(target_yaw - robot_yaw)
    
    # 输出转向指令（比例控制）
    steer_cmd = K_P * yaw_error
    return steer_cmd

4.4 速度曲线规划：快慢有度，稳字当头

路径定好了，速度怎么给？

很多新手直接给一个恒定速度——结果呢？过弯时离心力太大，货物侧翻；急停时惯性前冲，定位偏差好几厘米。

我早期做的一个项目，就因为速度曲线没规划好，机器人在终点前急刹，把货架上的精密仪器震坏了。赔了不少钱，教训深刻啊。

正确的做法是「S形速度曲线」——也叫梯形速度曲线或正弦加速曲线：

• 加速段：从0开始，加速度逐渐增加到最大，再逐渐减到0
• 匀速段：以最大速度巡航
• 减速段：对称于加速段，平滑减速到0

def s_curve_velocity(total_distance, max_speed, accel_time):
    """
    生成S形速度曲线
    """
    t = 0
    dt = 0.01
    positions = []
    velocities = []
    
    while t < total_distance / max_speed + accel_time:
        if t < accel_time:
            # 加速段：正弦加速
            v = max_speed * 0.5 * (1 - cos(pi * t / accel_time))
        elif t < total_distance / max_speed:
            # 匀速段
            v = max_speed
        else:
            # 减速段
            t_remain = total_distance / max_speed + accel_time - t
            v = max_speed * 0.5 * (1 - cos(pi * t_remain / accel_time))
        
        velocities.append(v)
        positions.append(positions[-1] + v * dt if positions else 0)
        t += dt
    
    return positions, velocities

速度曲线类型	优点	缺点	适用场景
梯形速度曲线	计算简单，实现容易	加速度突变，有冲击	低速、轻载场景
S形速度曲线	加速度连续，运动平滑	计算量稍大	中高速、精密定位
正弦速度曲线	最平滑，无任何冲击	总时间略长	载有易碎品、精密仪器

好了，这一章的内容就这些。多段拼接、动态避让、路径跟随、速度规划——这四个点串起来，就是一个完整的复杂路径规划系统。下一章我们会聊更高级的话题：多机器人协同路径规划。到时候见。