逐行hybrid astar路径规划 混合a星泊车路径规划 带你从头开始写hybridastar算法，逐行源码分析matlab版hybridastar算法

咱们今天唠唠混合A星（Hybrid A*）路径规划，这玩意儿在自动泊车场景用得贼溜。和传统A星最大的区别在于它能处理车辆运动学约束，说人话就是——车不是纸片人，不能原地转向，得按方向盘转动的实际轨迹走。

先看个典型场景：停车场里有个斜方形的空位，咱们要让车从入口位置倒车入库。这时候传统栅格A星规划的路径可能根本开不进去，而Hybrid A*生成的路径带转向弧度，这才是真能用的方案。

上代码！先定义车辆参数（Matlab版）：

vehicle.length = 4.7;  % 车长
vehicle.width = 1.8;   % 车宽
vehicle.wheelbase = 2.8; % 轴距
vehicle.max_steer = deg2rad(40); % 最大转向角

节点结构体是关键，得记录每个状态的位姿和运动轨迹：

function node = createNode(x, y, theta, steer, cost, parent)
    node.x = x;          % X坐标
    node.y = y;          % Y坐标
    node.theta = theta;  % 航向角（弧度）
    node.steer = steer;  % 当前转向角
    node.cost = cost;    % 累计代价
    node.parent = parent;% 父节点索引
end

扩展节点时得考虑车辆运动学，这里用自行车模型生成后继节点：

function successors = generateSuccessors(currentNode, vehicle)
    steer_steps = [-vehicle.max_steer, 0, vehicle.max_steer]; % 左转/直行/右转
    successors = [];
    
    for steer = steer_steps
        % 自行车模型计算新位姿
        R = vehicle.wheelbase / tan(steer);
        dtheta = 0.5 / R; % 步长0.5米对应的角度变化
        
        new_theta = currentNode.theta + dtheta;
        new_x = currentNode.x + 0.5 * cos(new_theta);
        new_y = currentNode.y + 0.5 * sin(new_theta);
        
        % 计算代价时加入转向惩罚
        new_cost = currentNode.cost + 0.5 + 0.3*abs(steer); 
        
        successors = [successors; 
                     createNode(new_x, new_y, new_theta, steer, new_cost, [])];
    end
end

重点来了——混合启发函数。传统A星用欧式距离，Hybrid A*得考虑航向角：

function h = heuristic(current, goal)
    % 欧式距离部分
    dx = goal.x - current.x;
    dy = goal.y - current.y;
    distance_cost = sqrt(dx^2 + dy^2);
    
    % 航向角偏差惩罚
    theta_diff = abs(wrapToPi(current.theta - atan2(dy, dx)));
    angle_cost = theta_diff * 2;  % 实验调整的权重
    
    h = distance_cost + angle_cost;
end

碰撞检测要同时考虑车体轮廓和障碍物膨胀区。这里用多边形相交检测：

function collision = checkCollision(node, map, vehicle)
    % 构建车体四个角点
    car_corners = computeCarCorners(node, vehicle);
    
    % 转换为栅格坐标
    [rows, cols] = size(map.obstacle);
    for i = 1:4
        x_idx = floor(car_corners(i,1)/map.resolution);
        y_idx = floor(car_corners(i,2)/map.resolution);
        
        if x_idx < 1 || x_idx > cols || y_idx < 1 || y_idx > rows
            collision = true;
            return
        end
        
        if map.obstacle(y_idx, x_idx) == 1
            collision = true;
            return
        end
    end
    collision = false;
end

实际跑算法时会出现路径抖动，得用二次优化平滑路径。分享个实用技巧：在找到路径后，用二次规划对节点进行弹性拉伸：

function smooth_path = pathSmoothing(raw_path)
    alpha = 0.5; % 平滑权重
    beta = 0.3;   % 弹性权重
    smooth_path = raw_path;
    
    for iter = 1:100
        for i = 2:length(raw_path)-1
            dx = smooth_path(i+1).x - 2*smooth_path(i).x + smooth_path(i-1).x;
            dy = smooth_path(i+1).y - 2*smooth_path(i).y + smooth_path(i-1).y;
            
            move_x = alpha*(raw_path(i).x - smooth_path(i).x) + beta*dx;
            move_y = alpha*(raw_path(i).y - smooth_path(i).y) + beta*dy;
            
            smooth_path(i).x = smooth_path(i).x + move_x;
            smooth_path(i).y = smooth_path(i).y + move_y;
        end
    end
end

最后给个可视化效果对比：原始Hybrid A*路径可能像醉汉画的线，经过平滑处理后变成老司机操作的流畅曲线。实际部署时还要考虑方向盘转角速度限制，别让转向变化太突兀——毕竟乘客的咖啡杯可经不起突然打方向。