Skip to content
Closed
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
338 changes: 338 additions & 0 deletions src/autonomous_driving_car/CAS/v4.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,338 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
CARLA 弯道自适应转向:根据弯道大小动态调整转弯幅度
"""

import sys
import os
import carla
import numpy as np
import math
import pygame
import traceback

# ===================== 全局配置(动态参数)=====================
# CARLA连接
CARLA_HOST = "localhost"
CARLA_PORT = 2000
CARLA_TIMEOUT = 10.0

# 车辆配置
VEHICLE_MODEL = "vehicle.tesla.model3"
VEHICLE_WHEELBASE = 2.9 # 特斯拉Model3轴距(米)
VEHICLE_REAR_AXLE_OFFSET = 1.45 # 后轴偏移(正数)

# 转向控制(动态参数基准)
LOOKAHEAD_DIST_STRAIGHT = 7.0 # 直道预瞄距离
LOOKAHEAD_DIST_CURVE = 4.0 # 急弯预瞄距离
STEER_GAIN_STRAIGHT = 0.7 # 直道转向增益
STEER_GAIN_CURVE = 1.0 # 急弯转向增益
STEER_DEADZONE = 0.05 # 转向死区
STEER_LOWPASS_ALPHA = 0.6 # 低通滤波系数
STEER_DIR_COEFF = 1.0 # 转向方向校准
MAX_STEER = 1.0 # 最大转向角

# 弯道等级划分(方向变化量阈值)
DIR_CHANGE_GENTLE = 0.03 # 缓弯阈值
DIR_CHANGE_SHARP = 0.08 # 急弯阈值

# 速度控制
BASE_SPEED = 28.0 # 直道基础速度
CURVE_SPEED_FACTOR = 0.6 # 弯道速度系数(更明显的速度衰减)
PID_KP = 0.15
PID_KI = 0.01
PID_KD = 0.01

# 相机
CAMERA_POS = carla.Transform(carla.Location(x=-5.0, z=2.0))
CAMERA_WIDTH = 800
CAMERA_HEIGHT = 600
CAMERA_FOV = 90

# ===================== 纯追踪控制器(动态适配)=====================
class AdaptivePurePursuit:
def __init__(self, wheelbase):
self.wheelbase = wheelbase
self.last_steer = 0.0 # 上一帧转向角
self.last_lookahead = LOOKAHEAD_DIST_STRAIGHT # 上一帧预瞄距离

def calculate_steer(self, vehicle_transform, target_point, dir_change):
"""
自适应纯追踪计算(根据弯道大小调整参数)
:param vehicle_transform: 车辆变换
:param target_point: 目标点(carla.Location)
:param dir_change: 方向变化量(弯道大小)
:return: 自适应后的转向角
"""
# 1. 获取车辆后轴位置
forward_vec = vehicle_transform.get_forward_vector()
rear_axle_loc = carla.Location(
x=vehicle_transform.location.x - forward_vec.x * VEHICLE_REAR_AXLE_OFFSET,
y=vehicle_transform.location.y - forward_vec.y * VEHICLE_REAR_AXLE_OFFSET,
z=vehicle_transform.location.z
)

# 2. 转换到车辆坐标系
dx = target_point.x - rear_axle_loc.x
dy = target_point.y - rear_axle_loc.y
yaw = math.radians(vehicle_transform.rotation.yaw)

dx_vehicle = dx * math.cos(yaw) + dy * math.sin(yaw)
dy_vehicle = -dx * math.sin(yaw) + dy * math.cos(yaw)

# 3. 动态计算转向增益(根据弯道大小线性插值)
# 方向变化量越大,增益越高
steer_gain = np.interp(
dir_change,
[0, DIR_CHANGE_SHARP],
[STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE]
)
steer_gain = np.clip(steer_gain, STEER_GAIN_STRAIGHT, STEER_GAIN_CURVE)

# 4. 纯追踪核心计算
if dx_vehicle < 0.1:
steer = self.last_steer
else:
# 纯追踪公式 + 弯道大小系数(dir_change*2 放大差异)
steer_rad = math.atan2(2 * self.wheelbase * dy_vehicle * (1 + dir_change * 2), dx_vehicle ** 2 + dy_vehicle ** 2)
steer = steer_rad / math.pi

# 应用增益和方向校准
steer *= steer_gain * STEER_DIR_COEFF

# 5. 应用死区
if abs(steer) < STEER_DEADZONE:
steer = 0.0

# 6. 低通滤波(平滑)
steer = STEER_LOWPASS_ALPHA * steer + (1 - STEER_LOWPASS_ALPHA) * self.last_steer

# 7. 限制范围
steer = np.clip(steer, -MAX_STEER, MAX_STEER)

# 8. 更新状态
self.last_steer = steer

return steer

def get_adaptive_lookahead(self, dir_change):
"""
动态获取预瞄距离(根据弯道大小线性插值)
:param dir_change: 方向变化量
:return: 自适应预瞄距离
"""
lookahead_dist = np.interp(
dir_change,
[0, DIR_CHANGE_SHARP],
[LOOKAHEAD_DIST_STRAIGHT, LOOKAHEAD_DIST_CURVE]
)
lookahead_dist = np.clip(lookahead_dist, LOOKAHEAD_DIST_CURVE, LOOKAHEAD_DIST_STRAIGHT)
self.last_lookahead = lookahead_dist
return lookahead_dist

# ===================== 速度控制器(动态调整)=====================
class SpeedController:
def __init__(self, kp, ki, kd):
self.kp = kp
self.ki = ki
self.kd = kd
self.last_error = 0.0
self.integral = 0.0

def calculate(self, target_speed, current_speed):
"""PID速度控制"""
error = target_speed - current_speed

p = self.kp * error
self.integral += self.ki * error
self.integral = np.clip(self.integral, -1.0, 1.0)
i = self.integral
d = self.kd * (error - self.last_error)
self.last_error = error

output = p + i + d
return np.clip(output, 0.0, 1.0)

# ===================== 辅助函数:计算方向变化(替代曲率)=====================
def calculate_dir_change(current_wp):
"""
计算相邻Waypoint的方向变化量(更精细的计算,增加采样点)
:param current_wp: 当前Waypoint
:return: 方向变化的绝对值之和,弯道等级(0=直道,1=缓弯,2=急弯)
"""
waypoints = [current_wp]
# 增加采样点到5个,更准确的判断弯道大小
for i in range(5):
next_wps = waypoints[-1].next(1.0)
if next_wps:
waypoints.append(next_wps[0])
else:
break

if len(waypoints) < 4:
return 0.0, 0

# 计算每个相邻点的方向
dirs = []
for i in range(1, len(waypoints)):
wp_prev = waypoints[i-1]
wp_curr = waypoints[i]
dir_rad = math.atan2(
wp_curr.transform.location.y - wp_prev.transform.location.y,
wp_curr.transform.location.x - wp_prev.transform.location.x
)
dirs.append(dir_rad)

# 计算方向变化的绝对值之和(放大差异)
dir_change = 0.0
for i in range(1, len(dirs)):
dir_change += abs(dirs[i] - dirs[i-1]) * 2 # 放大差异

# 划分弯道等级
if dir_change < DIR_CHANGE_GENTLE:
curve_level = 0 # 直道
elif dir_change < DIR_CHANGE_SHARP:
curve_level = 1 # 缓弯
else:
curve_level = 2 # 急弯

return dir_change, curve_level

# ===================== 相机管理器 =====================
class CameraManager:
def __init__(self, world, vehicle, display):
self.world = world
self.vehicle = vehicle
self.display = display
self.camera = None
self._create_camera()

def _create_camera(self):
bp = self.world.get_blueprint_library().find("sensor.camera.rgb")
bp.set_attribute("image_size_x", str(CAMERA_WIDTH))
bp.set_attribute("image_size_y", str(CAMERA_HEIGHT))
bp.set_attribute("fov", str(CAMERA_FOV))
self.camera = self.world.spawn_actor(bp, CAMERA_POS, attach_to=self.vehicle)
self.camera.listen(self._on_image)

def _on_image(self, image):
array = np.frombuffer(image.raw_data, dtype=np.uint8)
array = array.reshape((CAMERA_HEIGHT, CAMERA_WIDTH, 4))[:, :, :3]
array = array[:, :, ::-1].swapaxes(0, 1)
self.display.blit(pygame.surfarray.make_surface(array), (0, 0))
pygame.display.flip()

def destroy(self):
if self.camera:
self.camera.stop()
self.camera.destroy()

# ===================== 主函数(核心逻辑)=====================
def main():
pygame.init()
display = pygame.display.set_mode((CAMERA_WIDTH, CAMERA_HEIGHT))
pygame.display.set_caption("CARLA 弯道自适应转向(最终版)")

client = None
world = None
vehicle = None
camera_manager = None
pp_controller = None
speed_controller = None

try:
# 1. 连接CARLA并初始化
client = carla.Client(CARLA_HOST, CARLA_PORT)
client.set_timeout(CARLA_TIMEOUT)
world = client.get_world()
map = world.get_map()

# 清理现有车辆
for actor in world.get_actors().filter("vehicle.*"):
actor.destroy()

# 生成车辆
vehicle_bp = world.get_blueprint_library().find(VEHICLE_MODEL)
spawn_points = map.get_spawn_points()
vehicle = world.spawn_actor(vehicle_bp, spawn_points[0])
print(f"车辆生成成功:{vehicle.type_id}")

# 初始化控制器
pp_controller = AdaptivePurePursuit(VEHICLE_WHEELBASE)
speed_controller = SpeedController(PID_KP, PID_KI, PID_KD)
camera_manager = CameraManager(world, vehicle, display)

# 2. 主循环
print("仿真启动,按ESC退出...")
clock = pygame.time.Clock()
running = True

while running:
# 事件处理
for event in pygame.event.get():
if event.type == pygame.QUIT or (event.type == pygame.KEYDOWN and event.key == pygame.K_ESCAPE):
running = False

# 3. 获取关键数据
vehicle_transform = vehicle.get_transform()
vehicle_vel = vehicle.get_velocity()
current_speed = math.hypot(vehicle_vel.x, vehicle_vel.y) * 3.6 # m/s → km/h

# 获取当前Waypoint
current_wp = map.get_waypoint(vehicle_transform.location, project_to_road=True)

# 计算方向变化量和弯道等级
dir_change, curve_level = calculate_dir_change(current_wp)

# 动态获取预瞄距离
lookahead_dist = pp_controller.get_adaptive_lookahead(dir_change)

# 获取目标Waypoint(动态预瞄距离)
target_wps = current_wp.next(lookahead_dist)
target_point = target_wps[0].transform.location if target_wps else vehicle_transform.location

# 4. 计算目标速度(根据弯道等级调整)
curve_speed_factors = [1.0, 0.7, 0.4] # 直道、缓弯、急弯的速度系数
speed_factor = curve_speed_factors[min(curve_level, 2)]
target_speed = BASE_SPEED * speed_factor
target_speed = max(10.0, target_speed)

# 5. 计算转向角(传入方向变化量,实现自适应)
steer = pp_controller.calculate_steer(vehicle_transform, target_point, dir_change)

# 6. 计算油门/刹车
throttle = speed_controller.calculate(target_speed, current_speed)
brake = 1.0 - throttle if current_speed > target_speed + 5 else 0.0

# 7. 应用控制
control = carla.VehicleControl()
control.steer = steer
control.throttle = throttle
control.brake = brake
control.hand_brake = False
vehicle.apply_control(control)

# 8. 打印状态(调试用,显示弯道等级和动态参数)
curve_names = ["直道", "缓弯", "急弯"]
print(f"速度:{current_speed:.1f}km/h | 目标速度:{target_speed:.1f}km/h | 弯道:{curve_names[curve_level]} | 预瞄:{lookahead_dist:.1f}m | 转向:{steer:.3f}")

# 控制帧率
clock.tick(30)

except Exception as e:
print(f"错误:{e}")
traceback.print_exc()

finally:
# 清理资源
print("清理资源...")
if camera_manager:
camera_manager.destroy()
if vehicle:
vehicle.destroy()
pygame.quit()
print("仿真结束")

if __name__ == "__main__":
main()
Loading