Skip to content
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
203 changes: 53 additions & 150 deletions src/Unmanned_vehicle_AD_DQN/Environment.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,6 @@
import numpy as np
import cv2
import math
from collections import deque

import carla
from carla import ColorConverter
Expand All @@ -18,8 +17,8 @@
except ImportError:
# 如果在导入Hyperparameters时出错,使用默认值
SHOW_PREVIEW = False
IM_WIDTH = 160 # 修改默认值
IM_HEIGHT = 120 # 修改默认值
IM_WIDTH = 640
IM_HEIGHT = 480


class CarEnv:
Expand All @@ -41,13 +40,6 @@ def __init__(self):
self.episode_start_time = None # 每轮开始时间
self.last_ped_distance = float('inf') # 上次最近行人距离
self.current_episode = 1 # 当前episode编号
self.obstacle_detected_time = None # 障碍物检测到的时间
self.reaction_start_time = None # 反应开始时间
self.proactive_action_count = 0 # 主动避障动作计数

# 新增:多帧缓存用于运动检测
self.frame_buffer = deque(maxlen=3)
self.motion_detected = False

# 加载世界和蓝图
try:
Expand Down Expand Up @@ -263,11 +255,6 @@ def reset(self, episode=1):
self.last_action = 1
self.episode_start_time = time.time()
self.last_ped_distance = float('inf')
self.obstacle_detected_time = None
self.reaction_start_time = None
self.proactive_action_count = 0
self.frame_buffer.clear()
self.motion_detected = False

# 设置车辆生成点
spawn_point = carla.Transform()
Expand Down Expand Up @@ -379,31 +366,6 @@ def process_img(self, image):
processed_image = np.array(image.raw_data)
processed_image = processed_image.reshape((self.im_height, self.im_width, 4))
processed_image = processed_image[:, :, :3] # 移除alpha通道

# 添加简单的图像增强
# 增加对比度以突出障碍物
processed_image = cv2.convertScaleAbs(processed_image, alpha=1.2, beta=10)

# 存储到帧缓冲区用于运动检测
if len(self.frame_buffer) == 0:
self.frame_buffer.append(processed_image.copy())
else:
# 简单的运动检测
if len(self.frame_buffer) >= 2:
prev_frame = self.frame_buffer[-1]
gray_prev = cv2.cvtColor(prev_frame, cv2.COLOR_RGB2GRAY)
gray_current = cv2.cvtColor(processed_image, cv2.COLOR_RGB2GRAY)

# 计算帧间差异
diff = cv2.absdiff(gray_prev, gray_current)
_, diff_thresh = cv2.threshold(diff, 25, 255, cv2.THRESH_BINARY)

# 如果差异足够大,检测到运动
motion_pixels = np.sum(diff_thresh > 0)
motion_ratio = motion_pixels / (self.im_width * self.im_height)
self.motion_detected = motion_ratio > 0.01 # 1%的像素变化

self.frame_buffer.append(processed_image.copy())

# 显示预览(如果启用)
if self.SHOW_CAM:
Expand All @@ -415,27 +377,27 @@ def process_img(self, image):
print(f"处理图像时出错: {e}")

def reward(self, speed_kmh, current_steer):
"""增强的奖励函数 - 特别强调行人避障和快速反应"""
"""增强的奖励函数 - 特别强调行人避障"""
reward = 0
done = False

# 获取车辆状态
vehicle_location = self.vehicle.get_location()
vehicle_rotation = self.vehicle.get_transform().rotation.yaw

# 1. 道路保持奖励
# 1. 道路保持奖励(重要的基础)
heading_error = abs(vehicle_rotation)

if heading_error < 5: # 完美保持方向
reward += 0.5
reward += 0.8 # 略微降低权重,给行人避障更多空间
elif heading_error < 15: # 良好保持
reward += 0.2
reward += 0.4
elif heading_error < 30: # 可接受
reward += 0.05
reward += 0.1
else: # 方向偏差过大
reward -= 0.2 * (heading_error / 30.0)
reward -= 0.3 * (heading_error / 30.0) # 降低惩罚强度

# 2. 行人避障(最高优先级) - 增强反应机制
# 2. 行人避障(最高优先级) - 增强奖励机制
min_ped_distance = float('inf')
closest_pedestrian = None

Expand All @@ -460,24 +422,14 @@ def reward(self, speed_kmh, current_steer):
if active_pedestrians == 0:
min_ped_distance = float('inf')

# 行人检测和反应时间记录
current_time = time.time()

# 检测到行人进入危险区域
# 行人距离分级奖励 - 增强避障激励
if min_ped_distance < 100: # 只考虑100米内的行人
if self.obstacle_detected_time is None:
self.obstacle_detected_time = current_time
self.reaction_start_time = current_time

# 行人距离分级奖励/惩罚 - 强调提前反应
if min_ped_distance < 3.0: # 紧急避让距离
reward -= 15.0 # 大幅增加惩罚
reward -= 8.0 # 增加惩罚
done = True
print(f"Episode {self.current_episode}: 与行人碰撞! 距离: {min_ped_distance:.1f}m")

print(f"Episode {self.current_episode}: 与行人距离过近 ({min_ped_distance:.1f}m)!")
elif min_ped_distance < 5.0: # 危险距离
reward -= 5.0 # 增加惩罚

reward -= 3.0 # 增加惩罚
# 计算避让方向
if closest_pedestrian:
ped_y = closest_pedestrian.get_location().y
Expand All @@ -487,93 +439,58 @@ def reward(self, speed_kmh, current_steer):
self.suggested_action = 4 # 右转
else: # 行人在右侧
self.suggested_action = 3 # 左转

# 反应时间惩罚
if self.reaction_start_time:
reaction_time = current_time - self.reaction_start_time
if reaction_time > 0.5: # 反应超过0.5秒
reward -= 2.0 * (reaction_time - 0.5)

elif min_ped_distance < 8.0: # 预警距离
reward -= 1.5

# 计算避让方向(提前建议)
if closest_pedestrian:
ped_y = closest_pedestrian.get_location().y
veh_y = vehicle_location.y
if ped_y < veh_y: # 行人在左侧
self.suggested_action = 4 # 右转
else: # 行人在右侧
self.suggested_action = 3 # 左转

# 提前反应的奖励
if self.last_ped_distance < 12.0 and min_ped_distance > self.last_ped_distance:
# 如果上次距离更近,这次距离更远了,说明正在成功避让
reward += 1.0
self.proactive_action_count += 1

reward -= 0.8
elif min_ped_distance < 12.0: # 安全距离
# 检测到行人但距离安全,给予轻微奖励
reward += 0.3

# 如果检测到运动,给予额外奖励
if self.motion_detected:
reward += 0.2

# 提前规划奖励
if self.last_ped_distance < 8.0:
# 从危险距离进入安全距离,成功避障
reward += 2.0

else:
# 没有检测到行人,重置反应计时
self.obstacle_detected_time = None
self.reaction_start_time = None
reward += 0.5 # 增加安全距离奖励
else: # 非常安全
reward += 0.2

# 3. 成功避障奖励 - 当成功避开行人后给予额外奖励
if self.last_ped_distance < 8.0 and min_ped_distance > self.last_ped_distance:
# 如果上次距离危险,这次距离更远了,说明成功避让
reward += 0.3

self.last_ped_distance = min_ped_distance # 保存当前距离

# 3. 速度奖励(平衡避障和前进)
# 4. 速度奖励(平衡避障和前进)
if 20 <= speed_kmh <= 35: # 理想速度区间
reward += 0.5
elif 10 <= speed_kmh < 20: # 较慢但安全
reward += 0.3
reward += 0.4
elif 10 <= speed_kmh < 20: # 较慢但安全(避障时可能需要减速)
reward += 0.2
elif 35 < speed_kmh <= 40: # 稍快
reward += 0.1
elif speed_kmh > 40: # 过快,在行人环境中危险
reward -= 1.0 # 增加惩罚
reward -= 0.5 # 增加惩罚
else: # 停车或极慢
reward -= 0.1 # 轻微惩罚
reward -= 0.05 # 轻微惩罚

# 4. 转向平滑性奖励
steer_penalty = abs(current_steer) * 0.2 # 降低惩罚,给避障转向更多空间
# 5. 转向平滑性奖励(避障时可能需要适当转向)
steer_penalty = abs(current_steer) * 0.3 # 降低惩罚,给避障转向更多空间
reward -= steer_penalty

# 5. 碰撞检测
# 6. 碰撞检测
if len(self.collision_history) != 0:
reward = -20 # 增加碰撞惩罚
reward = -10 # 减少碰撞惩罚
done = True
print(f"Episode {self.current_episode}: 发生碰撞!")

# 6. 进度奖励
# 7. 进度奖励(次要于避障)
progress = (vehicle_location.x + 81) / 236.0 # 从-81到155
reward += progress * 0.3
reward += progress * 0.2 # 降低进度权重

# 7. 边界检查
# 8. 边界检查
if vehicle_location.x > 155: # 成功到达终点
reward += 20 # 增加到达终点的奖励
reward += 15 # 减少到达终点的奖励
done = True
print(f"Episode {self.current_episode}: 成功到达终点!")
elif vehicle_location.x < -90 or abs(vehicle_location.y + 195) > 30: # 偏离道路
reward -= 5 # 增加偏离惩罚
reward -= 3 # 降低偏离惩罚
done = True
print(f"Episode {self.current_episode}: 偏离道路!")

# 8. 主动避障奖励
if self.proactive_action_count > 0:
reward += self.proactive_action_count * 0.5

# 限制奖励范围
reward = max(min(reward, 30), -25)
reward = max(min(reward, 20), -15)

return reward, done

Expand All @@ -593,46 +510,32 @@ def step(self, action):
brake = 0.0
steer = 0.0

# 速度控制 - 优化动作响应
# 速度控制
if action == 0: # 减速
throttle = 0.0
brake = 0.7 # 增加制动强度
brake = 0.5
elif action == 1: # 保持/轻微加速
throttle = 0.4 # 增加油门
throttle = 0.3
brake = 0.0
elif action == 2: # 加速
throttle = 0.8
throttle = 0.7
brake = 0.0
elif action == 3: # 左转
throttle = 0.5 # 增加转向时的油门
throttle = 0.4
brake = 0.0
steer = -0.25 * speed_factor # 增加转向幅度
steer = -0.2 * speed_factor # 速度相关的转向幅度
elif action == 4: # 右转
throttle = 0.5
throttle = 0.4
brake = 0.0
steer = 0.25 * speed_factor # 增加转向幅度

# 如果有建议的避让动作,调整当前动作
if self.suggested_action is not None:
# 如果当前动作与建议动作不同,调整转向幅度
if self.suggested_action in [3, 4] and action in [3, 4]:
# 如果都是转向动作,但方向不同,增加建议方向的转向
if self.suggested_action != action:
if self.suggested_action == 3: # 建议左转
steer = -0.3 * speed_factor
else: # 建议右转
steer = 0.3 * speed_factor
elif self.suggested_action == 0: # 建议减速
brake = max(brake, 0.8) # 强制制动
throttle = 0.0
steer = 0.2 * speed_factor # 速度相关的转向幅度

# 限制连续同向转向 - 防止过度转向
if (action == 3 and self.last_action == 3) or (action == 4 and self.last_action == 4):
self.same_steer_counter += 1
if self.same_steer_counter > 2: # 连续2次同向转向后强制回正
steer *= 0.3 # 大幅减小转向幅度
throttle *= 0.6 # 减速
brake = 0.2 # 轻微制动
if self.same_steer_counter > 3: # 连续3次同向转向后强制回正
steer *= 0.5 # 减小转向幅度
throttle *= 0.8 # 减速
brake = 0.1 # 轻微制动
else:
self.same_steer_counter = 0

Expand All @@ -646,13 +549,13 @@ def step(self, action):
steer=steer
))

# 减少等待时间,加快反应
time.sleep(0.03) # 从0.05减少到0.03
# 等待物理更新
time.sleep(0.05)

# 计算奖励和完成状态
reward, done = self.reward(speed_kmh, steer)

# 限制极端奖励值
reward = np.clip(reward, -25, 30)
reward = np.clip(reward, -15, 20)

return self.front_camera, reward, done, None
Loading
Loading