Fast-Planner(一)前端详解kinodynamic A*

作为无人机自主运动的入门路径规划算法，详细表述了路径规划的前端和后端，前端为路径搜索，后端未路径优化。具体参考代码。

一、kinodynamic a_star 路径搜索

这里参考的文章是Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments

总的可以概括为融合动力学约束的A*搜索，通过共轭梯度下降求局部最优。

解析kinodynamic_astar.cpp

Pathnode类(文章中motion Primitives)反向链表

class PathNode {public:  /* -------------------- */  Eigen::Vector3i index;  //voxel坐标  Eigen::Matrix state;  //基元状态,分别为xyz及其一阶导(vel)  double g_score, f_score;   //代价函数、启发函数  Eigen::Vector3d input;    //输入，分别为xyz上的二阶导(acc)  double duration;    //输入持续时间  double time;  // dyn  int time_idx;  PathNode* parent;   //父节点  char node_state;  /* -------------------- */  PathNode() {    parent = NULL;    node_state = NOT_EXPAND;  }  ~PathNode(){};  EIGEN_MAKE_ALIGNED_OPERATOR_NEW};typedef PathNode* PathNodePtr;

主函数

/*  kinodynamic A*的主函数    parameter：      - start_pt：起点位置      - start_v: 起始速度      - start_a: 起始加速度      - end_pt:  终点位置      - end_v:   终点速度      - init：   初始化成功标志位      - dynamic：动静规划标志位      - time_start:起始时间*/int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a,                             Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic,                             double time_start)

读取传入参数

  //读取传入参数  start_vel_ = start_v;  start_acc_ = start_a;  PathNodePtr cur_node = path_node_pool_[0];  //取出第一个路径点赋给当前节点  cur_node->parent = NULL;  //父节点  cur_node->state.head(3) = start_pt;  cur_node->state.tail(3) = start_v;  cur_node->index = posToIndex(start_pt); //获得全局坐标系下的位置索引  cur_node->g_score = 0.0;    //记录当前成本代价  Eigen::VectorXd end_state(6);  Eigen::Vector3i end_index;  double time_to_goal;    //路径规划时间  end_state.head(3) = end_pt;  end_state.tail(3) = end_v;  end_index = posToIndex(end_pt);  cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);  //计算启发函数  cur_node->node_state = IN_OPEN_SET; //标记为OPEN  open_set_.push(cur_node);      //将该点放入OPEN集，即论文中的C  use_node_num_ += 1;            //扩展点数计数

启发函数

该文利用Pontryagins minimum principle计算从Xc到Xg轨迹的启发函数。笔者根据自身能力进行推导，具体参考A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation及D. P. Bertsekas的《动态规划和最优控制》。

3D运动的cost function，Jk为投影到三轴上的加加速度

状态： S k = ( p k , v k , a k ) S_k=(p_k,v_k,a_k) Sk​=(pk​,vk​,ak​)

状态方程： S ˙ k = f s ( S k , j k ) = ( v k , a k , j k ) \dot{S}_k=f_s(S_k,j_k)=(v_k,a_k,j_k) S˙k​=fs​(Sk​,jk​)=(vk​,ak​,jk​)

构建Hamiltonian函数和协态方程：

求解costate的微分方程得

最佳输入轨迹J*(t)通过求最小化Hamiltonian函数(PMP原理)获得，即

j*(t)为加加速度的轨迹， S 0 = ( p 0 , v 0 , a 0 ) S_0=(p_0,v_0,a_0) S0​=(p0​,v0​,a0​)通过积分可以获 a ∗ ( t ) , v ∗ ( t ) , p ∗ ( t ) a^*(t),v^*(t),p*(t) a∗(t),v∗(t),p∗(t)

目标状态 S f = ( p f , v f , a f ) S_f=(p_f,v_f,a_f) Sf​=(pf​,vf​,af​)，

解得：

FAST-Planner当中最小化加速度，故与上述最小化加加速度不同，文中给出的公式如下：

启发函数代码(即上述使J取最小的T)：

/*  计算启发函数    parameters：      - x1,x2:起点和终点状态      - optimal_time:达到最终点的最优时间    return:      - 代价值*/double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2,                                           double& optimal_time) {  const Vector3d dp = x2.head(3) - x1.head(3);  //位置变化量  const Vector3d v0 = x1.segment(3, 3);         //起点速度矩阵  const Vector3d v1 = x2.segment(3, 3);         //终点速度矩阵  double c1 = -36 * dp.dot(dp);  double c2 = 24 * (v0 + v1).dot(dp);  double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));  double c4 = 0;  double c5 = w_time_;  std::vector<double> ts = quartic(c5, c4, c3, c2, c1); //求解获得极值点的T  double v_max = max_vel_ * 0.5;  double t_bar = (x1.head(3) - x2.head(3)).lpNorm() / v_max;  ts.push_back(t_bar);  double cost = 100000000;  double t_d = t_bar;//将根带回启发函数求最优Ts  for (auto t : ts) {    if (t < t_bar) continue;    double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;    if (c < cost) {      cost = c;      t_d = t;    }  }  optimal_time = t_d;  return 1.0 * (1 + tie_breaker_) * cost;}

启发函数对T求导获得极值点或者鞍点对应的时间T，然后把多个根代回启发函数获得最优时间T。

搜索

循环搜索第一步判断当前节点是否满足停止搜索的要求

   // 判断但前节点是否超出horizon或是离终点较近了    bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;    bool near_end = abs(cur_node->index(0) - end_index(0)) index(1) - end_index(1)) index(2) - end_index(2))       terminate_node = cur_node;  //判断其中一个达到了要求，将该节点设为终点      retrievePath(terminate_node);//从终点到起点的反向路径存储到path_nodes_      if (near_end) {        // Check whether shot traj exist        estimateHeuristic(cur_node-state, end_state, time_to_goal);  //计算优化的最小时间        computeShotTraj(cur_node->state, end_state, time_to_goal);    //带入优化的时间判断轨迹是否合理        if (init_search) ROS_ERROR("Shot in first search loop!");      }    }    if (reach_horizon)     if (near_end) 

其中当满足终点条件时，要重新检查一下轨迹是否合理，即将轨迹带入地图，用到下面这个函数

//将计算得到的最优轨迹带入地图中检查轨迹是否安全，有没有发生碰撞，速度、加速度是否超过限制bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2,                                       double time_to_goal) 

当节点不满足终止条件，需要扩展节点，以当前节点为根，离散输入控制量，得到下一状态。

/* ---3）若当前节点没有抵达终点，就要进行节点扩张 ---------- */    //1、在open_list中删除节点，并在close_list中添加节点    open_set_.pop();  //弹出该节点    cur_node->node_state = IN_CLOSE_SET;  //将该节点放入close节点当中    iter_num_ += 1;    //2、初始化状态传递    double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0; //时间常数    Eigen::Matrix cur_state = cur_node->state;  //当前节点状态    Eigen::Matrix pro_state;    //下一状态    vector tmp_expand_nodes;     //临时扩展节点列表    Eigen::Vector3d um;     //控制量，本文是加速度    double pro_t;   //扩展节点的时间戳    vector inputs;    vector durations; //输入控制量的持续时间    //3、判断节点没有被扩展过，把这个节点存下来    if (init_search) {      inputs.push_back(start_acc_);      for (double tau = time_res_init * init_max_tau_; tau       for (double ax = -max_acc_; ax             um         um = inputs[i];        double tau = durations[j];        stateTransit(cur_state, pro_state, um, tau);  //状态传递，通过前向积分得到扩展节点的位置和速度        pro_t = cur_node-time + tau;        // Check inside map range        Eigen::Vector3d pro_pos = pro_state.head(3);        if (!edt_environment_-sdf_map_-isInBox(pro_pos)) {          if (init_search) std::cout           if (init_search) std::cout           if (init_search) std::cout           if (init_search) std::cout           double dt = tau * double(k) / double(check_num_);          stateTransit(cur_state, xt, um, dt);          pos = xt.head(3);          if (edt_environment_-sdf_map_-getInflateOccupancy(pos) == 1 ||              !edt_environment_-sdf_map_-isInBox(pos)) {            is_occ = true;            break;          }          if (!optimistic_ && edt_environment_-sdf_map_-getOccupancy(pos) == SDFMap::UNKNOWN) {            is_occ = true;            break;          }        }        if (is_occ) {          if (init_search) std::cout           PathNodePtr expand_node = tmp_expand_nodes[j];          // 首先判断当前临时扩展节点与current node的其他临时扩展节点是否在同一个voxel中，如果是的话就是扩展的节点多余了，就要进行剪枝。          if ((pro_id - expand_node-index).norm() == 0 &&              ((!dynamic) || pro_t_id == expand_node-time_idx)) {            prune = true;            if (tmp_f_score g_score = tmp_g_score;              expand_node->state = pro_state;              expand_node->input = um;              expand_node->duration = tau;              if (dynamic) expand_node->time = cur_node->time + tau;            }            break;          }        }        // This node end up in a voxel different from others        if (!prune) {          if (pro_node == NULL) {            pro_node = path_node_pool_[use_node_num_];            pro_node->index = pro_id;            pro_node->state = pro_state;            pro_node->f_score = tmp_f_score;            pro_node->g_score = tmp_g_score;            pro_node->input = um;            pro_node->duration = tau;            pro_node->parent = cur_node;            pro_node->node_state = IN_OPEN_SET;            if (dynamic) {              pro_node->time = cur_node->time + tau;              pro_node->time_idx = timeToIndex(pro_node->time);            }            open_set_.push(pro_node);            if (dynamic)              expanded_nodes_.insert(pro_id, pro_node->time, pro_node);            else              expanded_nodes_.insert(pro_id, pro_node);            tmp_expand_nodes.push_back(pro_node);            use_node_num_ += 1;            if (use_node_num_ == allocate_num_) {              cout             if (tmp_g_score index = pro_id;              pro_node->state = pro_state;              pro_node->f_score = tmp_f_score;              pro_node->g_score = tmp_g_score;              pro_node->input = um;              pro_node->duration = tau;              pro_node->parent = cur_node;              if (dynamic) pro_node->time = cur_node->time + tau;            }          }           // 如果不是存在于open集的话，则可以直接将pro_node加入open集中          else {            cout   vector    Vector3d ut = node-input;    double duration = node-duration;    x0 = node-parent-state;    for (double t = duration; t = -1e-5; t -= delta_t) {      stateTransit(x0, xt, ut, t);      state_list.push_back(xt.head(3));    }    node = node-parent;  }  reverse(state_list.begin(), state_list.end());  /* ---------- get traj of one shot ---------- */  if (is_shot_succ_) {    Vector3d coord;    VectorXd poly1d, time(4);    for (double t = delta_t; t       for (int j = 0; j