前言

初次接触HybridAstar，会感觉比起其他的规划算法，HybridAstar好像复杂了一点，但后面才发现其实也只是Astar的优化升级版本，但是更适用于实际的移动机器人规划，Hybrid_astar是一种考虑机器运动学（转弯半径、车辆运动模型等）的非完整约束的全局路径规划算法。

算法流程

传统Astar搜索和考虑车辆运动约束搜索的区别：

Astar:只需要将搜索空间进行栅格化，然后按照栅格坐标去扩展路径，即只需要考虑X，Y；

考虑车辆运动约束搜索：需要考虑车辆的坐标（X,Y）、车头朝向（决定下一时刻可搜索方向）、转弯半径（决定可到达点）；

graph TD
    A[初始化地图参数] --> B[设置起点/终点节点]
    B --> C{开放集是否空?}
    C -->|否| D[取出最优节点]
    D --> E[Reeds-Shepp直连检查]
    E -->|成功| F[返回路径]
    E -->|失败| G[生成邻居节点]
    G --> H[碰撞检测]
    H -->|通过| I[计算G/H代价]
    I --> J[更新开放集]
    J --> C
    C -->|是| K[搜索失败]

    subgraph 节点处理
        D -->|扩展| G
        G -->|多方向| H
        I -->|代价更新| J
    end
    subgraph 路径生成
        E -->|最终连接| F
        F --> L[回溯父节点]
        L --> M[生成路径序列]
    endAstar的核心部分在于两部分：

1. 下一个节点的搜索策略；
2. 引入启发函数的代价函数设计。

针对Astar的一些改进，要么是改进代价函数，要么是改进搜索策略。

节点搜索策略

​Astar的搜索即九宫格策略，以当前位置为中心的九宫格中寻找下一个最可能的节点进行拓展；最后将拓展出的所有节点转换为世界坐标系下的点即可。

​然而对于HybridAstar，则无法直接在图像格式的PGM地图中搜索，因为HybridAstar除了x，y坐标，为了符合机器人运动约束，需要引入机器人运动学模型，既然引入了运动学模型，必然涉及到角度，而一旦涉及到角度，这就意味着HybridAstar的搜索将不再是以地图中的每个点为下一个扩展点，其扩展点可能是任意位置，如下图所示：

最左边为普通Astar的扩展，每个点落在九宫格中间，一一对应图像格式的PGM地图中的每个图像像素点，最右边则是HybridAstar的扩展方式，其落点随机，无法通过将点的位置转换为对应的图像像素点，这也就意味着HybridAstar的搜索，必然是直接在世界坐标系下的搜索。

​那么对于Astar我们知道，在图像中搜索时，九宫格对应的拓展节点位置很容易获得，对当前位置的像素x，y坐标进行加一减一即可，那么对于世界坐标系下的HybridAstar，如何获取下一个节点的坐标呢？HybridAstar的搜索目的是为了得到符合运动约束的路径，那么自然而然必然要引入运动学模型，所以理论上模型越符合机器人或者车辆运动特性，搜索出的路径也就更加匹配，这里给出代码里使用的一个简单模型（参考了开源库）：

voidHybridAStar::DynamicModel(constdouble&step_size,constdouble&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));
}

当然搜索过程存在避障问题，Astar避障很容易，只要把图像中障碍物的节点过滤掉就可以，但是既然HybridAstar是要符合机器人或者车辆运动学约束的，那么避障就稍微复杂一点，但其实也不难，以一个机器人或者车辆为例，提取物体的外接四边形，然后判断四条边是否有膨胀即可（对于PGM地图则是将坐标转换为图像坐标并判断四条边是否碰撞），任何一条边碰到了障碍物则节点不可取。当然在判断边的碰撞的时候其实也是取边上的点进行判断。

inlineboolHybridAStar::LineCheck(doublex0,doubley0,doublex1,doubley1){
boolsteep=(std::abs(y1-y0)>std::abs(x1-x0));
//LineCheck使用Bresenham算法检查两点间直线是否有障碍物参数：两点的坐标返回：布尔值是否有障碍物
if(steep){
std::swap(x0,y0);
std::swap(y1,x1);
}
if(x0>x1){
std::swap(x0,x1);
std::swap(y0,y1);
}
autodelta_x=x1-x0;
autodelta_y=std::abs(y1-y0);
autodelta_error=delta_y/delta_x;
decltype(delta_x)error=0;
decltype(delta_x)y_step;
autoyk=y0;
if(y0<y1){
y_step=1;
}else{
y_step=-1;
}
autoN=static_cast<unsignedint>(x1-x0);
for(unsignedinti=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_))
){
returnfalse;
}
}else{
if(HasObstacle(Vec2i(x0+i*1.0,yk))
||BeyondBoundary(Vec2d((x0+i)*MAP_GRID_RESOLUTION_,
yk*MAP_GRID_RESOLUTION_))
){
returnfalse;
}
}
error+=delta_error;
if(error>=0.5){
yk+=y_step;
error=error-1.0;
}
}
returntrue;
}
boolHybridAStar::CheckCollision(constdouble&x,constdouble&y,constdouble&theta)
{
//检查车辆在某个位姿下是否与障碍物碰撞，通过车辆轮廓变换后检测各边是否与障碍物相交。
//参数：车辆位置以及朝向
Timertimer;
Mat2dR;
R<<std::cos(theta),-std::sin(theta),
std::sin(theta),std::cos(theta);

MatXdtransformed_vehicle_shape;
transformed_vehicle_shape.resize(8,1);
for(unsignedinti=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);
}

Vec2itransformed_pt_index_0=Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2,1>(0,0)
);
Vec2itransformed_pt_index_1=Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2,1>(2,0)
);

Vec2itransformed_pt_index_2=Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2,1>(4,0)
);

Vec2itransformed_pt_index_3=Coordinate2MapGridIndex(
transformed_vehicle_shape.block<2,1>(6,0)
);
doubley1,y0,x1,x0;
//pt1->pt0
x0=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)){
returnfalse;
}
//pt2->pt1
x0=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)){
returnfalse;
}

//pt3->pt2
x0=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)){
returnfalse;
}

//pt0->pt3
x0=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)){
returnfalse;
}

check_collision_use_time+=timer.End();
num_check_collision++;
returntrue;
}

代价函数设计

Astar的代价函数分为两部分：启发函数 h(x) ，与距离起点的代价 g(x) ，当然Astar其实是额外引入了启发函数。对于Astar的启发函数设计，通常是利用当前点到终点的距离，或是曼哈顿距离，或是欧式距离等等，距离起点的代价则是在搜索过程中一步步更新的。而至于HybridAstar的代价函数设计，同样是这两部分，当然各有改进。

启发函数 h(x)：

有了当前的位置信息 x,y,θ 以及设定好的一些参数，即可计算出扩展节点的坐标，这便是重写了Astar的节点搜索策略。但是不止于此，HybridAstar在搜索的时候，使用了Reeds-Shepp路径用于启发式估计，直接调用Reeds-Shepp路径库计算理论最短路径长度，并且设置了一个阈值，shot_distance_阈值：在距离目标3倍终局距离时触发Reeds-Shepp计算，并且分层启发式设计：远距离用L1/L2加速搜索，近距离用Reeds-Shepp提高精度，在搜索时如果可以搜索到当前位置到终点的曲线，则可以结束搜索。

doubleHybridAStar::ComputeH(constStateNode::Ptr&current_node_ptr,
constStateNode::Ptr&terminal_node_ptr){
doubleh;
//L2
//h=(current_node_ptr->state_.head(2)-terminal_node_ptr->state_.head(2)).norm();

//L1
h=(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());
}
returnh;
}//是启发式函数，估计当前节点到目标的代价，使用Reeds-Shepp路径长度或欧氏距离。

距离起点的代价 g(x)：

对于距离起点的代价，我们知道Astar比较简单，在图像中搜索时，每次像素加一或是减一的过程中对于起点的距离也随之更新了，但是HybridAstar在世界坐标系下进行搜索，它考虑了不同的移动方向和转向变化等因素，可能涉及到路径的转向惩罚和倒车惩罚。分成了两个大方向：FORWARD（前进）和BACKWARD（倒车）。每个方向下又有不同的情况判断，特别是转向是否变化，以及转向等级是否为0。在FORWARD的情况下，如果转向等级不同，即邻居节点的转向等级和当前节点不一致，这时候可能需要应用转向变化的惩罚。例如，如果邻居的转向等级是0，也就是直行，那么可能惩罚较小，而如果有转向的话，惩罚会更大。另外，如果转向等级相同，则根据是否是直行来决定是否添加转向惩罚。转向等级为0可能意味着直行，没有转向，所以没有额外的惩罚，否则会有转向惩罚。

在倒车的情况下，处理类似，但还增加了倒车的惩罚因子。无论转向是否变化，倒车本身就有惩罚，再加上转向变化的惩罚和转向的惩罚。例如，如果倒车时转向等级改变，并且新的转向等级是0，那么惩罚是段长度乘以转向变化惩罚乘以倒车惩罚。如果是转向等级非0，那么还要再乘以转向惩罚。

总体来说，根据移动方向和转向变化情况，结合不同的惩罚系数，计算出实际的行进代价，从而在搜索过程中选择更合适的路径，避免不必要的转向和倒车，使生成的路径更符合实际车辆的操控需求。

doubleHybridAStar::ComputeG(constStateNode::Ptr&current_node_ptr,
constStateNode::Ptr&neighbor_node_ptr)const
{
doubleg;
//判断邻居节点运动方向（前进/倒车）
if(neighbor_node_ptr->direction_==StateNode::FORWARD)
{
//转向角度是否变化
if(neighbor_node_ptr->steering_grade_!=current_node_ptr->steering_grade_){
//新转向是否为直行（steering_grade=0）
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_;//倒车转向惩罚
}
}
}
returng;
}

参考资料：

1. HybridA*规划算法-知乎
2. HybridA*算法原理-知乎
3. 【附C++源代码】HybridAStar(混合A星)算法实现，考虑车辆运动学约束的路径规划算法_哔哩哔哩_bilibili