ROS2自动驾驶
yolo5自动驾驶
1、重要!更换U盘的操作指引
2、关闭开机自启动大程序
3、Linux基础
4、YoloV5训练集
5、自动驾驶基础调试(代码解析)
6、自动驾驶特调
7、自动驾驶原理
8、PID算法理论
9、阿克曼运动学分析理论
10、建立运动学模型
常用命令
!重要!首次使用
一、原理分析
麦克纳姆轮运动学分析
二、AI大模型
3、AI大模型类型和原理
4、RAG检索增强和模型训练样本
5、具身智能机器人系统架构
6、具身智能玩法核心源码解读
7、配置AI大模型
8、配置API-KEY
三、深度相机
2、颜色标定
10、深度相机的基础使用
11、深度相机伪彩色图像
12、深度相机测距
13、深度相机色块体积测算
14、深度相机颜色跟随
15、深度相机人脸跟随
16、深度相机KCF物体跟随
17、深度相机Mediapipe手势跟随
18、深度相机视觉循迹自动驾驶
19、深度相机边缘检测
四、多模态视觉理解
20、多模态语义理解、指令遵循
21、多模态视觉理解
22、多模态视觉理解+自动追踪
23、多模态视觉理解+视觉跟随
24、多模态视觉理解+视觉巡线
25、多模态视觉理解+深度相机距离问答
26、多模态视觉理解+SLAM导航
27、多模态视觉理解+SLAM导航+视觉巡线
28、意图揣测+多模态视觉理解+SLAM导航+视觉功能
五、雷达
8、雷达基础使用
思岚系列雷达
六、建立地图
9、Gmapping建图
cartographer快速重定位导航
RTAB-Map导航
RTAB-Map建图
slam-toolbox建图
cartographer建图
Navigation2多点导航避障
Navigation2单点导航避障
手机APP建图与导航
七、新机器人自动驾驶与调整
多模态视觉理解+SLAM导航
新机器人自动驾驶
场地摆放及注意事项
启动测试
识别调试
无人驾驶的车道保持
无人驾驶路标检测
无人驾驶红绿灯识别
无人驾驶之定点停车
无人驾驶转向决策
无人驾驶之喇叭鸣笛
无人驾驶减速慢行
无人驾驶限速行驶
无人驾驶自主泊车
无人驾驶综合应用
无人驾驶融合AI大模型应用
八、路网规划
路网规划导航简介
构建位姿地图
路网标注
路网规划结合沙盘地图案例
路径重规划
九、模型训练
1、数据采集
2、数据集标注
3、YOLOv11模型训练
4、模型格式转换
十、YOLOV11开发
多机通讯配置
汝城县职业中等专业学校知识库-信息中心朱老师编辑
-
+
首页
七、新机器人自动驾驶与调整
无人驾驶转向决策
无人驾驶转向决策
# 无人驾驶之转向决策 ## 1.课程内容 **注意:这个程序是在沙盘地图上运行的,个人地图需要自行调整程序** 1.学习小车识别到右转之后做的动作 2.学习新出现的关键源码 ### 2.路标检测 路标检测源码路径: `/home/jetson/yahboomcar_auto/src/auto_driver/scripts/auto_drive.py` ### 3.程序启动 * 参数配置 程序路径: /home/jetson/yahboomcar_auto/src/auto_driver/config/auto_drive_node.yaml ``` turn_radius: 0.4 linear_speed: 0.13 angular_speed: -0.55 duration: 2.0 response_distance_1: 2900 response_distance_2: 132 cooldown_time: 0.2 DEBUG_MODE: False DEBUG_MODE_2: True START_AutoDrive: True Kp: -0.62 Ki: 0.0 Kd: -0.1 max_integral: 1000.0 image_size: [640, 480] ``` ```启动相机+底盘+自动驾驶节点 roslaunch auto_driver auto_drive.launch ```  启动成功之后会显示画面不会运动,为了测试路标识别情况。(如果想要运动,将auto_drive_node.yaml的参数START_AutoDrive改成 True)  程序运行之后会显示yolo识别画面,小车会车道线居中运行,同时会根据行驶过程中识别到的路标做对应的动作。 ### 3.2 识别到右转之后小车的运动 **注意:因为右转使用的固定动作,所以右转和停车A的路标是需要放在固定位置的**  程序识别到右转程序会打印  小车会运行固定动作进行转向决策,看到右转会停下来一秒然后前进5秒右转。  转向决策处理函数在auto_drive.py文件下,下面是识别到右转之后进入的函数,用户可以自行修改识别到路标之后小车运动 ``` def turn(self, direction): rospy.loginfo(f'Turn {direction}') if self.R == 0: angular_z = self.angular_speed if direction == 'left' else -self.angular_speed self._set_current_speed(angular_z=angular_z) else: self.flag = True # rospy.loginfo(f"angular_z : {angular_z}") self._set_current_speed(linear_x=0.0, angular_z=0.0) time.sleep(1) self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.05) time.sleep(5) self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.55) time.sleep(2.3) #可自行修改转弯时间 self.flag = False time.sleep(self.duration) self._set_current_speed(linear_x=self.drive_speed, angular_z=0.0) ``` ## 4.源码解析 ### 4.1、auto_drive.py yolo路标消息接收: ``` results = self.sign_detect.get_infer_result(frame, True) annotated_frame = frame.copy() ``` 多重过滤机制:主要为了防止一直识别路标并触发,识别到不同路标才会解锁动作。 ``` # 如果检测到需要冷却的标志,启动冷却 if detected_sign_requires_cooldown: self.sign_cooldown = True self.last_sign_time = current_time rospy.loginfo(f"Sign detected, starting {self.cooldown_duration}s cooldown") # 检查是否应该忽略重复路标 should_ignore = False if obj.class_name == self.last_sign: should_ignore = True ``` 事件队列处理 ``` def _event_handling_worker(self): '''事件处理工作线程''' # 标志处理映射表 sign_handlers = { "straight": self.go_straight, "turnright": lambda: self.turn('right'), "turnright_ground": self.do_nothing, "honking": self.car_horn, "sidewalk": self.slow_down, "sidewalk_ground": self.do_nothing, "speedlimit": self.slow_down, "stop": self.stop, "school": self.school, "parkA": self.parking_A, "parkB": self.parking_A, "greenlight": self.go_straight, "redlight": self.red_stop, "yellowlight": self.stop, "out_park": self.out_parking } # 标志启用索引映射 sign_enable_map = { "straight": 10, "turnright": 11, "honking": 1, "sidewalk": 6, "sidewalk_ground": 7, "speedlimit": 8, "stop": 9, "school": 5, "parkA": 2, "parkB": 3, "greenlight": 0, "redlight": 4, "yellowlight": 13 } ``` 具体动作实现 ``` def turn(self, direction): rospy.loginfo(f'Turn {direction}') if self.R == 0: angular_z = self.angular_speed if direction == 'left' else -self.angular_speed self._set_current_speed(angular_z=angular_z) else: self.flag = True # rospy.loginfo(f"angular_z : {angular_z}") self._set_current_speed(linear_x=0.0, angular_z=0.0) time.sleep(1) self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.05) time.sleep(5) self._set_current_speed(linear_x=self.drive_speed, angular_z=-0.55) time.sleep(2.3) #可自己修改转弯时间 self.flag = False time.sleep(self.duration) self._set_current_speed(linear_x=self.drive_speed, angular_z=0.0) ```
admin
2025年12月7日 11:07
32
转发
收藏文档
上一篇
下一篇
手机扫码
复制链接
手机扫一扫转发分享
复制链接
Markdown文件
Word文件
PDF文档
PDF文档(打印)
分享
链接
类型
密码
更新密码
有效期
AI