✨ 长期致力于动态避障、DWA算法、JPS算法、融合算法、移动机器人研究工作，擅长数据搜集与处理、建模仿真、程序编写、仿真设计。
✅ 专业定制毕设、代码
✅如需沟通交流，点击《获取方式》
（1）全局引导与局部DWA融合的层次化路径规划框架：
移动机器人装备二维激光雷达测距范围12米，角度分辨率0.5度，航向由惯性测量单元提供。首先利用改进JPS算法生成全局引导路径，将栅格地图划分为0.2米×0.2米的单元，障碍物膨胀半径设为0.35米。JPS的跳点扩展规则修改为允许对角跳跃但代价系数乘以根号2，同时加入强制邻居剪枝策略，使搜索节点数比标准A星减少约42%。生成的全局路径由一系列关键拐点构成，拐点间距在1.2米至3.5米之间。将这些拐点依次注入DWA的局部规划器作为滚动目标，每到达一个拐点则更新下一个。动态窗口法的速度空间采样范围设为线速度0至0.8米每秒，角速度-1.2至1.2弧度每秒，采样时间间隔0.1秒，预测时长2.5秒。评价函数权重原为固定值，现改为自适应动态调整机制：当机器人与最近障碍物距离小于0.5米时，安全权重系数从0.2线性增加到0.8，方位角权重相应减小；当机器人接近目标点(距离小于1米)时，速度权重降低以提升停靠精度。为应对动态障碍物，在每个控制周期重新计算障碍物速度并估计其未来2秒内的运动轨迹，在评价函数中加入动态障碍物危险度代价，采用高斯核函数计算相对速度方向的威胁值。
import numpy as np from collections import deque def dynamic_window_control(robot_pose, goal, obstacles, vel_current, global_path): # 速度采样空间 vs = [0.0, 0.8, 0.2] # 线速度范围 ws = [-1.2, 1.2, 0.1] # 角速度范围 best_u, best_cost = None, np.inf for v in np.arange(vs[0], vs[1]+vs[2], vs[2]): for w in np.arange(ws[0], ws[1]+ws[2], ws[2]): # 模拟运动轨迹 traj = simulate_motion(robot_pose, v, w, dt=0.1, steps=25) # 自适应权重 min_obs_dist = min([np.linalg.norm(p - obs) for p in traj for obs in obstacles]) if min_obs_dist < 0.3: continue # 碰撞 alpha_safe = np.clip((0.5 - min_obs_dist)/0.5, 0.2, 0.8) if min_obs_dist<0.5 else 0.2 alpha_heading = 1.0 - alpha_safe - 0.2 heading_cost = angle_diff(robot_pose[2], np.arctan2(goal[1]-robot_pose[1], goal[0]-robot_pose[0])) vel_cost = 1.0 - v/0.8 total_cost = alpha_safe*(1/min_obs_dist) + alpha_heading*heading_cost + 0.2*vel_cost if total_cost < best_cost: best_cost = total_cost; best_u = (v,w) return best_u def simulate_motion(pose, v, w, dt, steps): x,y,theta = pose traj = [] for _ in range(steps): theta += w*dt x += v*np.cos(theta)*dt y += v*np.sin(theta)*dt traj.append(np.array([x,y])) return traj def angle_diff(a, b): diff = (a - b) % (2*np.pi) return min(diff, 2*np.pi-diff) # 使用 current_pose = (2.5, 1.8, 0.2) goal_point = (8.0, 5.0) obs_list = [(3.2, 1.9), (4.0, 2.1), (5.5, 3.0)] velocity = (0.3, 0.0) cmd = dynamic_window_control(current_pose, goal_point, obs_list, velocity, []) print('下发速度指令:', cmd)
