Carla自动驾驶仿真九：车辆变道路径规划

2024-03-03 01:28
文章标签 规划 路径 仿真 自动 驾驶 车辆 变道 carla

本文主要是介绍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

效果

下述是变道规划简单的实现，轨迹跟踪效果比较一般，PID没有仔细调，紫色是车辆运行的点迹。

在这里插入图片描述
公众号：自动驾驶simulation

这篇关于Carla自动驾驶仿真九：车辆变道路径规划的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/768030。

相关文章

hdu2544（单源最短路径）

hdu2544（单源最短路径）

模板题： //题意：求1到n的最短路径，模板题#include<iostream>#include<algorithm>#include<cstring>#include<stack>#include<queue>#include<set>#include<map>#include<stdio.h>#include<stdlib.h>#include<ctype.h>#i
阅读更多...
动态规划---打家劫舍

动态规划---打家劫舍

题目： 你是一个专业的小偷，计划偷窃沿街的房屋。每间房内都藏有一定的现金，影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统，如果两间相邻的房屋在同一晚上被小偷闯入，系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组，计算你 不触动警报装置的情况下 ，一夜之内能够偷窃到的最高金额。 思路： 动态规划五部曲： 1.确定dp数组及含义 dp数组是一维数组，dp[i]代表
阅读更多...
poj 1734 （floyd求最小环并打印路径）

poj 1734 （floyd求最小环并打印路径）

题意： 求图中的一个最小环，并打印路径。 解析： ans 保存最小环长度。 一直wa，最后终于找到原因，inf开太大爆掉了。。。 虽然0x3f3f3f3f用memset好用，但是还是有局限性。 代码： #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#incl
阅读更多...
软考系统规划与管理师考试证书含金量高吗？

软考系统规划与管理师考试证书含金量高吗？

2024年软考系统规划与管理师考试报名时间节点： 报名时间：2024年上半年软考将于3月中旬陆续开始报名 考试时间：上半年5月25日到28日，下半年11月9日到12日 分数线：所有科目成绩均须达到45分以上（包括45分）方可通过考试 成绩查询：可在“中国计算机技术职业资格网”上查询软考成绩 出成绩时间：预计在11月左右 证书领取时间：一般在考试成绩公布后3~4个月，各地领取时间有所不同
阅读更多...
poj 2976 分数规划二分贪心（部分对总体的贡献度） poj 3111

poj 2976 分数规划二分贪心（部分对总体的贡献度） poj 3111

poj 2976： 题意： 在n场考试中，每场考试共有b题，答对的题目有a题。 允许去掉k场考试，求能达到的最高正确率是多少。 解析： 假设已知准确率为x，则每场考试对于准确率的贡献值为： a - b * x，将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码： #include <iostream>#include <cstdio>#incl
阅读更多...
基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

基于UE5和ROS2的激光雷达+深度RGBD相机小车的仿真指南(五)：Blender锥桶建模

前言 本系列教程旨在使用UE5配置一个具备激光雷达+深度摄像机的仿真小车，并使用通过跨平台的方式进行ROS2和UE5仿真的通讯，达到小车自主导航的目的。本教程默认有ROS2导航及其gazebo仿真相关方面基础，Nav2相关的学习教程可以参考本人的其他博客Nav2代价地图实现和原理–Nav2源码解读之CostMap2D(上)-CSDN博客往期教程： 第一期：基于UE5和ROS2的激光雷达+深度RG
阅读更多...
代码随想录冲冲冲 Day39 动态规划Part7

代码随想录冲冲冲 Day39 动态规划Part7

198. 打家劫舍 dp数组的意义是在第i位的时候偷的最大钱数是多少 如果nums的size为0 总价值当然就是0 如果nums的size为1 总价值是nums[0] 遍历顺序就是从小到大遍历 之后是递推公式 对于dp[i]的最大价值来说有两种可能 1.偷第i个 那么最大价值就是dp[i-2]+nums[i] 2.不偷第i个 那么价值就是dp[i-1] 之后取这两个的最大值就是d
阅读更多...
基于51单片机的自动转向修复系统的设计与实现

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍：✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师，一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生，希望您们都共创辉煌！✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机
阅读更多...
Python3 BeautifulSoup爬虫 POJ自动提交

Python3 BeautifulSoup爬虫 POJ自动提交

POJ 提交代码采用Base64加密方式 import http.cookiejarimport loggingimport urllib.parseimport urllib.requestimport base64from bs4 import BeautifulSoupfrom submitcode import SubmitCodeclass SubmitPoj():de
阅读更多...
数学建模笔记—— 非线性规划

数学建模笔记—— 非线性规划

数学建模笔记—— 非线性规划 非线性规划1. 模型原理1.1 非线性规划的标准型1.2 非线性规划求解的Matlab函数 2. 典型例题3. matlab代码求解3.1 例1 一个简单示例3.2 例2 选址问题1. 第一问 线性规划2. 第二问 非线性规划 非线性规划 非线性规划是一种求解目标函数或约束条件中有一个或几个非线性函数的最优化问题的方法。运筹学的一个重要分支。2
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2