局部路径规划算法之DWA

发表于 | 分类于

ROS中的DWA（dynamic window approach）局部路径规划算法python实现！
0

幸运的爱,与勇者长相随。

在上一篇文章中介绍了经典全局路径规划算法《全局路径规划算法之Dijkstra、A*》，结合局部路径规划算法可以使移动机器人可以在已知地图环境的情况下轻松地完成避障任务并且得到最优的路径。

算法简介

机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略，ROS中主要是使用了DWA算法（dynamic window approach）。在ROS中每当move_base处于规划状态就调用DWA算法计算出一条最佳的速度指令，发送给机器人运动底盘执行。

算法原理

DWA算法主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间（sim_period）内的运动轨迹，再通过一个评价函数对这些轨迹打分，选取得分最高的最优路径，并把相应的速度控制指令发送给机器人。该算法的突出点在于动态窗口这个概念，它的含义是依据移动机器人的加减速性能限定速度速度采样空间在一个可行的动态范围内。

算法流程

运动学建模

对机器人建立运动学模型，将线速度和角速度进行数学公式表示。
代码封装如下：

1
2
3
4
5
6
7
8
def motion(x, u, dt):
    # motion model
    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

速度采样

在速度空间（v,w）对速度进行采样，采样过程需要考虑到机器人本身和环境的限制，包括：

  1. 机器人速度限制：机器人的速度限定在一定范围内。
  2. 机器人加速度限制：受电机扭矩限制，机器人的加速度被限定在一定范围内。
  3. 机器人安全距离限制：考虑到机器人半径，需要考虑到安全距离。

代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
def __init__(self):
    # robot parameter
    self.max_speed = 1.0  # [m/s]
    self.min_speed = -0.5  # [m/s]
    self.max_yawrate = 40.0 * math.pi / 180.0  # [rad/s]
    self.max_accel = 0.2  # [m/ss]
    self.max_dyawrate = 40.0 * math.pi / 180.0  # [rad/ss]
    self.v_reso = 0.01  # [m/s]
    self.yawrate_reso = 0.1 * math.pi / 180.0  # [rad/s]
    self.dt = 0.1  # [s]
    self.predict_time = 3.0  # [s]
    self.to_goal_cost_gain = 1.0
    self.speed_cost_gain = 1.0
    self.robot_radius = 1.0  # [m]

1

评价函数

  1. 方位角评价函数heading(v,w)：模拟轨迹的末端点与目标点角度差。差值越小，评分越高。
  2. 空隙评价函数dist(v,w)：当前模拟轨迹上距障碍物最近距离，没有障碍物，设定常数。距离越远，评分越高。
  3. 速度评价函数velocity(v,w)：评价当前轨迹的速度值大小。速度越大，评分越高。
  4. 平滑处理（归一化）：使评价函数连续化，不至于使某一个评价函数过大影响最优轨迹的选取。

代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
def calc_obstacle_cost(traj, ob, config):
    # calc obstacle cost inf: collistion, 0:free
    skip_n = 2
    minr = float("inf")

    for ii in range(0, len(traj[:, 1]), skip_n):
        for i in range(len(ob[:, 0])):
            ox = ob[i, 0]
            oy = ob[i, 1]
            dx = traj[ii, 0] - ox
            dy = traj[ii, 1] - oy

            r = math.sqrt(dx**2 + dy**2)
            if r <= config.robot_radius:
                return float("Inf")  # collision

            if minr >= r:
                minr = r

    return 1.0 / minr  # OK

def calc_to_goal_cost(traj, goal, config):
    # calc to goal cost. It is 2D norm.
    goal_magnitude = math.sqrt(goal[0]**2 + goal[1]**2)
    traj_magnitude = math.sqrt(traj[-1, 0]**2 + traj[-1, 1]**2)
    dot_product = (goal[0] * traj[-1, 0]) + (goal[1] * traj[-1, 1])
    error = dot_product / (goal_magnitude * traj_magnitude)
    error_angle = math.acos(error)
    cost = config.to_goal_cost_gain * error_angle

    return cost

2

轨迹选取

初始化原始轨迹及成本值，根据计算得到的轨迹评分，得到最终的总的成本值final_cost，将轨迹的成本值同min_cost相比较，若得到的成本值小于最小成本值，则更新min_cost以及速度采样值，直到选取出采样速度中的成本值最低的轨迹即最优轨迹。代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
def calc_final_input(x, u, dw, config, goal, ob):

    xinit = x[:]
    min_cost = 10000.0
    min_u = u
    min_u[0] = 0.0
    best_traj = np.array([x])

    # evalucate all trajectory with sampled input in dynamic window
    for v in np.arange(dw[0], dw[1], config.v_reso):
        for y in np.arange(dw[2], dw[3], config.yawrate_reso):
            traj = calc_trajectory(xinit, v, y, config)

            # calc cost
            to_goal_cost = calc_to_goal_cost(traj, goal, config)
            speed_cost = config.speed_cost_gain * \
                         (config.max_speed - traj[-1, 3])
            ob_cost = calc_obstacle_cost(traj, ob, config)
            final_cost = to_goal_cost + speed_cost + ob_cost

            # search minimum trajectory
            if min_cost >= final_cost:
                min_cost = final_cost
                min_u = [v, y]
                best_traj = traj

    return min_u, best_traj

3

main()函数

在main()函数中我们设定初始位置和目标位置，并建立障碍物环境，这里我们设定动态窗口检测次数为1000，在刷新到400次的时候，我们在原有的轨迹上动态添加障碍物来测试DWA算法的鲁棒性。当轨迹点到目标点的距离小于设定的安全距离时，则认为到达目标点。将机器人实时选取的最优轨迹打印到屏幕上。代码封装如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
def main(gx=10, gy=10.5):
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
    # goal position [x(m), y(m)]
    goal = np.array([gx, gy])
    # obstacles [x(m) y(m), ....]
    ob = np.array([[-1, -1],
                   [0, 2],
                   [4.0, 2.0],
                   [5.0, 4.0],
                   [5.0, 5.0],
                   [5.0, 6.0],
                   [3.0, 9.0],
                   [4.0, 9.0],
                   [5.0, 9.0],
                   [6.0, 9.0],
                   [7.0, 9.0],
                   [8.0, 9.0],
                   [12.0, 12.0]
                   ])

    u = np.array([0.0, 0.0])
    config = Config()
    traj = np.array(x)

    for i in range(1000):
        if i == 140:
           ob[9] = [9.0, 9.0]
           ob[10] = [10.0, 9.0]
           ob[11] = [11.0, 9.0]

        u, ltraj = dwa_control(x, u, config, goal, ob)
        x = motion(x, u, config.dt)
        traj = np.vstack((traj, x)) 

        # print(traj)
        if show_animation:
            plt.cla()
            plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
            plt.plot(x[0], x[1], "xr")
            plt.plot(goal[0], goal[1], "xb")
            plt.plot(ob[:, 0], ob[:, 1], "ok")
            plot_arrow(x[0], x[1], x[2])
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.001)

        # check goal
        if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
            print("Goal!!")
            break

    print("Done")
    if show_animation:
        plt.plot(traj[:, 0], traj[:, 1], "-r")
        plt.pause(0.001)

    plt.show()

4

经验分享

  1. 可通过修改障碍物ob[]坐标来添加动态障碍物，来测试算法的实时避障性能。
  2. 在测试过程中发现对于个别动态障碍物或是死角障碍的情况，该算法容易陷入局部最优的情况。
  3. 该算法主要用于差分驱动的机器人模型，可以添加障碍物清理模块或是逃离模块来解决陷入局部最优的情况。
-------------本文结束 感谢阅读-------------
0%