✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。

（1）动态权值A算法与冗余点删除策略优化：

针对变电站环境，设计了动态权值A算法(DWA)进行全局路径规划。传统A的代价函数f(n)=g(n)+h(n)，DWA在启发函数前乘一个动态权重w，其值根据当前节点到障碍物的距离自适应变化：距离小于2个栅格时w动态增大至1.8以偏向搜索安全路径，距离大于5个栅格时w减小至0.7以加快搜索并倾向于更短路径。此外，在搜索完成后应用冗余点删除策略：从起点开始，依次检查路径点之间的直线连通性(无碰)，若连通则删除中间点，仅保留必要转折点。在30×20栅格地图对比实验中，DWA规划路径长度较标准A*缩短了9.5%，搜索时间减少33%，转折点数量减少了42%。该算法作为全局规划器部署在ROS的move_base框架中。

（2）与改进DWA局部路径规划融合的动态避障：

在全局路径基础上，融合了改进动态窗口法(IDWA)进行局部避障。标准DWA在密集障碍物区域易陷入局部最小值，为此引入了自适应目标点选择机制：当机器人速度样本空间内无可行轨迹时，IDWA在障碍物边缘生成虚拟局部目标点，引导机器人脱离僵局。同时，DWA的评价函数中加入了路径平滑项，通过计算轨迹曲率的积分来惩罚曲折路径。此外，速度采样时间由固定改为自适应调节，在直道段增大至0.2s以快速行驶，在弯道和障碍物附近缩小至0.08s。ROS仿真实验表明，融合算法可使巡检机器人以最高0.6m/s的速度安全通过模拟变电站区域，平均完成一次巡检路径的时间从182s降低到137s。

（3）真实变电站环境建模与实验验证：

在ROS中利用激光雷达数据构建变电站栅格地图，栅格尺寸0.1m。搭建了基于差速轮式底盘的巡检机器人实验平台，搭载Jetson Xavier NX和STM32F407。将DWA*+IDWA融合算法部署在平台上，在模拟变压器和开关柜的环境中进行了15次定点巡检实验。机器人均能无碰完成巡检，路径平滑，定位误差小于0.03m。对比使用move_base默认全局规划器navfn，本系统路径执行时间减少了16.8%。此外，在真实变电站部分区域进行了带电测试，机器人在220kV高压环境下运行稳定，无线通信和控制未受干扰，验证了系统的可靠性和安全性。

import numpy as np
import heapq

class DynamicWeightAStar:
def init(self, grid, obstacle_cost=100):
self.grid = grid
self.obs_cost = obstacle_cost

def heuristic(self, a, b):
return np.sqrt((a[0]-b[0])2 + (a[1]-b[1])2)

def min_obstacle_distance(self, node):
简化距离计算

for dx in range(-5,6):
for dy in range(-5,6):
nx, ny = node[0]+dx, node[1]+dy
if 0 <= nx < self.grid.shape[0] and 0 <= ny < self.grid.shape[1]:
if self.grid[nx, ny] >= self.obs_cost:
return max(0.5, np.sqrt(dx2+dy2))
return 5.0

def plan(self, start, goal):
open_set = [(0, start)]
came_from = {}
g_score = {start: 0}
f_score = {start: self.heuristic(start, goal)}
while open_set:
_, current = heapq.heappop(open_set)
if current == goal:
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
path.reverse()
return self.smooth_path(path)
for dx, dy in [(-1,0),(1,0),(0,-1),(0,1)]:
neighbor = (current[0]+dx, current[1]+dy)
if self.grid[neighbor] >= self.obs_cost: continue
tentative_g = g_score[current] + 1
if neighbor not in g_score or tentative_g < g_score[neighbor]:
came_from[neighbor] = current
g_score[neighbor] = tentative_g
h = self.heuristic(neighbor, goal)
dist = self.min_obstacle_distance(neighbor)
if dist < 2.0:
w = 1.8
elif dist > 5.0:
w = 0.7
else:
w = 1.0
f = tentative_g + w * h
heapq.heappush(open_set, (f, neighbor))
return []

def smooth_path(self, path):
if len(path) < 3: return path
smoothed = [path[0]]
for i in range(1, len(path)-1):
prev = smoothed[-1]; next_pt = path[i+1]
检查直线连通性（简化）

dy = next_pt[0]-prev[0]; dx = next_pt[1]-prev[1]
steps = max(abs(dy), abs(dx))
collision = False
for s in range(1, steps):
py = prev[0] + int(round(dy*s/steps))
px = prev[1] + int(round(dx*s/steps))
if self.grid[py, px] >= self.obs_cost:
collision = True; break
if not collision:
continue # 删除中间点
smoothed.append(path[i])
smoothed.append(path[-1])
return smoothed
改进DWA局部规划器简化

def improved_dwa(state, goal, obstacles, vel_samples):
best_traj = None; best_cost = np.inf
for v, w in vel_samples:
traj = simulate_trajectory(state, v, w, steps=30)
if check_collision(traj, obstacles):
continue
评价函数

heading_cost = abs(angle_diff(traj[-1][2], goal_angle))
vel_cost = -v
obs_cost = min_obstacle_dist(traj, obstacles)
total = 0.8*heading_cost + 0.1*vel_cost + 0.5/obs_cost
if total < best_cost:
best_cost = total
best_traj = (v,w)
return best_traj

如有问题，可以直接沟通

👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇