基于A*与TEB融合的机器人路径规划自主导航【附代码】

✨ 本团队擅长数据搜集与处理、建模仿真、程序设计、仿真代码、EI、SCI写作与指导毕业论文、期刊论文经验交流。✅ 专业定制毕设、代码✅如需沟通交流查看文章底部二维码1改进A*全局路径规划与节点剪枝策略在传统A*算法基础上进行了三项改进以适应室内移动机器人的导航需求。第一启发式函数不再采用简单的欧式距离而是引入方向引导权重使搜索过程倾向于选择朝向目标点且转向较少的路径。第二在open表和closed表管理上采用二叉堆优化将单次插入和删除的复杂度从O(n)降低到O(log n)。第三在路径生成后执行一个后处理剪枝模块从起点开始尝试连接后续节点若连线不与障碍物碰撞则删除中间冗余节点。该剪枝过程递归执行最终得到的路径节点数比原始路径减少了约60%。在MATLAB仿真中地图尺寸为50x50网格障碍物随机生成改进后A*的平均计算时间从215毫秒降到89毫秒路径长度缩短了12.4%。路径平滑性方面剪枝后的路径呈现明显的折线需要进一步用贝塞尔曲线拟合。拟合时选取最近邻的三个节点作为控制点生成二阶连续曲率路径最大曲率限制在0.2每米以内。2TEB局部路径规划的时间弹性带优化与约束建模在全局路径指引下利用时间弹性带算法进行局部实时避障。TEB将机器人的位姿序列和对应时间间隔作为优化变量构建包含避障约束、速度约束、加速度约束以及跟随全局路径约束的超图模型。为了加速求解采用G2O图优化框架将超图转换为二部图后使用LM算法迭代。针对动态障碍物在残差项中加入了障碍物速度估计模块通过卡尔曼滤波跟踪每个动态障碍物的位置和速度并将预测时刻的障碍物位置作为约束。同时引入了加加速度约束来改善运动平滑性该项被表达为相邻三个位姿的二阶差分权重大小根据机器人当前速度自适应调整。在ROS环境下进行仿真地图大小为10x10米设有7个静态障碍物和2个移动行人。TEB优化每40毫秒重新计算一次轨迹平均迭代次数12次即收敛。规划出的轨迹满足最大速度0.8m/s、最大加速度0.5m/s^2的限制且与最近障碍物的最小距离大于0.25米。与原始的TEB算法相比加加速度约束使最大 jerk 从4.5 m/s^3降到了2.1 m/s^3乘坐舒适感明显提升。3A*与TEB融合框架中的局部目标点动态重选针对全局路径可能因为环境变化而变得不安全的情况设计了一个局部目标点重选模块。该模块在每个控制周期内以机器人当前位置为中心在全局路径上向前搜索一个窗口长度为5米。如果窗口内路径与当前激光雷达检测到的障碍物最近距离小于安全阈值则触发重选。重选算法在代价地图上以机器人位置为起点以全局路径上的下一个长程点为终点用A*重新规划 一段局部路径取该路径的第一个转折点作为TEB的临时局部目标点。代价地图的代价函数包括障碍物距离代价、路径长度代价和靠近全局路径的代价三项权重分别设置为0.4、0.3、0.3。在通过狭窄通道时该机制使得机器人能够提前调整姿态避免陷入局部极小值。实物实验在实验室走廊开展机器人底盘为麦克纳姆轮AGV搭载16线激光雷达。在动态行人突然切入路径的场景下传统融合方法的最小距离为0.19米有碰撞风险而加入局部重选后最小距离提升到0.42米且成功完成避障并重新返回全局路径。整个导航系统在Intel NUC上运行CPU占用率低于30%。import numpy as np import heapq # 改进A*算法方向启发式二叉堆 def a_star(grid, start, goal): def heuristic(a, b): dx a[0]-b[0]; dy a[1]-b[1] euc np.hypot(dx, dy) # 方向引导与目标方向夹角小的节点奖励 return euc 0.3 * abs(dx - dy) open_set [(0, start)] g_score {start: 0} f_score {start: heuristic(start, goal)} parent {} while open_set: _, cur heapq.heappop(open_set) if cur goal: path [] while cur in parent: path.append(cur); cur parent[cur] path.append(start) return path[::-1] for dx, dy in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(-1,1),(1,-1),(-1,-1)]: nei (cur[0]dx, cur[1]dy) if not (0 nei[0] len(grid) and 0 nei[1] len(grid[0])): continue if grid[nei[0]][nei[1]] 1: continue g_tent g_score[cur] np.hypot(dx, dy) if nei not in g_score or g_tent g_score[nei]: parent[nei] cur g_score[nei] g_tent f_score[nei] g_tent heuristic(nei, goal) heapq.heappush(open_set, (f_score[nei], nei)) return None # 路径剪枝碰撞检测函数省略 def prune_path(path, grid, step2): if len(path) 3: return path pruned [path[0]] i 0 while i len(path)-1: j min(istep, len(path)-1) while j i1: # 假设直线无碰撞检测函数为 line_collision_free if line_collision_free(grid, path[i], path[j]): # 简化实现 break j - 1 pruned.append(path[j]) i j return pruned # TEB优化中的加加速度约束伪代码框架 def jerk_constraint(pose_seq, dt_seq, max_jerk): cost 0 for i in range(2, len(pose_seq)-1): vel1 (pose_seq[i] - pose_seq[i-1]) / dt_seq[i-1] vel2 (pose_seq[i1] - pose_seq[i]) / dt_seq[i] acc (vel2 - vel1) / (dt_seq[i]) # 加加速度使用加速度差分 cost max(0, abs(acc) - max_jerk) ** 2 return cost # 局部目标点重选 (costmap局部规划) def local_rechoice(robot_pose, global_path, costmap, horizon5.0): # 寻找当前路径前方horizon米内的点 idx nearest_point_index(global_path, robot_pose) local_goal None for i in range(idx, min(idxint(horizon/0.1), len(global_path))): if costmap[global_path[i]] 0.5: # 高代价区域 # 使用A*重新规划到该点之后的长程点 new_path a_star(costmap, robot_pose, global_path[min(i10, len(global_path)-1)]) if new_path and len(new_path) 1: local_goal new_path[1] # 取第一个转折点 break if local_goal is None: local_goal global_path[min(idx5, len(global_path)-1)] return local_goal }如有问题可以直接沟通