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

作者：weixin_40725706 | 2024-04-20 19:00:57

踩

fast-planner

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

一、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<double, 6, 1> 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;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

主函数

/*
  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)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

读取传入参数

  //读取传入参数
  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;            //扩展点数计数
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

启发函数

该文利用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)$
状态方程： $\dot{S}_k=f_s(S_k,j_k)=(v_k,a_k,j_k)$
构建Hamiltonian函数和协态方程：

求解costate的微分方程得

最佳输入轨迹J*(t)通过求最小化Hamiltonian函数(PMP原理)获得，即
在这里插入图片描述

j*(t)为加加速度的轨迹， $S_0=(p_0,v_0,a_0)$ 通过积分可以获 $a^*(t),v^*(t),p*(t)$
在这里插入图片描述
目标状态 $S_f=(p_f,v_f,a_f)$ ，
请添加图片描述

解得：

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

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

/*
  计算启发函数
    parameters：
      - x1,x2:起点和终点状态
      - optimal_time:达到最终点的最优时间
    return:
      - 代价值
*/
double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2,
                                           double&amp; 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&lt;double&gt; 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<Infinity>() / v_max;
  ts.push_back(t_bar);

  double cost = 100000000;
  double t_d = t_bar;

	//将根带回启发函数求最优Ts
  for (auto t : ts) {
    if (t &lt; t_bar) continue;
    double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;
    if (c &lt; cost) {
      cost = c;
      t_d = t;
    }
  }

  optimal_time = t_d;

  return 1.0 * (1 + tie_breaker_) * cost;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43

启发函数对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)) <= tolerance &&
        abs(cur_node->index(1) - end_index(1)) <= tolerance &&
        abs(cur_node->index(2) - end_index(2)) <= tolerance;
    if (reach_horizon || near_end) {
      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) 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

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

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

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

/* ---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<double, 6, 1> cur_state = cur_node->state;  //当前节点状态
    Eigen::Matrix<double, 6, 1> pro_state;    //下一状态
    vector<PathNodePtr> tmp_expand_nodes;     //临时扩展节点列表
    Eigen::Vector3d um;     //控制量，本文是加速度
    double pro_t;   //扩展节点的时间戳
    vector<Eigen::Vector3d> inputs;
    vector<double> durations; //输入控制量的持续时间

    //3、判断节点没有被扩展过，把这个节点存下来
    if (init_search) {
      inputs.push_back(start_acc_);
      for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
           tau += time_res_init * init_max_tau_)
        durations.push_back(tau);
      init_search = false;
    } else {
      for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
        for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
          for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res) {
            um << ax, ay, az;
            inputs.push_back(um);
          }
      for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)
        durations.push_back(tau);
    }

    //4、状态传递循环迭代，检查速度约束，检查碰撞，都通过的话，就计算当前节点的g_score以及f_score.并且对重复的扩展节点进行剪枝
    // cout << "cur state:" << cur_state.head(3).transpose() << endl;
    for (int i = 0; i < inputs.size(); ++i)
      for (int j = 0; j < durations.size(); ++j) {
        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 << "box" << std::endl;
          continue;
        }

        // Check if in close set
        Eigen::Vector3i pro_id = posToIndex(pro_pos);
        int pro_t_id = timeToIndex(pro_t);
        PathNodePtr pro_node =
            dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);
        if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET) {
          if (init_search) std::cout << "close" << std::endl;
          continue;
        }

        // Check maximal velocity
        Eigen::Vector3d pro_v = pro_state.tail(3);
        if (fabs(pro_v(0)) > max_vel_ || fabs(pro_v(1)) > max_vel_ || fabs(pro_v(2)) > max_vel_) {
          if (init_search) std::cout << "vel" << std::endl;
          continue;
        }

        // Check not in the same voxel
        Eigen::Vector3i diff = pro_id - cur_node->index;
        int diff_time = pro_t_id - cur_node->time_idx;
        if (diff.norm() == 0 && ((!dynamic) || diff_time == 0)) {
          if (init_search) std::cout << "same" << std::endl;
          continue;
        }

        // Check safety
        Eigen::Vector3d pos;
        Eigen::Matrix<double, 6, 1> xt;
        bool is_occ = false;
        for (int k = 1; k <= check_num_; ++k) {
          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 << "safe" << std::endl;
          continue;
        }

        //计算代价值
        double time_to_goal, tmp_g_score, tmp_f_score;
        //计算当前节点的真实代价
        tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;
        //计算启发函数代价
        tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal);

        /* ----------5)在循环中对比扩展节点,进行节点剪枝---------- */
        // Compare nodes expanded from the same parent
        bool prune = false;     //剪枝标志位
        for (int j = 0; j < tmp_expand_nodes.size(); ++j) {
          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 < expand_node->f_score) {
              expand_node->f_score = tmp_f_score;
              expand_node->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 << "run out of memory." << endl;
              return NO_PATH;
            }
          }
          // 如果不用剪枝的话，则首先判断当前临时扩展节点pro_node是否出现在open集中 
          else if (pro_node->node_state == IN_OPEN_SET) {
            if (tmp_g_score < pro_node->g_score) {
              // 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;
              if (dynamic) pro_node->time = cur_node->time + tau;
            }
          } 
          // 如果不是存在于open集的话，则可以直接将pro_node加入open集中
          else {
            cout << "error type in searching: " << pro_node->node_state << endl;
          }
        }
      }
    // init_search = false;
  }

  cout << "open set empty, no path!" << endl;
  cout << "use node num: " << use_node_num_ << endl;
  cout << "iter num: " << iter_num_ << endl;
  return NO_PATH;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183

获得规划得到的路径点

kinodynamic A*的最后一步

/*  作用：
      完成路径搜索后，按照预设的时间分辨率delta_t，通过节点回溯和状态前向积分得到分辨率最高的路径点。
    param：
      delta_t:时间分辨率
    return：
      state_list=节点扩张的回溯轨迹+两点边界轨迹
*/
std::vector<Eigen::Vector3d> KinodynamicAstar::getKinoTraj(double delta_t) {
  vector<Vector3d> state_list;

  /* ---------- get traj of searching ---------- */
  PathNodePtr node = path_nodes_.back();
  Matrix<double, 6, 1> x0, xt;

  while (node->parent != NULL) {
    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 <= t_shot_; t += delta_t) {
      for (int j = 0; j < 4; j++)
        time(j) = pow(t, j);

      for (int dim = 0; dim < 3; dim++) {
        poly1d = coef_shot_.row(dim);
        coord(dim) = poly1d.dot(time);
      }
      state_list.push_back(coord);
    }
  }

  return state_list;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

第一部分结束，未完待续。。。

本文内容由网友自发贡献，转载请注明出处：【wpsshop博客】