提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
前言
上一篇完成了一个简单的路径规划,本文将基于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,得到最优轨迹。