【运动控制】（7）路径跟踪及组合动作篇

系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对**【运动控制】路径跟踪及组合动作篇**做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

一、航向角yaw规划

1、为什么要进行航向角yaw规划

在路径规划中，对路径位置进行规划可以理解，对轨迹的速度和加速度进行规划也可以理解
为什么还要对轨迹的方向也要进行规划呢？
当路径的位置一定确定下来了，路径的航向角yaw不是已经确定下来了吗？那为什么还要对路径的航向角在规划呢？

我的想法如下：
当路径的位置一定确定下来了，路径的航向角yaw已经确定下来了，但是我们还是要把航向角yaw计算出来，作为一个属性添加到轨迹之中，因为航向角yaw有时候是作为控制器的输入的，怎么理解呢，就是像纯跟踪pure_suit这种控制器输入一系列离散的路径点就可以进行路径跟随，但是PID控制输入一系列离散的路径点显然是不行的，要先通过航向角yaw规划得到每个路径点的航向位置yaw作为PID控制器输入，才能实现PID的航向角位置闭环控制【防盗标记–盒子君hzj】

同理，若要实现机器人的（航向）角速度闭环控制，或者实现机器人的（航向）角加速度闭环控制，还要进一步进行航向角速度w的规划和航向角加速度a的规划

同理，计算航向角速度w与航向角加速度a，可以进行航向角yaw对时间就一阶、二阶导数实现，若有进行轨迹的速度、加速度规划（如mini_snap），则可以取出轨迹的速度曲线、加速度曲线，进行分段就对应角度（与求航向角yaw方法类似）

我在写什么呢？？？表述的有点冗余，哎，理科生就这样，大家凑合着理解吧~【防盗标记–盒子君hzj】

.

2、核心思想

把现在规划出来的轨迹进行线性分段，分段的方法是根据轨迹的总运行时间/人为设定的时间增量，进而得到单位时间增量下的，最小航向角增量dt_yaw

通过人为设定的时间增量不断迭代，取出对应时刻轨迹上的控制点，进而得到轨迹上两两相邻的控制点，通过两两控制点的相对位置即可计算得到该条轨迹上每个控制点的航向角yaw，这个航向角yaw也是该轨迹在该点的切线方向
【防盗标记–盒子君hzj】
.

3、代码示例（fast_planner)

//航向规划
void FastPlannerManager::planYaw(const Eigen::Vector3d& start_yaw) {
  ROS_INFO("plan yaw");
  auto t1 = ros::Time::now();

  //（1）对带段轨迹进行分段
  auto&  pos      = local_data_.position_traj_; //获取该轨迹的位置
  double duration = pos.getTimeSum();           //获取该轨迹的总运行时间
  double dt_yaw  = 0.3;                         //设置航向角yaw的时间增量--300ms
  int    seg_num = ceil(duration / dt_yaw);     //计算轨迹分段数，轨迹分段数 = 该轨迹的总运行时间/时间增量
  dt_yaw         = duration / seg_num;          //设置航向角yaw的角度增量，角度增量 = 该轨迹的总运行时间/轨迹分段数

  const double            forward_t = 2.0;
  double                  last_yaw  = start_yaw(0);
  vector<Eigen::Vector3d> waypts;
  vector<int>             waypt_idx;

  //seg_num->seg_num-1点用于约束，不包括边界状态
  //（2）计算路径点waypoints的航向角yaw
  for (int i = 0; i < seg_num; ++i) {   //遍历所有的轨迹分段
    double          tc = i * dt_yaw;             //迭代计算轨迹现在的运行时刻
    Eigen::Vector3d pc = pos.evaluateDeBoorT(tc);//根据轨迹运行时刻，获得B样条的当前的控制点，注意这是当前控制点
    
    double          tf = min(duration, tc + forward_t); //迭代计算轨迹下一段的运行时刻
    Eigen::Vector3d pf = pos.evaluateDeBoorT(tf);       //根据轨迹运行时刻，获得B样条的下一段的控制点，注意这是下一段控制点
    
    Eigen::Vector3d pd = pf - pc;           //计算当前控制点与下一段控制点的有向向量

    Eigen::Vector3d waypt;
    if (pd.norm() > 1e-6) {            //当前控制点与下一段控制点的有向向量达到阈值，就计算yaw
      waypt(0) = atan2(pd(1), pd(0));  //计算量控制角的夹角，即航向yaw
      waypt(1) = waypt(2) = 0.0;
      calcNextYaw(last_yaw, waypt(0)); //计算下一个路径点的航向角yaw
    } else {
      waypt = waypts.back();
    }
    waypts.push_back(waypt);
    waypt_idx.push_back(i);
  }

  // （3）使用边界状态约束计算初始控制点
  Eigen::MatrixXd yaw(seg_num + 3, 1);
  yaw.setZero();

  Eigen::Matrix3d states2pts;
  states2pts << 1.0, -dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw, 1.0, 0.0, -(1 / 6.0) * dt_yaw * dt_yaw, 1.0,
      dt_yaw, (1 / 3.0) * dt_yaw * dt_yaw;
  yaw.block(0, 0, 3, 1) = states2pts * start_yaw;

  Eigen::Vector3d end_v = local_data_.velocity_traj_.evaluateDeBoorT(duration - 0.1);
  Eigen::Vector3d end_yaw(atan2(end_v(1), end_v(0)), 0, 0);
  calcNextYaw(last_yaw, end_yaw(0));
  yaw.block(seg_num, 0, 3, 1) = states2pts * end_yaw;

  //优化器 solve
  bspline_optimizers_[1]->setWaypoints(waypts, waypt_idx);
  int cost_func = BsplineOptimizer::SMOOTHNESS | BsplineOptimizer::WAYPOINTS;
  yaw           = bspline_optimizers_[1]->BsplineOptimizeTraj(yaw, dt_yaw, cost_func, 1, 1);

  //最后，更新轨迹信息 update traj info
  local_data_.yaw_traj_.setUniformBspline(yaw, 3, dt_yaw);
  local_data_.yawdot_traj_    = local_data_.yaw_traj_.getDerivative();
  local_data_.yawdotdot_traj_ = local_data_.yawdot_traj_.getDerivative();

  vector<double> path_yaw;
  for (int i = 0; i < waypts.size(); ++i) path_yaw.push_back(waypts[i][0]);
  plan_data_.path_yaw_    = path_yaw;
  plan_data_.dt_yaw_      = dt_yaw;
  plan_data_.dt_yaw_path_ = dt_yaw;

  std::cout << "plan heading: " << (ros::Time::now() - t1).toSec() << std::endl;
}



//计算下一个路径点的航向角yaw
void FastPlannerManager::calcNextYaw(const double& last_yaw, double& yaw) {
  // round yaw to [-PI, PI]

  double round_last = last_yaw;

  while (round_last < -M_PI) {
    round_last += 2 * M_PI;
  }
  while (round_last > M_PI) {
    round_last -= 2 * M_PI;
  }

  double diff = yaw - round_last;

  if (fabs(diff) <= M_PI) {
    yaw = last_yaw + diff;
  } else if (diff > M_PI) {
    yaw = last_yaw + diff - 2 * M_PI;
  } else if (diff < -M_PI) {
    yaw = last_yaw + diff + 2 * M_PI;
  }
}

---

总结

参考资料