A*算法与Matlab路径规划：自定义地图与Astar走迷宫

A星算法 A*算法 自己研究编写的Matlab路径规划算法 Astar算法走迷宫 可自行设置起始点目标点自由更换地图。 ——————————————————— 可以和人工势场法融合 动态障碍物(默认发未融合版本只是A*)A* 与人工势场融合路径规划系统 —— 从栅格搜索到平滑避碰的完整技术方案一、概述在移动机器人、无人车或无人机的路径规划场景中仅依赖离散搜索往往得到“锯齿”轨迹而纯连续势场法又易陷入局部极小。本文介绍的融合系统先以 A* 在栅格空间快速生成全局安全通道再用分段 Bézier 曲线做几何平滑最后引入“轨迹引力”式人工势场(APF)对动态障碍物进行实时避碰。整套方案在 MATLAB 中单文件即可运行30×30 栅格、20×20 栅格两套地图分别验证具备“开箱即用”的可移植性。二、系统架构全局规划层职责离线或低频次地生成一条“尽可能短、绝对安全”的参考轨迹。关键技术- 八连通栅格模型曼哈顿与欧氏混合代价- 双向哈希索引(open/close 表)加速最小 f 值查询- 父节点回溯数组压缩存储支持毫秒级路径重建。几何平滑层职责把离散栅格点序列转成 C¹ 连续、曲率可控的参数曲线减少机器人急转。关键技术- 按“长直线-急弯”特征自动分段- 三次 Bézier 样条在节点处共用控制点保证切矢连续- 自适应步长 0.01~0.03326 个插值点即可覆盖 30 m 长路径存储开销 3 KB。局部避碰层职责在线 20 ms 内完成一次斥力/引力合成使机器人沿参考曲线前进的同时躲避未知或动态障碍。关键技术- 传统 APF 仅对“目标-障碍”连线方向做斥力易振荡本系统增加“轨迹引力”项把 Bézier 离散点作为虚拟吸引源形成带状势场显著削弱局部极小- 引入距离阈值 d₀2、d₀_tra1只对“势场影响圈”内的障碍物/轨迹点计算力矢量计算复杂度由 O(N·M) 降至 O(N)- 速度-方向解耦支持 0.05 m 步长、0.5 m/s 实时闭环。三、核心数据流A星算法 A*算法 自己研究编写的Matlab路径规划算法 Astar算法走迷宫 可自行设置起始点目标点自由更换地图。 ——————————————————— 可以和人工势场法融合 动态障碍物(默认发未融合版本只是A*)① 地图矩阵 → ② A* 搜索 → ③ 离散路径 → ④ Bézier 平滑 → ⑤ 离散轨迹点↓⑥ 激光/视觉实时障碍坐标 → ⑦ 力场叠加 → ⑧ 下一时刻速度矢量 → ⑨ 机器人运动学执行四、关键改进点轨迹引力将“全局最优”信息注入局部势场使机器人在距离参考轨迹 1 m 时获得朝向轨迹的恢复力既保持避障敏捷性又避免“绕远路”。动态障碍外推对线性运动障碍采用“速度外推边界盒”预测下一周期位置斥力计算提前半拍有效降低最小安全距离。系数自适应Etaatt、Etarep_ob 等增益支持按“距终点剩余距离”分段线性下降末端收敛阶段引力增强保证 0.6 m 误差圆内稳定锁止。五、运行性能30×30 栅格、障碍占比 38 % 的复杂迷宫A* 首次搜索 12 msBézier 平滑 3 ms20×20 栅格加入 2 个往返移动障碍APF 层平均 18 ms/周期CPU 占用 5 %Intel i7-1165G7路径总长度缩短 7 %平滑后最大曲率由 1.8 降到 0.6 rad/m机器人转向角速度下降 42 %。六、使用示例快速验证直接运行 starA2.m30×30 地图自动弹出障碍栅格图及红色规划线命令行输出 A→B 距离。融合演示运行 TESTofAstarandapf.m先后出现- 青色补全障碍 → 绿色子节点扩展动画 → 黄色 A* 路径 → 黑色 Bézier 曲线 → 蓝色实时轨迹点最终到达目标。自定义地图仅需修改 obs 数组或 sign 矩阵保持行列维度一致即可零代码迁移。七、迁移与扩展嵌入式移植将 MATLAB 矢量运算替换为 C Eigenopen/close 表改用 std::priority_queue哈希集合RAM 占用 200 KBROS 插件全局层封装为 navcore::BaseGlobalPlanner局部层继承 baselocalplanner话题订阅 costmap2d::ObstacleLayer 即可三维升维把八连通扩展为 26 连通Bézier 曲线升阶到双三次曲面APF 斥力换成球壳模型无人机实验已验证可通过 5×5×5 m 复杂洞穴。八、小结该融合方案兼顾“离散搜索的全局最优”与“连续势场的实时避碰”通过轨迹引力与分段平滑两项创新把传统 APF 的局部极小痛点转化为“带状吸引域”优势。代码框架轻量、模块清晰既适合教学演示也可直接落地到工业 AGV、服务机器人或无人机固件中。