DWA(Dynamic Window Approach) 기법 정리

DWA(Dynamic Window Approach란?

DWA는 로봇의 동적 제약(속도, 가속도, 회전 속도 등)을 고려하여, 싥시간으로 안전하고 최적 제어 입력을 선택하는 로컬 플래닝 기법

• 동적 제약 고려 : 로봇의 현재 속도와 가속도 한계를 바탕으로, 다음 시간 간격 내에 도달 가능한 제어 입력의 범위를 동적 창(Dynamic Window)으로 설정
• 실시간 경로 예측 : 각 제어 입력에 대해 일정 시간 동안의 경로를 시뮬레이션하여, 장애물 회피, 목표 도달 여부, 속도 유지 등의 요소 평가
• 비용 기반 선택 : 각 경로에 대해 비용 함수를 적용하여 최적의 경로 및 제어 입력을 선택

DWA 구현 예시 정리

모바일 로봇의 모션 플래닝을 위한 Dynamic Window approach (DWA)를 구현한 예제

• https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
  현재 상태에서 가속도, 감속도, 회전 속도 등의 동적 제약 조건을 고려하여 일정 시간 동안 가능한 제어 입력 (속도, 회전속도)들을 샘플링하고,
  각 입력에 대해 예측 경로 (trajectory)를 생성한 후, 목적지 도달 비용, 속도 비용, 장애물 회피 비용 등을 평가하여 가장 비용이 낮은 제어 입력을 선택

robot (go2)와 elevation mapping (perception)과 연동 목표

import math
from enum import Enum

import matplotlib.pyplot as plt
import numpy as np

show_animation = True

# 현재 상태 x에서 장애물 정보 ob와 목표 goal을 고려하여, DWA를 통해 최적의 제어 입력과 예측 경로를 계산
def dwa_control(x, config, goal, ob):
    # 현재 상태와 로봇의 제원에서 가능한 속도 및 회전 속도의 범위를 계산
    dw = calc_dynamic_window(x, config)
    # 계산된 동적 창 내에서 샘플링한 입력값들에 대해 경로를 예측하고, 각 경로의 비용을 계산한 후 최적의 입력 u와 해당 예측 경로 trajectory를 반환
    u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
    return u, trajectory

# 로봇의 형태를 원형 또는 직사각형으로 구분하기 위한 열거형(enum)
class RobotType(Enum):
    # 충돌 시 로봇을 원으로 간주
    circle = 0
    # 직사각형 형태로 간주하며, 더 복잡한 충돌 체크 로직을 사용
    rectangle = 1

# 로봇의 파라미터를 설정
class Config:
    def __init__(self):
        # robot parameter
        self.max_speed = 1.0  # [m/s]
        self.min_speed = -0.5  # [m/s]
        self.max_yaw_rate = 40.0 * math.pi / 180.0  # [rad/s]
        self.max_accel = 0.2  # [m/ss]
        self.max_delta_yaw_rate = 40.0 * math.pi / 180.0  # [rad/ss]
        self.v_resolution = 0.01  # [m/s]
        self.yaw_rate_resolution = 0.1 * math.pi / 180.0  # [rad/s]
        self.dt = 0.1  # [s] Time tick for motion prediction 시간 간격
        self.predict_time = 3.0  # [s] 예측 시간
        # 비용 gain : 목표 도달 비용, 속도 비용, 장애물 비용 등의 가중치
        self.to_goal_cost_gain = 0.15
        self.speed_cost_gain = 1.0
        self.obstacle_cost_gain = 1.0
        self.robot_stuck_flag_cons = 0.001  # constant to prevent robot stucked
        self.robot_type = RobotType.circle

        # if robot_type == RobotType.circle
        # Also used to check if goal is reached in both types
        self.robot_radius = 1.0  # [m] for collision check

        # if robot_type == RobotType.rectangle
        self.robot_width = 0.5  # [m] for collision check
        self.robot_length = 1.2  # [m] for collision check

        # obstacles [x(m) y(m), ....]
        self.ob = np.array([[-1, -1],
                            [0, 2],
                            [4.0, 2.0],
                            [5.0, 4.0],
                            [5.0, 5.0],
                            [5.0, 6.0],
                            [5.0, 9.0],
                            [8.0, 9.0],
                            [7.0, 9.0],
                            [8.0, 10.0],
                            [9.0, 11.0],
                            [12.0, 13.0],
                            [12.0, 12.0],
                            [15.0, 15.0],
                            [13.0, 13.0]
                            ])

    @property
    def robot_type(self):
        return self._robot_type

    @robot_type.setter
    def robot_type(self, value):
        if not isinstance(value, RobotType):
            raise TypeError("robot_type must be an instance of RobotType")
        self._robot_type = value


config = Config()

# 주어진 제어 입력 u = [v, omega] (전진 속도와 회전 속도)를 사용하여, 시간 dt 동안 로봇의 상태 x(위치, 방향, 속도 등)를 업데이트
def motion(x, u, dt):
    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

# 로봇의 제원과 현재 속도를 기반으로 동적 창 (Dynamic Window)을 게산
def calc_dynamic_window(x, config):
    # 제원 기반 동적 창 (로봇이 가질 수 있는 이론적인 속도 범위)
    Vs = [config.min_speed, config.max_speed,
          -config.max_yaw_rate, config.max_yaw_rate]

    # 모션 모델 기반 동적 창 (현재 속도에서 가속/감속 가능한 범위를 계산)
    Vd = [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]

    #  실제로 적용가능한 범위는, 두 창의 교집합
    dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
          max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]

    return dw

# 초기 상태 x_init에서 주어진 (v,y)에 따라 예측 시간을 두고 시뮬레이션을 수행, 해당 제어 입력에 대한 경로 trajectory를 생성
def predict_trajectory(x_init, v, y, config):
    # 초기 상태를 복사하고, trajectory 배열에 저장
    x = np.array(x_init)
    trajectory = np.array(x)
    # 예측 시간 동안 반복하면서 motion() 함수를 호출해 상태를 업데이트하고, 그 결과를 경로에 추가
    time = 0
    while time <= config.predict_time:
        x = motion(x, [v, y], config.dt)
        trajectory = np.vstack((trajectory, x))
        time += config.dt

    return trajectory

# 동적 창 dw 내에서 가능한 모든 제어 입력을 샘플링하여, 각 제어 입력에 따른 예측 경로를 계산한 후, 비용을 평가하여 최적의 제어 입력 선택
def calc_control_and_trajectory(x, dw, config, goal, ob):
    x_init = x[:]
    min_cost = float("inf") # 무한대로 초기화
    # 최적 제어 입력과 최적 경로 초기화
    best_u = [0.0, 0.0] 
    best_trajectory = np.array([x])

    #  선형 속도 v와 회적 속도 y를 각각 동적 창의 범위 내에서 일정 해상도 v_resolution, yaw_rate_resolution으로 샘플링
    for v in np.arange(dw[0], dw[1], config.v_resolution):
        for y in np.arange(dw[2], dw[3], config.yaw_rate_resolution):

            # 각 v,y 쌍에 대해 predict_trajectory를 호출하여 경로를 생성
            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) # 장애물 비용

            # 3개 비용의 합
            final_cost = to_goal_cost + speed_cost + ob_cost

            # 최적 입력 선택
            # 계산된 비용이 현재 최소 비용보다 낮으면 해당 입력과 경로를 최적값으로 갱신
            # 로봇이 정체(stuck)되지 않도록 작은 속도 일 때 회전 속도를 강제로 조정하는 조건 포함
            if min_cost >= final_cost:
                min_cost = final_cost
                best_u = [v, y]
                best_trajectory = trajectory
                if 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_rate

    # 최적 제어 입력과 예측 경로 반환
    return best_u, best_trajectory

# 예측 경로가 장애물과 얼마나 가까워 충돌 위험이 있는지를 비용으로 평가
# 1. 장애물 좌표 추출 : 장애물 배열 ob에서 각 장애물의 x,y 좌표를 분리
# 2. 거리 계산 : 경로 상의 각 위치와 장애물 사리 거리 계산
# 3. 충돌 체크 : 로봇 형태에 따라 충돌 여부 검사 / 충돌 감지되면 무한대 비용 반환
# 4. 비용 산출 : 가장 가까운 장애물과의 거리에 대해 역수 비용으로 사용
def calc_obstacle_cost(trajectory, ob, config):
    ox = ob[:, 0]
    oy = ob[:, 1]
    dx = trajectory[:, 0] - ox[:, None]
    dy = trajectory[:, 1] - oy[:, None]
    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 / 2
        right_check = local_ob[:, 1] <= config.robot_width / 2
        bottom_check = local_ob[:, 0] >= -config.robot_length / 2
        left_check = local_ob[:, 1] >= -config.robot_width / 2
        if (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

# 경로의 마지막 사태와 목표 지점 간의 방향 오차를 계산하여, 그 오차가 클수록 높은 비용을 부여
def calc_to_goal_cost(trajectory, goal):
    dx = goal[0] - trajectory[-1, 0]
    dy = goal[1] - trajectory[-1, 1]
    # 목표와 마지막 위치 사이의 각도를 atan2를 사용해 계산
    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 plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover
    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
              head_length=width, head_width=width)
    plt.plot(x, y)

# 시각화 함수 : 로봇의형태에 따라 현재 위치를 시각화
def plot_robot(x, y, yaw, config):  # pragma: no cover
    if config.robot_type == RobotType.rectangle:
        outline = np.array([[-config.robot_length / 2, config.robot_length / 2,
                             (config.robot_length / 2), -config.robot_length / 2,
                             -config.robot_length / 2],
                            [config.robot_width / 2, config.robot_width / 2,
                             - config.robot_width / 2, -config.robot_width / 2,
                             config.robot_width / 2]])
        Rot1 = np.array([[math.cos(yaw), math.sin(yaw)],
                         [-math.sin(yaw), math.cos(yaw)]])
        outline = (outline.T.dot(Rot1)).T
        outline[0, :] += x
        outline[1, :] += y
        plt.plot(np.array(outline[0, :]).flatten(),
                 np.array(outline[1, :]).flatten(), "-k")
    elif config.robot_type == RobotType.circle:
        circle = plt.Circle((x, y), config.robot_radius, color="b")
        plt.gcf().gca().add_artist(circle)
        out_x, out_y = (np.array([x, y]) +
                        np.array([np.cos(yaw), np.sin(yaw)]) * config.robot_radius)
        plt.plot([x, out_x], [y, out_y], "-k")

# 전체 시뮬레이션을 초기화하고 실행하는 main 함수
def main(gx=10.0, gy=10.0, robot_type=RobotType.circle):
    print(__file__ + " start!!")
    # initial state [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])
    # goal position [x(m), y(m)]
    goal = np.array([gx, gy])

    # input [forward speed, yaw_rate]

    # 로봇의 형태를 설정하고, 장애물 정보를 가져옴
    config.robot_type = robot_type
    trajectory = np.array(x)
    ob = config.ob

    # 시뮬레이션 루프
    # 반복문 내에서, dwa_control를 호출하여 최적 제어 입력과 에측 경로를 계산
    # motion 함수를 사용해 로봇의 상태를 업데이트하고, 경로를 누적
    # 각 반복마다 시각화를 업데이트하여 로봇, 경로, 장애물, 목표를 표시
    while True:
        u, predicted_trajectory = dwa_control(x, config, goal, ob)
        x = motion(x, u, config.dt)  # simulate robot
        trajectory = np.vstack((trajectory, x))  # store state history

        if show_animation:
            plt.cla()
            # for stopping simulation with the esc key.
            plt.gcf().canvas.mpl_connect(
                'key_release_event',
                lambda event: [exit(0) if event.key == 'escape' else None])
            plt.plot(predicted_trajectory[:, 0], predicted_trajectory[:, 1], "-g")
            plt.plot(x[0], x[1], "xr")
            plt.plot(goal[0], goal[1], "xb")
            plt.plot(ob[:, 0], ob[:, 1], "ok")
            plot_robot(x[0], x[1], x[2], config)
            plot_arrow(x[0], x[1], x[2])
            plt.axis("equal")
            plt.grid(True)
            plt.pause(0.0001)

        # check reaching goal
        dist_to_goal = math.hypot(x[0] - goal[0], x[1] - goal[1])
        if dist_to_goal <= config.robot_radius:
            print("Goal!!")
            break

    print("Done")
    if show_animation:
        plt.plot(trajectory[:, 0], trajectory[:, 1], "-r")
        plt.pause(0.0001)
        plt.show()


if __name__ == '__main__':
    main(robot_type=RobotType.rectangle)
    # main(robot_type=RobotType.circle)

reference (같이 보면 좋은 블로그)

• https://jhrobotics.tistory.com/42