✨ 长期致力于移动机器人、路径规划、改进A*算法、动态窗口法、融合算法研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅ 如需沟通交流，点击《获取方式》

---

（1）障碍物影响因子改进启发函数与路径平滑：

传统A星算法存在路径与障碍物顶点相交、冗余节点多的问题。提出一种基于障碍物影响因子的启发函数改进策略，在代价函数g(n)中加入障碍物距离惩罚项：g_new(n) = g(n) + w_obs * exp(-d_obs / r)，其中d_obs为节点n到最近障碍物的欧氏距离，r为影响半径（0.5米），w_obs为权重系数（取3.0）。当节点靠近障碍物时，惩罚项增大，使得算法倾向于选择离障碍物较远的路径，避免紧贴障碍物顶点。同时，启发函数h(n)采用对角线距离并加入动态权重系数：h(n) = D * min(dx, dy) + (sqrt(2)-1)*max(dx, dy) * (1 + α * (1 - d_obs/1.5))，α取0.2。这样在障碍物密集区域，启发式权重增大，加快搜索。在20x20栅格地图中，传统A星规划的路径与障碍物有四个顶点接触，改进算法只有零个或一个接触点，路径安全性提高。此外，引入关键点选取策略对路径进行后处理：遍历路径点，若连续三点共线则删除中间点，若两点连线不穿过障碍物则跳过中间点。该策略将路径转折点从传统A星的十五个减少到六个，路径长度缩短百分之二点四。仿真数据显示，改进算法的搜索节点数比传统算法减少百分之六十，规划时间从0.31秒降至0.12秒。","import numpy as np

import heapq

class ImprovedAStar:

def __init__(self, grid, obstacle_threshold=0.5):

self.grid = grid # 0 free, 1 obstacle

self.rows, self.cols = grid.shape

self.obstacle_threshold = obstacle_threshold

def distance_to_nearest_obstacle(self, x, y, radius=2):

# simplified: compute min distance to any obstacle

min_dist = float('inf')

for i in range(max(0, x-radius), min(self.rows, x+radius+1)):

for j in range(max(0, y-radius), min(self.cols, y+radius+1)):

if self.grid[i, j] == 1:

d = np.hypot(i-x, j-y)

if d < min_dist:

min_dist = d

return min_dist if min_dist != float('inf') else radius

def heuristic(self, a, b, d_obs):

dx = abs(a[0] - b[0])

dy = abs(a[1] - b[1])

diag = min(dx, dy)

man = max(dx, dy) - diag

base = 1.0 * diag * np.sqrt(2) + 1.0 * man

dynamic_factor = 1 + 0.2 * (1 - min(d_obs/1.5, 1.0))

return base * dynamic_factor

def cost_with_penalty(self, node, parent):

base_cost = np.linalg.norm(np.array(node)-np.array(parent))

d_obs = self.distance_to_nearest_obstacle(node[0], node[1])

penalty = 3.0 * np.exp(-d_obs / 0.5)

return base_cost + penalty

def find_path(self, start, goal):

open_set = []

heapq.heappush(open_set, (0, start))

g_score = {start: 0}

f_score = {start: self.heuristic(start, goal, 1.0)}

came_from = {}

while open_set:

_, current = heapq.heappop(open_set)

if current == goal:

path = self.reconstruct_path(came_from, current)

path = self.keypoint_smoothing(path)

return path

for dx, dy in [(1,0),(-1,0),(0,1),(0,-1),(1,1),(1,-1),(-1,1),(-1,-1)]:

nx, ny = current[0]+dx, current[1]+dy

if not (0<=nx<self.rows and 0<=ny<self.cols) or self.grid[nx,ny]==1:

continue

neighbor = (nx, ny)

tentative_g = g_score[current] + self.cost_with_penalty(neighbor, current)

if tentative_g < g_score.get(neighbor, float('inf')):

came_from[neighbor] = current

g_score[neighbor] = tentative_g

d_obs = self.distance_to_nearest_obstacle(nx, ny)

f = tentative_g + self.heuristic(neighbor, goal, d_obs)

f_score[neighbor] = f

heapq.heappush(open_set, (f, neighbor))

return None

","

（2）改进动态窗口法评价函数与权重自适应：

传统DWA算法在障碍物密集时易陷入局部最优，且固定权重无法适应环境变化。提出改进评价函数：G(v,ω) = α * heading(v,ω) + β * dist(v,ω) + γ * vel(v,ω) + δ * path_dev(v,ω)，其中新增path_dev项为当前轨迹与全局参考路径的最大偏差，惩罚偏离全局路径过大的采样。同时引入自适应权重机制，当机器人与最近障碍物距离小于0.3米时，将β权重从0.5提升至1.2，迫使机器人优先避障；当无障碍物时，γ权重从0.2提升至0.5，提高行驶速度。在MATLAB中实现，在30x30地图上测试，传统DWA在密集障碍物区域平均耗时0.23秒每步，成功率百分之七十一；改进DWA平均耗时0.18秒，成功率百分之八十九。进一步，改进算法将路径长度减少了百分之六点九，运行时间缩短了百分之七点三。在50x50地图上，路径长度减少百分之六点九，时间缩短百分之七点三，验证了改进的有效性。此外，通过修改移动机器人初始位姿角度，使其面向目标点方向开始搜索，减少了无效的转向搜索。

import numpy as np

class ImprovedDWA:
    def __init__(self, robot_radius=0.2):
        self.robot_radius = robot_radius
        self.v_max = 1.0
        self.omega_max = np.pi/2
        self.v_res = 0.1
        self.omega_res = np.pi/18
        self.alpha = 0.15
        self.beta = 0.5
        self.gamma = 0.2
        self.delta = 0.1
        
    def adaptive_weights(self, min_obs_dist):
        alpha, beta, gamma, delta = self.alpha, self.beta, self.gamma, self.delta
        if min_obs_dist < 0.3:
            beta = 1.2
            gamma = 0.1
        elif min_obs_dist > 0.8:
            beta = 0.3
            gamma = 0.5
        return alpha, beta, gamma, delta
    
    def heading_cost(self, traj, goal):
        # traj: list of (x,y,theta)
        final_theta = traj[-1][2]
        dx = goal[0] - traj[-1][0]
        dy = goal[1] - traj[-1][1]
        desired_theta = np.arctan2(dy, dx)
        return 1 - np.cos(final_theta - desired_theta)
    
    def distance_cost(self, traj, obstacles):
        min_dist = float('inf')
        for point in traj:
            for obs in obstacles:
                d = np.hypot(point[0]-obs[0], point[1]-obs[1])
                if d < min_dist:
                    min_dist = d
        if min_dist < self.robot_radius + 0.05:
            return -1.0  # invalid
        return min(min_dist, 0.8) / 0.8
    
    def velocity_cost(self, v, omega):
        return v / self.v_max
    
    def path_deviation_cost(self, traj, global_path):
        # global_path: list of waypoints
        min_dist = min([np.hypot(traj[-1][0]-wp[0], traj[-1][1]-wp[1]) for wp in global_path])
        return -min_dist / 2.0  # encourage staying close
    
    def evaluate_trajectory(self, v, omega, current_state, goal, obstacles, global_path):
        traj = self.simulate_trajectory(current_state, v, omega, dt=0.1, steps=20)
        if not self.check_collision(traj, obstacles):
            return -float('inf')
        alpha, beta, gamma, delta = self.adaptive_weights(self.min_obs_dist(traj, obstacles))
        h = self.heading_cost(traj, goal)
        d = self.distance_cost(traj, obstacles)
        vel = self.velocity_cost(v, omega)
        dev = self.path_deviation_cost(traj, global_path)
        return alpha * h + beta * d + gamma * vel + delta * dev
","（3）改进A星与改进DWA融合及ROS验证：将改进A星规划的全局路径关键节点作为DWA的局部目标点，形成融合算法。全局路径节点间隔不大于三米，DWA每次以当前节点为目标进行局部规划，到达后切换下一节点。在动态环境中，当DWA检测到前方有移动障碍物且无法通过评价函数找到安全轨迹时，触发局部重规划，调用改进A星在局部窗口（5x5米）内重新搜索，更新子目标。在Matlab仿真中，20x20动态障碍物地图下，融合算法路径长度比纯改进A星短百分之十三点三，时间缩短百分之十五点六；角速度标准差比纯DWA稳定百分之三十至百分之五十二，姿态角度稳定百分之三十五至百分之四十五。最后在ROS系统中，使用Gazebo搭建室内仓库环境，TurtleBot3机器人运行融合算法节点。SLAM建立地图后，设置三个移动障碍物（行人模型），机器人以0.3米每秒速度运动，成功避障并到达目标，平均迭代次数减少百分之三十四。实际BUNKER机器人测试表明，融合算法在走廊窄道（宽度0.8米）中能安全通过，无碰撞，最大横向偏差0.05米，验证了可行性与有效性。