跳转至

运动规划与轨迹优化

路径规划

路径规划（Path Planning）是在给定环境中找到从起点到终点的无碰撞路径，是机器人导航和自动驾驶的核心问题。

基于图搜索的方法

A* 算法

A*是路径规划中最经典的算法，结合了Dijkstra的最优性和贪心搜索的效率：

\[f(n) = g(n) + h(n)\]
  • \(g(n)\)：从起点到节点 \(n\) 的实际代价
  • \(h(n)\)：从节点 \(n\) 到目标的启发式估计
  • 当 \(h\) 是可容的（admissible）时，A*保证找到最优路径

常见启发式：欧几里得距离、曼哈顿距离、对角距离。

D 与 D Lite

适用于动态环境的增量搜索算法：

  • D*：在环境变化时高效重规划（从目标向起点搜索）
  • D* Lite：简化版本，基于LPA（Lifelong Planning A）
  • 广泛应用于火星探测器和移动机器人导航

跳点搜索 (JPS)

在均匀网格上大幅加速A*：

  • 利用网格的对称性跳过不必要的节点扩展
  • 保持A*的最优性保证
  • 在大规模网格地图中速度提升一个数量级

基于采样的方法

RRT（快速探索随机树）

适用于高维连续空间的路径规划：

  1. 在空间中随机采样点 \(q_{\text{rand}}\)
  2. 找到树中最近节点 \(q_{\text{near}}\)
  3. 沿方向延伸固定步长得到 \(q_{\text{new}}\)
  4. 若路径无碰撞则加入树中

RRT*：RRT的渐近最优变体，通过重连接（rewiring）保证随采样数增加收敛到最优路径。

PRM（概率路线图）

适用于多查询场景：

  1. 构建阶段：在自由空间中随机采样节点，连接可行边
  2. 查询阶段：将起点和终点连入路线图，用图搜索找路径

PRM更适合环境不变但需要规划多条路径的场景。

游戏AI和室内导航中常用的表示方法：

  • 将可行区域分解为凸多边形网格
  • 在多边形之间建立邻接关系
  • 路径搜索在多边形图上进行（粗搜索 + 漏斗算法细化）

优点：搜索空间小、路径自然、支持不同尺寸的智能体。

势场法

将目标建模为引力源、障碍物建模为斥力源：

\[\mathbf{F}(\mathbf{q}) = \mathbf{F}_{\text{att}}(\mathbf{q}) + \mathbf{F}_{\text{rep}}(\mathbf{q})\]
  • 优点：计算简单、实时性好
  • 缺点：局部最小值问题——智能体可能陷入非目标的平衡点
  • 解决方案：随机扰动、虚拟障碍、与全局规划器结合

轨迹规划

轨迹规划（Trajectory Planning）在路径规划的基础上加入时间维度，生成满足运动学和时间约束的平滑轨迹。

路径与轨迹的区别

路径 (Path) 轨迹 (Trajectory)
时间 无时间信息 包含时间参数 \(\mathbf{q}(t)\)
输出 空间中的几何曲线 位置-速度-加速度随时间的函数
约束 几何约束（避障） 运动学/动力学约束（速度、加速度限制）

样条插值方法

三次样条 (Cubic Spline)

在路径点之间用三次多项式插值，保证 \(C^2\) 连续性：

\[\mathbf{q}(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3\]

B样条 (B-Spline)

提供更好的局部控制——修改一个控制点只影响局部曲线段。

贝塞尔曲线 (Bézier Curve)

\(n\) 阶贝塞尔曲线由 \(n+1\) 个控制点定义：

\[\mathbf{B}(t) = \sum_{i=0}^{n} \binom{n}{i} (1-t)^{n-i} t^i \mathbf{P}_i, \quad t \in [0, 1]\]

轨迹优化

将轨迹生成表述为优化问题：

\[\min_{\mathbf{q}(t)} \int_0^T L(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \, dt\]

常见优化目标：

  • 最小snap（四阶导数）：\(\min \int \|\dddot{\mathbf{q}}(t)\|^2 dt\)，生成平滑轨迹（无人机常用）
  • 最小加加速度（jerk）：\(\min \int \|\ddot{\mathbf{q}}(t)\|^2 dt\)，适用于机械臂
  • 最短时间：在满足运动约束下最小化到达时间

基于QP的多项式轨迹优化

将轨迹参数化为分段多项式，目标函数和约束化为二次规划（QP）问题：

\[\min_{\mathbf{c}} \mathbf{c}^T \mathbf{Q} \mathbf{c} \quad \text{s.t.} \quad \mathbf{A}\mathbf{c} = \mathbf{b}\]

其中 \(\mathbf{c}\) 是多项式系数向量。这是无人机轨迹规划中的标准方法。

时间分配

给定路径点后，需要决定每段的通过时间：

  • 均匀分配：简单但不够好
  • 梯形速度规划：加速-匀速-减速
  • S型速度规划：加加速度有限，更平滑
  • 时间最优分配：考虑速度/加速度约束的最优时间

Dubins路径与Reeds-Shepp路径

受运动学约束的最短路径：

  • Dubins路径：只能前进、有最小转弯半径的车辆——最短路径由直线(S)和圆弧(L/R)组成，共6种类型
  • Reeds-Shepp路径：允许前进和后退——46种可能的路径类型

这些是自动驾驶中底层轨迹生成的基础。

运动规划与动力学

运动规划（Motion Planning）在构型空间中寻找满足动力学约束的可行轨迹，是机器人学中的核心问题。

构型空间 (Configuration Space)

将机器人的所有可能姿态用一个高维空间表示：

  • 构型 \(\mathbf{q}\)：完整描述机器人姿态的最小参数集
    • 平面移动机器人：\(\mathbf{q} = (x, y, \theta)\)，3维
    • 6轴机械臂：\(\mathbf{q} = (\theta_1, \ldots, \theta_6)\)，6维
    • 人形机器人：可达30维以上
  • C-free：无碰撞的构型集合
  • C-obstacle：与障碍物碰撞的构型集合

运动规划的本质就是在 C-free 中寻找连续路径。

运动学约束

完整约束 (Holonomic)

约束只涉及构型（位置），不涉及速度：

\[f(\mathbf{q}) = 0\]

例：机械臂关节角度限制。具有完整约束的系统可以在任意方向瞬时移动。

非完整约束 (Nonholonomic)

约束涉及速度，无法积分为纯位置约束：

\[g(\mathbf{q}, \dot{\mathbf{q}}) = 0\]

例：车辆不能横向移动（\(\dot{y} = v \sin\theta\)）。非完整约束使得规划问题显著更难——系统的可达空间比构型空间维度更高。

动力学约束

考虑力和质量的影响：

\[\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}\]
  • \(\mathbf{M}\)：惯性矩阵
  • \(\mathbf{C}\)：科里奥利力和向心力
  • \(\mathbf{g}\)：重力项
  • \(\boldsymbol{\tau}\)：驱动力矩

动力学约束使规划空间从构型空间扩展到状态空间 \((\mathbf{q}, \dot{\mathbf{q}})\)。

基于采样的运动规划

Kinodynamic RRT

将动力学约束融入RRT：

  • 采样状态为 \((\mathbf{q}, \dot{\mathbf{q}})\)
  • 扩展时通过前向模拟（forward simulation）应用控制输入
  • 使用局部控制器连接近邻节点

SST（Stable Sparse RRT）

  • 渐近近最优的kinodynamic规划算法
  • 通过稀疏化和修剪保持树的效率

CHOMP与TrajOpt

CHOMP（Covariant Hamiltonian Optimization for Motion Planning）：

  • 将轨迹建模为函数，优化平滑性 + 碰撞代价
  • 使用协变梯度下降

TrajOpt（Trajectory Optimization）：

  • 使用序列凸优化（Sequential Convex Programming）
  • 将碰撞约束线性化，迭代求解

多机器人运动规划

多机器人联合运动规划的维度是单机器人的倍数：

  • 耦合方法：在联合构型空间规划（维度爆炸）
  • 解耦方法：分别规划后协调（可能无解）
  • 基于优先级的方法：按优先级依次规划，后续机器人避让前序
  • CBS（Conflict-Based Search）：先独立规划，发现冲突后分支求解

规划与控制衔接

规划与控制的衔接是将高层决策转化为可执行动作的关键环节。现实系统中，规划和控制并非割裂的，而是需要紧密协作。

规划-控制的层次架构

典型的自主系统采用分层架构：

层次 功能 频率 示例
任务规划 高层目标分解 ~0.1 Hz "从A到B取包裹"
行为决策 选择行为模式 ~1 Hz 换道、跟车、超车
运动规划 生成参考轨迹 ~10 Hz 局部轨迹优化
控制执行 跟踪参考轨迹 ~100 Hz PID、MPC

每层以不同的时间尺度运行，高层提供参考，底层保证执行。

模型预测控制 (MPC)

MPC是连接规划与控制最重要的方法之一，本质上是一个滚动规划器：

\[\min_{\mathbf{u}_0, \ldots, \mathbf{u}_{N-1}} \sum_{k=0}^{N-1} \ell(\mathbf{x}_k, \mathbf{u}_k) + V_f(\mathbf{x}_N)\]
\[\text{s.t.} \quad \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k), \quad \mathbf{x}_k \in \mathcal{X}, \quad \mathbf{u}_k \in \mathcal{U}\]

核心思想：

  1. 在每个时间步求解有限时域的优化问题
  2. 只执行第一步控制输入
  3. 获取新的状态反馈后重新求解

优点：

  • 自然地处理约束（状态约束、输入约束）
  • 结合了规划（前瞻性）和控制（反馈性）
  • 对模型误差和扰动有一定鲁棒性

计算挑战：需要在控制周期内求解优化问题，实时性要求高。

线性MPC vs 非线性MPC

线性MPC 非线性MPC
模型 线性：\(\mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k\) 非线性：\(\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)\)
优化 QP（高效求解） NLP（计算量大）
应用 局部线性化场景 大范围非线性运动

实时重规划

真实环境是动态的，规划必须能够快速重做：

触发重规划的条件：

  • 障碍物状态变化（检测到新障碍物）
  • 当前轨迹不可行（偏差过大）
  • 目标变更
  • 环境预测更新

实时性策略：

  • 任意时间算法（Anytime）：随时可返回当前最优解，计算时间越长解越好（如ARA*）
  • 增量式方法：复用上次规划结果，只更新变化的部分（如D* Lite）
  • 并行规划：在执行当前轨迹的同时，后台规划新轨迹

规划与学习的融合

现代系统越来越多地将学习方法引入规划-控制框架：

  • 学习型MPC：用神经网络学习MPC的代价函数或约束
  • 学习型路径规划：用深度学习预测启发式函数或直接输出路径
  • 扩散模型规划：将规划问题建模为从噪声中去噪生成轨迹（Diffuser）
  • 端到端方法：从感知直接到控制，跳过显式规划（但可解释性差）

关键挑战：如何在学习的灵活性和规划的安全保证之间取得平衡。

评论 #