dynamic window approach

发表于 |

动态窗口：

窗口框的是速度空间的采样点$(v_t, w_t)$，一对$(v_t, w_t)$就代表一段轨迹，轨迹通过机器人底盘的运动学建模得到。

1
2
3
4
5
6
7
8
9
10
// base_local_planner的simple_trajectory_generator.cpp
// 直线模型
double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(theta) - vy * sin(theta)) * dt;
double delta_y = (vx * sin(theta) + vy * cos(theta)) * dt;
double delta_th = vth * dt;

x += delta_x;
y += delta_y;
theta += delta_th;

窗口的选择：

  1. 速度限制

  2. 加速度限制

  3. 障碍物制动限制

    $dist(v,w)$表示采样点$(v, w)$对应轨迹上离障碍物最近的距离。

确定窗口后进行采样，可以得到一系列轨迹：

轨迹的选择：

原始论文中采用评价函数：

  1. 方位角评价函数：采用当前采样点设定下，达到模拟轨迹末端时机器人的朝向角与目标点朝向角的差距。
  2. 空隙评价：当前模拟轨迹上与最近障碍物之间的距离。
  3. 速度评价：采样点速度与最大速度的差距。

上述评价函数要进行归一化。

算法流程：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
BEGIN DWA(robotPose,robotGoal,robotModel)
    laserscan = readScanner()
    allowable_v = generateWindow(robotV, robotModel)
    allowable_w  = generateWindow(robotW, robotModel)
	for each v in allowable_v
        for each w in allowable_w
            dist = find_dist(v,w,laserscan,robotModel)
            breakDist = calculateBreakingDistance(v)
            if (dist > breakDist)  //can stop in time
                heading = hDiff(robotPose,goalPose, v,w) 
                clearance = (dist-breakDist)/(dmax - breakDist) 
                cost = costFunction(heading,clearance, abs(desired_v - v))
                if (cost > optimal)
                    best_v = v
                    best_w = w
                    optimal = cost
	set robot trajectory to best_v, best_w
END