自动泊车混合A星规划算法学习-HybridAstar

最近想学习一下自动泊车的规划算法，在Github上找到了一个关于HybridAstar代码，做一下学习记录。
代码地址：HAStar_ParkingPlanner

1 泊车场景创建

代码中通过文件parking_scenario.txt来配置泊车场景，包括起始位姿，终点位姿，场景边界和障碍物顶点坐标（从右上角逆时针排序）：

start_pose: -6.0, 10.5, 3.1416;
end_pose: 0.0, 3.5, 1.5708;
boundary: -15, 15, 0, 40;
obstacle: {13, 8},{1.5, 8},{1.5, 0},{13, 0};{-1.5, 8},{-13, 8},{-13, 0},{-1.5, 0};{1.5, 2},{-1.5, 2},{-1.5, 0},{1.5, 0}

画出来如下图所示，其中实心点代表车辆后轴中心，蓝色点表示起始位置，绿色点表示终点位置。障碍物顶点的连线构成一个个的障碍区域（红色区域）。

2 HAStar算法配置参数

代码通过hya_param.conf文件来定义算法运行时的一些配置参数：

xy_grid_resolution : 0.2            // 
phi_grid_resolution : 0.0
int next_node_num : 10              // 扩展节点时，每个节点往下扩展的新节点数量
step_size : 0.5                     // 节点扩展步长
traj_forward_penalty : 0.0          // 向前搜索的惩罚代价
traj_back_penalty : 0.0             // 向后搜索的惩罚代价
traj_gear_switch_penalty : 10.0     // 换挡的惩罚代价
traj_steer_penalty : 100.0          // 转向的惩罚代价
traj_steer_change_penalty : 10.0    // 转向变化的惩罚代价
grid_a_star_xy_resolution : 0.1     // 计算cost map时用的网格分辨率
node_radius : 0.5                   // 扩展节点时，节点跟障碍边缘的距离上限，超过则节点不可取
delta_t : 1.0

3 车辆参数

车辆参数通过vehicle_param.conf来配置：

front_edge_to_center : 3.89         //车辆前边缘到后轴中心的距离
back_edge_to_center : 1.043         //车辆后边缘到后轴中心的距离
length : 4.933                      //车长
width : 2.11                        //车宽
max_steer_angle : 8.20304748437     //这个值与steer_ratio的比值为等效转向轮的等效转角，用于计算
steer_ratio : 16                    //转弯半径
wheel_base : 2.8448                 //轴距

4 算法流程

代码中对HybridAstar的实现主要通过HybridAstar类来实现，利用HAStar配置参数和车辆参数创建类对象，直接调用Plan成员函数：

bool HybridAStar::Plan(
    const Pos3d& start_pose, 
    const Pos3d& end_pose,    
    const std::vector<double>& XYbounds,
    const std::vector<std::vector<Vec2d>>& obstacles_vertices_vec,   // 障碍区域顶点
    HybridAStartResult* result)   // 结果

4.1 计算障碍区域边线

遍历障碍区域，将障碍区的每条线都生成LineSegment2d对象：

struct LineSegment2d {
  Vec2d start;
  Vec2d end;
  Vec2d unit_direction;    // 单位方向向量（ux, uy）
  double heading = 0.0;    // 方向角，弧度
  double length = 0.0;
};

每个障碍区的边线作为一组，放入成员变量：

std::vector<std::vector<LineSegment2d>> obstacles_linesegments_vec_;

4.2 Node3d节点

路径搜索时每一步都会生成一个节点，代码里是Node3d类：

Node3d::Node3d(const std::vector<double> &traversed_x,
               const std::vector<double> &traversed_y,
               const std::vector<double> &traversed_phi,
               const std::vector<double> &XYbounds,
               const double xy_grid_resolution) {
  x_ = traversed_x.back();       // 节点x坐标
  y_ = traversed_y.back();       // 节点y坐标
  phi_ = traversed_phi.back();   // 节点方向

  // XYbounds in xmin, xmax, ymin, ymax
  x_grid_ = static_cast<int>((x_ - XYbounds[0]) / xy_grid_resolution);  // 网格坐标
  y_grid_ = static_cast<int>((y_ - XYbounds[2]) / xy_grid_resolution);  // 网格坐标
  phi_grid_ = static_cast<int>((phi_ - (-M_PI)) / xy_grid_resolution);  // 网格坐标

  traversed_x_ = traversed_x;      // 当前节点的路径点（从上一个节点的坐标到本节点的坐标）
  traversed_y_ = traversed_y;      // 当前节点的路径点
  traversed_phi_ = traversed_phi;  // 当前节点的路径点

  index_ = ComputeStringIndex(x_grid_, y_grid_, phi_grid_);  // 节点名称
  step_size_ = traversed_x.size();
}

4.2.1 节点有效性判断

遍历节点的每个轨迹点，计算车辆的轮廓，对每一条障碍边缘线，判断是否与车辆轮廓有交点，如果有则该节点无效。如果车辆轮廓与障碍边缘线的距离过近（代码中是1e-10），也无效。

4.3 计算cost map

代码中定义了GridSearch类，其成员函数GenerateDpMap用于计算cost map（作为后续HA*搜索时的启发值），
从终点位置开始，搜索每个网格点到终点的cost，每个网格点的搜索方向为上下左右和对角，共八个方向，如下图所示，上下左右代价为1，对角代价为1.414：

搜索过程为典型涉及三个容器变量：

std::priority_queue<std::pair<std::string, double>,
                      std::vector<std::pair<std::string, double>>, cmp> open_pq; // 待处理的网格点，按cost值从小到大排序
std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;  // 已扩展到的网格点
std::unordered_map<std::string, std::shared_ptr<Node2d>> dp_map_;   // 结果

open_pq和open_set最开始只有终点网格点，每次循环将open_pq中最小cost网格点放入dp_map_中，并弹出open_pq中的这个最小cost点，按上面所说的扩展方式扩展该最小cost网格点，检查扩展的8个网格点的有效性：

// 检查网格点是否越界
// 是否与障碍边缘的距离过近（＜node_radius）

如果网格点已经在dp_map_中，则跳过后续处理；
如果网格点不在open_set里，则将网格点加入到open_set和open_pq中；
如果网格点已经在open_set里，则更新其cost值和PreNode；

4.4 Hybrid A*

这里也涉及到三个容器变量（HybridAStar类成员）：

std::priority_queue<std::pair<std::string, double>,
                      std::vector<std::pair<std::string, double>>, cmp> open_pq_; // 待check的节点，按cost值从小到大排列 
std::unordered_map<std::string, std::shared_ptr<Node3d>> open_set_;   // 扩展到的节点
std::unordered_map<std::string, std::shared_ptr<Node3d>> close_set_;  // 已check过的节点

open_pq和open_set最开始只有起点节点。

4.4.1 节点扩展

4.4.1.1 检查当前节点到终点节点有无连接曲线

std::vector<Pos3d> GetTrajFromCurvePathsConnect(const Pos3d &start_pos,
                                                const Pos3d &end_pos,
                                                double r_min,         // 最小转弯半径
                                                double resolution) {  // 节点网格分辨率
  std::vector<Pos3d> traj_points;
  std::vector<CurvePath> curve_paths =
      CalCurvePathConnectTwoPose(start_pos, end_pos, r_min);
  if (curve_paths.empty()) {
    return traj_points;
  }

  for (const auto &path : curve_paths) {
    int traj_point_num = std::abs(path.dist) / resolution;
    for (int i = 0; i < traj_point_num; ++i) {
      const Pos3d path_start_pos{path.x, path.y, path.phi};
      Pos3d temp_pos = CalEndPosWithACurvePath(
          path_start_pos, sign(path.dist) * i * resolution, path.radius);
      traj_points.push_back(temp_pos);
    }
  }

  return traj_points;
}

其中CalCurvePathConnectTwoPose用于计算两个位姿之间有没有直线和圆弧组成的连接线（代码中也有关于RS曲线的内容）：

std::vector<CurvePath> CalCurvePathConnectTwoPose(const Pos3d &start_pos,
                                                  const Pos3d &end_pos,
                                                  double r_min) 

double rx_start_pos_end_pos = CalProjectInX(start_pos, end_pos);   // end_pos在start_pos坐标系下的坐标x
double ry_start_pos_end_pos = CalProjectInY(start_pos, end_pos);   // end_pos在start_pos坐标系下的坐标y
double ry_end_pos_start_pos = CalProjectInY(end_pos, start_pos);   // start_pos在end_pos坐标系下的坐标y
double theta = NormalizeAngle(end_pos.phi - start_pos.phi);        // end_pos在start_pos坐标系下的航向角

（1）如果终点位置在起点位姿坐标系下的x值过小（1e-4），则认为没有连接线：

std::vector<CurvePath> curve_paths;
  if (std::abs(rx_start_pos_end_pos) < k_min_value) {   
    return curve_paths;
  }

这里有疑问，实际上通过一个半圆弧是可以连接起来的，如下图所示：

（2）如果车辆终点位置在起点位姿坐标系的x轴上且与x轴基本平行，则可通过直线路径连接：

if (std::abs(ry_start_pos_end_pos) < k_min_value &&
      std::abs(theta) < k_min_value) {
    CurvePath line_path{start_pos.x, start_pos.y, start_pos.phi,
                        rx_start_pos_end_pos, 0};
    curve_paths.push_back(line_path);
    return curve_paths;
  }

（3）当终点与起点连线与起始位姿x轴夹角小于终点位姿航向角的一半时，可以用圆弧连接：

if (std::abs(std::atan(ry_start_pos_end_pos / rx_start_pos_end_pos) -
               theta / 2.0) < k_min_value) {
    double r = rx_start_pos_end_pos / std::sin(theta);
    if (std::abs(r) < r_min) {
      return curve_paths;
    }
    CurvePath circle_path{start_pos.x, start_pos.y, start_pos.phi, r * theta,
                          r};
    curve_paths.push_back(circle_path);
    return curve_paths;
  }

（4）与x轴平行同向且不在x轴上，没法只通过一段圆弧到达：

if (std::abs(1.0 - std::cos(theta)) <= k_min_value) {  
    return curve_paths;
}

（5）直线+圆弧 or 圆弧+直线

if (std::abs(ry_end_pos_start_pos) > std::abs(ry_start_pos_end_pos)) {
    double r = ry_start_pos_end_pos / (1.0 - std::cos(theta));
    double line_length =
        ry_end_pos_start_pos / std::sin(theta) - r * std::tan(theta / 2.0);

    if (std::abs(r) < r_min) {
      return curve_paths;
    }

    CurvePath line_path{start_pos.x, start_pos.y, start_pos.phi, line_length,
                        0};
    Pos3d min_pos = CalEndPosWithACurvePath(line_path);
    CurvePath circle_path{min_pos.x, min_pos.y, min_pos.phi, r * theta, r};
    curve_paths.push_back(line_path);
    curve_paths.push_back(circle_path);
  } else { // 圆弧+直线
    double r = ry_end_pos_start_pos / (1.0 - std::cos(theta));
    double line_length =
        ry_start_pos_end_pos / std::sin(theta) - r * std::tan(theta / 2.0);

    if (std::abs(r) < r_min) {
      return curve_paths;
    }

    CurvePath circle_path{start_pos.x, start_pos.y, start_pos.phi, r * theta,
                          r};
    Pos3d min_pos = CalEndPosWithACurvePath(circle_path);
    CurvePath line_path{min_pos.x, min_pos.y, min_pos.phi, line_length, 0};
    curve_paths.push_back(circle_path);
    curve_paths.push_back(line_path);
  }

4.4.1.2 若有当前节点与终点连接曲线

生成离散的路径点，计算每个路径点的有效性（参考4.2.1），若路径有效，则生成最终的节点，并停止节点扩展，生成最终的路径结果。

4.4.1.3 若无当前节点与终点连接曲线

4.4.1.3.1 当前节点往下扩展新的节点

std::shared_ptr<Node3d> HybridAStar::Next_node_generator(
    std::shared_ptr<Node3d> current_node, size_t next_node_index)

根据配置文件中参数，代码中实际每次只往前扩展了1个step_size:

for (size_t i = 0; i < arc / step_size_; ++i) {
    
    const double next_x = last_x + traveled_distance * std::cos(last_phi); // 代码中traveled_distance = step_size_，向前取正，向后取负
    const double next_y = last_y + traveled_distance * std::sin(last_phi);
    const double next_phi = math::NormalizeAngle(
        last_phi +
        traveled_distance / vehicle_param_.wheel_base * std::tan(steering));  // 弧长/R
    intermediate_x.push_back(next_x);
    intermediate_y.push_back(next_y);
    intermediate_phi.push_back(next_phi);
    last_x = next_x;
    last_y = next_y;
    last_phi = next_phi;
  }

这里是把圆弧离散成了一段段（每段长度为traveled_distance）的。

4.4.1.3.2 新扩展的节点的cost

新节点的cost包括：
（1）路径cost，上一节点的路径cost加上上一节点到该节点的cost：

double HybridAStar::TrajCost(std::shared_ptr<Node3d> current_node,
                             std::shared_ptr<Node3d> next_node) {
  // evaluate cost on the trajectory and add current cost
  double piecewise_cost = 0.0;
  if (next_node->GetDirec()) {  // true 表示向前走，false表示后退
    piecewise_cost += static_cast<double>(next_node->GetStepSize() - 1) *  // 节点的stepsize指的是离散步的步数
                      step_size_ * traj_forward_penalty_;   // 往前走的代价
  } else {
    piecewise_cost += static_cast<double>(next_node->GetStepSize() - 1) *
                      step_size_ * traj_back_penalty_;      // 往后退的代价
  }
  if (current_node->GetDirec() != next_node->GetDirec()) {  // 改变行驶方向
    piecewise_cost += traj_gear_switch_penalty_;
  }
  piecewise_cost += traj_steer_penalty_ * std::abs(next_node->GetSteer());  // 转角代价，转角越大，代价越大
  piecewise_cost += traj_steer_change_penalty_ *  
                    std::abs(next_node->GetSteer() - current_node->GetSteer());   // 变转角代价
  return piecewise_cost;
}

（2）到终点的cost（启发值），等于2.4.3中计算得到的cost；

4.4.2 生成路径结果

从final_node_开始依次往前遍历到起点，得到整条路径的离散点的（x, y, phi）；

struct HybridAStartResult {
  std::vector<double> x;       //路径离散点x坐标
  std::vector<double> y;       //路径离散点y坐标
  std::vector<double> phi;     //路径离散点航向角
  std::vector<double> v;
  std::vector<double> a;
  std::vector<double> steer;
  std::vector<double> accumulated_s;
};

对整条路径进行拆分，将同一挡位下的路径点组成一个HybridAStartResult对象；为每一个HybridAStartResult对象计算速度和加速度：

bool HybridAStar::GenerateSpeedAcceleration(HybridAStartResult* result) 

速度计算：
每段路径的起点和终点速度都是0；

for (size_t i = 1; i + 1 < x_size; ++i) {
    double discrete_v = (((result->x[i + 1] - result->x[i]) / delta_t_) *
                             std::cos(result->phi[i]) +
                         ((result->x[i] - result->x[i - 1]) / delta_t_) *
                             std::cos(result->phi[i])) /
                            2.0 +        // x方向速度在当前航向上的投影
                        (((result->y[i + 1] - result->y[i]) / delta_t_) *
                             std::sin(result->phi[i]) +
                         ((result->y[i] - result->y[i - 1]) / delta_t_) *
                             std::sin(result->phi[i])) /
                            2.0;         // y方向速度在当前航向上的投影
    result->v.push_back(discrete_v);
  }

加速度计算：

for (size_t i = 0; i + 1 < x_size; ++i) {
    const double discrete_a = (result->v[i + 1] - result->v[i]) / delta_t_;
    result->a.push_back(discrete_a);
  }

转向角计算：

for (size_t i = 0; i + 1 < x_size; ++i) {
    double discrete_steer = (result->phi[i + 1] - result->phi[i]) *
                            vehicle_param_.wheel_base / step_size_;
    if (result->v[i] > 0.0) {
      discrete_steer = std::atan(discrete_steer);
    } else {
      discrete_steer = std::atan(-discrete_steer);
    }
    result->steer.push_back(discrete_steer);
  }

将结果画出来：