逐行hybrid astar路径规划 混合a星泊车路径规划 带你从头开始写hybridastar算法逐行源码分析matlab版hybridastar算法咱们今天唠唠混合A星Hybrid A*路径规划这玩意儿在自动泊车场景用得贼溜。和传统A星最大的区别在于它能处理车辆运动学约束说人话就是——车不是纸片人不能原地转向得按方向盘转动的实际轨迹走。逐行hybrid astar路径规划 混合a星泊车路径规划 带你从头开始写hybridastar算法逐行源码分析matlab版hybridastar算法先看个典型场景停车场里有个斜方形的空位咱们要让车从入口位置倒车入库。这时候传统栅格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(i1).x - 2*smooth_path(i).x smooth_path(i-1).x; dy smooth_path(i1).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*路径可能像醉汉画的线经过平滑处理后变成老司机操作的流畅曲线。实际部署时还要考虑方向盘转角速度限制别让转向变化太突兀——毕竟乘客的咖啡杯可经不起突然打方向。