逐行讲解Hybrid A*路径规划算法及Matlab版源码分析

逐行讲解hybrid astar路径规划 混合a星泊车路径规划 带你从头开始写hybridastar算法逐行源码分析matlab版hybridastar算法一、算法整体架构与核心定位Hybrid A混合A算法是专为非完整约束车辆如汽车设计的路径规划算法在泊车场景中具有精准、高效的优势。本文档解读的代码库基于MATLAB开发实现了从地图构建、状态校验到路径生成的全流程泊车路径规划方案支持垂直车位、斜车位、平行车位等多场景泊车需求核心架构分为地图场景模块、状态校验模块、路径规划模块和路径生成模块四大组件各模块协同实现“场景定义-碰撞检测-路径搜索-轨迹输出”的完整链路。二、核心模块功能解析一地图场景模块map_scenario_1~6.m该模块负责构建不同泊车场景的栅格地图定义起点、终点位姿并初始化规划环境。每个场景脚本对应一种典型泊车场景核心功能包括地图创建与障碍物设置- 基于binaryOccupancyMap类创建12×12或15×7等规格的栅格地图分辨率设为10即1m对应10个栅格栅格尺寸0.1m。- 通过setOccupancy函数在指定坐标范围内设置障碍物如车位边界、通道障碍例如在垂直车位场景mapscenario1.m中通过循环生成x/y坐标范围定义车位的左右边界和底部边界。起点与终点位姿定义- 起点位姿startPose格式为[x, y, θ]其中θ为航向角弧度制通常设置在泊车区域外的通道位置如[6, 8, -0.1]。- 终点位姿goalPose根据车位类型动态计算例如垂直车位的终点设为车位中心区域如[(x1x2)/2, y2-3.5, pi/2]斜车位则需结合车位倾斜角度调整航向角如75°或105°。场景可视化- 通过show(map)显示栅格地图使用plot函数标记起点红色“x”和终点蓝色“o”并添加场景标题如“垂直车位狭窄通道”便于直观观察规划环境。二状态校验模块VehicleMapStateValidator.m该模块是路径规划的“安全卫士”继承自MATLAB的nav.StateValidator类负责校验车辆状态位置、航向是否合法无碰撞、在地图边界内核心功能包括类属性初始化- 存储地图数据Map和校验步长ValidationDistance默认0.14m确保校验精度与车辆尺寸匹配。状态合法性校验isStateValid-边界校验先判断车辆位姿是否在地图边界内通过StateBounds获取地图范围对比位姿坐标。-碰撞校验将车辆抽象为10个校验点组成的多边形覆盖车头、车尾、车身两侧关键位置通过坐标变换旋转矩阵M计算校验点在世界坐标系中的实际位置再调用checkOccupancy函数判断校验点是否落在障碍物区域若任意一点占用则状态无效。运动合法性校验isMotionValid- 对两点之间的运动轨迹进行插值默认生成100个插值点逐一校验每个插值点的合法性若存在无效点则判定运动轨迹碰撞并返回最后一个有效点lastValid确保车辆运动过程无碰撞风险。三路径规划模块plannerMyHybridAStar.m / plannerVehParkingHybridAStar.m该模块是算法核心实现Hybrid A*的路径搜索逻辑支持自定义规划参数与MATLAB官方版本兼容核心功能分为基础参数配置、节点扩展、代价计算和解析扩展四部分基础参数配置- 关键参数包括最小转弯半径MinTurningRadius默认5m、运动基元长度MotionPrimitiveLength默认0.5m、运动基元数量NumMotionPrimitives默认3、方向切换代价DirectionSwitchingCost默认100等可根据车辆性能和场景复杂度调整。- 初始化OpenSet优先队列和NodeMap节点映射表用于存储待扩展节点和已扩展节点的父子关系确保路径可回溯。节点扩展逻辑-运动基元生成生成两类运动基元——转弯运动圆形轨迹和直线运动直线轨迹。转弯运动通过曲率curvature计算圆心位置和新节点位姿直线运动则根据航向角和运动长度直接计算新节点坐标。-有效性校验对生成的新节点先通过checkNodeValidity判断其所在栅格是否已被访问避免重复扩展再通过isPrimitiveValid校验运动轨迹转弯/直线上的所有插值点是否无碰撞仅保留有效节点。代价计算-路径代价gScore累计从起点到当前节点的运动代价包括运动长度代价直线/转弯长度×方向代价前进代价ForwardCost默认3后退代价ReverseCost默认3和方向切换代价若当前运动方向与上一节点不同叠加DirectionSwitchingCost。-启发代价hScore估算当前节点到终点的最小代价采用“角度偏差代价直线距离代价”即abs(angdiff(goalθ, currentθ))×最小转弯半径 欧氏距离确保搜索方向向终点收敛。-总代价fScorefScore gScore hScoreOpenSet按fScore升序排序优先扩展代价最小的节点保证路径最优性。解析扩展Analytic Expansion- 为提升搜索效率每迭代指定次数AnalyticExpansionInterval尝试从当前代价最小的节点直接生成到终点的无碰撞路径基于Reeds-Shepp曲线支持前进、后退、转弯的组合运动。- 通过checkAnalyticExpansion函数生成Reeds-Shepp曲线的插值点校验所有点无碰撞则解析扩展成功直接回溯路径否则继续节点扩展流程。四路径生成模块GetSlotPath.m / GetSlotPath1.m该模块针对平行车位等需要“揉库”多次进退调整方向的场景生成精细化的库内路径核心功能包括初始退车阶段- 从终点位姿泊车完成位置反向计算初始退车路径车辆先直行退至库内安全位置N1点计算退车长度N0Len时考虑车辆后悬距VehRear默认1m避免车尾碰撞。循环揉库阶段- 交替执行“前进转弯FLPath”和“后退转弯BRPath”- FLPath计算前进转弯的碰撞点如右前点与车位边界碰撞确定转弯终止位置和航向角确保前进过程不超出车位范围。- BRPath计算后退转弯的碰撞点如左后点与后边界、右后点与侧边界碰撞优先处理先发生的碰撞调整后退转弯轨迹。路径点格式转换- 将揉库过程的关键节点PathPoints转换为连续的路径点集refPath通过calcpoints函数生成直线或圆弧轨迹的插值点同时记录每个路径点的运动方向directions1为前进-1为后退为车辆控制提供精准参考。三、规划流程与场景适配一通用规划流程初始化运行对应场景脚本如mapscenario1.m创建地图、定义起止点初始化状态校验器VehicleMapStateValidator和规划器plannerMyHybridAStar。参数配置设置规划器参数如最小转弯半径、运动基元长度根据场景复杂度调整如狭窄通道场景需减小运动基元长度提升灵活性。路径搜索调用plan函数规划器通过节点扩展和解析扩展搜索从起点到终点的无碰撞路径记录路径点refPath.States和运动方向directions。结果可视化绘制车辆轮廓通过多边形校验点生成、规划路径蓝色实线前进段标红后退段标蓝直观展示路径可行性。二多场景适配能力车位类型核心适配逻辑代表脚本垂直车位终点航向角设为90°pi/2解析扩展优先生成“直角转弯直线”路径mapscenario1.m垂直车位狭窄通道在垂直车位基础上增加顶部障碍物yob11限制通道高度规划器通过减小运动基元长度提升避障精度mapscenario2.m斜车位75°/105°终点航向角匹配车位倾斜角度运动基元生成时考虑倾斜方向的碰撞风险解析扩展采用斜向Reeds-Shepp曲线mapscenario3.m / 4.m平行车位调用GetSlotPath生成揉库路径通过多次进退调整车身方向确保车辆平行驶入车位mapscenario5.m / 6.m四、关键技术亮点与应用价值非完整约束适配通过运动基元转弯直线和最小转弯半径限制严格匹配汽车的运动学特性避免生成“不可行驶”的路径如超小半径转弯。高效搜索策略结合A*的启发式搜索hScore引导方向和解析扩展Reeds-Shepp曲线缩短搜索路径平衡路径最优性和搜索效率在复杂场景中也能快速收敛。多场景通用性通过模块化设计支持垂直、斜向、平行等多种车位类型只需调整场景脚本的地图参数和起止点位姿即可适配不同泊车需求。工程化校验基于多边形校验点的碰撞检测逻辑可直接对接实际车辆的尺寸参数如车宽、车长、前后悬距降低从仿真到实车落地的适配成本。该代码库为泊车路径规划提供了完整的技术方案可作为自动驾驶低速泊车功能开发的基础框架也可通过调整参数如最小转弯半径、代价权重适配不同车型如轿车、SUV或场景如室内停车场、室外露天车位。逐行讲解hybrid astar路径规划 混合a星泊车路径规划 带你从头开始写hybridastar算法逐行源码分析matlab版hybridastar算法