DWA（dynamic window approach）算法学习

本文主要是介绍DWA（dynamic window approach）算法学习，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

系列文章目录

A*算法学习-CSDN博客

弗洛伊德算法（Floyd）和路径平滑弗洛伊德算法（Smooth Floyd）学习-CSDN博客

D*算法学习-CSDN博客

系列文章目录

前言

搜索空间 —减小速度搜索空间

优化过程 —最大化目标函数

算法实现

总结

前言

在机器人的路径规划中少不了DWA算法，学习！！！

DWA动态窗口法的原理及应用:The Dynamic Window Approach to Collision Avoidance - 知乎 (zhihu.com)

自动驾驶决策规划算法—DWA 动态窗口法 - 知乎 (zhihu.com)

动态窗口法（Dynamic Window Approach， DWA）是一种避障规划方法，DWA算法通过对速度空间施加约束以确保动力学模型和避障的要求，在速度空间中搜索机器人最优控制速度，最终实现快速安全地到达目的地。

DWA算法的实现主要由两部分组成： 1.减小速度空间2.定义目标函数，并最大化目标函数

搜索空间 —减小速度搜索空间

通过施加弧线轨迹、允许速度、滑动窗口约束以减小搜索空间。

弧线轨迹 Circular trajectories
在原论文中，作者对简化了机器人的运动学模型，并得出了机器人运动轨迹可由一系列弧线和直线组成。所以，作者将速度空间约束在由机器人的平移速度和旋转速度(v,w)组成的二维速度搜索空间。
允许速度 Admissible velocities
允许速度（Admissible velocities）确保机器人可以在障碍前停下，最大的允许速度取决于当前轨迹距最近障碍的距离dist(v,w)。允许速度集被定义为：

下图中展示了Vs,Va速度空间：
滑动窗口 Dynamic window
考虑到机器人存在加速度限制，搜索空间被限定动态窗口Vd中（在下一个规划间隔可达到的速度），具体如下：

其中，v˙,w˙表示机器人的加速度。
最终的搜索空间：Vr=Vs∩Va∩Vd,如下图所示：

优化过程 —最大化目标函数

目标函数考虑了方位角、安全距离和速度：

方位角 Target heading
方位角项heading(v,w)可确保机器人在运动过程中快速对准目标点。
安全距离 Clearance
dist(v,w)确保机器人不发生任何碰撞
速度 Velocity
速度项Velocity(v,w)可确保机器人尽快到达目标点

目标函数被定义为：

其中， $\alpha$ , $\beta$ , $\gamma$ 可以根据需求调整。这三个指标是目标函数的重要组成部分，缺一不可。仅使clearance和velocity最大化，机器人始终在无障碍空间运动，但不会有向目标位置移动的趋势。单独最大化heading，机器人很快就会被阻碍其前进的第一个障碍所阻挡，无法在其周围移动。通过组合三个指标，机器人在上述限制条件下能够快速地绕过碰撞，同时朝着目标方向运动。

算法实现

具体的DWA算法参考：https://github.com/AtsushiSakai/PythonRoboticsGifsplanning

函数讲解

动态窗口
创建动态窗口

def calc_dynamic_window(x, config):"""calculation dynamic window based on current state x"""# Dynamic window from robot specificationVs = [config.min_speed, config.max_speed,-config.max_yaw_rate, config.max_yaw_rate]# Dynamic window from motion modelVd = [x[3] - config.max_accel * config.dt,x[3] + config.max_accel * config.dt,x[4] - config.max_delta_yaw_rate * config.dt,x[4] + config.max_delta_yaw_rate * config.dt]#  [v_min, v_max, yaw_rate_min, yaw_rate_max]dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]return dw

计算动态窗口内最优速度和最优轨迹

在动态窗口dw中采样并计算可能的轨迹，并求解各条轨迹的目标函数
选取令目标函数最小化（最小化指标，即对每个指标取倒数）的轨迹作为最优轨迹

def calc_control_and_trajectory(x, dw, config, goal, ob):"""calculation final input with dynamic window"""x_init = x[:]min_cost = float("inf")best_u = [0.0, 0.0]best_trajectory = np.array([x])# evaluate all trajectory with sampled input in dynamic windowfor v in np.arange(dw[0], dw[1], config.v_resolution):# v_resolution: speed intervalfor y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):# yaw_rate_resolution: yaw_rate intervaltrajectory = predict_trajectory(x_init, v, y, config)# calc costto_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(trajectory, goal)speed_cost = config.speed_cost_gain * (config.max_speed - trajectory[-1, 3])ob_cost = config.obstacle_cost_gain * calc_obstacle_cost(trajectory, ob, config)final_cost = to_goal_cost + speed_cost + ob_cost# search minimum trajectoryif min_cost >= final_cost:min_cost = final_costbest_u = [v, y]best_trajectory = trajectoryif abs(best_u[0]) < config.robot_stuck_flag_cons \and abs(x[3]) < config.robot_stuck_flag_cons:# to ensure the robot do not get stuck in# best v=0 m/s (in front of an obstacle) and# best omega=0 rad/s (heading to the goal with# angle difference of 0)best_u[1] = -config.max_delta_yaw_ratereturn best_u, best_trajectory

目标函数

方位角目标函数

def calc_to_goal_cost(trajectory, goal):"""calc to goal cost with angle difference"""dx = goal[0] - trajectory[-1, 0]dy = goal[1] - trajectory[-1, 1]error_angle = math.atan2(dy, dx)cost_angle = error_angle - trajectory[-1, 2]cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))return cost

障碍目标函数
计算机器人与最近障碍的距离

def calc_obstacle_cost(trajectory, ob, config):"""calc obstacle cost inf: collision"""ox = ob[:, 0]oy = ob[:, 1]dx = trajectory[:, 0] - ox[:, None]dy = trajectory[:, 1] - oy[:, None]# r = sqrt(dx^2 + dy^2)r = np.hypot(dx, dy)if config.robot_type == RobotType.rectangle:yaw = trajectory[:, 2]rot = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]])rot = np.transpose(rot, [2, 0, 1])local_ob = ob[:, None] - trajectory[:, 0:2]local_ob = local_ob.reshape(-1, local_ob.shape[-1])local_ob = np.array([local_ob @ x for x in rot])local_ob = local_ob.reshape(-1, local_ob.shape[-1])upper_check = local_ob[:, 0] <= config.robot_length / 2right_check = local_ob[:, 1] <= config.robot_width / 2bottom_check = local_ob[:, 0] >= -config.robot_length / 2left_check = local_ob[:, 1] >= -config.robot_width / 2if (np.logical_and(np.logical_and(upper_check, right_check),np.logical_and(bottom_check, left_check))).any():return float("Inf")elif config.robot_type == RobotType.circle:if np.array(r <= config.robot_radius).any():return float("Inf")min_r = np.min(r)return 1.0 / min_r  # OK

最终机器人规划避障轨迹：