### DWA算法的Python实现 以下是基于DWA（Dynamic Window Approach）算法的一个简化版本，该代码可以直接在Python IDLE环境中运行。此实现主要关注机器人如何通过动态窗口法找到最优路径并避开障碍物。 #### 算法核心逻辑 1. **采样窗口**：定义机器人的速度范围和加速度约束。 2. **评价函数**：评估候选轨迹的安全性和目标接近程度。 3. **优化选择**：选取评分最高的轨迹作为当前控制指令。 下面是完整的Python代码： ```python import numpy as np import matplotlib.pyplot as plt def dwa_control(x, config, goal, ob): """ 实现DWA的核心控制逻辑。 参数: x (list): 当前状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] config (class): 配置参数类 goal (list): 目标位置 [x(m), y(m)] ob (np.ndarray): 障碍物列表 [[x1,y1], [x2,y2],...] 返回: u (list): 控制输入 [v(m/s), omega(rad/s)] trajectory (list of lists): 轨迹预测点 [(x0,y0), (x1,y1)...] """ dw = calc_dynamic_window(x, config) min_cost = float("inf") best_u = [0.0, 0.0] best_trajectory = [] for v in np.arange(dw[0], dw[1], config.v_reso): for w in np.arange(dw[2], dw[3], config.w_reso): trajectory = predict_trajectory(x, v, w, config) # 判断轨迹是否碰撞障碍物 if not check_collision(trajectory, ob, config): continue # 计算综合成本 to_goal_cost = calc_to_goal_cost(trajectory[-1][0:2], goal) * config.to_goal_cost_gain speed_cost = config.max_speed - trajectory[-1][3] * config.speed_cost_gain obstacle_cost = calc_obstacle_cost(trajectory, ob, config) * config.obstacle_cost_gain final_cost = to_goal_cost + speed_cost + obstacle_cost # 更新最佳动作 if min_cost >= final_cost: min_cost = final_cost best_u = [v, w] best_trajectory = trajectory return best_u, best_trajectory def motion(x, u, dt): """运动模型""" x[2] += u[1] * dt x[0] += u[0] * np.cos(x[2]) * dt x[1] += u[0] * np.sin(x[2]) * dt x[3] = u[0] x[4] = u[1] return x def calc_dynamic_window(x, config): """计算动态窗口""" vs = [config.min_speed, config.max_speed, -config.max_yaw_rate, config.max_yaw_rate] vd = [max(config.min_speed, x[3] - config.max_accel * config.dt), min(config.max_speed, x[3] + config.max_accel * config.dt), max(-config.max_yaw_rate, x[4] - config.max_delta_yaw_rate * config.dt), min(config.max_yaw_rate, x[4] + config.max_delta_yaw_rate * config.dt)] vw = [max(vs[0], vd[0]), min(vs[1], vd[1]), max(vs[2], vd[2]), min(vs[3], vd[3])] return vw def predict_trajectory(x_init, v, w, config): """预测轨迹""" x = np.array(x_init) trajectory = [] time = 0 while time <= config.predict_time: x = motion(x, [v, w], config.dt) trajectory.append([x[0], x[1], x[2], x[3]]) time += config.dt return trajectory def check_collision(trajectory, ob, config): """检测碰撞""" for i in range(len(trajectory)): px = trajectory[i][0] py = trajectory[i][1] for j in range(len(ob)): dx = px - ob[j][0] dy = py - ob[j][1] dist = np.sqrt(dx**2 + dy**2) if dist <= config.robot_radius: return False return True def calc_to_goal_cost(xy, goal): """计算到目标的距离代价""" cost = np.linalg.norm(np.array(goal) - np.array(xy)) return cost def calc_obstacle_cost(traj, ob, config): """计算障碍物距离代价""" min_r = float('inf') for i in range(len(traj)): for j in range(len(ob)): dx = traj[i][0] - ob[j][0] dy = traj[i][1] - ob[j][1] r = np.hypot(dx, dy) if r < min_r: min_r = r return 1 / min_r # 倒数形式表示越近代价越高 # 定义配置参数 class Config(): def __init__(self): self.max_speed = 1.0 # 最大线速度 [m/s] self.min_speed = -0.5 # 最小线速度 [m/s] self.max_yaw_rate = 40.0 * np.pi / 180.0 # 最大角速度 [rad/s] self.max_accel = 0.2 # 最大加速度 [m/ss] self.max_delta_yaw_rate = 40.0 * np.pi / 180.0 # 最大角加速度 [rad/ss] self.v_reso = 0.01 # 线速度分辨率 [m/s] self.w_reso = 0.1 * np.pi / 180.0 # 角速度分辨率 [rad/s] self.dt = 0.1 # 时间间隔 [s] self.predict_time = 3.0 # 预测时间长度 [s] self.to_goal_cost_gain = 0.15 # 到达目标的成本增益 self.speed_cost_gain = 1.0 # 速度成本增益 self.obstacle_cost_gain = 1.0 # 障碍物成本增益 self.robot_radius = 0.2 # 机器人半径 [m] if __name__ == "__main__": # 初始化设置 config = Config() obstacles = np.array([[1, 1], [2, 2], [3, 4]]) # 障碍物位置 start = [-2.0, -2.0, np.deg2rad(90.0), 0.0, 0.0] # 初始状态[x, y, theta, v, omega] goal = [5.0, 5.0] # 目标位置 path_x, path_y = [], [] state = start.copy() plt.figure(figsize=(8, 8)) for _ in range(500): u, predicted_traj = dwa_control(state, config, goal, obstacles) state = motion(state, u, config.dt) path_x.append(state[0]) path_y.append(state[1]) plt.cla() plt.plot(predicted_traj[:, 0], predicted_traj[:, 1], "-g") # 预测轨迹 plt.plot(path_x, path_y, "-r", label="Path") # 实际路径 plt.plot(start[0], start[1], "xr", label="Start Point") # 起始点 plt.plot(goal[0], goal[1], "*b", label="Goal Point") # 终点 plt.plot(obstacles[:, 0], obstacles[:, 1], "sk", label="Obstacles") # 障碍物 plt.axis("equal") plt.grid(True) plt.pause(0.001) if np.linalg.norm(np.array(state[:2]) - np.array(goal)) <= config.robot_radius: break plt.show() ``` 这段代码实现了DWA算法的主要部分，并能在简单的二维平面上模拟移动机器人避障过程[^1]^。它包含了动态窗口生成、轨迹预测以及碰撞检测等功能模块。