使用DWA算法规划小车的最优路径

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

上一篇完成了一个简单的路径规划，本文将基于DWA滑动窗口法和PID反馈调节来控制小车按规划出的路径行驶

---

一、定义小车的状态量

class State:
    """
    vehicle state class
    """
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, omega = 0.0):
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.omega = omega

控制量为速度v和角速度omega，单步控制后的状态更新

def update_state(state, v, omega):

    if omega >= MAX_STEER:
        omega = MAX_STEER
    elif omega <= -MAX_STEER:
        omega = -MAX_STEER

    if v > MAX_SPEED:
        v = MAX_SPEED

    state_ = copy.deepcopy(state)
    v += random.uniform(-0.01, 0.01)
    omega += random.uniform(-0.02, 0.03)
    state_.yaw = state.yaw + omega * DT
    state_.v = v
    state_.x = state.x + v * DT * math.cos(state.yaw)
    state_.y = state.y + v * DT * math.sin(state.yaw)
    state_.omega = omega

    return state_

二、DWA计算当前位置的最优控制量

1.控制空间采样

定义小车线速度和角速度的范围

predict_time = 1 # [s] 
v_sample = 0.1  # [m/s] #速度采样间隔
w_sample = 1 * math.pi / 180.0  # [rad/s] #角速度采样间隔
v_max = 1.1  # [m/s]
v_min = 0.8  # [m/s]
# 角速度边界
w_max = 40.0 * math.pi / 180.0  # [rad/s]
w_min = -40.0 * math.pi / 180.0  # [rad/s]

2.预测评估

def my_control(state,path):
    state_ = copy.deepcopy(state)
    G_max = -float('inf') # 最优评价
    indx = calc_target_index(state_,path)
    for v in np.arange(v_min,v_max,v_sample):
        for w in np.arange(w_min,w_max,w_sample):
            trajectory = trajectory_predict(state_,v,w)
            d =__heading(trajectory,path.x[indx],path.y[indx])
            G = d
            if G_max<=G:
                G_max = G
                control_opt = [v,w]
    return control_opt,indx

预测每一种控制量下小车在predict_time内的轨迹

def trajectory_predict(state_init, v, w):
    state_ = copy.deepcopy(state_init)
    trajectory = Trajectory()
    trajectory.x.append(state_init.x)
    trajectory.y.append(state_init.y)
    trajectory.v.append(state_init.v)
    trajectory.yaw.append(state_init.yaw)
    time = 0
    # 在预测时间段内,Front View Distance ：v*predict_time
    while time < predict_time:
        state_ = update_state(state_, v, w) # 运动学模型
        trajectory.x.append(state_.x)
        trajectory.y.append(state_.y)
        trajectory.v.append(state_.v)
        trajectory.yaw.append(state_.yaw)
        time += DT

    return trajectory

最优评价指标可以是多种指标的综合，这里只计算了当前控制量的预测值与实际目标的角度偏差，也可以拿距离偏差作为评价指标。

#计算path中距离当前位置最近的点，以该点作为实际值评价不同预测轨迹的cost
def calc_target_index(state, path):
    # Search view target point index
    dx = [state.x - icx for icx in path.x]
    dy = [state.y - icy for icy in path.y]
    d = np.hypot(dx, dy)
    current_idx = np.argmin(d)
    new_d = abs(d[current_idx:-1] - view_dis)
    target_idx = np.argmin(new_d)+current_idx

    return target_idx

def __heading(trajectory,x,y):
    dx = x - trajectory.x[-1]
    dy = y - trajectory.y[-1]
    error_angle = math.atan2(dy, dx)
    cost_angle = error_angle - trajectory.yaw[-1]
    cost = math.pi-abs(cost_angle)

    return cost

3.用plt工具画小车

def pi_to_pi(angle):
    while angle < -math.pi:
        angle += 2 * math.pi
    
    while angle >= math.pi:
        angle -= 2 * math.pi
        
    return angle

def rot_mat_2d(angle):
    return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]

def plot_car(x, y, yaw):
    car_color = '-k'
    rot = rot_mat_2d(-yaw)
    car_outline_x, car_outline_y = [], []
    VRX = [.1,.1,-.1,-.1,.1]
    VRY = [.08,-.08,-.08,.08,.08]
    for rx, ry in zip(VRX, VRY):
        converted_xy = np.stack([rx, ry]).T @ rot
        car_outline_x.append(converted_xy[0]+x)
        car_outline_y.append(converted_xy[1]+y)

    plt.plot(car_outline_x, car_outline_y, car_color)

initial_state = State(x=0, y=0, yaw=math.pi/4, v=0.0, omega = 0.0)
state = initial_state
while True:
    control_opt,indx_ = my_control(state,path) # path详见上一篇文章
    state = update_state(state,control_opt[0],control_opt[1])
    plt.cla()
    plt.plot(path.x,path.y,color = 'r',linewidth=1.0)
    plt.grid(True)
    plt.axis("equal")
    plot_car(state.x,state.y,state.yaw)
    plt.scatter(path.x[indx_],path.y[indx_],c='green',s = 4)
    plt.pause(0.01)
    dist_to_goal = math.hypot(state.x - C[0], state.y - C[1])
    if dist_to_goal <= 0.3:
        print("Goal!!")
        break
plt.plot(path.x, path.y, "-r")
plt.grid(True)
plt.axis("equal")
plt.show()

总结

DWA算法思路总结：1、确定采样空间；（状态空间+控制空间）本文只考虑了控制空间。
2、预测轨迹；预测每一种控制量在predict_time时间内的轨迹。
3、确定评价指标（可以综合多个指标）cost，比较所有轨迹的cost，得到最优轨迹。