最近想学习一下自动泊车的规划算法,在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);
}
将结果画出来: