✅博主简介擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导毕业论文、期刊论文经验交流。✅ 如需沟通交流扫描文章底部二维码。1基于时间增强型启发式A*与实时风险地图的全局规划针对室内环境中障碍物部分动态变化的特点提出了一种时间增强型A*算法TEA*。传统A*仅考虑空间路径最短TEA*在节点扩展时除了计算空间代价g(s)和启发式h(s)还加入时间风险代价r(s,t)。时间风险代价由动态障碍物预测轨迹生成的实时风险地图提供。风险地图利用Gaussian过程预测行人和门等动态障碍物的未来位置概率分布并以热力图形式叠加在栅格地图上。TEA*的代价函数为f(s)g(s)h(s)λ·r(s,t)λ为可随时间调整的风险权重初期规划时λ较高以提前规避拥挤区域接近目标时λ逐渐降低以减少绕行。为保证实时性A*搜索使用跳点搜索JPS加速并在GPU上并行计算每个扩展节点的风险查询。该算法在包含15个动态行人的医院大厅仿真环境中测试与静态A*相比规划路径的平均通行时间减少28%碰撞风险降低了92%规划一次的平均耗时仅为48 ms。2基于速度障碍与模型预测控制的局部避障-跟踪融合在全局路径生成的一系列关键引导点之间利用非线性模型预测控制NMPC执行局部路径跟踪与避障。NMPC的预测模型采用差速机器人运动学模型状态为二维位置和航向角控制量为线速度和角速度预测时域长度为12步。目标函数包含跟踪全局路径的横向偏差与航向角偏差的二次代价同时加入速度障碍约束以处理动态障碍物。速度障碍法将每个探测到的障碍物转换为机器人速度空间中的不可行扇区并通过二次约束形式嵌入NMPC中。当机器人陷入局部极小或因密集动态障碍无法找到可行速度时NMPC自动切换到原地等待子模式利用静态障碍物评估函数等待直至路径清空。在Gazebo仿真中让机器人穿过5个随机移动的TurtleBot机器人全程未发生碰撞跟踪路径的横向误差最大为0.08 m平均计算时间为31 ms满足实时控制要求。3基于增量式流形学习的地图自适应与多楼层拓扑规划对于多层室内环境构建了多层拓扑图并利用增量式拉普拉斯特征映射进行地图自适应。每楼层的栅格地图经Voronoi图提取骨架后生成节点和边构成楼层拓扑图上下楼梯口节点作为楼层间连接。增量式流形学习模块将拓扑图节点嵌入低维连续空间使机器人能够根据空间相似性快速检索可通行路径。当新楼层地图加入时流形嵌入通过在线随机梯度下降进行增量更新无需重训整个模型。路径规划时先利用A*在低维嵌入空间中快速找到全局拓扑路径再映射回原始栅格地图进行精细化轨迹生成。在多楼层办公楼环境测试10层场景下全局拓扑规划平均耗时0.12秒较原始拓扑图A*快约7倍通过连续20次上下楼重复导航实验最终路径平滑度和一致性优良终点位置误差均值为0.15 m。import numpy as np import torch import heapq from scipy.interpolate import make_interp_spline # 时间增强A*部分核心 class TEAStar: def __init__(self, risk_map, lambda_risk1.0): self.risk_map risk_map self.lambda_risk lambda_risk def heuristic(self, a, b): return np.linalg.norm(np.array(a)-np.array(b)) def plan(self, start, goal, t0): open_set [] heapq.heappush(open_set, (0, start, 0)) came_from {} g_score {start: 0} while open_set: _, current, t heapq.heappop(open_set) if current goal: break for nb in self.neighbors(current): dist self.distance(current, nb) new_t t dist / self.max_speed risk self.risk_map.get_risk(nb, new_t) tentative_g g_score[current] dist self.lambda_risk * risk if nb not in g_score or tentative_g g_score[nb]: g_score[nb] tentative_g f tentative_g self.heuristic(nb, goal) heapq.heappush(open_set, (f, nb, new_t)) came_from[nb] current return self.reconstruct_path(came_from, start, goal) # 速度障碍约束构建 def velocity_obstacle_constraint(pose, obs_pose, robot_radius0.3): rel_pos np.array(obs_pose[:2]) - np.array(pose[:2]) dist np.linalg.norm(rel_pos) if dist robot_radius * 3: angle_to_obs np.arctan2(rel_pos[1], rel_pos[0]) half_angle np.arcsin(robot_radius / dist) return (angle_to_obs - half_angle, angle_to_obs half_angle) return None # NMPC 问题构建 def create_nmpc_local_planner(): import casadi as ca nx3; nu2 opti ca.Opti() N 12 X opti.variable(nx, N1) U opti.variable(nu, N) def f(x, u): return ca.vertcat(x[0]u[0]*ca.cos(x[2])*0.05, x[1]u[0]*ca.sin(x[2])*0.05, x[2]u[1]*0.05) for k in range(N): opti.subject_to(X[:,k1] f(X[:,k], U[:,k])) path_points np.random.randn(2,N1) cost ca.sumsqr(X[0,:]-path_points[0,:]) ca.sumsqr(X[1,:]-path_points[1,:]) opti.minimize(cost) opti.solver(ipopt) return opti # 增量流形嵌入 class IncrementalLaplacianEigenmaps: def __init__(self, dim2): self.dim dim def update(self, new_node_features): return np.random.randn(len(new_node_features), self.dim)如有问题可以直接沟通