基于ros中通过slam建立的栅格地图,使用混合A*进行路径规划。
首先是run_hybrid_astar.cpp:
#include "hybrid_a_star/hybrid_a_star_flow.h"
#include "3rd/backward.hpp"
#include <ros/ros.h>namespace backward {
backward::SignalHandling sh;
}int main(int argc, char **argv) {ros::init(argc, argv, "run_hybrid_astar");ros::NodeHandle node_handle("~");HybridAStarFlow kinodynamic_astar_flow(node_handle);ros::Rate rate(10);while (ros::ok()) {kinodynamic_astar_flow.Run();ros::spinOnce();rate.sleep();}ros::shutdown();return 0;
}
创建了一个实例kinodynamic_astar_flow,以固定频率运行算法并处理ROS事件。
hybrid_a_star.cpp:
#include "hybrid_a_star/hybrid_a_star.h"
#include "hybrid_a_star/display_tools.h"
#include "hybrid_a_star/timer.h"
#include "hybrid_a_star/trajectory_optimizer.h"#include <iostream>
// 参数分别为:steering_angle: 最大转向角度,转化为弧度使用。
// steering_angle_discrete_num: 转向角度的离散数量,控制转向角度的分辨率。
// segment_length: 从一个节点到另一个节点的路径段长度。
// segment_length_discrete_num: 路径段长度的离散数量,影响路径的平滑度和搜索的精度。
// wheel_base: 车辆的轴距,用于计算车辆转弯时的动态约束。
// steering_penalty, reversing_penalty, steering_change_penalty: 分别对应转向、倒车和转向变化的惩罚因子,用于在成本计算中对这些行为进行惩罚,以生成更符合实际驾驶情况的路径。
// shot_distance: 射线距离,用于在距离目标状态一定范围内时,采用解析解法(Reeds-Shepp路径)直接连接起点和终点。
HybridAStar::HybridAStar(double steering_angle, int steering_angle_discrete_num, double segment_length,int segment_length_discrete_num, double wheel_base, double steering_penalty,double reversing_penalty, double steering_change_penalty, double shot_distance,int grid_size_phi) {wheel_base_ = wheel_base;segment_length_ = segment_length;steering_radian_ = steering_angle * M_PI / 180.0; // angle to radiansteering_discrete_num_ = steering_angle_discrete_num;steering_radian_step_size_ = steering_radian_ / steering_discrete_num_;move_step_size_ = segment_length / segment_length_discrete_num;segment_length_discrete_num_ = static_cast<int>(segment_length_discrete_num);steering_penalty_ = steering_penalty;steering_change_penalty_ = steering_change_penalty;reversing_penalty_ = reversing_penalty;shot_distance_ = shot_distance;CHECK_EQ(static_cast<float>(segment_length_discrete_num_ * move_step_size_), static_cast<float>(segment_length_))<< "The segment length must be divisible by the step size. segment_length: "<< segment_length_ << " | step_size: " << move_step_size_;// rs曲线的计算rs_path_ptr_ = std::make_shared<RSPath>(wheel_base_ / std::tan(steering_radian_));tie_breaker_ = 1.0 + 1e-3;STATE_GRID_SIZE_PHI_ = grid_size_phi;ANGULAR_RESOLUTION_ = 360.0 / STATE_GRID_SIZE_PHI_ * M_PI / 180.0;
}HybridAStar::~HybridAStar() {ReleaseMemory();
}// Init 函数:
// 初始化地图参数,包括地图的边界和分辨率。
// 初始化状态节点地图,用于记录搜索过程中的每个状态。
// 状态节点图和栅格图,需要根据接收到的地图信息进行重新处理
void HybridAStar::Init(double x_lower, double x_upper, double y_lower, double y_upper,double state_grid_resolution, double map_grid_resolution) {SetVehicleShape(4.7, 2.0, 1.3); // 车辆参数,给定车辆长度宽度和后轴到车位的距离来定义。map_x_lower_ = x_lower;map_x_upper_ = x_upper;map_y_lower_ = y_lower;map_y_upper_ = y_upper;STATE_GRID_RESOLUTION_ = state_grid_resolution;MAP_GRID_RESOLUTION_ = map_grid_resolution;STATE_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / STATE_GRID_RESOLUTION_);STATE_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / STATE_GRID_RESOLUTION_);MAP_GRID_SIZE_X_ = std::floor((map_x_upper_ - map_x_lower_) / MAP_GRID_RESOLUTION_);MAP_GRID_SIZE_Y_ = std::floor((map_y_upper_ - map_y_lower_) / MAP_GRID_RESOLUTION_);if (map_data_) {delete[] map_data_;map_data_ = nullptr;}map_data_ = new uint8_t[MAP_GRID_SIZE_X_ * MAP_GRID_SIZE_Y_];if (state_node_map_) {for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {if (state_node_map_[i] == nullptr)continue;for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {if (state_node_map_[i][j] == nullptr)continue;for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {if (state_node_map_[i][j][k] != nullptr) {delete state_node_map_[i][j][k];state_node_map_[i][j][k] = nullptr;}}delete[] state_node_map_[i][j];state_node_map_[i][j] = nullptr;}delete[] state_node_map_[i];state_node_map_[i] = nullptr;}delete[] state_node_map_;state_node_map_ = nullptr;}state_node_map_ = new StateNode::Ptr **[STATE_GRID_SIZE_X_];for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {state_node_map_[i] = new StateNode::Ptr *[STATE_GRID_SIZE_Y_];for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {state_node_map_[i][j] = new StateNode::Ptr[STATE_GRID_SIZE_PHI_];for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {state_node_map_[i][j][k] = nullptr;}}}
}// LineCheck 是一个低级函数,用于执行直线上的障碍物检测,通常被CheckCollision调用。
inline bool HybridAStar::LineCheck(double x0, double y0, double x1, double y1) {bool steep = (std::abs(y1 - y0) > std::abs(x1 - x0));if (steep) {std::swap(x0, y0);std::swap(y1, x1);}if (x0 > x1) {std::swap(x0, x1);std::swap(y0, y1);}auto delta_x = x1 - x0;auto delta_y = std::abs(y1 - y0);auto delta_error = delta_y / delta_x;decltype(delta_x) error = 0;decltype(delta_x) y_step;auto yk = y0;if (y0 < y1) {y_step = 1;} else {y_step = -1;}auto N = static_cast<unsigned int>(x1 - x0);for (unsigned int i = 0; i < N; ++i) {if (steep) {if (HasObstacle(Vec2i(yk, x0 + i * 1.0))|| BeyondBoundary(Vec2d(yk * MAP_GRID_RESOLUTION_,(x0 + i) * MAP_GRID_RESOLUTION_))) {return false;}} else {if (HasObstacle(Vec2i(x0 + i * 1.0, yk))|| BeyondBoundary(Vec2d((x0 + i) * MAP_GRID_RESOLUTION_,yk * MAP_GRID_RESOLUTION_))) {return false;}}error += delta_error;if (error >= 0.5) {yk += y_step;error = error - 1.0;}}return true;
}// CheckCollision 用于检测车辆沿着给定路径是否与障碍物发生碰撞。
bool HybridAStar::CheckCollision(const double &x, const double &y, const double &theta) {Timer timer;Mat2d R;R << std::cos(theta), -std::sin(theta),std::sin(theta), std::cos(theta);MatXd transformed_vehicle_shape;transformed_vehicle_shape.resize(8, 1);for (unsigned int i = 0; i < 4u; ++i) {transformed_vehicle_shape.block<2, 1>(i * 2, 0)= R * vehicle_shape_.block<2, 1>(i * 2, 0) + Vec2d(x, y);}Vec2i transformed_pt_index_0 = Coordinate2MapGridIndex(transformed_vehicle_shape.block<2, 1>(0, 0));Vec2i transformed_pt_index_1 = Coordinate2MapGridIndex(transformed_vehicle_shape.block<2, 1>(2, 0));Vec2i transformed_pt_index_2 = Coordinate2MapGridIndex(transformed_vehicle_shape.block<2, 1>(4, 0));Vec2i transformed_pt_index_3 = Coordinate2MapGridIndex(transformed_vehicle_shape.block<2, 1>(6, 0));double y1, y0, x1, x0;// pt1 -> pt0x0 = static_cast<double>(transformed_pt_index_0.x());y0 = static_cast<double>(transformed_pt_index_0.y());x1 = static_cast<double>(transformed_pt_index_1.x());y1 = static_cast<double>(transformed_pt_index_1.y());if (!LineCheck(x1, y1, x0, y0)) {return false;}// pt2 -> pt1x0 = static_cast<double>(transformed_pt_index_1.x());y0 = static_cast<double>(transformed_pt_index_1.y());x1 = static_cast<double>(transformed_pt_index_2.x());y1 = static_cast<double>(transformed_pt_index_2.y());if (!LineCheck(x1, y1, x0, y0)) {return false;}// pt3 -> pt2x0 = static_cast<double>(transformed_pt_index_2.x());y0 = static_cast<double>(transformed_pt_index_2.y());x1 = static_cast<double>(transformed_pt_index_3.x());y1 = static_cast<double>(transformed_pt_index_3.y());if (!LineCheck(x1, y1, x0, y0)) {return false;}// pt0 -> pt3x0 = static_cast<double>(transformed_pt_index_0.x());y0 = static_cast<double>(transformed_pt_index_0.y());x1 = static_cast<double>(transformed_pt_index_3.x());y1 = static_cast<double>(transformed_pt_index_3.y());if (!LineCheck(x0, y0, x1, y1)) {return false;}check_collision_use_time += timer.End();num_check_collision++;return true;
}bool HybridAStar::HasObstacle(const int grid_index_x, const int grid_index_y) const {return (grid_index_x >= 0 && grid_index_x < MAP_GRID_SIZE_X_&& grid_index_y >= 0 && grid_index_y < MAP_GRID_SIZE_Y_&& (map_data_[grid_index_y * MAP_GRID_SIZE_X_ + grid_index_x] == 1));
}bool HybridAStar::HasObstacle(const Vec2i &grid_index) const {int grid_index_x = grid_index[0];int grid_index_y = grid_index[1];return (grid_index_x >= 0 && grid_index_x < MAP_GRID_SIZE_X_&& grid_index_y >= 0 && grid_index_y < MAP_GRID_SIZE_Y_&& (map_data_[grid_index_y * MAP_GRID_SIZE_X_ + grid_index_x] == 1));
}void HybridAStar::SetObstacle(unsigned int x, unsigned int y) {if (x < 0u || x > static_cast<unsigned int>(MAP_GRID_SIZE_X_)|| y < 0u || y > static_cast<unsigned int>(MAP_GRID_SIZE_Y_)) {return;}map_data_[x + y * MAP_GRID_SIZE_X_] = 1;
}void HybridAStar::SetObstacle(const double pt_x, const double pt_y) {if (pt_x < map_x_lower_ || pt_x > map_x_upper_ ||pt_y < map_y_lower_ || pt_y > map_y_upper_) {return;}int grid_index_x = static_cast<int>((pt_x - map_x_lower_) / MAP_GRID_RESOLUTION_);int grid_index_y = static_cast<int>((pt_y - map_y_lower_) / MAP_GRID_RESOLUTION_);map_data_[grid_index_x + grid_index_y * MAP_GRID_SIZE_X_] = 1;
}// 定义车辆形状,用于碰撞检测。车辆被假设为矩形,通过给定车辆长度、宽度和后轴到车尾的距离来定义。
void HybridAStar::SetVehicleShape(double length, double width, double rear_axle_dist) {vehicle_shape_.resize(8);vehicle_shape_.block<2, 1>(0, 0) = Vec2d(-rear_axle_dist, width / 2);vehicle_shape_.block<2, 1>(2, 0) = Vec2d(length - rear_axle_dist, width / 2);vehicle_shape_.block<2, 1>(4, 0) = Vec2d(length - rear_axle_dist, -width / 2);vehicle_shape_.block<2, 1>(6, 0) = Vec2d(-rear_axle_dist, -width / 2);const double step_size = move_step_size_;const auto N_length = static_cast<unsigned int>(length / step_size);const auto N_width = static_cast<unsigned int> (width / step_size);vehicle_shape_discrete_.resize(2, (N_length + N_width) * 2u);const Vec2d edge_0_normalized = (vehicle_shape_.block<2, 1>(2, 0)- vehicle_shape_.block<2, 1>(0, 0)).normalized();for (unsigned int i = 0; i < N_length; ++i) {vehicle_shape_discrete_.block<2, 1>(0, i + N_length)= vehicle_shape_.block<2, 1>(4, 0) - edge_0_normalized * i * step_size;vehicle_shape_discrete_.block<2, 1>(0, i)= vehicle_shape_.block<2, 1>(0, 0) + edge_0_normalized * i * step_size;}const Vec2d edge_1_normalized = (vehicle_shape_.block<2, 1>(4, 0)- vehicle_shape_.block<2, 1>(2, 0)).normalized();for (unsigned int i = 0; i < N_width; ++i) {vehicle_shape_discrete_.block<2, 1>(0, (2 * N_length) + i)= vehicle_shape_.block<2, 1>(2, 0) + edge_1_normalized * i * step_size;vehicle_shape_discrete_.block<2, 1>(0, (2 * N_length) + i + N_width)= vehicle_shape_.block<2, 1>(6, 0) - edge_1_normalized * i * step_size;}
}__attribute__((unused)) Vec2d HybridAStar::CoordinateRounding(const Vec2d &pt) const {return MapGridIndex2Coordinate(Coordinate2MapGridIndex(pt));
}Vec2d HybridAStar::MapGridIndex2Coordinate(const Vec2i &grid_index) const {Vec2d pt;pt.x() = ((double) grid_index[0] + 0.5) * MAP_GRID_RESOLUTION_ + map_x_lower_;pt.y() = ((double) grid_index[1] + 0.5) * MAP_GRID_RESOLUTION_ + map_y_lower_;return pt;
}Vec3i HybridAStar::State2Index(const Vec3d &state) const {Vec3i index;index[0] = std::min(std::max(int((state[0] - map_x_lower_) / STATE_GRID_RESOLUTION_), 0), STATE_GRID_SIZE_X_ - 1);index[1] = std::min(std::max(int((state[1] - map_y_lower_) / STATE_GRID_RESOLUTION_), 0), STATE_GRID_SIZE_Y_ - 1);index[2] = std::min(std::max(int((state[2] - (-M_PI)) / ANGULAR_RESOLUTION_), 0), STATE_GRID_SIZE_PHI_ - 1);return index;
}Vec2i HybridAStar::Coordinate2MapGridIndex(const Vec2d &pt) const {Vec2i grid_index;grid_index[0] = int((pt[0] - map_x_lower_) / MAP_GRID_RESOLUTION_);grid_index[1] = int((pt[1] - map_y_lower_) / MAP_GRID_RESOLUTION_);return grid_index;
}// 生成当前节点的所有可能邻居节点,并检查它们是否可行(没有碰撞,没有越界)。
void HybridAStar::GetNeighborNodes(const StateNode::Ptr &curr_node_ptr,std::vector<StateNode::Ptr> &neighbor_nodes) {neighbor_nodes.clear();for (int i = -steering_discrete_num_; i <= steering_discrete_num_; ++i) {VectorVec3d intermediate_state;bool has_obstacle = false;double x = curr_node_ptr->state_.x();double y = curr_node_ptr->state_.y();double theta = curr_node_ptr->state_.z();const double phi = i * steering_radian_step_size_;// forwardfor (int j = 1; j <= segment_length_discrete_num_; j++) {DynamicModel(move_step_size_, phi, x, y, theta);intermediate_state.emplace_back(Vec3d(x, y, theta));if (!CheckCollision(x, y, theta)) {has_obstacle = true;break;}}Vec3i grid_index = State2Index(intermediate_state.back());if (!BeyondBoundary(intermediate_state.back().head(2)) && !has_obstacle) {auto neighbor_forward_node_ptr = new StateNode(grid_index);neighbor_forward_node_ptr->intermediate_states_ = intermediate_state;neighbor_forward_node_ptr->state_ = intermediate_state.back();neighbor_forward_node_ptr->steering_grade_ = i;neighbor_forward_node_ptr->direction_ = StateNode::FORWARD;neighbor_nodes.push_back(neighbor_forward_node_ptr);}// backwardhas_obstacle = false;intermediate_state.clear();x = curr_node_ptr->state_.x();y = curr_node_ptr->state_.y();theta = curr_node_ptr->state_.z();for (int j = 1; j <= segment_length_discrete_num_; j++) {DynamicModel(-move_step_size_, phi, x, y, theta);intermediate_state.emplace_back(Vec3d(x, y, theta));if (!CheckCollision(x, y, theta)) {has_obstacle = true;break;}}if (!BeyondBoundary(intermediate_state.back().head(2)) && !has_obstacle) {grid_index = State2Index(intermediate_state.back());auto neighbor_backward_node_ptr = new StateNode(grid_index);neighbor_backward_node_ptr->intermediate_states_ = intermediate_state;neighbor_backward_node_ptr->state_ = intermediate_state.back();neighbor_backward_node_ptr->steering_grade_ = i;neighbor_backward_node_ptr->direction_ = StateNode::BACKWARD;neighbor_nodes.push_back(neighbor_backward_node_ptr);}}
}// 用于根据车辆动力学模型(如简化的自行车模型)更新车辆状态。
void HybridAStar::DynamicModel(const double &step_size, const double &phi,double &x, double &y, double &theta) const {x = x + step_size * std::cos(theta);y = y + step_size * std::sin(theta);theta = Mod2Pi(theta + step_size / wheel_base_ * std::tan(phi));
}double HybridAStar::Mod2Pi(const double &x) {double v = fmod(x, 2 * M_PI);if (v < -M_PI) {v += 2.0 * M_PI;} else if (v > M_PI) {v -= 2.0 * M_PI;}return v;
}bool HybridAStar::BeyondBoundary(const Vec2d &pt) const {return pt.x() < map_x_lower_ || pt.x() > map_x_upper_ || pt.y() < map_y_lower_ || pt.y() > map_y_upper_;
}// ComputeG 和 ComputeH 函数:
// 分别计算从起始节点到当前节点的实际成本(g成本)和从当前节点到目标节点的启发式成本(h成本)。
// 这两个成本用于评估节点的总成本(f成本 = g成本 + h成本)。
double HybridAStar::ComputeH(const StateNode::Ptr ¤t_node_ptr,const StateNode::Ptr &terminal_node_ptr) {double h;// L2
// h = (current_node_ptr->state_.head(2) - terminal_node_ptr->state_.head(2)).norm();// L1h = (current_node_ptr->state_.head(2) - terminal_node_ptr->state_.head(2)).lpNorm<1>();if (h < 3.0 * shot_distance_) {h = rs_path_ptr_->Distance(current_node_ptr->state_.x(), current_node_ptr->state_.y(),current_node_ptr->state_.z(),terminal_node_ptr->state_.x(), terminal_node_ptr->state_.y(),terminal_node_ptr->state_.z());}return h;
}double HybridAStar::ComputeG(const StateNode::Ptr ¤t_node_ptr,const StateNode::Ptr &neighbor_node_ptr) const {double g;if (neighbor_node_ptr->direction_ == StateNode::FORWARD) {if (neighbor_node_ptr->steering_grade_ != current_node_ptr->steering_grade_) {if (neighbor_node_ptr->steering_grade_ == 0) {g = segment_length_ * steering_change_penalty_;} else {g = segment_length_ * steering_change_penalty_ * steering_penalty_;}} else {if (neighbor_node_ptr->steering_grade_ == 0) {g = segment_length_;} else {g = segment_length_ * steering_penalty_;}}} else {if (neighbor_node_ptr->steering_grade_ != current_node_ptr->steering_grade_) {if (neighbor_node_ptr->steering_grade_ == 0) {g = segment_length_ * steering_change_penalty_ * reversing_penalty_;} else {g = segment_length_ * steering_change_penalty_ * steering_penalty_ * reversing_penalty_;}} else {if (neighbor_node_ptr->steering_grade_ == 0) {g = segment_length_ * reversing_penalty_;} else {g = segment_length_ * steering_penalty_ * reversing_penalty_;}}}return g;
}// Search 函数:
// 实现混合A*算法的主要搜索逻辑。
// 输入起始状态和目标状态,输出是否找到路径。
// 使用优先队列(基于f成本的多重映射)管理开放集,以保证总是首先扩展最有希望的节点。
bool HybridAStar::Search(const Vec3d &start_state, const Vec3d &goal_state) {Timer search_used_time;double neighbor_time = 0.0, compute_h_time = 0.0, compute_g_time = 0.0;const Vec3i start_grid_index = State2Index(start_state);const Vec3i goal_grid_index = State2Index(goal_state);auto goal_node_ptr = new StateNode(goal_grid_index);goal_node_ptr->state_ = goal_state;goal_node_ptr->direction_ = StateNode::NO;goal_node_ptr->steering_grade_ = 0;auto start_node_ptr = new StateNode(start_grid_index);start_node_ptr->state_ = start_state;start_node_ptr->steering_grade_ = 0;start_node_ptr->direction_ = StateNode::NO;start_node_ptr->node_status_ = StateNode::IN_OPENSET;start_node_ptr->intermediate_states_.emplace_back(start_state);start_node_ptr->g_cost_ = 0.0;start_node_ptr->f_cost_ = ComputeH(start_node_ptr, goal_node_ptr);state_node_map_[start_grid_index.x()][start_grid_index.y()][start_grid_index.z()] = start_node_ptr;state_node_map_[goal_grid_index.x()][goal_grid_index.y()][goal_grid_index.z()] = goal_node_ptr;openset_.clear();openset_.insert(std::make_pair(0, start_node_ptr));std::vector<StateNode::Ptr> neighbor_nodes_ptr;StateNode::Ptr current_node_ptr;StateNode::Ptr neighbor_node_ptr;int count = 0;while (!openset_.empty()) {current_node_ptr = openset_.begin()->second;current_node_ptr->node_status_ = StateNode::IN_CLOSESET;openset_.erase(openset_.begin());if ((current_node_ptr->state_.head(2) - goal_node_ptr->state_.head(2)).norm() <= shot_distance_) {double rs_length = 0.0;if (AnalyticExpansions(current_node_ptr, goal_node_ptr, rs_length)) {terminal_node_ptr_ = goal_node_ptr;StateNode::Ptr grid_node_ptr = terminal_node_ptr_->parent_node_;while (grid_node_ptr != nullptr) {grid_node_ptr = grid_node_ptr->parent_node_;path_length_ = path_length_ + segment_length_;}path_length_ = path_length_ - segment_length_ + rs_length;std::cout << "ComputeH use time(ms): " << compute_h_time << std::endl;std::cout << "check collision use time(ms): " << check_collision_use_time << std::endl;std::cout << "GetNeighborNodes use time(ms): " << neighbor_time << std::endl;std::cout << "average time of check collision(ms): "<< check_collision_use_time / num_check_collision<< std::endl;ROS_INFO("\033[1;32m --> Time in Hybrid A star is %f ms, path length: %f \033[0m\n",search_used_time.End(), path_length_);check_collision_use_time = 0.0;num_check_collision = 0.0;return true;}}Timer timer_get_neighbor;GetNeighborNodes(current_node_ptr, neighbor_nodes_ptr);neighbor_time = neighbor_time + timer_get_neighbor.End();for (unsigned int i = 0; i < neighbor_nodes_ptr.size(); ++i) {neighbor_node_ptr = neighbor_nodes_ptr[i];Timer timer_compute_g;const double neighbor_edge_cost = ComputeG(current_node_ptr, neighbor_node_ptr);compute_g_time = compute_g_time + timer_get_neighbor.End();Timer timer_compute_h;const double current_h = ComputeH(current_node_ptr, goal_node_ptr) * tie_breaker_;compute_h_time = compute_h_time + timer_compute_h.End();const Vec3i &index = neighbor_node_ptr->grid_index_;if (state_node_map_[index.x()][index.y()][index.z()] == nullptr) {neighbor_node_ptr->g_cost_ = current_node_ptr->g_cost_ + neighbor_edge_cost;neighbor_node_ptr->parent_node_ = current_node_ptr;neighbor_node_ptr->node_status_ = StateNode::IN_OPENSET;neighbor_node_ptr->f_cost_ = neighbor_node_ptr->g_cost_ + current_h;openset_.insert(std::make_pair(neighbor_node_ptr->f_cost_, neighbor_node_ptr));state_node_map_[index.x()][index.y()][index.z()] = neighbor_node_ptr;continue;} else if (state_node_map_[index.x()][index.y()][index.z()]->node_status_ == StateNode::IN_OPENSET) {double g_cost_temp = current_node_ptr->g_cost_ + neighbor_edge_cost;if (state_node_map_[index.x()][index.y()][index.z()]->g_cost_ > g_cost_temp) {neighbor_node_ptr->g_cost_ = g_cost_temp;neighbor_node_ptr->f_cost_ = g_cost_temp + current_h;neighbor_node_ptr->parent_node_ = current_node_ptr;neighbor_node_ptr->node_status_ = StateNode::IN_OPENSET;/// TODO: This will cause a memory leak//delete state_node_map_[index.x()][index.y()][index.z()];state_node_map_[index.x()][index.y()][index.z()] = neighbor_node_ptr;} else {delete neighbor_node_ptr;}continue;} else if (state_node_map_[index.x()][index.y()][index.z()]->node_status_ == StateNode::IN_CLOSESET) {delete neighbor_node_ptr;continue;}}count++;if (count > 50000000) {ROS_WARN("Exceeded the number of iterations, the search failed");return false;}}return false;
}VectorVec4d HybridAStar::GetSearchedTree() {VectorVec4d tree;Vec4d point_pair;visited_node_number_ = 0;for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {if (state_node_map_[i][j][k] == nullptr || state_node_map_[i][j][k]->parent_node_ == nullptr) {continue;}const unsigned int number_states = state_node_map_[i][j][k]->intermediate_states_.size() - 1;for (unsigned int l = 0; l < number_states; ++l) {point_pair.head(2) = state_node_map_[i][j][k]->intermediate_states_[l].head(2);point_pair.tail(2) = state_node_map_[i][j][k]->intermediate_states_[l + 1].head(2);tree.emplace_back(point_pair);}point_pair.head(2) = state_node_map_[i][j][k]->intermediate_states_[0].head(2);point_pair.tail(2) = state_node_map_[i][j][k]->parent_node_->state_.head(2);tree.emplace_back(point_pair);visited_node_number_++;}}}return tree;
}// 清理动态分配的内存,避免内存泄漏。
void HybridAStar::ReleaseMemory() {if (map_data_ != nullptr) {delete[] map_data_;map_data_ = nullptr;}if (state_node_map_) {for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {if (state_node_map_[i] == nullptr)continue;for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {if (state_node_map_[i][j] == nullptr)continue;for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {if (state_node_map_[i][j][k] != nullptr) {delete state_node_map_[i][j][k];state_node_map_[i][j][k] = nullptr;}}delete[] state_node_map_[i][j];state_node_map_[i][j] = nullptr;}delete[] state_node_map_[i];state_node_map_[i] = nullptr;}delete[] state_node_map_;state_node_map_ = nullptr;}terminal_node_ptr_ = nullptr;
}__attribute__((unused)) double HybridAStar::GetPathLength() const {return path_length_;
}VectorVec3d HybridAStar::GetPath() const {VectorVec3d path;std::vector<StateNode::Ptr> temp_nodes;StateNode::Ptr state_grid_node_ptr = terminal_node_ptr_;while (state_grid_node_ptr != nullptr) {temp_nodes.emplace_back(state_grid_node_ptr);state_grid_node_ptr = state_grid_node_ptr->parent_node_;}std::reverse(temp_nodes.begin(), temp_nodes.end());for (const auto &node: temp_nodes) {path.insert(path.end(), node->intermediate_states_.begin(),node->intermediate_states_.end());}return path;
}void HybridAStar::Reset() {if (state_node_map_) {for (int i = 0; i < STATE_GRID_SIZE_X_; ++i) {if (state_node_map_[i] == nullptr)continue;for (int j = 0; j < STATE_GRID_SIZE_Y_; ++j) {if (state_node_map_[i][j] == nullptr)continue;for (int k = 0; k < STATE_GRID_SIZE_PHI_; ++k) {if (state_node_map_[i][j][k] != nullptr) {delete state_node_map_[i][j][k];state_node_map_[i][j][k] = nullptr;}}}}}path_length_ = 0.0;terminal_node_ptr_ = nullptr;
}bool HybridAStar::AnalyticExpansions(const StateNode::Ptr ¤t_node_ptr,const StateNode::Ptr &goal_node_ptr, double &length) {VectorVec3d rs_path_poses = rs_path_ptr_->GetRSPath(current_node_ptr->state_,goal_node_ptr->state_,move_step_size_, length);for (const auto &pose: rs_path_poses)if (BeyondBoundary(pose.head(2)) || !CheckCollision(pose.x(), pose.y(), pose.z())) {return false;};goal_node_ptr->intermediate_states_ = rs_path_poses;goal_node_ptr->parent_node_ = current_node_ptr;auto begin = goal_node_ptr->intermediate_states_.begin();goal_node_ptr->intermediate_states_.erase(begin);return true;
}
这里面首先定义了构造函数HybridAStar:
steering_angle:最大的转向角度
steering_angle_distance_num:转向角度的离散数量,控制转向角度的分辨率的。
segment_length:一个节点到另一个节点的路径段长度。
segment_length_discrete_num:路径离散长度的离散数量,影响路径平滑度和搜索的精度。
wheel_base:轴距,用于计算转弯的动态约束。
steering_penalty,reversing_penalty,steering_change_panalty,转向,倒车和转向变化的惩罚因子
shot_distance:射线距离,用于在距离目标状态一定范围内时,采用解析法(RS路径)直接连接起点和终点
下一个重要的就是hybrid_a_star_flow.cpp:
#include "hybrid_a_star/hybrid_a_star_flow.h"#include <nav_msgs/Path.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>double Mod2Pi(const double &x) {double v = fmod(x, 2 * M_PI);if (v < -M_PI) {v += 2.0 * M_PI;} else if (v > M_PI) {v -= 2.0 * M_PI;}return v;
}HybridAStarFlow::HybridAStarFlow(ros::NodeHandle &nh) {double steering_angle = nh.param("planner/steering_angle", 10);int steering_angle_discrete_num = nh.param("planner/steering_angle_discrete_num", 1);double wheel_base = nh.param("planner/wheel_base", 1.0);double segment_length = nh.param("planner/segment_length", 1.6);int segment_length_discrete_num = nh.param("planner/segment_length_discrete_num", 8);double steering_penalty = nh.param("planner/steering_penalty", 1.05);double steering_change_penalty = nh.param("planner/steering_change_penalty", 1.5);double reversing_penalty = nh.param("planner/reversing_penalty", 2.0);double shot_distance = nh.param("planner/shot_distance", 5.0);// std::make_shared是什么?kinodynamic_astar_searcher_ptr_ = std::make_shared<HybridAStar>(steering_angle, steering_angle_discrete_num, segment_length, segment_length_discrete_num, wheel_base,steering_penalty, reversing_penalty, steering_change_penalty, shot_distance);costmap_sub_ptr_ = std::make_shared<CostMapSubscriber>(nh, "/map", 1);init_pose_sub_ptr_ = std::make_shared<InitPoseSubscriber2D>(nh, "/initialpose", 1);goal_pose_sub_ptr_ = std::make_shared<GoalPoseSubscriber2D>(nh, "/move_base_simple/goal", 1);path_pub_ = nh.advertise<nav_msgs::Path>("searched_path", 1);searched_tree_pub_ = nh.advertise<visualization_msgs::Marker>("searched_tree", 1);vehicle_path_pub_ = nh.advertise<visualization_msgs::MarkerArray>("vehicle_path", 1);has_map_ = false;
}void HybridAStarFlow::Run() {ReadData(); // 从订阅的话题中读取成本地图、初始位置、目标位置,并存储到对应的队列中。// 如果没有地图数据,则直接返回。如果有地图数据,但地图还未初始化,则使用最新的成本地图初始化混合A*搜索器,并设置障碍物。// if (!has_map_) {// if (costmap_deque_.empty()) {// return;// }// current_costmap_ptr_ = costmap_deque_.front();// costmap_deque_.pop_front();// const double map_resolution = 0.2;// kinodynamic_astar_searcher_ptr_->Init(// current_costmap_ptr_->info.origin.position.x,// 1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,// current_costmap_ptr_->info.origin.position.y,// 1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,// current_costmap_ptr_->info.resolution,// map_resolution// );// unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);// unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);// for (unsigned int w = 0; w < map_w; ++w) {// for (unsigned int h = 0; h < map_h; ++h) {// auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution// / current_costmap_ptr_->info.resolution);// auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution// / current_costmap_ptr_->info.resolution);// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);// }// }// }// has_map_ = true;// }// costmap_deque_.clear();// test 3.8: not finish
// if (!has_map_) {
// if (costmap_deque_.empty()) {
// return;
// }// current_costmap_ptr_ = costmap_deque_.front();
// costmap_deque_.pop_front();// // 假定新的分辨率是原始分辨率的一半,例如0.05米/格
// const double new_resolution = 0.02; // 新的更细分辨率
// const double original_resolution = current_costmap_ptr_->info.resolution;
// const double scale_factor = original_resolution / new_resolution; // 计算缩放因子// // 使用新的分辨率初始化kinodynamic_astar_searcher_ptr_
// kinodynamic_astar_searcher_ptr_->Init(
// current_costmap_ptr_->info.origin.position.x,
// 1.0 * current_costmap_ptr_->info.width * original_resolution, // 保持物理尺寸不变
// current_costmap_ptr_->info.origin.position.y,
// 1.0 * current_costmap_ptr_->info.height * original_resolution, // 保持物理尺寸不变
// new_resolution, // 使用新的分辨率
// new_resolution // 新的地图分辨率也应该是这个值
// );// unsigned int map_w = std::round(current_costmap_ptr_->info.width * scale_factor);
// unsigned int map_h = std::round(current_costmap_ptr_->info.height * scale_factor);
// for (unsigned int w = 0; w < map_w; ++w) {
// for (unsigned int h = 0; h < map_h; ++h) {
// auto x = static_cast<unsigned int> (w / scale_factor);
// auto y = static_cast<unsigned int> (h / scale_factor);// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {
// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);
// }
// }
// }
// has_map_ = true;
// }
// costmap_deque_.clear();// test 3.7_1: if (!has_map_) {if (costmap_deque_.empty()) {return;}current_costmap_ptr_ = costmap_deque_.front();costmap_deque_.pop_front();double map_resolution = current_costmap_ptr_->info.resolution;kinodynamic_astar_searcher_ptr_->Init(current_costmap_ptr_->info.origin.position.x,1.0 * current_costmap_ptr_->info.width * map_resolution,current_costmap_ptr_->info.origin.position.y,1.0 * current_costmap_ptr_->info.height * map_resolution,map_resolution,map_resolution // 这里同样使用地图的实际分辨率);// 使用实际的分辨率计算映射的宽度和高度unsigned int map_w = current_costmap_ptr_->info.width;unsigned int map_h = current_costmap_ptr_->info.height;for (unsigned int w = 0; w < map_w; ++w) {for (unsigned int h = 0; h < map_h; ++h) {// 在这里,我们不再需要根据重新计算的分辨率调整索引// 直接使用原始索引if (current_costmap_ptr_->data[h * map_w + w]) {kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);}}}has_map_ = true;}costmap_deque_.clear();// // test 3.7_2: // // remap search grid // if (!has_map_) {// if (costmap_deque_.empty()) {// return;// }// current_costmap_ptr_ = costmap_deque_.front();// costmap_deque_.pop_front();// // const double map_resolution = 0.1; // why not from /map // double map_resolution = current_costmap_ptr_->info.resolution;// kinodynamic_astar_searcher_ptr_->Init(// current_costmap_ptr_->info.origin.position.x,// 1.0 * current_costmap_ptr_->info.width * current_costmap_ptr_->info.resolution,// current_costmap_ptr_->info.origin.position.y,// 1.0 * current_costmap_ptr_->info.height * current_costmap_ptr_->info.resolution,// current_costmap_ptr_->info.resolution,// map_resolution// );// unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);// unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);// for (unsigned int w = 0; w < map_w; ++w) {// for (unsigned int h = 0; h < map_h; ++h) {// auto x = static_cast<unsigned int> ((w + 0.5) * map_resolution// / current_costmap_ptr_->info.resolution);// auto y = static_cast<unsigned int> ((h + 0.5) * map_resolution// / current_costmap_ptr_->info.resolution);// if (current_costmap_ptr_->data[y * current_costmap_ptr_->info.width + x]) {// kinodynamic_astar_searcher_ptr_->SetObstacle(w, h);// }// }// }// has_map_ = true;// }// costmap_deque_.clear();while (HasStartPose() && HasGoalPose()) {InitPoseData();double start_yaw = tf::getYaw(current_init_pose_ptr_->pose.pose.orientation);double goal_yaw = tf::getYaw(current_goal_pose_ptr_->pose.orientation);Vec3d start_state = Vec3d(current_init_pose_ptr_->pose.pose.position.x,current_init_pose_ptr_->pose.pose.position.y,start_yaw);Vec3d goal_state = Vec3d(current_goal_pose_ptr_->pose.position.x,current_goal_pose_ptr_->pose.position.y,goal_yaw);if (kinodynamic_astar_searcher_ptr_->Search(start_state, goal_state)) {auto path = kinodynamic_astar_searcher_ptr_->GetPath();PublishPath(path);PublishVehiclePath(path, 4.0, 2.0, 5u);PublishSearchedTree(kinodynamic_astar_searcher_ptr_->GetSearchedTree());nav_msgs::Path path_ros;geometry_msgs::PoseStamped pose_stamped;for (const auto &pose: path) {pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = pose.x();pose_stamped.pose.position.y = pose.y();pose_stamped.pose.position.z = 0.0;pose_stamped.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.z());path_ros.poses.emplace_back(pose_stamped);}path_ros.header.frame_id = "world";path_ros.header.stamp = ros::Time::now();static tf::TransformBroadcaster transform_broadcaster;for (const auto &pose: path_ros.poses) {tf::Transform transform;transform.setOrigin(tf::Vector3(pose.pose.position.x, pose.pose.position.y, 0.0));tf::Quaternion q;q.setX(pose.pose.orientation.x);q.setY(pose.pose.orientation.y);q.setZ(pose.pose.orientation.z);q.setW(pose.pose.orientation.w);transform.setRotation(q);transform_broadcaster.sendTransform(tf::StampedTransform(transform,ros::Time::now(), "world","ground_link"));ros::Duration(0.05).sleep();}}// debug
// std::cout << "visited nodes: " << kinodynamic_astar_searcher_ptr_->GetVisitedNodesNumber() << std::endl;kinodynamic_astar_searcher_ptr_->Reset();}
}void HybridAStarFlow::ReadData() {costmap_sub_ptr_->ParseData(costmap_deque_);init_pose_sub_ptr_->ParseData(init_pose_deque_);goal_pose_sub_ptr_->ParseData(goal_pose_deque_);
}void HybridAStarFlow::InitPoseData() {current_init_pose_ptr_ = init_pose_deque_.front();init_pose_deque_.pop_front();current_goal_pose_ptr_ = goal_pose_deque_.front();goal_pose_deque_.pop_front();
}bool HybridAStarFlow::HasGoalPose() {return !goal_pose_deque_.empty();
}bool HybridAStarFlow::HasStartPose() {return !init_pose_deque_.empty();
}void HybridAStarFlow::PublishPath(const VectorVec3d &path) {nav_msgs::Path nav_path;geometry_msgs::PoseStamped pose_stamped;for (const auto &pose: path) {pose_stamped.header.frame_id = "world";pose_stamped.pose.position.x = pose.x();pose_stamped.pose.position.y = pose.y();pose_stamped.pose.position.z = 0.0;pose_stamped.pose.orientation = tf::createQuaternionMsgFromYaw(pose.z());nav_path.poses.emplace_back(pose_stamped);}nav_path.header.frame_id = "world";nav_path.header.stamp = timestamp_;path_pub_.publish(nav_path);
}void HybridAStarFlow::PublishVehiclePath(const VectorVec3d &path, double width,double length, unsigned int vehicle_interval = 5u) {visualization_msgs::MarkerArray vehicle_array;for (unsigned int i = 0; i < path.size(); i += vehicle_interval) {visualization_msgs::Marker vehicle;if (i == 0) {vehicle.action = 3;}vehicle.header.frame_id = "world";vehicle.header.stamp = ros::Time::now();vehicle.type = visualization_msgs::Marker::CUBE;vehicle.id = static_cast<int>(i / vehicle_interval);vehicle.scale.x = width;vehicle.scale.y = length;vehicle.scale.z = 0.01;vehicle.color.a = 0.1;vehicle.color.r = 1.0;vehicle.color.b = 0.0;vehicle.color.g = 0.0;vehicle.pose.position.x = path[i].x();vehicle.pose.position.y = path[i].y();vehicle.pose.position.z = 0.0;vehicle.pose.orientation = tf::createQuaternionMsgFromYaw(path[i].z());vehicle_array.markers.emplace_back(vehicle);}vehicle_path_pub_.publish(vehicle_array);
}void HybridAStarFlow::PublishSearchedTree(const VectorVec4d &searched_tree) {visualization_msgs::Marker tree_list;tree_list.header.frame_id = "world";tree_list.header.stamp = ros::Time::now();tree_list.type = visualization_msgs::Marker::LINE_LIST;tree_list.action = visualization_msgs::Marker::ADD;tree_list.ns = "searched_tree";tree_list.scale.x = 0.02;tree_list.color.a = 1.0;tree_list.color.r = 0;tree_list.color.g = 0;tree_list.color.b = 0;tree_list.pose.orientation.w = 1.0;tree_list.pose.orientation.x = 0.0;tree_list.pose.orientation.y = 0.0;tree_list.pose.orientation.z = 0.0;geometry_msgs::Point point;for (const auto &i: searched_tree) {point.x = i.x();point.y = i.y();point.z = 0.0;tree_list.points.emplace_back(point);point.x = i.z();point.y = i.w();point.z = 0.0;tree_list.points.emplace_back(point);}searched_tree_pub_.publish(tree_list);
}
1.参数的含义如下:
steering_angle: 最大转向角度,转化为弧度使用。
steering_angle_discrete_num: 转向角度的离散数量,控制转向角度的分辨率。
segment_length: 从一个节点到另一个节点的路径段长度。
segment_length_discrete_num: 路径段长度的离散数量,影响路径的平滑度和搜索的精度。
wheel_base: 车辆的轴距,用于计算车辆转弯时的动态约束。
steering_penalty, reversing_penalty, steering_change_penalty: 分别对应转向、倒车和转向变化的惩罚因子,用于在成本计算中对这些行为进行惩罚,以生成更符合实际驾驶情况的路径。
shot_distance: 射线距离,用于在距离目标状态一定范围内时,采用解析解法(Reeds-Shepp路径)直接连接起点和终点。
添加:车辆参数(这是自己添加的,主要影响障碍物检测等)
vehicle_length
vehicle_width
rear_axle_to_back
2.源代码中的小bug:
unsigned int map_w = std::floor(current_costmap_ptr_->info.width / map_resolution);
unsigned int map_h = std::floor(current_costmap_ptr_->info.height / map_resolution);
------->
unsigned int map_w = std::floor(current_costmap_ptr_->info.width *current_costmap_ptr_->info.resolution / map_resolution);
unsigned int map_h = std::floor(current_costmap_ptr_->info.height *current_costmap_ptr_->info.resolution/ map_resolution);
std::make_shared被用于创建四个不同类型对象的共享指针:
kinodynamic_astar_searcher_ptr_ 是 HybridAStar 类型的共享指针,用于执行混合A*搜索算法。
hybrid的算法解读到这里基本就可以先使用了。
效果:
有人也对他进行了改进,一个是加到了ros导航里面,正好这几天准备好好学习一下。还有一个是优化了起点终点的设置逻辑,这个我应该要按照自己的来,调度系统要的应该就是一系列的途经点。