Fast-Planner中的多项式轨迹生成算法解析

概述

Fast-Planner是一个高效的无人机路径规划系统，其中多项式轨迹生成是其核心功能之一。本文将深入解析polynomial_traj.cpp文件中的关键算法实现，包括最小加加速度轨迹生成(minSnapTraj)和快速直线轨迹生成(fastLine4deg/fastLine3deg)两种主要方法。

最小加加速度轨迹生成(minSnapTraj)

算法原理

最小加加速度轨迹生成的目标是找到一条通过给定路径点、满足速度/加速度边界条件，并且使加加速度(加速度的导数)最小的多项式轨迹。这种方法在无人机轨迹规划中广泛应用，因为它能生成平滑且能量效率高的轨迹。

实现细节

1. 输入参数：

   • Pos：路径点位置矩阵
   • start_vel/end_vel：起点/终点速度
   • start_acc/end_acc：起点/终点加速度
   • Time：各段轨迹的时间分配

2. 核心步骤：

PolynomialTraj minSnapTraj(const Eigen::MatrixXd& Pos, const Eigen::Vector3d& start_vel,
                           const Eigen::Vector3d& end_vel, const Eigen::Vector3d& start_acc,
                           const Eigen::Vector3d& end_acc, const Eigen::VectorXd& Time)

3. 关键计算：

   • 构建导数向量Dx, Dy, Dz
   • 构造映射矩阵A
   • 生成选择矩阵C'
   • 计算最小加加速度矩阵Q
   • 构建R矩阵并求解闭式解

4. 多项式系数计算：

   • 通过矩阵运算求解各段多项式的系数
   • 将系数组织成PolynomialTraj对象返回

快速直线轨迹生成

Fast-Planner提供了两种快速直线轨迹生成方法：4阶(fastLine4deg)和3阶(fastLine3deg)多项式轨迹。

fastLine4deg方法

特点

• 使用4阶多项式(5次)
• 考虑了最大速度、加速度和加加速度约束
• 由7段轨迹组成，形成完整的加速-匀速-减速过程

实现流程

1. 计算位移向量和方向
2. 确定缩放向量
3. 分段计算轨迹：
   • 第一段：加加速度阶段
   • 第二段：匀速加速阶段
   • 第三段：减加速度阶段
   • 第四段：匀速阶段
   • 第五段：减加速度阶段
   • 第六段：匀速减速阶段
   • 第七段：加加速度阶段

fastLine3deg方法

特点

• 使用3阶多项式(4次)
• 考虑了最大速度和加速度约束
• 由3段轨迹组成：加速-匀速-减速

实现流程

1. 计算位移向量和方向
2. 确定缩放向量
3. 计算各段时间：
   • 头尾段加速/减速时间
   • 中间段匀速时间
4. 计算中间路径点
5. 分段构建轨迹多项式

多项式轨迹类(PolynomialTraj)

两种轨迹生成方法都返回PolynomialTraj对象，该类具有以下特点：

1. 分段存储：每段轨迹独立存储其多项式系数
2. 时间参数化：每段轨迹有明确的时间长度
3. 连续性保证：段与段之间满足位置、速度、加速度的连续性

实际应用建议

1. 选择轨迹类型：

   • 对平滑性要求高时使用minSnapTraj
   • 对计算速度要求高时使用fastLine系列

2. 参数调优：

   • 根据无人机动力学性能设置合理的最大速度、加速度
   • 时间分配会影响轨迹的平滑性和可行性

3. 实时性考虑：

   • fastLine系列计算更快，适合实时性要求高的场景
   • minSnapTraj计算量较大，适合离线规划或对轨迹质量要求高的场景

总结

Fast-Planner中的多项式轨迹生成算法提供了灵活高效的轨迹生成方案，minSnapTraj适合复杂路径规划，而fastLine系列适合简单直线轨迹。理解这些算法的实现原理有助于在实际应用中做出合理选择和参数调优。