FastPlanner论文解读（三）——后端时间优化及代码流程

最新推荐文章于 2025-03-26 02:11:38 发布

原创

最新推荐文章于 2025-03-26 02:11:38 发布 · 1.6k 阅读

35 ·

CC 4.0 BY-SA版权

文章标签：

#算法 #无人机 #机器人 #c++

一、后端时间优化

尽管我们在路径搜索和优化中限制了 kinodynamic 的可行性，但有时我们会得到不可行的轨迹。基本原因是梯度信息往往会拉长整体轨迹，同时将其推离障碍物。因此，四旋翼飞行器必须更激进地飞行，才能在相同的时间内行驶更长的距离，如果原始运动已经接近物理极限，这不可避免地会导致过度激进的运动。

Fast Planner中是通过非均匀B样条来进行时间重分配的，与均匀 B 样条的唯一区别是它的每个结跨度 ∆tm = tm+1 − tm 都独立于其他结点区间。只改变时间分配，不改变控制点的位置，生成轨迹的几何情况是不会发生变化的。

非均匀B样条的一阶导数和二阶导数如下：

我们需要调整时间之间的间隔

速度调整：

速度超限的控制点可以写作

其中，最大的一个超出速度限制的分量记为： $\left | V^{'} _{i}\right |=v_{m}$ ，速度的最大限制为 $v_{max}$ ，该速度控制点的超限主要受时间区间ti+pb+1-ti+1大小的影响，因此我们要延长该时间区间，即：

$t\hat{}_{i+pb+1}-t\hat{}_{i+1}=\mu _{v}(t{}_{i+pb+1}-t{}_{i+1})$ ，其中， $\mu _{v}=\frac{v_{m}}{v_{max}}$

该速度控制点变为：

对加速度控制点做同样的调整，

这里的 $\mu _{a}=\left ( \frac{a_{m}}{a_{max}} \right )^{\frac{1}{2}}$

这样通过调整时间区间就使得轨迹满足动力学可行性，同时没有调整控制点的位置保证了轨迹没有发生改变。

对应的算法流程为：

首先找出不满足动力学可行性的控制点集合V和A，分别对其做调整，这里在选择调整的参数时，有两个预设的参数αv和αa，因为结跨度 ∆tm 会影响几个控制点，反之亦然，所以用两个常数 αv 和 αa 略大于 1（第 5、9 行）来界定 μ' v 和 μ' a 可以防止任何时间跨度过度延长。

对应的实现函数为reallocateTime

bool NonUniformBspline::reallocateTime(bool show)
  {
    // SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
    // cout << "origin knots:\n" << u_.transpose() << endl;
    bool fea = true;

    Eigen::MatrixXd P = control_points_;
    int dimension = control_points_.cols();

    double max_vel, max_acc;

    /* check vel feasibility and insert points */
    for (int i = 0; i < P.rows() - 1; ++i)
    {
      Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));

      if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
          fabs(vel(2)) > limit_vel_ + 1e-4)
      {

        fea = false;
        if (show)
          cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl;

        max_vel = -1.0;
        for (int j = 0; j < dimension; ++j)
        {
          max_vel = max(max_vel, fabs(vel(j)));
        }

        double ratio = max_vel / limit_vel_ + 1e-4;
        if (ratio > limit_ratio_)//初始设置了一个调整因子为av
          ratio = limit_ratio_;

        double time_ori = u_(i + p_ + 1) - u_(i + 1);//原本的时间段
        double time_new = ratio * time_ori;//新的时间段变
        double delta_t = time_new - time_ori;//新的时间间隔
        double t_inc = delta_t / double(p_);//将时间均匀分配到p阶样条的每段时间内的增量
        for (int j = i + 2; j <= i + p_ + 1; ++j)//从i+1到i+pb+1的时间段按照比例调整，在i+pb+1之后相同比例调整，调整的点为i+2到i+pb+1
        {
          u_(j) += double(j - i - 1) * t_inc;//这几个点每个增加j-i-1*tinc的值
          if (j <= 5 && j >= 1)
          {
            // cout << "vel j: " << j << endl;
          }
        }

        for (int j = i + p_ + 2; j < u_.rows(); ++j)//这之后的点增加delta_t的时间，相同的时间增加量
        {
          u_(j) += delta_t;
        }
      }
    }

    /* acc feasibility */
    for (int i = 0; i < P.rows() - 2; ++i)
    {

      Eigen::VectorXd acc = p_ * (p_ - 1) *
                            ((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
                             (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
                            (u_(i + p_ + 1) - u_(i + 2));

      if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
          fabs(acc(2)) > limit_acc_ + 1e-4)
      {

        fea = false;
        if (show)
          cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl;

        max_acc = -1.0;
        for (int j = 0; j < dimension; ++j)
        {
          max_acc = max(max_acc, fabs(acc(j)));
        }

        double ratio = sqrt(max_acc / limit_acc_) + 1e-4;
        if (ratio > limit_ratio_)//初始有一个调整因子为aa
          ratio = limit_ratio_;
        // cout << "ratio: " << ratio << endl;

        double time_ori = u_(i + p_ + 1) - u_(i + 2);//原本的时间
        double time_new = ratio * time_ori;//新的时间
        double delta_t = time_new - time_ori;//新的时间间隔
        double t_inc = delta_t / double(p_ - 1);

        if (i == 1 || i == 2)
        {
          // cout << "acc i: " << i << endl;
          for (int j = 2; j <= 5; ++j)
          {
            u_(j) += double(j - 1) * t_inc;
          }

          for (int j = 6; j < u_.rows(); ++j)
          {
            u_(j) += 4.0 * t_inc;
          }
        }
        else
        {

          for (int j = i + 3; j <= i + p_ + 1; ++j)
          {
            u_(j) += double(j - i - 2) * t_inc;
            if (j <= 5 && j >= 1)
            {
              // cout << "acc j: " << j << endl;
            }
          }

          for (int j = i + p_ + 2; j < u_.rows(); ++j)
          {
            u_(j) += delta_t;
          }
        }
      }
    }

    return fea;//返回是否调整成功，这里是不断循环调整，直到所有的控制点都满足动力学可行性
  }

首先是参数的定义

fea：标记轨迹是否满足约束，如果调整成功，最终返回 true。
P：存储当前 B-Spline 的控制点。
dimension：轨迹的维度（通常为 3D）。
max_vel & max_acc：记录轨迹的最大速度 & 加速度

然后检查速度控制点，

最低0.47元/天解锁文章