基于改进人工势场法的移动机器人动态避障路径规划仿真实验方案
(适合笔记本电脑运行,基于 Python+PyGame 可视化实现)
一、实验名称
融合虚拟目标点的改进人工势场法移动机器人动态避障路径规划仿真
二、实验目的
掌握人工势场法的核心原理(引力场、斥力场建模)及在路径规划中的应用,理解传统智能控制算法的局限性。
针对传统人工势场法 “局部极小值陷阱” 缺陷,设计 “虚拟目标点引导” 改进策略,深化 “算法优化与创新” 的工程思维。
学会使用 Python+PyGame 搭建可视化仿真平台,实现移动机器人路径规划的动态演示与性能评估。
对比传统与改进算法在静态 / 动态障碍物场景下的路径规划效果,验证智能控制算法优化的实际价值。
三、实验环境
3.1 硬件要求
笔记本电脑(CPU i3 及以上,内存 4GB 及以上,集成显卡即可满足可视化需求)
无额外硬件(全程软件仿真,无需实体机器人)
3.2 软件要求
操作系统:Windows 10/11、macOS 或 Linux(跨平台兼容)
核心工具:Python 3.7 及以上版本
依赖库:
pygame:用于搭建可视化仿真界面(动态显示机器人、障碍物、路径)
numpy:用于向量运算与数值计算
matplotlib:用于路径数据的离线分析与绘图
四、实验原理
4.1 核心问题与场景建模
以 “室内移动机器人自主导航” 为实际场景(模拟扫地机器人、配送机器人),核心需求是:机器人从起点到终点的过程中,自主避开静态障碍物(如家具)和动态障碍物(如行人),且路径满足 “短、平滑、无碰撞”。
4.1.1 实体建模
移动机器人:简化为半径 5cm 的圆形质点,最大移动速度 0.2m/s,可实时计算速度向量(方向由势场力决定)。
静态障碍物:矩形(如桌子)或圆形(如垃圾桶),位置固定,斥力场作用范围为障碍物边界外 10cm。
动态障碍物:圆形质点,速度大小 0.1~0.15m/s,方向随机(模拟行人移动),斥力场作用范围同静态障碍物。
环境边界:100cm×80cm 的矩形区域(模拟房间),边界产生斥力防止机器人越界。
4.1.2 传统人工势场法原理
通过模拟 “引力 - 斥力平衡” 引导机器人运动:
引力场:终点对机器人产生引力,方向指向终点,大小与距离成正比:
F
att
(q)=k
att
⋅∥q−q
goal
∥
其中,
k
att
为引力系数,
q
为机器人当前位置,
q
goal
为终点位置。
斥力场:障碍物对机器人产生斥力,方向背离障碍物,距离越近斥力越大:
F
rep
(q)={
k
rep
⋅(
ρ(q,q
obs
)
1
−
ρ
0
1
)⋅
ρ(q,q
obs
)
2
1
⋅
ρ(q,q
obs
)
q−q
obs
0
ρ(q,q
obs
)≤ρ
0
ρ(q,q
obs
)>ρ
0
其中,
k
rep
为斥力系数,
q
obs
为障碍物位置,
ρ(q,q
obs
)
为机器人与障碍物的距离,
ρ
0
为斥力作用半径。
合力计算:机器人的运动方向由引力与斥力的合力决定:
F
total
(q)=F
att
(q)+F
rep
(q)
。
4.1.3 改进策略:虚拟目标点引导(解决局部极小值)
传统算法易陷入 “引力与斥力抵消” 的局部极小值(如机器人被障碍物包围时,合力为零无法移动)。改进方案:
局部极小值判断:当机器人连续 3 秒移动距离小于 0.5cm,且未到达终点时,判定陷入局部极小值。
虚拟目标点生成:在机器人与终点的连线方向上,远离障碍物的一侧生成虚拟目标点(距离机器人 15cm)。
引导脱离:机器人先向虚拟目标点运动,脱离局部极小值后,重新以真实终点为目标继续规划路径。
五、实验步骤(分 7 步,含代码与操作细节)
步骤 1:环境搭建与依赖库安装(15 分钟)
安装 Python:从Python 官网下载 3.9 版本,勾选 “Add Python to PATH”,默认安装。
安装依赖库:打开笔记本 “命令提示符”(Windows)或 “终端”(macOS/Linux),输入以下命令并回车:
bash
pip install pygame numpy matplotlib
验证安装:输入python -m pygame.examples.aliens,若弹出游戏窗口,说明 PyGame 安装成功。
步骤 2:基础仿真框架搭建(30 分钟)
新建 Python 文件,命名为robot_path_planning.py,导入依赖库:
python
import pygame
import numpy as np
import matplotlib.pyplot as plt
定义全局参数(可直接复制,后续可调整):
python
# 环境参数(单位:cm,转换为像素时×2便于显示)
ENV_WIDTH, ENV_HEIGHT = 100, 80
PIXEL_SCALE = 2 # 1cm = 2像素
# 机器人参数
ROBOT_RADIUS = 5
ROBOT_SPEED = 0.2 # cm/frame(帧率30,对应0.2×30=6cm/s)
# 势场参数
k_att, k_rep = 0.5, 100 # 引力/斥力系数
rho0 = 10 # 斥力作用半径(cm)
# 颜色定义(RGB)
COLOR_WHITE = (255,255,255)
COLOR_ROBOT = (0,255,0) # 绿色机器人
COLOR_GOAL = (255,0,0) # 红色终点
COLOR_OBSTACLE_STATIC = (128,128,128) # 灰色静态障碍物
COLOR_OBSTACLE_DYNAMIC = (255,165,0) # 橙色动态障碍物
COLOR_PATH = (0,0,255) # 蓝色路径
初始化 PyGame 窗口:
python
pygame.init()
screen = pygame.display.set_mode((ENV_WIDTH*PIXEL_SCALE, ENV_HEIGHT*PIXEL_SCALE))
pygame.display.set_caption("改进人工势场法路径规划仿真")
clock = pygame.time.Clock()
步骤 3:核心实体类编写(45 分钟)
3.1 机器人类(含位置、速度、路径记录)
python
class Robot:
def __init__(self, start_pos):
self.x = start_pos[0] # x坐标(cm)
self.y = start_pos[1] # y坐标(cm)
self.vx = 0 # x方向速度
self.vy = 0 # y方向速度
self.path = [(self.x, self.y)] # 记录路径
def update_pos(self):
# 更新位置(速度×时间步,帧率30→时间步1/30s)
self.x += self.vx * (1/30)
self.y += self.vy * (1/30)
# 边界碰撞检测(碰到边界反弹)
if self.x - ROBOT_RADIUS <= 0 or self.x + ROBOT_RADIUS >= ENV_WIDTH:
self.vx *= -0.5
if self.y - ROBOT_RADIUS <= 0 or self.y + ROBOT_RADIUS >= ENV_HEIGHT:
self.vy *= -0.5
# 记录路径(每5帧记录一次,减少数据量)
if len(self.path) % 5 == 0:
self.path.append((self.x, self.y))
def draw(self, screen):
# 绘制机器人(圆形)
pygame.draw.circle(screen, COLOR_ROBOT,
(int(self.x*PIXEL_SCALE), int(self.y*PIXEL_SCALE)),
ROBOT_RADIUS*PIXEL_SCALE)
# 绘制路径(线段连接历史位置)
if len(self.path) > 1:
for i in range(1, len(self.path)):
pygame.draw.line(screen, COLOR_PATH,
(int(self.path[i-1][0]*PIXEL_SCALE), int(self.path[i-1][1]*PIXEL_SCALE)),
(int(self.path[i][0]*PIXEL_SCALE), int(self.path[i][1]*PIXEL_SCALE)),
1)
3.2 障碍物类(区分静态 / 动态)
python
class Obstacle:
def __init__(self, pos, radius, is_dynamic=False):
self.x = pos[0]
self.y = pos[1]
self.radius = radius # 障碍物半径(cm)
self.is_dynamic = is_dynamic
# 动态障碍物随机速度
if is_dynamic:
angle = np.random.uniform(0, 2*np.pi)
self.vx = np.cos(angle) * np.random.uniform(0.1, 0.15)
self.vy = np.sin(angle) * np.random.uniform(0.1, 0.15)
def update_pos(self):
# 动态障碍物更新位置
if self.is_dynamic:
self.x += self.vx * (1/30)
self.y += self.vy * (1/30)
# 边界反弹
if self.x - self.radius <= 0 or self.x + self.radius >= ENV_WIDTH:
self.vx *= -1
if self.y - self.radius <= 0 or self.y + self.radius >= ENV_HEIGHT:
self.vy *= -1
def draw(self, screen):
color = COLOR_OBSTACLE_DYNAMIC if self.is_dynamic else COLOR_OBSTACLE_STATIC
pygame.draw.circle(screen, color,
(int(self.x*PIXEL_SCALE), int(self.y*PIXEL_SCALE)),
self.radius*PIXEL_SCALE)
步骤 4:传统人工势场法实现(30 分钟)
编写势场力计算函数:
python
def calculate_force_traditional(robot, goal, obstacles):
# 1. 计算引力
dx_att = goal[0] - robot.x
dy_att = goal[1] - robot.y
dist_att = np.sqrt(dx_att**2 + dy_att**2)
F_att_x = k_att * dx_att
F_att_y = k_att * dy_att
# 2. 计算斥力(所有障碍物的斥力之和)
F_rep_x, F_rep_y = 0, 0
for obs in obstacles:
dx_rep = robot.x - obs.x
dy_rep = robot.y - obs.y
dist_rep = np.sqrt(dx_rep**2 + dy_rep**2)
# 仅在斥力作用范围内计算
if dist_rep <= rho0 and dist_rep > 0:
rep_mag = k_rep * (1/dist_rep - 1/rho0) / (dist_rep**2)
F_rep_x += rep_mag * (dx_rep / dist_rep)
F_rep_y += rep_mag * (dy_rep / dist_rep)
# 3. 计算合力与速度
F_total_x = F_att_x + F_rep_x
F_total_y = F_att_y + F_rep_y
# 速度大小限制在最大速度内
speed = np.sqrt(F_total_x**2 + F_total_y**2)
if speed > 0:
robot.vx = (F_total_x / speed) * ROBOT_SPEED
robot.vy = (F_total_y / speed) * ROBOT_SPEED
else:
robot.vx, robot.vy = 0, 0 # 合力为零(局部极小值)
编写仿真主函数(传统算法):
python
def simulate_traditional():
# 初始化实体:起点(10,10),终点(90,70)
robot = Robot((10, 10))
goal = (90, 70)
# 障碍物:2个静态(圆形,半径5cm),1个动态(半径4cm)
obstacles = [
Obstacle((30, 30), 5, is_dynamic=False),
Obstacle((70, 50), 5, is_dynamic=False),
Obstacle((50, 20), 4, is_dynamic=True)
]
running = True
step = 0
while running:
screen.fill(COLOR_WHITE)
# 事件处理(关闭窗口退出)
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
# 计算势场力与更新位置
calculate_force_traditional(robot, goal, obstacles)
robot.update_pos()
for obs in obstacles:
obs.update_pos()
# 绘制所有实体
robot.draw(screen)
for obs in obstacles:
obs.draw(screen)
# 绘制终点(圆形)
pygame.draw.circle(screen, COLOR_GOAL,
(int(goal[0]*PIXEL_SCALE), int(goal[1]*PIXEL_SCALE)),
3*PIXEL_SCALE)
# 到达终点判断(距离<3cm)
if np.sqrt((robot.x - goal[0])**2 + (robot.y - goal[1])**2) < 3:
print("传统算法:到达终点!")
running = False
# 防止无限循环(超过3000步退出)
if step > 3000:
print("传统算法:陷入局部极小值,仿真终止!")
running = False
step += 1
pygame.display.flip()
clock.tick(30) # 帧率30
# 保存路径数据(用于后续对比)
np.save("traditional_path.npy", robot.path)
pygame.quit()
步骤 5:改进人工势场法实现(45 分钟)
编写改进算法的势场力计算函数(加入虚拟目标点逻辑):
python
def calculate_force_improved(robot, goal, obstacles, virtual_goal=None):
# 目标选择:优先向虚拟目标点运动,无虚拟目标则用真实终点
current_goal = virtual_goal if virtual_goal is not None else goal
# 1. 计算引力(指向当前目标)
dx_att = current_goal[0] - robot.x
dy_att = current_goal[1] - robot.y
dist_att = np.sqrt(dx_att**2 + dy_att**2)
F_att_x = k_att * dx_att
F_att_y = k_att * dy_att
# 2. 计算斥力(同传统算法)
F_rep_x, F_rep_y = 0, 0
for obs in obstacles:
dx_rep = robot.x - obs.x
dy_rep = robot.y - obs.y
dist_rep = np.sqrt(dx_rep**2 + dy_rep**2)
if dist_rep <= rho0 and dist_rep > 0:
rep_mag = k_rep * (1/dist_rep - 1/rho0) / (dist_rep**2)
F_rep_x += rep_mag * (dx_rep / dist_rep)
F_rep_y += rep_mag * (dy_rep / dist_rep)
# 3. 计算合力与速度
F_total_x = F_att_x + F_rep_x
F_total_y = F_att_y + F_rep_y
speed = np.sqrt(F_total_x**2 + F_total_y**2)
if speed > 0:
robot.vx = (F_total_x / speed) * ROBOT_SPEED
robot.vy = (F_total_y / speed) * ROBOT_SPEED
else:
robot.vx, robot.vy = 0, 0
return current_goal
编写改进算法的仿真主函数(核心:局部极小值判断与虚拟目标点生成):
python
def simulate_improved():
robot = Robot((10, 10))
goal = (90, 70)
obstacles = [
Obstacle((30, 30), 5, is_dynamic=False),
Obstacle((70, 50), 5, is_dynamic=False),
Obstacle((50, 20), 4, is_dynamic=True)
]
running = True
step = 0
virtual_goal = None
stuck_count = 0 # 记录陷入停滞的步数
last_pos = (robot.x, robot.y) # 上一帧位置
while running:
screen.fill(COLOR_WHITE)
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
# 1. 局部极小值判断:连续移动距离<0.01cm→停滞
current_pos = (robot.x, robot.y)
dist_moved = np.sqrt((current_pos[0]-last_pos[0])**2 + (current_pos[1]-last_pos[1])**2)
if dist_moved < 0.01:
stuck_count += 1
else:
stuck_count = 0
virtual_goal = None # 脱离停滞→清除虚拟目标
last_pos = current_pos
# 2. 生成虚拟目标点(停滞超过90步→3秒)
if stuck_count > 90 and virtual_goal is None:
# 虚拟目标点:在机器人→真实终点方向上,远离障碍物一侧15cm
dx_goal = goal[0] - robot.x
dy_goal = goal[1] - robot.y
dir_goal = np.array([dx_goal, dy_goal]) / np.sqrt(dx_goal**2 + dy_goal**2)
# 随机偏移(远离障碍物)
offset = np.array([-dir_goal[1], dir_goal[0]]) * np.random.uniform(5, 8)
virtual_goal = (
robot.x + dir_goal[0]*15 + offset[0],
robot.y + dir_goal[1]*15 + offset[1]
)
print("生成虚拟目标点:", virtual_goal)
# 3. 计算势场力与更新位置
current_goal = calculate_force_improved(robot, goal, obstacles, virtual_goal)
robot.update_pos()
for obs in obstacles:
obs.update_pos()
# 4. 绘制实体(额外绘制虚拟目标点)
robot.draw(screen)
for obs in obstacles:
obs.draw(screen)
pygame.draw.circle(screen, COLOR_GOAL,
(int(goal[0]*PIXEL_SCALE), int(goal[1]*PIXEL_SCALE)),
3*PIXEL_SCALE)
if virtual_goal is not None:
pygame.draw.circle(screen, (128,0,128), # 紫色虚拟目标
(int(virtual_goal[0]*PIXEL_SCALE), int(virtual_goal[1]*PIXEL_SCALE)),
2*PIXEL_SCALE, 1) # 空心圆
# 5. 到达判断(真实终点或虚拟目标)
if np.sqrt((robot.x - goal[0])**2 + (robot.y - goal[1])**2) < 3:
print("改进算法:到达真实终点!")
running = False
if virtual_goal and np.sqrt((robot.x - virtual_goal[0])**2 + (robot.y - virtual_goal[1])**2) < 2:
virtual_goal = None # 到达虚拟目标→清除
if step > 5000:
print("改进算法:仿真超时!")
running = False
step += 1
pygame.display.flip()
clock.tick(30)
np.save("improved_path.npy", robot.path)
pygame.quit()
步骤 6:仿真运行与结果记录(60 分钟)
6.1 分场景测试
静态障碍物场景:修改obstacles列表,删除动态障碍物(仅保留 2 个静态障碍物),分别运行传统与改进算法:
传统算法调用:在代码末尾添加simulate_traditional(),运行python robot_path_planning.py。
改进算法调用:替换为simulate_improved(),重新运行。
观察现象:传统算法可能在两个障碍物之间陷入停滞(局部极小值),改进算法生成虚拟目标点后脱离。
动态障碍物场景:使用原始obstacles列表(含 1 个动态障碍物),重复上述运行,观察机器人是否能实时避开移动的障碍物。
6.2 数据记录
仿真结束后,笔记本会生成traditional_path.npy和improved_path.npy两个路径数据文件,记录机器人的运动轨迹。
步骤 7:结果分析与可视化(30 分钟)
新建result_analysis.py文件,编写数据可视化代码:
python
import numpy as np
import matplotlib.pyplot as plt
# 加载路径数据
path_trad = np.load("traditional_path.npy")
path_impr = np.load("improved_path.npy")
# 障碍物与目标位置(同仿真参数)
obstacles = [(30,30), (70,50), (50,20)]
goal = (90,70)
start = (10,10)
# 绘制路径对比图
plt.figure(figsize=(10,8))
# 绘制障碍物
for obs in obstacles:
circle = plt.Circle(obs, 5, color='gray', alpha=0.5)
plt.gca().add_patch(circle)
# 绘制路径
plt.plot(path_trad[:,0], path_trad[:,1], 'b--', label='传统人工势场法', linewidth=1.5)
plt.plot(path_impr[:,0], path_impr[:,1], 'r-', label='改进人工势场法', linewidth=1.5)
# 绘制起点和终点
plt.scatter(start[0], start[1], c='green', s=100, label='起点', zorder=5)
plt.scatter(goal[0], goal[1], c='red', s=100, label='终点', zorder=5)
# 设置坐标轴
plt.xlim(0, 100)
plt.ylim(0, 80)
plt.xlabel('X坐标 (cm)')
plt.ylabel('Y坐标 (cm)')
plt.title('传统与改进人工势场法路径规划对比')
plt.legend()
plt.grid(True, alpha=0.3)
plt.savefig("path_comparison.png", dpi=300)
plt.show()
运行分析代码,生成路径对比图,填写性能评估表:
算法类型 场景 是否陷入局部极小值 路径长度 (cm) 避障成功率 到达终点时间 (s)
传统人工势场法 静态障碍物
动态障碍物
改进人工势场法 静态障碍物
动态障碍物
指标计算方法:
路径长度:累加路径点之间的欧氏距离;
避障成功率:无碰撞次数 / 总测试次数(重复 3 次取平均值);
到达终点时间:仿真终止时的step/30(帧率 30)。
六、实验创新点与价值
算法创新:针对传统人工势场法的经典缺陷,提出 “虚拟目标点引导” 改进策略,可直观展示智能控制算法的优化过程。
可视化创新:通过 PyGame 实现动态仿真,实时显示机器人运动、障碍物移动、虚拟目标点生成,比 MATLAB 仿真更具直观性和交互性。
场景创新:涵盖静态与动态障碍物,贴近真实机器人导航场景,解决 “动态避障” 这一实际工程问题。
工具创新:采用 Python 生态实现,代码开源可复用,且 PyGame 学习成本低于 MATLAB,便于学生二次开发。
七、实验拓展(可选)
多机器人协同规划:增加机器人数量,设计 “优先级避让” 规则(如小机器人避让大机器人),验证多智能体控制逻辑。
混合算法优化:融合 A算法的全局路径引导,先用 A生成粗略路径,再用改进人工势场法进行局部避障,实现 “全局 - 局部” 双层规划。
参数自适应调整:通过遗传算法优化k_att、k_rep等势场参数,使机器人在不同场景下均达到最优性能(可调用scipy库的遗传算法工具)。
关键说明
若笔记本性能较弱,可降低帧率(如clock.tick(20))或减少障碍物数量,不影响实验结论。
局部极小值场景可通过调整障碍物位置强化(如将两个静态障碍物对称放置在起点 - 终点连线上)。
代码可从 GitHub 获取简化版(搜索 “robot-path-planning-pygame”),适合编程基础薄弱的学生快速上手。完成上面的实验内容
最新发布