DWA算法python实现
时间: 2025-01-04 10:20:44 浏览: 20
### 动态窗口法(DWA)简介
动态窗口法(DWA)是一种高效的局部路径规划算法,特别适合于动态环境中移动机器人的导航需求。该方法通过计算一系列可能的速度组合来预测未来轨迹,并评估这些轨迹的安全性和目标接近程度,从而选择最优动作[^2]。
### DWA算法的核心要素
- **线速度范围** 和 **角速度范围** 的定义决定了机器人可以采取的动作集合。
- 对每一个可行的速度组合,模拟一段时间内的运动并检查是否会碰撞障碍物。
- 计算各条候选路径的成本函数值,通常包括距离终点的距离、前进方向偏差等因素。
- 选取成本最低的一组参数作为最终控制指令发送给执行器。
### Python实现示例代码
下面是一个简化版的DWA算法Python实现:
```python
import numpy as np
from math import pow, sqrt
def dwa_control(x, config, goal, ob):
"""
Dynamic Window Approach control.
:param x: Robot state vector [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)].
:param config: Configuration parameters of the robot and environment.
:param goal: Goal position [x(m), y(m)].
:param ob: Obstacle positions [[x1(m), y1(m)], ...].
"""
dw = calc_dynamic_window(x, config)
u, trajectory = calc_final_input(x, dw, config, goal, ob)
return u, trajectory
def motion(x, u, dt):
""" Motion Model """
# Update vehicle kinematics based on input velocity (u[0]) and angular velocity (u[1])
x[2] += u[1] * dt
x[0] += u[0] * math.cos(x[2]) * dt
x[1] += u[0] * math.sin(x[2]) * dt
x[3] = u[0]
x[4] = u[1]
return x
def predict_trajectory(x_init, v, o, config):
"""
Predicts a new trajectory given initial conditions and controls.
:param x_init: Initial condition [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
:param v: Velocity command (m/s).
:param o: Angular velocity command (rad/s).
:param config: Configuration object containing simulation settings.
"""
x_pred = np.array(x_init)
traj = np.array(x_init)
time = 0
while time <= config.predict_time:
x_pred = motion(x_pred, [v, o], config.dt)
traj = np.vstack((traj, x_pred))
time += config.dt
return traj
def calc_obstacle_cost(traj, ob, config):
"""
Calculates cost associated with proximity to obstacles along predicted path.
:param traj: Trajectory array consisting of multiple states over prediction horizon.
:param ob: List of obstacle coordinates.
:param config: Simulation configuration details.
"""
ox = ob[:, 0]
oy = ob[:, 1]
dx = traj[:, 0] - ox[:, None]
dy = traj[:, 1] - oy[:, None]
r = np.hypot(dx, dy)
if not any(r <= config.robot_radius):
return 0.0 # No collision detected
min_r = min(r)
return 1.0 / min_r # Higher penalty when closer to an obstacle
def calc_to_goal_cost(traj, goal, config):
"""
Computes cost related to distance from end point of trajectory towards target location.
:param traj: Array representing sequence of poses during planning interval.
:param goal: Desired destination coordinate pair.
:param config: System setup information including dimensions etc..
"""
dx = goal[0] - traj[-1, 0]
dy = goal[1] -1, 2]
error_angle = normalize_angle(error_angle)
dist_to_line = abs(-error_angle * config.robot_length / 2.0)
heading_diff = abs(normalize_angle(math.atan2(goal[1]-config.start_pose[1],
goal[0]-config.start_pose[0])-traj[-1][2]))
return dist_to_line + heading_diff
def normalize_angle(angle):
""" Normalize angle between [-pi, pi]."""
while angle > np.pi:
angle -= 2*np.pi
while angle < -np.pi:
angle += 2*np.pi
return angle
def calc_final_input(x, dw, config, goal, ob):
"""
Select best action according to evaluation criteria defined by costs functions.
:param x: Current pose estimate [x(m), y(m), theta(rad), linear vel.(m/s), ang.vel.(rad/s)].
:param dw: Discretized dynamic window bounds for admissible velocities.
:param config: Environment & agent specifications used throughout computations.
:param goal: Target waypoint specified in world frame reference system.
:param ob: Collection of static or moving objects within vicinity.
"""
max_speed = float('-inf')
best_u = [0.0, 0.0]
best_traj = []
for v in np.arange(dw[0], dw[1], config.v_reso):
for w in np.arange(dw[2], dw[3], config.yawrate_reso):
temp_traj = predict_trajectory(x, v, w, config)
# Calculate three types of penalties/costs
to_goal_cost = calc_to_goal_cost(temp_traj, goal, config)
speed_cost = config.speed_weight * v / config.max_speed
obs_cost = calc_obstacle_cost(temp_traj, ob, config)
final_cost = to_goal_cost + speed_cost + obs_cost
if final_cost >= max_speed:
max_speed = final_cost
best_u = [v, w]
best_traj = temp_traj
return best_u, best_traj
def calc_dynamic_window(x, config):
"""
Defines feasible region where potential solutions lie inside limits imposed both physically and logically.
:param x: Present status report about mobile platform's spatial attributes plus its instantaneous rates of change.
:param config: Data structure holding all necessary constants required across various stages of computation process.
"""
Vs = [config.min_speed, config.max_speed,
-config.max_yawrate, config.max_yawrate]
Vd = [(x[3] - config.max_accel * config.dt),
(x[3] + config.max_accel * config.dt),
(x[4] - config.max_dyawrate * config.dt),
(x[4] + config.max_dyawrate * config.dt)]
dw = [
max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])
]
return dw
```
此段程序实现了基本的DWA逻辑框架,可以根据具体应用场景调整配置项以及优化性能指标。注意这里省略了一些辅助性的子功能模块,在实际开发过程中还需要完善边界条件处理等内容。
阅读全文