📚 课程目录第1章课程导入

1. 课程导入

在自动驾驶、移动机器人、无人机集群与智能仓储等系统中，轨迹规划从来不是“一次算完就结束”的问题。真实世界充满动态障碍、临时管制、通信延迟、执行误差与任务变更，任何一条在静态地图上看似完美的轨迹，都可能在几秒后失效。因此，动态场景轨迹重规划与优化关注的是：当环境、任务或系统状态发生变化时，如何快速、安全、平滑地生成新的可行轨迹，并使其在动力学约束、实时性约束与舒适性约束下尽可能最优。

一句话定义：动态场景轨迹重规划与优化，是在线感知—预测—决策—规划—控制闭环中，面向时变环境与不确定扰动，对已有轨迹进行快速修正、局部重构或全局再优化的工程方法体系。

1.1 为什么需要“重规划”而不是“重新规划”

很多初学者会把重规划理解为“每次变化都从头算一条新轨迹”。这在工程上往往不可行：全局规划器计算量大、耗时长，且频繁切换轨迹会导致控制抖动、乘坐不适甚至执行机构磨损。重规划的核心思想是增量式修正：保留原轨迹中仍然有效的部分，只对受影响的时间窗口或空间区域进行局部调整；当局部调整无法满足安全或可行性时，再触发更大范围的重构。

与之配套的“优化”则解决另一个问题：新轨迹不仅要可行，还要在多个目标之间取得平衡，例如安全性、通行效率、平滑性、能耗与乘坐舒适性。常见做法是把轨迹表示为多项式、样条或离散路点序列，构造带约束的优化问题，并用二次规划、非线性优化或采样优化求解。

1.2 整体知识框架：从感知到执行的五层结构

本课程的知识框架可以概括为“五层一闭环”。下图展示了动态场景下轨迹重规划与优化的整体数据流与模块关系：感知层提供动态障碍与地图更新，预测层输出未来轨迹分布，决策层判断是否需要重规划以及重规划范围，规划优化层生成候选轨迹并求解，控制层跟踪执行，同时执行偏差与新的感知结果反馈回重规划触发模块。

1.3 方法流程：一次重规划的典型生命周期

下面给出一个可落地的重规划流程，适用于大多数移动机器人与自动驾驶场景：

状态输入：读取当前位姿、速度、加速度，以及最新感知到的动态障碍列表与预测轨迹。
触发判断：计算原轨迹与预测障碍的最小安全距离、跟踪误差、剩余时间预算，判断是否需要重规划。
范围选择：若冲突发生在未来 2 秒内且局部可绕行，则选择局部重规划窗口；否则扩大至全局或分段重构。
轨迹表示：选择多项式、B 样条、离散路点或时空图表示，确定优化变量维度。
约束构造：将碰撞 avoidance、动力学限制、边界条件写成等式或不等式约束。
目标函数：加权组合平滑性、偏离原轨迹程度、通行时间、能耗等项。
求解与校验：调用 QP/NLP 求解器，检查可行性、安全性与实时性；失败则降级到保守策略。
输出与跟踪：将新轨迹送入控制器，并记录本次重规划的触发原因与求解耗时。

# 重规划触发判断伪代码
def need_replan(current_state, old_traj, obstacles, dt):
    min_dist = compute_min_distance(old_traj, obstacles)
    tracking_err = norm(current_state.pose - old_traj.pose_at(0))
    time_left = old_traj.duration - current_state.t
    if min_dist < SAFE_MARGIN:
        return True, "collision_risk"
    if tracking_err > ERR_THRESHOLD:
        return True, "tracking_error"
    if time_left < MIN_HORIZON:
        return True, "horizon_short"
    return False, "none"

1.4 工程应用全景与典型场景

动态场景轨迹重规划与优化并非单一算法，而是一套面向工程落地的系统能力。下表列出典型应用场景、核心挑战与常用方法，帮助建立“场景—问题—方法”的对应关系。

应用场景	动态性来源	核心挑战	常用方法
城市自动驾驶	行人、车辆、施工区	交互博弈、安全距离、实时性	时空走廊、QP、MPC、采样优化
仓储 AGV	其他 AGV、临时货架	多机冲突、死锁、窄通道	优先级规划、时间窗搜索、局部重规划
无人机集群	气流、禁飞区、同伴	三维避障、通信延迟、编队保持	分布式优化、势场+重规划、B 样条
服务机器人	行人、宠物、移动家具	人机共融、舒适性、社会规范	社会力模型、轨迹预测、局部优化
高速巡航	前车变速、匝道汇入	高速稳定性、纵向舒适性	纵向 MPC、换道决策、多项式轨迹

1.5 常见误区与复盘要点

误区一：重规划频率越高越安全。频繁重规划会导致轨迹抖动、控制饱和，反而降低稳定性。应设置合理的触发阈值与最小重规划间隔。

误区二：只关注几何避障，忽略动力学可行性。一条几何上无碰撞的轨迹，可能因曲率或加速度超限而无法跟踪。

误区三：优化目标权重拍脑袋。权重直接决定行为风格，应通过场景测试与数据标定，而不是一次调参后长期不变。

复盘建议：每次重规划失败或触发紧急接管后，记录触发原因、求解耗时、约束冲突项与最终执行结果。长期积累可形成“场景—失败模式—参数调整”的知识库，这是工程能力提升的关键。

1.6 学习目标

理解动态场景轨迹重规划与优化的定义、边界与工程价值。
掌握“感知—预测—决策—规划—控制”闭环中重规划所处的位置与数据流。
能够根据场景选择局部重规划、全局重构或分段优化策略。
掌握轨迹表示、约束构造、目标函数设计与求解器选型的基本方法。
具备分析重规划失败原因、设计降级策略与评价指标的能力。