Carla自動駕駛仿真九：車輛變道路徑規劃

文章目錄

前言
一、關鍵函數
二、完整代碼
效果

前言

本文介紹一種在carla中比較簡單的變道路徑規劃方法，主要核心是調用carla的GlobalRoutePlanner模塊和PID控制模塊實現變道，大體的框架如下圖所示。

在這里插入圖片描述

一、關鍵函數

1、get_spawn_point(),該函數根據指定road和lane獲得waypoint（這里之所以這么用是為了選擇一條比較長的直路）。具體用法可以參考上篇文章：Carla自動駕駛仿真八：兩種查找CARLA地圖坐標點的方法

def get_spawn_point(self,target_road_id,target_lane_id):#每隔5m生成1個waypointwaypoints = self.map.generate_waypoints(5.0)# 遍歷路點for waypoint in waypoints:if waypoint.road_id == target_road_id:lane_id = waypoint.lane_id# 檢查是否已經找到了特定車道ID的路點if lane_id == target_lane_id:location = waypoint.transform.locationlocation.z = 1ego_spawn_point = carla.Transform(location, waypoint.transform.rotation)breakreturn ego_spawn_point

2、should_cut_in()，用于主車和目標車的相對距離判斷，當目標車超越自車一定距離時，開始給cut_in_flag置Ture，并在下一步驟規劃變道路徑和執行變道操作。

 def should_cut_in(self,npc_vehicle, ego_vehicle, dis_to_cut=5):location1 = npc_vehicle.get_transform().locationlocation2 = ego_vehicle.get_transform().locationrel_x = location1.x - location2.xrel_y = location1.y - location2.ydistance = math.sqrt(rel_x * rel_x + rel_y * rel_y)print("relative dis",distance)#rel_x 大于等于0，說明目標車在前方if rel_x >= 0:distance = distanceelse:distance = -distanceif distance >= dis_to_cut:print("The conditions for changing lanes are met.")cut_in_flag = Trueelse:cut_in_flag = Falsereturn cut_in_flag

3、cal_target_route()，函數中調用了Carla的GlobalRoutePlanner模塊，能根據起點和終點自動生成車輛行駛的路徑（重點），我這里的變道起點是兩車相對距離達到（閾值）時目標車的當前位置，而終點就是左側車道前方target_dis米。將起點和終點代入到route = grp.trace_route(current_location, target_location)就能獲取到規劃路徑route。

在這里插入圖片描述

 def cal_target_route(self,vehicle=None,lanechange="left",target_dis=20):#實例化道路規劃模塊grp = GlobalRoutePlanner(self.map, 2)#獲取npc車輛當前所在的waypointcurrent_location = vehicle.get_transform().locationcurrent_waypoint = self.map.get_waypoint(current_location)#選擇變道方向if "left" in lanechange:target_org_waypoint = current_waypoint.get_left_lane()elif "right" in lanechange:target_org_waypoint = current_waypoint.get_right_lane()#獲取終點的位置target_location = target_org_waypoint.next(target_dis)[0].transform.location#根據起點和重點生成規劃路徑route = grp.trace_route(current_location, target_location)return route

4、speed_con_by_pid(),通過PID控制車輛的達到目標速度，pid是通過實例化Carla的PIDLongitudinalController實現。由于pid.run_step()只返回油門的控制，需要增加剎車的邏輯。

 control_signal = pid.run_step(target_speed=target_speed, debug=False)throttle = max(min(control_signal, 1.0), 0.0)  # 確保油門值在0到1之間brake = 0.0  # 根據需要設置剎車值if control_signal < 0:throttle = 0.0brake = abs(control_signal)  # 假設控制器輸出的負值可以用來剎車vehilce.apply_control(carla.VehicleControl(throttle=throttle, brake=brake))

5、PID = VehiclePIDController()是carla的pid橫縱向控制模塊，通過設置目標速度和目標終點來實現軌跡控制control = PID.run_step(target_speed, target_waypoint)，PID參數我隨便調了一組，有興趣的可以深入調一下。

二、完整代碼

import carla
import time
import math
import sys#修改成自己的carla路徑
sys.path.append(r'D:\CARLA_0.9.14\WindowsNoEditor\PythonAPI\carla')
from agents.navigation.global_route_planner import GlobalRoutePlanner
from agents.navigation.controller import VehiclePIDController,PIDLongitudinalController
from agents.tools.misc import draw_waypoints, distance_vehicle, vector, is_within_distance, get_speedclass CarlaWorld:def __init__(self):self.client = carla.Client('localhost', 2000)self.world = self.client.load_world('Town06')# self.world = self.client.get_world()self.map = self.world.get_map()# 開啟同步模式settings = self.world.get_settings()settings.synchronous_mode = Truesettings.fixed_delta_seconds = 0.05def spawm_ego_by_point(self,ego_spawn_point):vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.*')[0]ego_vehicle = self.world.try_spawn_actor(vehicle_bp,ego_spawn_point)return ego_vehicledef spawn_npc_by_offset(self,ego_spawn_point,offset):vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.*')[0]# 計算新的生成點rotation = ego_spawn_point.rotationlocation = ego_spawn_point.locationlocation.x += offset.xlocation.y += offset.ylocation.z += offset.znpc_transform = carla.Transform(location, rotation)npc_vehicle = self.world.spawn_actor(vehicle_bp, npc_transform)return npc_vehicledef get_spawn_point(self,target_road_id,target_lane_id):#每隔5m生成1個waypointwaypoints = self.map.generate_waypoints(5.0)# 遍歷路點for waypoint in waypoints:if waypoint.road_id == target_road_id:lane_id = waypoint.lane_id# 檢查是否已經找到了特定車道ID的路點if lane_id == target_lane_id:location = waypoint.transform.locationlocation.z = 1ego_spawn_point = carla.Transform(location, waypoint.transform.rotation)breakreturn ego_spawn_pointdef cal_target_route(self,vehicle=None,lanechange="left",target_dis=20):#實例化道路規劃模塊grp = GlobalRoutePlanner(self.map, 2)#獲取npc車輛當前所在的waypointcurrent_location = vehicle.get_transform().locationcurrent_waypoint = self.map.get_waypoint(current_location)#選擇變道方向if "left" in lanechange:target_org_waypoint = current_waypoint.get_left_lane()elif "right" in lanechange:target_org_waypoint = current_waypoint.get_right_lane()#獲取終點的位置target_location = target_org_waypoint.next(target_dis)[0].transform.location#根據起點和重點生成規劃路徑route = grp.trace_route(current_location, target_location)return routedef draw_target_line(self,waypoints):# 獲取世界和調試助手debug = self.world.debug# 設置繪制參數life_time = 60.0  # 點和線將持續顯示的時間（秒）color = carla.Color(255, 0, 0)thickness = 0.3  # 線的厚度for i in range(len(waypoints) - 1):debug.draw_line(waypoints[i][0].transform.location + carla.Location(z=0.5),waypoints[i + 1][0].transform.location + carla.Location(z=0.5),thickness=thickness,color=color,life_time=life_time)def draw_current_point(self,current_point):self.world.debug.draw_point(current_point,size=0.1, color=carla.Color(b=255), life_time=60)def speed_con_by_pid(self,vehilce=None,pid=None,target_speed=30):control_signal = pid.run_step(target_speed=target_speed, debug=False)throttle = max(min(control_signal, 1.0), 0.0)  # 確保油門值在0到1之間brake = 0.0  # 根據需要設置剎車值if control_signal < 0:throttle = 0.0brake = abs(control_signal)  # 假設控制器輸出的負值可以用來剎車vehilce.apply_control(carla.VehicleControl(throttle=throttle, brake=brake))def set_spectator(self,vehicle):self.world.get_spectator().set_transform(carla.Transform(vehicle.get_transform().location +carla.Location(z=50), carla.Rotation(pitch=-90)))def should_cut_in(self,npc_vehicle, ego_vehicle, dis_to_cut=5):location1 = npc_vehicle.get_transform().locationlocation2 = ego_vehicle.get_transform().locationrel_x = location1.x - location2.xrel_y = location1.y - location2.ydistance = math.sqrt(rel_x * rel_x + rel_y * rel_y)print("relative dis",distance)if rel_x >= 0:distance = distanceelse:distance = -distanceif distance >= dis_to_cut:print("The conditions for changing lanes are met.")cut_in_flag = Trueelse:cut_in_flag = Falsereturn cut_in_flagif __name__ == '__main__':try:CARLA = CarlaWorld()#根據road_id和lane_id選擇出生點start_point = CARLA.get_spawn_point(target_road_id=40, target_lane_id=-5)#生成自車ego_vehicle = CARLA.spawm_ego_by_point(start_point)#設置初始的觀察者視角CARLA.set_spectator(ego_vehicle)#相對ego生成目標車relative_ego = carla.Location(x=-10, y=3.75, z=0)npc_vehicle = CARLA.spawn_npc_by_offset(start_point, relative_ego)# 設置ego自動巡航ego_vehicle.set_autopilot(True)#設置目標車初始速度的縱向控制PIDinitspd_pid = PIDLongitudinalController(npc_vehicle, K_P=1.0, K_I=0.1, K_D=0.05)#設置目標車的cut_in的橫縱向控制PIDargs_lateral_dict = {'K_P': 0.8, 'K_D': 0.8, 'K_I': 0.70, 'dt': 1.0 / 10.0}args_long_dict = {'K_P': 1, 'K_D': 0.0, 'K_I': 0.75, 'dt': 1.0 / 10.0}PID = VehiclePIDController(npc_vehicle, args_lateral_dict, args_long_dict)waypoints = Nonewaypoint_index = 0need_cal_route = Truecut_in_flag = Falsearrive_target_point = Falsetarget_distance_threshold = 2.0  # 切換waypoint的距離start_sim_time = time.time()while not arrive_target_point:CARLA.world.tick()# 更新觀察者的視野CARLA.set_spectator(ego_vehicle)#計算目標車的初始速度ego_speed = (ego_vehicle.get_velocity().x  * 3.6) #km/htarget_speed = ego_speed + 8 #目標車的目標速度#是否滿足cut_in條件if cut_in_flag:if need_cal_route:#生成車側車道前方30m的waypointwaypoints = CARLA.cal_target_route(npc_vehicle,lanechange= "left",target_dis=30)CARLA.draw_target_line(waypoints)need_cal_route = False# 如果已經計算了路線if waypoints is not None and waypoint_index < len(waypoints):# 獲取當前目標路點target_waypoint = waypoints[waypoint_index][0]# 獲取車輛當前位置transform = npc_vehicle.get_transform()#繪制當前運行的點CARLA.draw_current_point(transform.location)# 計算車輛與當前目標路點的距離distance_to_waypoint = distance_vehicle(target_waypoint, transform)# 如果車輛距離當前路點的距離小于閾值，則更新到下一個路點if distance_to_waypoint < target_distance_threshold:waypoint_index += 1  # 移動到下一個路點if waypoint_index >= len(waypoints):arrive_target_point = Trueprint("npc_vehicle had arrive target point.")break  # 如果沒有更多的路點，退出循環else:# 計算控制命令control = PID.run_step(target_speed, target_waypoint)# 應用控制命令npc_vehicle.apply_control(control)else:#設置NPC的初始速度CARLA.speed_con_by_pid(npc_vehicle,initspd_pid,target_speed)#判斷是否可以cut incut_in_flag = CARLA.should_cut_in(npc_vehicle,ego_vehicle,dis_to_cut=8)# 判斷是否達到模擬時長if time.time() - start_sim_time > 60:print("Simulation ended due to time limit.")break#到達目的地停車npc_vehicle.apply_control(carla.VehicleControl(throttle=0, steer=0, brake=-0.5))print("Control the target car to brake.")time.sleep(10)except Exception as e:print(f"An error occurred: {e}")finally:# 清理資源print("Cleaning up the simulation...")if ego_vehicle is not None:ego_vehicle.destroy()if npc_vehicle is not None:npc_vehicle.destroy()settings = CARLA.world.get_settings()settings.synchronous_mode = False  # 禁用同步模式settings.fixed_delta_seconds = None