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

news2024/9/23 3:19:52

文章目录

  • 前言
  • 一、关键函数
  • 二、完整代码
  • 效果

前言

本文介绍一种在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个waypoint
    waypoints = 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.location
                location.z = 1
                ego_spawn_point = carla.Transform(location, waypoint.transform.rotation)
                break
    return 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().location
     location2 = ego_vehicle.get_transform().location
     rel_x = location1.x - location2.x
     rel_y = location1.y - location2.y
     distance = math.sqrt(rel_x * rel_x + rel_y * rel_y)
     print("relative dis",distance)
     #rel_x 大于等于0，说明目标车在前方
     if rel_x >= 0:
         distance = distance
     else:
         distance = -distance
     if distance >= dis_to_cut:
         print("The conditions for changing lanes are met.")
         cut_in_flag = True
     else:
         cut_in_flag = False
     return 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车辆当前所在的waypoint
     current_location = vehicle.get_transform().location
     current_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.0
     brake = 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_speed


class 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 = True
        settings.fixed_delta_seconds = 0.05

    def 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_vehicle

    def spawn_npc_by_offset(self,ego_spawn_point,offset):
        vehicle_bp = self.world.get_blueprint_library().filter('vehicle.tesla.*')[0]
        # 计算新的生成点
        rotation = ego_spawn_point.rotation
        location = ego_spawn_point.location
        location.x += offset.x
        location.y += offset.y
        location.z += offset.z
        npc_transform = carla.Transform(location, rotation)
        npc_vehicle = self.world.spawn_actor(vehicle_bp, npc_transform)

        return npc_vehicle

    def get_spawn_point(self,target_road_id,target_lane_id):
        #每隔5m生成1个waypoint
        waypoints = 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.location
                    location.z = 1
                    ego_spawn_point = carla.Transform(location, waypoint.transform.rotation)
                    break
        return ego_spawn_point
    
    def cal_target_route(self,vehicle=None,lanechange="left",target_dis=20):
        #实例化道路规划模块
        grp = GlobalRoutePlanner(self.map, 2)
        #获取npc车辆当前所在的waypoint
        current_location = vehicle.get_transform().location
        current_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

    def 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.0
            brake = 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().location
        location2 = ego_vehicle.get_transform().location
        rel_x = location1.x - location2.x
        rel_y = location1.y - location2.y
        distance = math.sqrt(rel_x * rel_x + rel_y * rel_y)
        print("relative dis",distance)
        if rel_x >= 0:
            distance = distance
        else:
            distance = -distance
        if distance >= dis_to_cut:
            print("The conditions for changing lanes are met.")
            cut_in_flag = True
        else:
            cut_in_flag = False
        return cut_in_flag


if __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)

        #设置目标车初始速度的纵向控制PID
        initspd_pid = PIDLongitudinalController(npc_vehicle, K_P=1.0, K_I=0.1, K_D=0.05)

        #设置目标车的cut_in的横纵向控制PID
        args_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 = None
        waypoint_index = 0
        need_cal_route = True
        cut_in_flag = False
        arrive_target_point = False
        target_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/h
            target_speed = ego_speed + 8 #目标车的目标速度
            #是否满足cut_in条件
            if cut_in_flag:
                if need_cal_route:
                    #生成车侧车道前方30m的waypoint
                    waypoints = 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 = True
                            print("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 in
                cut_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

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1484038.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

360文件夹（窗口标签化工具）使用：windows系统的文件管理标签化

360文件夹（窗口标签化工具）使用：windows系统的文件管理标签化

软件介绍 360 文件夹是一款单窗口多标签的资源管理器&#xff0c;提高了用户使用各类文件夹操作效率。单窗口多标签&#xff0c;像浏览器一样用多标签管理每个文件夹&#xff0c;以便更加快速高效地切换文件夹&#xff0c;告别凌乱的窗口&#xff0c;加快办公效率&#xff1b;…
阅读更多...
剑指offer面试题28：对称的二叉树

剑指offer面试题28：对称的二叉树

#试题28&#xff1a;对称的二叉树 题目&#xff1a; 请设计一个函数判断一棵二叉树是否 轴对称 。 示例 1&#xff1a; 输入&#xff1a;root [6,7,7,8,9,9,8] 输出&#xff1a;true 解释&#xff1a;从图中可看出树是轴对称的。示例 2&#xff1a; 输入&#xff1a;root …
阅读更多...
ssm个人学习01

ssm个人学习01

Spring配置文件: spring环境的搭建: 1:导入对应的spring坐标 也就是依赖 2:编写controller, service, dao相关的代码 3:创建配置文件(在resource下面配置文件) 例如:applicationContext.xml <bean id "" class ""> <property name "&…
阅读更多...
SQL窗口函数， 测试题

SQL窗口函数， 测试题

第一题 create table user_score (logday date, -- 考试时间 userid VARCHAR(20), -- 考试用户 score int); -- 考试成绩Insert into user_score values (2019-10-20,11111,85) ,(2019-10-20,22222,83) ,(2019-10-20,33333,86) ,(2019-10-21,11111,87) ,(2019-10-2…
阅读更多...
node 之 http模块

node 之 http模块

1.什么是http模块 在网络节点中&#xff0c;负责消费资源的电脑叫做客户端&#xff1b;负责对外提供网络资源的电脑&#xff0c;叫做服务器 http模块是node.js官方提供的&#xff0c;用来创建web服务器的模块&#xff0c;通过http模块提供的http.createServer()方法&#xff0c…
阅读更多...
烧脑问题解决办法：如何选择一款合适自己的手机流量卡

烧脑问题解决办法：如何选择一款合适自己的手机流量卡

现在社会人们越来越离不开手机了&#xff0c;手机给我们生活带来了翻天覆地的变化&#xff0c;手机需要最多的就是流量了&#xff0c;所以选择一款合适自己的手机流量卡就显得尤为重要了&#xff0c;今天小编就给大家来分享一下我的经验&#xff0c;希望对大家能有帮助&#xf…
阅读更多...
构建大语言模型的四个主要阶段

构建大语言模型的四个主要阶段

大规模语言模型的发展历程虽然只有短短不到五年的时间&#xff0c;但是发展速度相当惊人&#xff0c;国内外有超过百种大模型相继发布。中国人民大学赵鑫教授团队在文献按照时间线给出 2019 年至 2023 年比较有影响力并且模型参数量超过 100 亿的大规模语言模型。大规模语言模型…
阅读更多...
关于synchronized介绍

关于synchronized介绍

synchronized的特性 1. 乐观锁/悲观锁自适应,开始时是乐观锁,如果锁冲突频繁,就转换为悲观锁 2.轻量级/重量级锁自适应 开始是轻量级锁实现,如果锁被持有的时间较长,就转换成重量级锁 3.自旋/挂起等待锁自适应 4.不是读写锁 5.非公平锁 6,可重入锁 synchronized的使用 1&#…
阅读更多...
01背包（详细）

01背包（详细）

背包最大重量为4。 有物品3件&#xff0c;分别有其质量和价值。 vector<int> weight{1,3,4}; vector<int> value{15,20,30}; int bag4; 问背包能背的物品最大价值是多少&#xff1f; 这是标准的动态规划问题&#xff0c;每一个问题鱼鳍前面的子问题相联。 目…
阅读更多...
structuredClone() 详解

structuredClone() 详解

您是否知道&#xff0c;现在 JavaScript 中有一种原生的方式可以深拷贝对象&#xff1f; 没错&#xff0c;这个内置于 JavaScript 运行时的structuredClone函数就是这样&#xff1a; const calendarEvent {title: "Builder.io大会",date: new Date(123),attendees…
阅读更多...
#WEB前端（CSS基础）

#WEB前端（CSS基础）

1.实验&#xff1a;HTML是网页骨架&#xff0c;CCS是网页装修 2.IDE&#xff1a;VSCODE 3.记录&#xff1a; style 4.代码&#xff1a; <!DOCTYPE html> <html lang"en"> <head><meta charset"UTF-8"><meta name"view…
阅读更多...
Ajax(黑马学习笔记)

Ajax(黑马学习笔记)

Ajax介绍 Ajax概述 我们前端页面中的数据&#xff0c;如下图所示的表格中的学生信息&#xff0c;应该来自于后台&#xff0c;那么我们的后台和前端是互不影响的2个程序&#xff0c;那么我们前端应该如何从后台获取数据呢&#xff1f;因为是2个程序&#xff0c;所以必须涉及到…
阅读更多...
风电机组来说，CMS振动数据和SCADA数据各有其优点和缺点

风电机组来说，CMS振动数据和SCADA数据各有其优点和缺点

对于风电机组来说&#xff0c;CMS振动数据和SCADA数据各有其优点和缺点。 CMS振动数据的缺点主要包括&#xff1a; 数据解读难度高&#xff1a;振动数据包含大量的专业信息&#xff0c;如振动幅度、频率、相位等&#xff0c;需要专业的知识和技能才能准确解读。受环境影响大&…
阅读更多...
DETR详解

DETR详解

1. 动机 传统的目标检测任务需要大量的人工先验知识&#xff0c;例如预定义的先验anchor&#xff0c;NMS后处理策略等。这些人工先验知识引入了很多人为因素&#xff0c;且较难处理。如果能够端到端到直接生成目标检测结果&#xff0c;将会使问题变得很优雅。 2. 主要贡献 提…
阅读更多...
2024最新算法：电鳗觅食优化算法（Electric eel foraging optimization,EEFO）求解23个基准函数（提供MATLAB代码）

2024最新算法：电鳗觅食优化算法（Electric eel foraging optimization,EEFO）求解23个基准函数（提供MATLAB代码）

一、电鳗觅食优化算法 电鳗觅食优化算法&#xff08;Electric eel foraging optimization,EEFO&#xff09;由Weiguo Zhao等人提出的一种元启发算法&#xff0c;EEFO从自然界中电鳗表现出的智能群体觅食行为中汲取灵感。该算法对四种关键的觅食行为进行数学建模&#xff1a;相…
阅读更多...
ESP8266智能家居（5）——开发APP深入篇

ESP8266智能家居（5）——开发APP深入篇

1.代码解析 接下来重点介绍一下逻辑代码 这里面主要是设置mqtt服务器的IP地址和端口号&#xff0c;设置服务器的用户名和登录密码 绑定好订阅主题和发布主题&#xff08;和8266上的订阅、发布交叉就行&#xff09; 绑定界面&#xff0c;设置界面标题 绑定6个文本控件 将从mq…
阅读更多...
【C语言】熟悉文件基础知识

【C语言】熟悉文件基础知识

欢迎关注个人主页&#xff1a;逸狼 创造不易&#xff0c;可以点点赞吗~ 如有错误&#xff0c;欢迎指出~ 文件 为了数据持久化保存&#xff0c;使用文件&#xff0c;否则数据存储在内存中&#xff0c;程序退出&#xff0c;内存回收&#xff0c;数据就会丢失。 程序设计中&…
阅读更多...
在您的下一个项目中选择 Golang 和 Node.js 之间的抉择

在您的下一个项目中选择 Golang 和 Node.js 之间的抉择

作为一名软件开发者&#xff0c;我总是在寻找构建应用程序的最快、最高效的工具。在速度和处理复杂任务方面&#xff0c;我认为 Golang 和 Node.js 是顶尖技术。两者在性能方面都享有极高的声誉。但哪一个更快——Golang 还是 Node&#xff1f;我决定深入一些硬核基准测试&…
阅读更多...
抽象类、模板方法模式

抽象类、模板方法模式

抽象类概述 在Java中abstract是抽象的意思&#xff0c;如果一个类中的某个方法的具体实现不能确定&#xff0c;就可以申明成abstract修饰的抽象方法&#xff08;不能写方法体了&#xff09;&#xff0c;这个类必须用abstract修饰&#xff0c;被称为抽象类。 抽象方法定义&…
阅读更多...
吸猫毛空气净化器哪个好？推荐除猫毛效果好宠物空气净化器品牌

吸猫毛空气净化器哪个好？推荐除猫毛效果好宠物空气净化器品牌

当下有越来越多的家庭选择养宠物&#xff01;尽管家里变得更加温馨&#xff0c;但养宠可能会带来异味和空气中的毛发增多可能会带来健康问题&#xff0c;这是一个大问题&#xff01; 不想家里弥漫着异味&#xff0c;特别是来自宠物便便的味道&#xff0c;所以需要一款能够处理…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023