机器人运动规划与轨迹优化

目录

  1. 动力学前端：从几何到微分约束的跨越
  2. 多项式后端：最小 Snap 与闭式解
  3. 安全约束优化：硬走廊 vs 软 ESDF
  4. Pipeline 架构与实战避坑
  5. 附录：核心代码与数学模板

1. 动力学前端：从几何到微分约束的跨越

1.1 为什么需要 Kinodynamic Planning？

传统 A*/RRT 输出的是几何路径（点列或折线），隐含假设：机器人是质点，且能瞬时改变速度/转向角。但真实系统受限于：

工程总结：
“前端定骨架，后端赋血肉”。动力学前端不追求全局最优，而是保证输出的轨迹初值严格满足物理边界，为后端优化提供安全的收敛域。

1.2 状态格点规划 (State Lattice)

放弃手工定义 4/8 连通网格，改为在控制空间或状态空间离散化，生成满足动力学约束的 Motion Primitives（运动基元），拼接成搜索图。

采样方向 实现方式 优点 缺点 工业应用
正向 (控制空间) 离散 $(v, \delta, a)$，固定时长 $T$ 数值积分 $\dot{s}=f(s,u)$ 极易实现，天然满足约束 无目标导向，末端分布不均，搜索效率低 局部避障、固定场景AGV
反向 (状态空间) 指定目标邻域，求解 OBVP 反推连接轨迹 强目标导向，规划效率高 需解析/数值求解 OBVP，实现复杂 自动驾驶泊车、结构化道路

⚠️ 注意：Lattice 不是“图搜索”而是“轨迹库”

工业界（如 Apollo Lattice Planner）通常离线或启动时预计算 Lattice 库，在线仅做轨迹评估与选择，而非实时建图搜索。代价函数为多目标加权：

$$ J = w_1 \cdot J_{safety} + w_2 \cdot J_{comfort} + w_3 \cdot J_{progress} + w_4 \cdot J_{reference} $$

1.3 混合 A* (Hybrid A*)

1.4 动力学 RRT* (Kinodynamic RRT*)

将传统 RRT* 扩展至全状态空间 $\mathcal{X}$，核心改造点：

  1. Sample：在状态空间 $(x,y,\theta,v)$ 采样。
  2. Near：距离不再是欧氏距离，而是最优控制代价 $c = \int_0^\tau (1 + u^T R u) dt$。注意该代价非对称（$c(s_i \to s_j) \neq c(s_j \to s_i)$）。
  3. ChooseParent/Rewire：求解 OBVP 获取最优控制律 $u^*(t)$。
  4. 高效邻域搜索：利用可达集椭球计算 AABB 包围盒，KD-Tree 范围查询剪枝，避免 $O(N)$ 全量 OBVP 求解。

备注：
Kinodynamic RRT* 在无人机/机械臂等高维系统中有优势，但在车载平台上极少直接使用。原因：状态空间维度高、OBVP 求解慢、随机性导致产线调试困难。工业界更倾向 Hybrid A* + Frenet 多项式 的确定性架构。

2. 多项式后端：最小 Snap 与闭式解

2.1 微分平坦性：降维的艺术

对于欠驱动系统（如四旋翼），非线性动力学极其复杂。但若存在平坦输出 $\sigma = [x, y, z, \psi]^T$，则：

2.2 最小 Snap QP 与闭式解

轨迹分段多项式：$p(t) = \sum_{i=0}^{N} c_i t^i$。最小化 $J = \int (\frac{d^4 p}{dt^4})^2 dt$ 转化为凸 QP：

$$ \begin{aligned} \min_{\mathbf{p}} \quad & \mathbf{p}^T \mathbf{Q} \mathbf{p} \\ \text{s.t.} \quad & \mathbf{A}_{eq} \mathbf{p} = \mathbf{b}_{eq} \quad \text{(起点/终点/连续性)} \end{aligned} $$

闭式解 (Closed-Form) 的本质

将端点导数分为固定 (Fixed, $d_F$) 与自由 (Free, $d_P$)：

$$ J = \begin{bmatrix} d_F \\ d_P \end{bmatrix}^T \begin{bmatrix} R_{FF} & R_{FP} \\ R_{PF} & R_{PP} \end{bmatrix} \begin{bmatrix} d_F \\ d_P \end{bmatrix} \Rightarrow d_P^* = -R_{PP}^{-1} R_{FP}^T d_F $$

优势：无需外部求解器，纯矩阵运算，适合嵌入式实时部署。
劣势：$R_{PP}$ 在轨迹段数多或时长差异大时病态 (Ill-conditioned)，数值爆炸。

2.3 时间分配 (Time Allocation)

多项式质量高度依赖分段时长 $\Delta T_j$。

备注：
闭式解在学术代码中常见，但工业部署首选 QP 求解器 (OSQP/qpOASES)。现代求解器支持热启动、稀疏矩阵优化，数值稳定性远超手算闭式。不要为了“零依赖”牺牲鲁棒性。

3. 安全约束优化：硬走廊 vs 软 ESDF

3.1 最小 Snap 的致命缺陷：超调 (Overshoot)

最小 Snap 仅约束航点通过，不限制段间轨迹。高阶多项式在航点间极易产生振荡，导致轨迹穿出安全区域。“路径无碰撞 ≠ 轨迹无碰撞”。

3.2 硬约束：飞行走廊与贝塞尔凸包

核心思路：前端搜索生成飞行走廊 (Flight Corridor)，后端强制轨迹完全包含于凸多面体内。

注意：多项式基转换

工业实现中，通常先在 Monomial 基下构建连续性约束，再转换至 Bernstein (Bezier) 基施加凸包约束：

$$ \mathbf{p}_{mono} = \mathbf{M} \cdot \mathbf{c}_{bez} $$

转换矩阵 $\mathbf{M}$ 为固定下三角矩阵，计算开销极小。

3.3 软约束：ESDF 距离惩罚与梯度优化

当环境高度动态或走廊生成失败时，采用软约束惩罚：

$$ J_{total} = \lambda_1 J_{smooth} + \lambda_2 J_{collision} + \lambda_3 J_{dyn} $$

3.4 均匀 B-Spline：实时重规划的利器

多项式优化需全局重算，而 Uniform B-Spline 具有局部支撑性：修改单个控制点仅影响局部 $p+1$ 段曲线。

备注：
硬约束 (Corridor) 适合结构化/静态环境（保证绝对安全，但走廊生成失败则规划中断）。
软约束 (ESDF) 适合非结构化/动态环境（容错率高，但依赖传感器质量与梯度平滑）。
工业主流：B-Spline + 凸包软约束 + 滑窗优化，兼顾安全性与实时性。

4. Pipeline 架构与实战避坑

4.1 标准分层架构

graph LR
    A[全局地图/感知] --> B(前端: Hybrid A*/Lattice)
    B --> C[无碰撞几何路径]
    C --> D{环境类型?}
    D -->|静态/结构化| E[硬约束: 走廊提取 + Bezier QP]
    D -->|动态/未知| F[软约束: ESDF + B-Spline 梯度优化]
    E --> G[轨迹输出]
    F --> G
    G --> H[底层跟踪: MPC/PID]
    H -->|状态反馈/地图更新| B
    
    style B fill:#e3f2fd
    style E fill:#e8f5e9
    style F fill:#fff3e0
    style H fill:#f3e5f5

4.2 常见陷阱与解决方案

陷阱现象 根本原因 工业级解决方案
QP 求解失败/病态 时间分段 $\Delta T \to 0$ 或空间尺度 $>100m$ 双重归一化：时间映射到 $[0,1]$，空间缩放到沙盒求解后反变换
轨迹超调碰撞 仅约束航点，未限制段间极值 切换至 Bezier/B-Spline 凸包约束，或增加 Jerk/Snap 边界惩罚
优化震荡不收敛 ESDF 梯度噪声大、步长不当 使用 L-BFGS + Backtracking Line Search，ESDF 添加高斯平滑
动态障碍躲避不及 规划频率低，预测时域短 提升优化频率至 30Hz+，采用 滑窗 B-Spline 局部重规划
实车执行延迟/跟踪误差 规划输出未考虑执行器带宽 规划输出加 MPC 跟踪层，预留 15% 控制裕度，降阶下发

4.3 调参 Checklist

附录：核心代码与数学模板

A.1 OBVP 闭式解矩阵构建 (C++/Eigen)

// 最小化Jerk的5次多项式OBVP求解 (固定起止位置/速度/加速度)
Eigen::Matrix3d buildOBVPMatrix(double T) {
    Eigen::Matrix3d M;
    M << std::pow(T,3), std::pow(T,2), T,
         3*std::pow(T,2), 2*T, 1,
         6*T, 2, 0;
    return M;
}

Eigen::Vector3d solveOBVP(const State& start, const State& end, double T) {
    Eigen::Matrix3d M = buildOBVPMatrix(T);
    Eigen::Vector3d delta;
    delta << end.p - (start.p + start.v*T + 0.5*start.a*T*T),
             end.v - (start.v + start.a*T),
             end.a - start.a;
    return M.ldlt().solve(delta); // 返回 [c5, c4, c3] 系数
}

A.2 Bezier 凸包约束构建

// 将走廊线性约束 A*x <= b 映射到 Bezier 控制点
// control_points: (N+1) x 3, corridor: M x 4 (Ax+By+Cz <= D)
bool buildBezierConstraints(const std::vector<Eigen::Vector3d>& ctrl_pts,
                            const std::vector<Eigen::Vector4d>& corridor,
                            Eigen::MatrixXd& A_ineq, Eigen::VectorXd& b_ineq) {
    int n_ctrl = ctrl_pts.size();
    int m_planes = corridor.size();
    A_ineq.resize(m_planes * n_ctrl, 3 * n_ctrl);
    b_ineq.resize(m_planes * n_ctrl);
    
    for (int i = 0; i < n_ctrl; i++) {
        for (int j = 0; j < m_planes; j++) {
            int row = j * n_ctrl + i;
            int col_start = 3 * i;
            A_ineq.block(row, col_start, 1, 3) = corridor[j].head<3>().transpose();
            b_ineq(row) = corridor[j](3);
        }
    }
    return true;
}

A.3 B-Spline 导数递推与安全凸包检查

// 检查 B-Spline 控制点是否全部位于安全走廊内
bool isBSplineSafe(const std::vector<Eigen::Vector3d>& ctrl_pts, 
                   const std::vector<Eigen::Vector4d>& corridor, double margin = 0.1) {
    for (const auto& pt : ctrl_pts) {
        for (const auto& plane : corridor) {
            if (plane.head<3>().dot(pt) > plane(3) - margin) {
                return false; // 控制点越界 → 曲线必越界
            }
        }
    }
    return true;
}

// B-Spline 一阶导数控制点递推 (阶数 pb, 时间步 dt)
std::vector<Eigen::Vector3d> computeBSplineDerivative(
    const std::vector<Eigen::Vector3d>& ctrl_pts, double dt) {
    std::vector<Eigen::Vector3d> vel_ctrl;
    vel_ctrl.reserve(ctrl_pts.size() - 1);
    double scale = (double)(ctrl_pts.size() - 2) / dt; // 根据实际阶数调整
    for (size_t i = 0; i < ctrl_pts.size() - 1; ++i) {
        vel_ctrl.push_back(scale * (ctrl_pts[i+1] - ctrl_pts[i]));
    }
    return vel_ctrl;
}