参考网址:

(114条消息) DWA动态窗口法的原理及应用_gophae的博客-CSDN博客_动态窗口法

ROS 2D导航原理系列教程合集（WHEELTEC）_哔哩哔哩_bilibili

ShisatoYano/dynamic_window_approach (github.com)

(114条消息) SLAM学习：DWA算法原理和Python编程实现_YuriFan的博客-CSDN博客_dwa算法实现

流程

20200621101602368.png

代码流程

1
2
3
4
5
6
7
8
9
10
11
for 1:1000 % 迭代1000次
	dwa_control();
	motion();
	if 接近目标点:
		break

dwa_control():
	calc_dynamic_window(); -> calc_final_input();

calc_final_input():
	calc_trajectory()->to_goal_cost->speed_cost->obst_cost->u_traj(v,w)

参考代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// 初始化状态信息
DynamicWindow calc_dynamic_window(State *st)
{
    DynamicWindow dw = {0.0, 0.0, 0.0, 0.0};

    // from robot specification
    float vs[4] = {MIN_SPD_MS, MAX_SPD_MS, -MAX_YAWRATE_RS, MAX_YAWRATE_RS};

    // from motion model
    float vd[4] = {st->v_ms - MAX_ACCEL_MS2 * DT_S,
                   st->v_ms + MAX_ACCEL_MS2 * DT_S,
                   st->omega_rads - MAX_DYAWRATE_RS2 * DT_S,
                   st->omega_rads + MAX_DYAWRATE_RS2 * DT_S};

    dw.min_v = max(vs[0], vd[0]);
    dw.max_v = min(vs[1], vd[1]);
    dw.min_yr = max(vs[2], vd[2]);
    dw.max_yr = min(vs[3], vs[3]);

    return dw;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
// 生成一系列轨迹
for (float v = dw->min_v; v < dw->max_v; v+=V_RESO) {
    for (float y = dw->min_yr; y < dw->max_yr; y+=YAWRATE_RESO) {
        traj = calc_trajectory(&st_init, v, y);

        // calculate cost
        to_goal_cost = GOAL_COST_GAIN * calc_to_goal_cost(&traj, goal);
        speed_cost = SPD_COST_GAIN * (MAX_SPD_MS - traj.back().v_ms);
        obst_cost = OBST_COST_GAIN*calc_obstacle_cost(&traj, obst, obst_num);
        final_cost = to_goal_cost + speed_cost + obst_cost;

        // search minimum trajectory
        if (min_cost >= final_cost)
        {
            min_cost = final_cost;
            min_u.v_ms = v;
            min_u.omega_rads = y;
            best_traj = traj;
        }
    }
}
// 最后输出