本文共 3155 字，大约阅读时间需要 10 分钟。

动态窗口法(Dynamic Window Approach)概述

DWA是一种基于速度的局部规划器，可计算达到目标所需的机器人的最佳无碰撞速度。

程序实现

DWA算法主要分三步：

• 计算动态窗口
• 计算最优$[v,\omega ]$
• 更新机器人状态

流程图如下：

以下代码参考：

初始化机器人状态、目标位置、障碍物位置

# 初始化机器人状态 [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])# 目标位置 [x(m), y(m)]goal = np.array([gx, gy])# 障碍物位置 [x(m), y(m)]ob = np.array([[-1, -1], ...... , [13.0, 13.0]])

获取动态窗口

这个动态窗口就是机器人在当前状态下能达到的速度$v$和转速$\omega$范围，受到自身机械特性以及当前状态的影响。

def calc_dynamic_window(x, config):    """    calculation dynamic window based on current state x    """    # Dynamic window from robot specification    Vs = [config.min_speed, config.max_speed,          -config.max_yawrate, config.max_yawrate]    # Dynamic window from motion model    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]    #  [vmin, vmax, 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

计算最优$[v,\omega ]$

对动态窗口采样，得到$N\times M$组$[v_{ij},{\omega }_{ij}]|i<N,j<M$，并计算$v=v_{ij},\omega ={\omega }_{ij}$时机器人的预测轨迹。接下来计算机器人$v=v_{ij},\omega ={\omega }_{ij}$时的评价函数：

• $heading(v,\omega )$表示机器人运动方向与目标方向的偏差$\theta$(如下图)。

• $dist(v,\omega )$表示与预测轨迹相交并且距离当前机器人位置最近的障碍物的距离，当没有障碍物处于预测轨迹上时，这个函数取一个很大的常数。
• $velocity(v,\omega )$表示机器人处于预测轨迹最后一点时的速度$v$

计算机器人处于$v=v_{ij},\omega ={\omega }_{ij}$时的评价函数，得到一系列的代价，代价最小时的$[v_{ij},{\omega }_{ij}]$即为最优$[v,\omega ]$

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])    # 计算动态窗口内所有的采样样本的代价函数    for v in np.arange(dw[0], dw[1], config.v_reso):        for y in np.arange(dw[2], dw[3], config.yawrate_reso):            trajectory = predict_trajectory(x_init, v, y, config)            # 计算代价函数            to_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            # 寻找具有最小代价的样本以及它的轨迹            if min_cost >= final_cost:                min_cost = final_cost                best_u = [v, y]                best_trajectory = trajectory    return best_u, best_trajectory

更新状态

根据最优$u=[v,\omega ]$更新机器人状态

x = motion(x, u, config.dt)

完整代码参见

转载地址：http://olvgn.baihongyu.com/