简介资源包为基于Matlab实现的混合A*Hybrid A*路径规划算法源码面向智能车辆、自动驾驶及路径规划领域的开发者和研究者用于在考虑车辆运动学约束的条件下生成平滑可行的行驶路径。包内共27个文件全部为.m脚本整体大小仅13KB包含主程序、A搜索、Reeds-Shepp曲线生成、车辆状态变换与路径可视化等功能模块结构紧凑易于阅读和二次修改。算法综合利用A启发式搜索与RS曲线信息通过取当前点到终点的A距离与RS距离中的较大值作为启发函数在保证搜索效率的同时兼顾转向半径等运动限制避免了直接A路径不平滑或不可执行的问题。用户使用Matlab 2016a及以上版本即可直接运行体验也可根据实际地图和车辆参数调整相关设置适合作为课程设计、算法对比实验或工程预研的参考实现。资源包已有11972人学习下载在同类路径规划源码中具有较高的参考价值。1. 混合A星算法Hybrid_Astar在MATLAB里落地为什么传统A*算出的路径开不了车混合A星算法Hybrid_Astar最常被问的一句话是A都出来了为什么还要搞一个新的答案在停车场窄位调头这类低速场景里——普通A在栅格地图上搜出的是八方向折线最小转弯半径这一条物理约束就对不上路径规划器给出的轨迹车辆大概率跟踪失败。混合A星把车辆运动学模型直接塞进A*搜索框架将x、y、theta三个维度的位姿当作状态节点扩展时沿自行车模型积分出一簇满足转向限制的圆弧轨迹输出的是车辆能跟的路径。这篇笔记面向正在用MATLAB搭路径规划原型、做自动泊车或AGV课题的工程师从原理讲到最小可运行实现再把最容易翻车的参数与排查经验一次性说清楚。2. 混合A星的核心差异状态空间、节点扩展与启发式的设计传统A在二维栅格上做四邻域或八邻域扩展节点只有x、y两个自由度。用它规划自动泊车路径时最典型的翻车是一段由横移和直角弯组成的路径看起来绕开了障碍但实车低速下没有横移能力转弯半径也撑不起直角弯路径只能看不能用。混合A星把状态空间从(x, y)扩展为(x, y, theta)搜索过程前置考虑了车辆能不能走而不是先给路径再事后修正这是它区别于A系列算法的根本点。2.1 状态空间从二维变三维x、y、theta如何离散化成状态栅格混合A星里的“状态栅格”不是普通栅格地图而是把x、y、theta三个维度都离散化的三维栅格。x、y的离散粒度取地图分辨率比如0.2m一格theta方向则按角度分区常见做法是把360度切成8到16个扇区。每到达一个位姿就把它映射到对应的栅格单元这个单元就是状态key。搜索只保留每个状态key下代价最小的节点其他同格节点直接丢弃目的是限制搜索规模。stateKey函数是这一步的核心。我一般用floor取整而不是round原因放在第5章的排查里讲先看代码function key stateKey(x, y, theta, params) ix floor(x / params.resolution); iy floor(y / params.resolution); % 把角度归到 [0, 2*pi)再按分区数取整 it floor(mod(theta 2*pi, 2*pi) / params.angle_res); key sprintf(%d_%d_%d, ix, iy, it); end逻辑说明输入是车辆在米制坐标系下的位姿输出是字符串形式的栅格索引三元组。ix和iy分别对应x、y方向的栅格编号it对应角度扇区编号。字符串拼接是为了能直接作为containers.Map的key避免用数组做key时遇到的哈希问题。参数说明params.resolution是栅格边长单位米决定空间离散粒度params.angle_res是每个扇区的弧度大小比如2*pi/12约等于0.5236rad。这里用floor而不是round是为了让扇区边界上的点稳定归入低半区避免边界抖动导致同一个位姿在不同次进入时映射到不同key从而破坏closed集合的判重一致性。2.2 节点扩展一组转向角沿自行车模型积分出一簇圆弧节点扩展是混合A星区别于普通A最直观的部分。普通A一个节点往上下左右最多8个方向走一步混合A星则是从当前位姿出发对一组离散的转向角比如-0.4到0.4rad步长0.1rad逐档做车辆运动学积分每一档都得到一条固定弧长的圆弧轨迹圆弧末端就是候选后继节点。车辆运动学用最常用的自行车模型描述车辆的前轮转角delta、轴距L与转弯半径满足R L / tan(delta)。沿弧长ds走一步航向角变化dtheta ds / R。这里要用圆弧闭式解做单步积分而不是普通的欧拉直线逼近否则累计航向误差会在长弧长上明显漂移。function [x_next, y_next, theta_next] kinematic_step(x, y, theta, delta, ds, L) if abs(delta) 1e-6 % 接近直行时直接走直线避免大半径带来的数值问题 x_next x ds * cos(theta); y_next y ds * sin(theta); theta_next theta; else R L / tan(delta); dtheta ds / R; x_next x R * (sin(theta dtheta) - sin(theta)); y_next y - R * (cos(theta dtheta) - cos(theta)); theta_next theta dtheta; end end逻辑说明输入当前位姿(x,y,theta)、前轮转角delta、弧长步长ds和轴距L输出积分后的位姿。|delta|小于1e-6时直接走直线因为tan(delta)趋近0会使R巨大闭式解在数值上不稳定。圆弧闭式解用的是“航向角变化后沿圆弧走ds”的几何关系它对ds没有截断误差比欧拉积分更贴合真实转向。每次扩展把这段积分循环跑满max_arc/ds步就得到一条完整的候选弧。参数说明delta是前轮转角单位弧度实际车辆可达的最大转角通常由转向机构限制比如±0.4radds是单步弧长常见取0.3到0.5m它同时决定碰撞检测的空间间隔取太大容易漏检取太小节点密度过高L是轴距单位米一个典型的低速小车轴距在2到3m之间。有了单步积分扩展函数做两件事对每个转向角采样值积分出一段弧再沿弧逐点做碰撞检测。代码如下function succ expandNode(node, model, params) nStep round(params.max_arc / params.ds); succ struct(x, {}, y, {}, theta, {}, delta, {}, g_cost, {}); for i 1:length(params.delta_samples) xi node.x; yi node.y; thi node.theta; for j 1:nStep [xi, yi, thi] kinematic_step(xi, yi, thi, params.delta_samples(i), params.ds, model.L); if collisionCheck(xi, yi, thi, model, params) 0 xi NaN; break; end end if isnan(xi), continue; end s struct(x, xi, y, yi, theta, thi, ... delta, params.delta_samples(i), ... g_cost, node.g_cost params.max_arc params.w_steer * abs(params.delta_samples(i))); succ(end1) s; end end逻辑说明外层循环遍历每个转向角采样值内层循环做积分和逐点碰撞检测。一旦某一步撞上障碍这一档转向角整条弧作废。通过检测的弧其末端作为候选节点g_cost等于父节点g_cost加上弧长代价再加上转向代价w_steer乘以转向角绝对值。转向角绝对值越大惩罚越大搜索自然倾向于少打大方向盘。参数说明params.max_arc是单次扩展的弧长上限建议1.5到3mparams.delta_samples是转向角采样向量比如-0.4:0.1:0.4params.w_steer是转向代价权重后面第4章会详细展开。2.3 代价函数与启发式为什么h取max而不是取和混合A星的代价结构是f g h。g是已累积的路径代价包括弧长、转向代价和倒车代价h是从当前节点到目标的估计代价。关键在h的取法。经典的混合A星做法是同时算两个启发式一个是忽略障碍、只考虑运动学约束的Reeds-Shepp最短路径长度另一个是忽略运动学、只考虑障碍距离的普通A*最短路径长度然后让h取两者中的最大值。% 每个节点生成时计算一次不再重复 h max(reedsSheppLength(node, goal), astarDistMap(node, goal, params));逻辑说明Reeds-Shepp长度描述的是“一辆车完全不理会障碍用最少的转向和倒车从A到B需要走多远”它天然满足运动学约束astarDistMap则是普通栅格A*从当前点到目标点的无障碍距离。一个覆盖了运动学下端界一个覆盖了障碍物下端界。两者都小于真实最短路径长度取max之后仍然小于等于真实最短路径长度所以启发式保持可采纳不会剪掉最优解。如果取和启发式会高估真实代价搜索可能提前丢弃真正的最优分支路径质量明显变差。参数说明reedsSheppLength可以按公开的RS曲线几何公式自行改写常见实现是查表加角度归一化astarDistMap建议离线预计算——从目标点反向跑一遍普通BFS或A*把整张地图每个栅格到目标的距离存成一张距离场搜索时直接查表避免每个节点都现场做一次完整A*。距离场只和x、y有关和theta无关因此存一张二维数组即可代码和算力都能省一个量级。3. 在MATLAB里跑通Hybrid A*最小实现碰撞检测、Open List与主循环原理立住之后把最小可运行版本跑一遍。下面代码按能看到主循环的形状来组织地图是一个0/1矩阵1是障碍0可通行model结构体含轴距L和车宽Wparams含resolution、angle_res、delta_samples、ds、max_arc、w_steer、w_back等。主函数hybridAstarSearch输入地图、起点、终点、参数和车辆模型返回路径点列和成功标志。3.1 车辆包络碰撞检测一段圆弧每隔一小段检查一次车壳碰撞检测要检查的不是一个点而是车辆的整个外壳扫过的区域。常见做法是每积完一个ds检查车辆矩形四个角是否落到障碍栅格。外接圆包围盒更保守但会浪费窄通道空间。我习惯先做地图膨胀把障碍向外扩一个车宽的一半再用四角检查。四角坐标按航向角theta旋转代码如下function hit collisionCheck(x, y, theta, model, params) map params.map; res params.resolution; L model.L; W model.W; corners [L/2, W/2; -L/2, W/2; -L/2, -W/2; L/2, -W/2]; Rmat [cos(theta), -sin(theta); sin(theta), cos(theta)]; hit 0; for i 1:4 p Rmat * corners(i,:) [x; y]; ix floor(p(1)/res) 1; iy floor(p(2)/res) 1; if ix1 || ixsize(map,2) || iy1 || iysize(map,1) || map(iy, ix) hit 1; return; end end end逻辑说明把车辆近似成长方形车头车尾各取半个轴距左右各取半个车宽。每个角点先按theta做旋转再平移到当前位姿然后将米制坐标除以分辨率得到栅格下标。只要任何一个角落在障碍栅格或地图边界外就判定碰撞。map(iy, ix)的行列对应y和x顺序写反会出现搜索路径穿过障碍但不报错的诡异现象。参数说明model.W是车宽单位米。地图膨胀这一步不在函数里做而是在加载地图后对整个map调用一次imerode或手动外扩膨胀半径取车辆宽度的一半加上一个栅格比较稳妥。提示碰撞检测的空间间隔由ds决定ds太大时车辆弧线扫过的包络可能连到两个检测点之间的空隙里。保守做法是ds不超过车辆宽度的一半。3.2 状态去重与Open List用containers.Map做closed集合用结构体数组做openMATLAB没有内置堆节点规模在几万以内时直接用结构体数组存open list每次取f最小的节点复杂度O(n)是能接受的。closed集合用containers.Mapkey是stateKey生成的字符串。这样代码最直白也方便新手跟踪调试。如果地图到几百乘几百、节点数上十万再换成java.util.PriorityQueue也不迟接口上只要把pop和push两个操作替换掉。openList startNode; closedMap containers.Map(); while ~isempty(openList) [~, idx] min([openList.f_cost]); cur openList(idx); openList(idx) []; key stateKey(cur.x, cur.y, cur.theta, params); if isKey(closedMap, key) continue; end closedMap(key) true; % 扩展与更新见3.3 end逻辑说明f_cost g_cost h_costmin取最小f的节点作为当前扩展点。节点被弹出后先检查closed已在closed就跳过否则立刻塞进closed。这个check-then-add的顺序容易写反写反会导致同一个状态被重复扩展规划时间成倍增长。closedMap存的是key不存节点本身回溯路径靠openList里的parent链完成。参数说明startNode是一个结构体字段至少包含x、y、theta、g_cost、h_cost、f_cost、parentparent初始化为空。openList删除节点用openList(idx) []如果节点数量上万这一步的移动开销会变大到那时再换优先队列。3.3 主循环与路径回溯扩展、代价更新与回溯扩展与更新是混合A星主循环的实质部分。对当前节点调用expandNode拿到后继节点逐个判断是否在closed、是否更新open。open列表里如果已有相同状态key的节点且g_cost更小就替换否则跳过。终止条件有两个一是当前节点与目标点的距离小于terminal_dist且角度差小于terminal_angle直接回溯退出二是启发式小于某个阈值时尝试Reeds-Shepp收尾收尾成功也退出。后者路径更短更顺我一般优先用它。succNodes expandNode(cur, model, params); for s succNodes sk stateKey(s.x, s.y, s.theta, params); if isKey(closedMap, sk) continue; end h max(reedsSheppLength(s, goal), astarDistMap(s, goal, params)); s.f_cost s.g_cost h; s.parent idx; openList insertOrReplace(openList, s, sk, params); end逻辑说明expandNode返回的每个后继节点都带有g_cost这里补上h得到f_cost。parent记录的是当前扩展节点在openList中的位置回溯时顺着parent索引一路走回起点。需要特别注意的是insertOrReplace在替换同key节点时不能破坏其他节点parent索引的指向实际工程里更稳的做法是每个节点存parent的完整坐标再用stateKey去openList里查代码多几行但不容易出索引错乱。回溯代码path []; node cur; while ~isempty(node.parent) path [path; node.x, node.y, node.theta]; node openList(node.parent); end path flipud(path);逻辑说明从终点节点开始通过parent索引逐级回跳直到parent为空即到达起点最后把路径翻转成从起点到终点的顺序。path每行是一个位姿(x, y, theta)三列分别对应米制坐标和弧度航向角后续画图、平滑、发给控制模块都用这个格式。主函数入口的组织方式如下params struct(map, map, resolution, 0.2, angle_res, pi/6, ... delta_samples, -0.4:0.1:0.4, ds, 0.3, max_arc, 2.0, ... w_steer, 0.5, w_back, 0.0, terminal_dist, 0.3, ... terminal_angle, deg2rad(5), rs_threshold, 5.0); model struct(L, 2.5, W, 1.8); start struct(x, 2.0, y, 2.0, theta, 0); goal struct(x, 18.0, y, 15.0, theta, pi/2); path hybridAstarSearch(map, start, goal, params);画图时把path的x、y两列取出来plot即可注意map的行列方向与坐标轴y轴翻转问题否则路径会上下颠倒。第一次跑通的标准是路径所有点都落在0栅格上且终点附近的朝向与goal.theta一致。4. Hybrid_Astar必调参数分辨率、转向角采样与代价权重的搭配混合A星跑通很容易跑好用全看参数。这部分没什么理论能直接给出最优解参数之间的耦合关系比单个参数的绝对大小更值得先理解。我按“先定空间离散再定扩展弧最后定代价权重”的顺序讲。4.1 栅格分辨率与角度分区数先量最小转弯半径再定粒度的上下界开始前先算两个值车辆轴距L和前轮最大转角delta_max以及由此决定的最小转弯半径。角度分区数要和delta_max匹配假如前轮转角采样范围是±0.4rad角度分区如果只有8个45度一格两个相邻转向角选出来的节点极可能落在同一个角度扇区里状态key冲突搜索多样性直接丢了一截。经验值转弯半径3到5m的设备栅格0.2m时角度分区取12到16个约22到30度一格。分辨率越细角度分区可以越多但搜索空间按三个维度乘数膨胀0.1m栅格加16个扇区很容易让open list上几万节点。先跑通再加密。4.2 转向角采样与弧长积分步长搜索质量与发散速度的平衡转向角采样决定了扩展弧的方向覆盖。常见做法是取[-delta_max : 0.1 : delta_max]delta_max按实际车辆限制来。弧长步长ds是另一个关键量它既参与碰撞检测的空间间隔也决定积分精度。ds取0.3到0.5m比较常见。max_arc是单次扩展的弧长一般在1.5到3m之间。一个直接的调参现象max_arc加大搜索发散变快但窄通道里容易跳过可行入口max_arc太小节点密集搜索变慢。把经验和影响列成表参数建议区间调大影响调小影响resolution0.1-0.2m状态栅格更粗路径粗糙但快状态栅格更细路径精细但慢角度分区数8-16个状态区分度低容易丢角度状态区分度高搜索空间大delta_samples步长0.05-0.1rad覆盖更密路径更顺覆盖更疏路径锯齿更明显ds0.3-0.5m碰撞检查间隔大漏检风险节点多计算慢max_arc1.5-3m发散快窄通道易失败发散慢节点多4.3 代价权重与终止阈值倒车惩罚、转向惩罚与RS终端进入条件代价权重直接影响路径性格。w_steer是转向代价调大让搜索尽量走大圆弧、少频繁换向w_back是倒车代价调大让路径尽量不倒车但代价过大会让搜索绕远路去找一个可以一把掉头的位姿。倒车权重常见在0到2之间。terminal_dist与terminal_angle组合判断主搜索是否到达目标附近rs_threshold控制什么时候尝试Reeds-Shepp收尾取值一般3到6m。调参顺序是玄学但我的血泪经验是先把resolution和delta_samples固定只动w_steer和w_back再看路径形状决定要不要加密转向角采样最后才动栅格分辨率。因为分辨率一变状态key、碰撞检测、规划时间全部联动早期调它等于给后面留了一堆不确定因素。注意w_back设成0不代表允许无限倒车只是给搜索更大的路径选择自由度倒车路径本身依然要满足全程无碰撞RS收尾的碰撞校验不能省。5. 混合A星常见问题排查发散撞墙、原地打转与路径毛刺调试混合A星时搜索一旦出问题观察对象集中在三类路径是否碰障碍、搜索是否在局部反复、规划时间是否失控。下面的问题按出现频率排序都是我调试时见过多次的每条按现象到原因再到解决展开。5.1 搜索发散撞向障碍碰撞检测间隔大于车辆扫掠包络现象生成的路径贴着障碍边缘“擦”过甚至有一段明显压过障碍物。原因expandNode里只在弧长末端做碰撞检测一段2m的弧中间扫过的区域完全没有检查。车辆转向时车头甩过的包络远远大于两个离散点之间的连线漏检几乎必然。解决在expandNode的积分循环里每积一个ds做一次collisionCheck而不是只在末端。保守方案是先用外接圆做粗检测再用矩形四角精化。同时把地图障碍做1到2个栅格的膨胀给控制误差留余量。5.2 搜索在局部来回绕圈状态栅格把不同角度压进同一个key现象open list不空但节点一直在同一小块区域反复进出路径出不去。原因角度分区数太少比如只有4个相邻转向角的多个节点落在同一个stateKey里closed集合把本应继续探索的位姿当成已访问状态丢弃搜索被锁死在局部。解决角度分区至少12个stateKey改用floor加mod不要用round——round会把相邻扇区边界上的角度分错区造成状态误判。改完再看路径是否还绕圈如果还绕再把delta_samples步长缩小一档。5.3 Reeds-Shepp收尾路径穿墙终段完全没有碰撞校验现象主搜索正常但算法在接近目标时突然丢出一条穿过障碍的直线圆弧组合。原因rs_threshold触发后直接返回RS路径没有按ds重新离散采样并做碰撞检测。RS曲线只在几何上是可行的能不能走完全取决于这一段扫过的区域是否干净。解决RS路径生成后按0.2m步长逐点调用collisionCheck中途撞障碍就放弃RS继续主搜索并适当降低rs_threshold。收尾路径要做平滑后再给下游控制模块。5.4 规划时间爆炸open list反复排序、启发式重复计算现象很小的地图80x80规划耗时超过十秒节点数看起来却不多。原因多出现在纯MATLAB实现里[~, idx] min([openList.f_cost])每次O(n)节点建立几万后排序开销很大同时每个节点重复计算reedsSheppLength和astarDistMap又把常数拉高几倍。解决把openList换成java.util.PriorityQueue节点生成时算一次h并缓存closed检查放在扩展前避免对已关闭节点重复计算。这样通常能把80x80地图的规划时间压到2秒以内。5.5 路径毛刺与反复修正弧长步长和转向角采样不匹配现象路径上出现很密的锯齿形修正车头方向频繁小幅左右摆。原因ds太小比如0.1m而转向角采样步长0.1rad导致相邻节点的转向角变化在状态栅格里表现为离散跳变搜索倾向用连续小修正抵消跳变。解决把ds增大到0.3m以上同时转向角采样步长保持在0.05到0.1rad之间。如果毛刺还在说明是代价权重问题而不是离散问题加大w_steer、适当降低w_back让路径倾向大弧线而不是密集修正。6. 把混合A星从演示变成好用轨迹平滑与批量验证6.1 轨迹平滑用梯度下降把分段圆弧修成连续曲率混合A星输出的路径由多段圆弧拼接拼接点曲率突变直接给控制模块会让转向角高频波动。常见做法是在原图障碍信息上做梯度下降优化目标函数包含贴近原路径项、障碍排斥项和曲率平滑项。别急着调MATLAB优化工具箱里的fmincon路径几百个点全量优化很慢手写几百轮梯度下降更快也更方便控制迭代步长。% 目标函数贴近原路径 远离障碍 角度平滑 theta_vec atan2(diff(p(:,2)), diff(p(:,1))); J sum((p - p_ref).^2) ... w_obs * sum(1 ./ (d_obs eps)) ... w_curv * sum(diff(theta_vec).^2);逻辑说明第一项把路径拉回参考路径附近第二项对离障碍太近的点施加惩罚第三项让相邻点之间的航向角变化变小。三项各自乘以权重对路径逐点求梯度后沿负梯度方向迭代。d_obs是路径点到最近障碍的距离可以从距离变换场里查。平滑后路径的曲率突变会被摊平但需要重新做一遍碰撞检测确保平滑没有把路径推出可通行区域。6.2 批量验证固定地图、固定随机种子、统计三件事调参得验证验证要说统计。我一般固定一张地图随机撒起点终点每个case固定随机种子跑5次统计三件事成功率、平均规划时间、路径平均曲率。把参数和统计结果打印下来才能比较。验证脚本骨架rng(42); for i 1:20 start randStart(); goal randGoal(); for j 1:5 tic; path hybridAstarSearch(map, start, goal, params); t toc; if ~isempty(path) success success 1; timeSum timeSum t; end end end fprintf(成功率 %.1f%% 平均耗时 %.2fs\n, ... success/(20*5)*100, timeSum/max(success, 1));这段验证逻辑我一直保留在工程目录里每次调参后先跑它再去看单条路径。印象最深的一次教训是我把栅格分辨率从0.2m加密到0.1m单条路径看着更平滑但批量验证里成功率从95%掉到80%原因是状态栅格膨胀让窄通道可通行性变差参数耦合往往从统计里才看得到。希望帮到你。本文还有配套的精品资源点击获取