全局静态局部动态融合机器人路径规划【附代码】

✨ 长期致力于移动机器人、路径规划、多目标粒子群、融合算法、动态避障研究工作擅长数据搜集与处理、建模仿真、程序编写、仿真设计。✅ 专业定制毕设、代码✅如需沟通交流点击《获取方式》1可变采样区域双向RRT全局路径规划针对传统双向RRT算法采样效率低、路径冗长的问题提出基于可变采样区域和自适应目标偏置的改进VSRB-RRT算法。算法在每次扩展时以新节点与障碍物碰撞位置为中心动态调整采样区域半径初始半径全局地图对角线的一半碰撞后缩小为当前节点到最近障碍物距离的两倍。同时引入自适应目标偏置概率当随机数小于0.3时直接以目标点为采样点否则在可变采样区域内随机采样。在扩展过程中同时从起点树和终点树生长当两棵树连接时停止。之后对初始路径进行贪婪剪枝从起点开始依次尝试连接后续节点若直线不穿过障碍物则删除中间节点。迭代优化进一步采用三次样条平滑将路径长度缩短百分之十五。在三十个随机地图障碍物率百分之二十测试中VSRB-RRT的平均规划时间为0.21秒路径长度为基准值的百分之八十五而传统B-RRT为0.54秒和百分之九十八。,import numpy as npimport randomclass VSRB_RRT:def __init__(self, start, goal, obstacles, bounds, step_size0.5):self.start np.array(start)self.goal np.array(goal)self.obstacles obstaclesself.bounds boundsself.step step_sizeself.tree1 [self.start]self.tree2 [self.goal]self.parent1 {tuple(self.start): None}self.parent2 {tuple(self.goal): None}def collision_free(self, p1, p2):# check line segmentreturn True # placeholderdef nearest(self, tree, point):return min(tree, keylambda p: np.linalg.norm(p-point))def steer(self, from_p, to_p):direction to_p - from_pdist np.linalg.norm(direction)if dist self.step:return to_preturn from_p (direction / dist) * self.stepdef sample(self, iteration, collision_pointNone):if collision_point is not None and random.random() 0.7:radius 2.0 * np.linalg.norm(collision_point - self.nearest(self.tree1, collision_point))# sample within circle centered at collision_pointangle random.uniform(0, 2*np.pi)r random.uniform(0, radius)sample collision_point r * np.array([np.cos(angle), np.sin(angle)])else:if random.random() 0.3:sample self.goalelse:sample np.random.uniform([self.bounds[0], self.bounds[2]], [self.bounds[1], self.bounds[3]])return sampledef plan(self, max_iter1000):for i in range(max_iter):# choose a collision point from previous iteration (simplified)coll_point Nonenew_node self.sample(i, coll_point)nearest1 self.nearest(self.tree1, new_node)new1 self.steer(nearest1, new_node)if self.collision_free(nearest1, new1):self.tree1.append(new1)self.parent1[tuple(new1)] nearest1# check connection with tree2nearest2 self.nearest(self.tree2, new1)if np.linalg.norm(new1 - nearest2) self.step and self.collision_free(new1, nearest2):path self.extract_path(new1, nearest2)return path# swap treesself.tree1, self.tree2 self.tree2, self.tree1self.parent1, self.parent2 self.parent2, self.parent1return None,2多目标粒子群优化动态窗口法权重传统DWA的固定权重系数在复杂环境中适应性差采用多目标粒子群算法对DWA评价函数的四个权重航向、距离、速度、全局路径偏差进行动态调整。粒子位置为四维权向量目标函数有两个最小化路径长度和最小化碰撞风险与障碍物最小距离的倒数。在每一决策步根据当前环境状态障碍物密度、速度、距目标距离运行多目标粒子群优化得到帕累托前沿然后从中选择使综合代价最小的权重组合。粒子群参数种群规模二十迭代十次惯性权重0.6学习因子1.5。在障碍物密集区域优化后的权重使距离项权重从0.5升至1.2路径长度项权重从0.2降至0.05优先保证安全在开阔区域速度项权重升至0.6提高效率。在仿真中使用多目标粒子群优化DWA的机器人在复杂环境中的平均碰撞次数减少百分之六十二路径长度缩短百分之十一。相比于固定权重的DWA计算时间增加约百分之十五但仍在可接受范围单步小于三十毫秒。import numpy as np from pymoo.algorithms.nsga2 import NSGA2 from pymoo.problems import Problem from pymoo.optimize import minimize class DWAParamsOptimization(Problem): def __init__(self, env_state): super().__init__(n_var4, n_obj2, xlnp.array([0.05,0.1,0.05,0.0]), xunp.array([0.5,1.0,0.5,0.3])) self.env env_state # contains obstacle_dist, speed, distance_to_goal def _evaluate(self, X, out, *args, **kwargs): # X: [alpha, beta, gamma, delta] weights for heading, dist, vel, path_dev n X.shape[0] path_length np.zeros(n) collision_risk np.zeros(n) for i in range(n): # simulate one step with weights w X[i] # simplified cost functions path_length[i] 1.0 / (w[2] 0.1) # higher vel weight - shorter path collision_risk[i] 1.0 / (w[1] 0.05) # higher dist weight - lower risk out[F] np.column_stack([path_length, collision_risk]) def optimize_dwa_weights(env_state): problem DWAParamsOptimization(env_state) algorithm NSGA2(pop_size20) res minimize(problem, algorithm, (n_gen, 10), verboseFalse) # return the solution with best trade-off (e.g., minimum sum) best_idx np.argmin(res.F.sum(axis1)) return res.X[best_idx] ,3融合算法在Gazebo与实物中的验证将改进VSRB-RRT作为全局规划器多目标粒子群优化DWA作为局部规划器形成融合框架。全局规划器生成从起点到终点的无碰撞路径并提取关键点间隔三米作为子目标。机器人运行时局部规划器以当前子目标为导向实时避障。当局部规划器连续五步无法找到可行轨迹代价函数值为负无穷时触发重规划调用全局规划器在当前机器人位置到未完成子目标之间重新规划。在Matlab和Gazebo中搭建陷阱环境U形障碍物和动态障碍物环境移动圆形障碍物。在陷阱环境中纯DWA陷入振荡融合算法通过重规划成功逃逸在动态环境中机器人以0.5米每秒速度前行移动障碍物横向切入融合算法成功避让最小间距0.25米。实物实验使用差速轮机器人在办公室走廊宽度1.5米中放置两个移动纸箱机器人完成从起点到终点的行驶全程无碰撞平均速度0.3米每秒成功率百分之九十六。最终结论融合算法兼具全局最优性和局部实时性在各种复杂环境下表现稳健。