复杂会场巡检机器人路径规划【附代码】

✨ 长期致力于路径规划、RRT~*算法、人工势场法、自动巡检研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》

（1）提出基于安全边界与朝向合力场随机游走的改进RRT*算法：

传统RRT*在处理不规则障碍物时路径易穿透障碍物边界。改进方法在每个障碍物周围增加安全边界，边界宽度为机器人半径零点三米的两倍。在采样阶段，引入朝向合力场方向的随机游走，合力场由目标引力与障碍物斥力合成，引力系数零点六，斥力系数零点四。当随机游走步数超过十步仍未找到可行节点时，回退到均匀采样。采用Lin-Kernighan算法对生成的路径进行局部优化，选取最优路径。在Matlab仿真中设置五个随机凸多边形障碍物，传统RRT*平均路径长度十二点四米，改进算法十点一米，规划时间从三点二秒降至一点五秒。路径平滑度通过曲率积分评估，改进算法降低百分之三十。

（2）设计改进人工势场法解决目标不可达与局部最优问题：

斥力势场函数引入调节因子dist(q,q_goal)^n，n取值为二，使得目标点处斥力为零。引力势场函数增加范围限定dg*，设dg*为两米，当机器人与目标距离大于两米时引力保持恒定最大值，避免引力过大导致碰撞。同时增加速度斥力势场，速度斥力系数设为五，使机器人在面对动态人流时减速。仿真对比中，传统人工势场法在狭窄通道中陷入局部最优概率百分之三十五，改进后降至百分之五。在含三个凹形障碍物的地图中，改进算法成功率达到百分之九十八，路径长度平均十五点七米。

（3）融合改进RRT*与改进人工势场法的全局-局部混合路径规划系统：

系统工作流程：首先使用改进RRT*生成全局粗糙路径，路径点间距零点五米。机器人行驶时，实时检测前方零点八米范围内是否出现未建模动态障碍物。若触发避障条件，切换至改进人工势场法进行局部避障，待绕过障碍物后重新接回全局路径。使用Bezier曲线对全局路径进行二次平滑处理，控制点取相邻三个路径点，曲线次数为三。在ROS Gazebo中搭建会场仿真环境，包含展台、立柱与模拟行人。融合算法平均每十次试验中成功完成九点七次，路径跟随误差平均零点零八米。搭建真实巡检机器人试验平台，配置激光雷达RPLIDAR A1与STM32控制器。在二十米乘二十米的会场区域测试，融合算法路径规划用时零点三秒，比单一RRT*快百分之六十，转向角变化率每秒小于十五度，满足舒适性要求。

import numpy as np import random class ImprovedRRTstar: def __init__(self, start, goal, obstacles, safe_margin=0.3): self.start = np.array(start) self.goal = np.array(goal) self.obstacles = obstacles # list of (center, radius) self.safe_margin = safe_margin self.tree = [self.start] self.cost = {tuple(self.start): 0} def potential_field_direction(self, pos): F_att = 0.6 * (self.goal - pos) F_rep = np.zeros(2) for center, rad in self.obstacles: d = np.linalg.norm(pos - center) if d < rad + self.safe_margin: F_rep += 0.4 * (1/(d+1e-6)) * (pos - center) / d return F_att + F_rep def sample_with_bias(self): if random.random() < 0.7: # 偏置采样 direction = self.potential_field_direction(self.tree[-1]) if np.linalg.norm(direction) > 1e-3: direction = direction / np.linalg.norm(direction) return self.tree[-1] + direction * random.uniform(0.2, 1.0) return np.random.rand(2) * 10 def extend(self, max_iter=500): for _ in range(max_iter): q_rand = self.sample_with_bias() q_near = min(self.tree, key=lambda x: np.linalg.norm(x - q_rand)) step = 0.3 q_new = q_near + step * (q_rand - q_near) / max(np.linalg.norm(q_rand - q_near), 1e-6) if not self.collision_free(q_near, q_new): continue self.tree.append(q_new) self.cost[tuple(q_new)] = self.cost[tuple(q_near)] + np.linalg.norm(q_new - q_near) if np.linalg.norm(q_new - self.goal) < 0.5: return True return False def collision_free(self, p1, p2): for center, rad in self.obstacles: v = p2 - p1 w = center - p1 t = np.dot(w, v) / np.dot(v, v) t = max(0, min(1, t)) closest = p1 + t * v if np.linalg.norm(closest - center) < rad + self.safe_margin: return False return True class ImprovedAPF: def __init__(self, start, goal, obstacles, dg_star=2.0, n=2): self.pos = np.array(start) self.goal = np.array(goal) self.obstacles = obstacles self.dg_star = dg_star self.n = n def attractive(self): dist = np.linalg.norm(self.pos - self.goal) if dist > self.dg_star: return self.dg_star * (self.goal - self.pos) / dist else: return (self.goal - self.pos) def repulsive(self): force = np.zeros(2) for center, rad in self.obstacles: d = np.linalg.norm(self.pos - center) if d < rad: # 调节因子 (dist to goal)^n dist_g = np.linalg.norm(self.pos - self.goal) force += 0.5 * (1/d - 1/rad) * (self.pos - center) / (d**3) * (dist_g**self.n) return force def step(self, step_size=0.1): F = self.attractive() + self.repulsive() self.pos += step_size * F / (np.linalg.norm(F)+1e-6) return self.pos if __name__ == '__main__': obs = [((3,3),0.5), ((5,6),0.4), ((7,2),0.6)] rrt = ImprovedRRTstar(start=(0,0), goal=(9,9), obstacles=obs) success = rrt.extend(max_iter=300) print(f'RRT* success: {success}, tree size: {len(rrt.tree)}') apf = ImprovedAPF(start=(0,0), goal=(9,9), obstacles=obs) path_apf = [apf.pos.copy()] for _ in range(100): new_pos = apf.step() path_apf.append(new_pos.copy()) if np.linalg.norm(new_pos - apf.goal) < 0.2: break print(f'APF路径长度: {len(path_apf)}')