行为预测与运动规划：轨迹预测与决策

FreeGuideOnline 最新 2026-06-20

行为预测与规划：从轨迹预测到智能决策

1. 为什么预测与规划是智能系统的核心

自动驾驶汽车要安全变道，机器人要无碰撞地递送物品，游戏 AI 要预判对手走位……这些场景背后，都依赖两项关键能力：

行为预测：基于历史状态和环境信息，推测其他智能体未来会如何运动。
运动规划：根据预测结果，制定自身安全、高效且符合物理约束的运动方案。

本教程将带你从零开始，系统地理解轨迹预测与决策规划的基本概念、经典方法和前沿思路，并附有可动手的简单实践。

2. 轨迹预测：猜透他人意图

轨迹预测的目标是：给定一个智能体过去的位置序列，输出其未来最可能走出的路径。输入通常是历史轨迹点，输出可以是单一轨迹或多条可能的轨迹及其概率。

2.1 预测问题的形式化

设 $t$ 时刻，智能体 $i$ 的状态为 $s_t^i$（可包含位置、速度、朝向等）。观测到过去 $T_{obs}$ 个时间步的状态序列 ${s_{t-T_{obs}+1}, \dots, s_t}$，我们要预测未来 $T_{pred}$ 步的状态 ${\hat{s}{t+1}, \dots, \hat{s}{t+T_{pred}}}$。

轨迹预测的难点在于：行为是多模态的（可以直行、左转或右转），且高度依赖与其他智能体的交互（社交互动）。

2.2 经典模型：从物理模型到数据驱动

物理模型
最简单的方案是假设运动服从某种物理规律，例如恒速度模型 (CV)、恒加速度模型 (CA) 或恒转向率和速度模型 (CTRV)。优点是计算极快、可解释性强，但完全忽略环境约束和交互，只适合短时预测。

基于机器学习的方法

高斯过程 (GP) 回归：能输出预测的不确定性，但在高维和复杂交互场景下表达能力有限。
隐马尔可夫模型 (HMM)：将运动模式聚类为离散状态，适合简单的分岔路口场景。
循环神经网络 (RNN)：利用 LSTM/GRU 编码历史序列，输出未来位置。可处理变长序列，但单独使用时多模态捕捉能力弱。

现代主流：深度生成模型与注意力机制

CVAE (条件变分自动编码器)：从历史信息中学习潜在变量 $z$，再解码出多条可能的未来轨迹，天然适合多模态预测。
GAN (生成对抗网络)：生成器产生轨迹，判别器迫使轨迹逼真，常用于社会交互轨迹生成。
Transformer/图网络：将场景中的智能体视为图的节点，用注意力机制建模两两交互，输出所有智能体协同的未来轨迹。代表性工作如 VectorNet、AgentFormer、Wayformer 等。

2.3 交互建模：你的行为影响我的预测

个体运动很少相互独立。交互建模就是要捕捉“因为旁边有车，所以我将减速”这类关系。

Social LSTM (2016) 是里程碑：为每个行人分配一个 LSTM，通过“社会池化”层将邻近行人的隐藏状态聚合后输入各自的 LSTM，隐式建模交互。后来演进为基于图注意力网络 (GAT) 的方法，动态学习交互权重。

当前主流思路是将场景表示为占据栅格、向量地图或全景图，联合编码所有智能体的历史轨迹和地图信息，一次性预测所有智能体的未来轨迹。

3. 运动规划：从预测到行动

获得他车的预测轨迹后，自车如何规划出安全、舒适的路径？运动规划回答“我将去哪里”以及“速度如何变化”。

3.1 规划问题的基本元素

构型空间 (C-space)：智能体所有可能的位置和朝向组成的空间。规划就是在 C-space 中找出一条从起点到目标区域的连续路径。
约束：运动学约束（最大转向角、加速度限制）、碰撞约束、交通规则、舒适度约束等。
代价函数：时间最优、能量最优、平滑度、安全性等加权组合。

3.2 经典规划算法家族

基于搜索的规划
将状态空间离散化为网格或晶格，使用 A*、Hybrid A* 等算法搜索最优路径。Hybrid A* 额外考虑车辆运动学，生成的路径可直接执行。

基于采样的规划
在高维空间中随机采样点并连接成图，再图搜索。代表作：

PRM (概率路标图)：离线学习路线图，在线查询。
RRT (快速扩展随机树)：从起点出发快速探索未知空间，适合动态环境，衍生版本 RRT* 可渐进收敛到最优路径。
CL-RRT：在 RRT 中加入闭环预测，常用于自动驾驶车辆。

优化类方法
将规划描述为一个带约束的优化问题，目标函数包含平滑性、避障、跟随参考线等项，用梯度下降或图优化求解。常用 EM Planner、Lattice Planner 等。优点是可以精细调控车辆行为，输出曲率连续的轨迹。

3.3 融入行为预测的决策规划

单纯“预测-规划”流水线的弊端是：规划时假设预测是确定的，而实际预测充满不确定性。因此现代系统引入联合预测与规划或交互感知规划。

部分可观马尔可夫决策过程 (POMDP)：将他车意图作为隐变量，规划时考虑所有可能意图的信念分布，选择期望代价最小的动作。代表性算法为自适应信念树 (ABT)、DESPOT 等。
博弈论规划：将多车交互建模为动态博弈，寻找纳什均衡或斯塔克尔伯格均衡，使自车策略对其他车辆理性反应具有鲁棒性。如基于迭代线性二次规划 (iLQG) 的博弈规划器。
模仿学习与离线强化学习：直接从大量人类驾驶数据中学习策略，端到端输出规划轨迹，预测隐式包含在神经网络黑色的推理中。例如 ChauffeurNet、PlanT 等。

4. 经典算法动手实践：恒速度预测 + RRT 规划

以下是一个用 Python 实现的简单示例，展示“先预测他车未来位置，再基于预测用 RRT 规划自车路径”的完整思路。

import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial import KDTree

# 1. 恒速度轨迹预测
def predict_trajectory(past_positions, future_steps, dt=0.5):
    # past_positions: (T, 2) 过去位置数组
    velocity = (past_positions[-1] - past_positions[0]) / (len(past_positions) * dt)
    future = [past_positions[-1]]
    for i in range(future_steps):
        future.append(future[-1] + velocity * dt)
    return np.array(future)

# 2. 简单2D RRT路径规划
class RRT:
    def __init__(self, start, goal, obstacle_list, bounds, step_size=0.5, max_iter=500):
        self.start = Node(start)
        self.goal = Node(goal)
        self.bounds = bounds
        self.step_size = step_size
        self.max_iter = max_iter
        self.obstacle_kdtree = KDTree(obstacle_list)
        self.node_list = [self.start]

    class Node:
        def __init__(self, pos):
            self.pos = np.array(pos)
            self.parent = None

    def is_collision_free(self, pos):
        # 检查是否与障碍物碰撞 (距离<0.3视为碰撞)
        dist, _ = self.obstacle_kdtree.query(pos)
        return dist > 0.3

    def plan(self):
        for _ in range(self.max_iter):
            # 随机采样 (带小概率直接采样目标点)
            if np.random.rand() < 0.1:
                sample_pos = self.goal.pos
            else:
                sample_pos = np.random.uniform(self.bounds[0], self.bounds[1], 2)

            # 找最近节点
            nearest_node = min(self.node_list, key=lambda n: np.linalg.norm(n.pos - sample_pos))
            direction = (sample_pos - nearest_node.pos)
            dist = np.linalg.norm(direction)
            if dist < 1e-6:
                continue
            new_pos = nearest_node.pos + direction/dist * min(self.step_size, dist)

            if self.is_collision_free(new_pos):
                new_node = self.Node(new_pos)
                new_node.parent = nearest_node
                self.node_list.append(new_node)

                if np.linalg.norm(new_pos - self.goal.pos) < self.step_size:
                    return self.extract_path(new_node)
        return None  # 未找到路径

    def extract_path(self, node):
        path = []
        while node is not None:
            path.append(node.pos)
            node = node.parent
        return path[::-1]

# 3. 运行示例
if __name__ == "__main__":
    # 预测他车未来轨迹 (假设它从(2,2)以(0.5,0.2)速度移动)
    past = np.array([[1.5, 1.8], [2.0, 2.0]])
    pred_traj = predict_trajectory(past, future_steps=20)
    print("预测轨迹点:\n", pred_traj)

    # 把他车未来位置当作动态障碍物
    obstacles = pred_traj[::2]  # 下采样作为障碍物点

    # 规划自车从(0,0)到(8,8)的路径
    rrt = RRT(start=(0,0), goal=(8,8), obstacle_list=obstacles, bounds=([0,0],[10,10]))
    path = rrt.plan()

    # 可视化
    plt.figure()
    if path:
        path = np.array(path)
        plt.plot(path[:,0], path[:,1], '-b', label='自车规划路径')
    plt.scatter(pred_traj[:,0], pred_traj[:,1], c='r', marker='o', label='预测他车轨迹')
    plt.scatter(0,0, marker='s', s=100, label='起点')
    plt.scatter(8,8, marker='*', s=100, label='终点')
    plt.legend()
    plt.grid()
    plt.axis('equal')
    plt.show()

这个例子直观展示了“预测-规划”流水线：先利用恒速度模型推测他车未来占据的位置，再将这些位置作为障碍物，使用 RRT 规划出一条无碰撞路径。你可以尝试替换为更复杂的预测器（如 Social LSTM 的简化版）或更精细的规划器（如 B 样条平滑）。

5. 进阶方向与工程落地的注意事项

不确定性量化与风险感知规划：预测输出的不应只是单点轨迹，而是带置信度的概率分布。规划时需引入碰撞概率约束，而非仅考虑最可能的情况。
多智能体交互闭环：他车也会对自车的行为做出反应。在线求解动态博弈计算量大，工程中常使用“预期轨迹预测 + 代价敏感规划”的简化方案。
实时性优化：模型推理和规划更新必须满足毫秒级延迟。采用模型轻量化、并行搜索、预计算轨迹库等手段。
感知-预测-规划联合学习：一些最新工作尝试端到端从传感器数据直接输出轨迹或控制量，减少模块间信息丢失，但可解释性和安全验证仍是挑战。

6. 总结

行为预测与运动规划是智能体实现自主、安全交互的关键技术。预测从早期的物理模型演进到数据驱动的生成式图网络，规划从几何搜索发展到优化和博弈方法。掌握“先预测、再规划”以及“联合预测规划”两条路径的核心思想，将帮助你构建出更智能的机器人、自动驾驶或游戏 AI 系统。

推荐的进阶阅读材料：

“Multimodal Trajectory Prediction: A Survey” (arXiv: 2305.16114)
“Planning and Decision-Making for Autonomous Vehicles: A Survey” (IEEE ITS)
OpenAI 的 gym 环境 + stable_baselines3 可快速搭建强化学习规划实验