04-车道线检测（USB）

"""

# 导入系统和第三方库
	import time
	import os
	import sys
	import cv2 as cv
	import numpy as np
	from cap import VideoCapture  # 摄像头捕获模块
	from tpu_factory import TPUFactory  # TPU加速模块
	from bot.utils import Voice  # 语音模块
	from bot.ugv import Motor, PID  # 电机控制和PID控制器
	from tool import has_digit_input, global_exception_handler  # 工具函数
	sys.excepthook = global_exception_handler   # 设置全局异常处理钩子
	# =============================== 全局配置参数 ===============================
	class Config:
		# 车速控制相关参数
		SPEED_NORMAL = 3500       # 正常行驶时的车速设定
		SPEED_DECELERATE = 2500   # 减速行驶时的车速设定
		TURN_MAX = 1500           # 最大转向速度差，用于转弯控制
		STRAIGHT_MAX = 1000       # 直行时允许的最大转向速度差

# PID控制器参数（比例、积分、微分系数）
		PID_P = 10.0
		PID_I = 0.0
		PID_D = 0.0

# 车道中心点设定值，根据不同环岛行驶方向设置不同的目标点
		SET_POINTS = {
			"Clockwise_outer_ring": 140,       # 顺时针外环
			"Counterclockwise_outer_ring": 560, # 逆时针外环
			"Counterclockwise_inner_ring": 520, # 逆时针内环
			"Clockwise_inner_ring": 180        # 顺时针内环
		}
		# 摄像头参数
		ROI_HEIGHT = 300          # 车道线检测感兴趣区域的高度，减少处理区域
	# =============================== 车道线跟踪模块 ===============================
	class LaneFollower:
		def __init__(self, pid_p, pid_i, pid_d):
			self.pid = PID(pid_p, pid_i, pid_d)  # 初始化PID控制器
			self.flag_turn = 0                   # 转弯标志位(0:直行,1:右转,2:左转)
			self.straight_count = 0              # 直行状态计数器
			self.current_direction = "Clockwise_outer_ring"  # 当前行驶方向
			self.current_speed = Config.SPEED_NORMAL  # 当前车速初始化为正常速度

def follow_lane(self, image, tpu_lanenet):
			"""
			车道线跟踪主函数，实现完整的车道保持控制流程
			:param image: 输入图像帧
			:param tpu_lanenet: LaneNet模型实例
			:return: 电机控制指令(左轮速度, 右轮速度)
			"""
			# Step 1: 使用LaneNet模型预测车道线
			output_img, lanes_points = tpu_lanenet.predict(image)
			# Step 2: 从模型输出中提取有效车道线特征点
			lane_points = self._extract_lane_points(lanes_points)
			# Step 3: 处理车道线丢失的情况
			if not lane_points['valid']:
				return self._handle_no_lane_detected()
			# Step 4: 根据检测到的车道线更新行驶方向判断
			self._update_direction(lane_points)
			# Step 5: 计算转向控制量，基于PID算法
			speed_diff = self._calculate_steering(lane_points)
			# Step 6: 生成电机控制指令，转化为左右轮速度
			return self._generate_motor_command(speed_diff)

def _extract_lane_points(self, lanes_points):
			"""提取并处理有效的车道线特征点，过滤无效点"""
			points = {
				'max_x0': -float('inf'), 'min_x0': float('inf'),
				'max_x1': -float('inf'), 'min_x1': float('inf'),
				'valid': False
			}
			# 处理模型输出为空的情况
			if lanes_points is None:
				return points
			# 处理第一条车道线的特征点
			if len(lanes_points) > 0:
				for x, y in lanes_points[0]:
					if y > Config.ROI_HEIGHT:  # 只处理感兴趣区域内的点
						if x > points['max_x0']:
							points['max_x0'] = x
							points['max_x0_y'] = y
						if x < points['min_x0']:
							points['min_x0'] = x
							points['min_x0_y'] = y

# 处理第二条车道线的特征点
			if len(lanes_points) > 1:
				for x, y in lanes_points[1]:
					if y > Config.ROI_HEIGHT:  # 只处理感兴趣区域内的点
						if x > points['max_x1']:
							points['max_x1'] = x
							points['max_x1_y'] = y
						if x < points['min_x1']:
							points['min_x1'] = x
							points['min_x1_y'] = y

# 判断是否检测到有效车道点
			points['valid'] = any(val != float('inf') and val != -float('inf') 
								 for key, val in points.items() if key.startswith(('max', 'min')))
			return points

def _handle_no_lane_detected(self):
			"""处理未检测到车道线时的应急控制逻辑"""
			self.straight_count = 0  # 重置直行计数器

# 根据当前转弯状态执行不同的应急动作
			if self.flag_turn == 1:
				Control_Motor.Rotate_Right(2000)  # 执行右转动作
				return 2000, -2000  # 返回右转速度指令
			elif self.flag_turn == 2:
				Control_Motor.Rotate_Left(2000)    # 执行左转动作
				return -2000, 2000  # 返回左转速度指令
			else:
				Control_Motor.GS_run(0, 0)         # 执行停车动作

return 0, 0  # 返回零速度指令

def _update_direction(self, points):
			"""根据检测到的车道线特征更新车辆行驶方向"""
			self.straight_count += 1  # 直行计数器加1

# 如果处于转弯状态且直行计数器超过阈值，重置转弯标志
			if self.flag_turn and self.straight_count > 5:
				self.flag_turn = 0
				self.straight_count = 0

# 根据车道线特征点计算斜率，判断行驶方向
			if points['max_x0'] == -float('inf') and points['min_x1'] != float('inf'):
				if points['max_x1'] != points['min_x1']:
					k = (points['max_x1_y'] - points['min_x1_y']) / (points['max_x1'] - points['min_x1'])
					self.current_direction = "Counterclockwise_inner_ring" if k > 0 else "Clockwise_inner_ring"
			elif points['max_x0'] != -float('inf') and points['min_x1'] != float('inf'):
				if points['max_x0'] != points['min_x0']:
					k = (points['max_x0_y'] - points['min_x0_y']) / (points['max_x0'] - points['min_x0'])
					self.current_direction = "Counterclockwise_outer_ring" if k > 0 else "Clockwise_outer_ring"

def _calculate_steering(self, points):
			"""基于PID算法计算车辆转向控制量"""
			target_point = Config.SET_POINTS[self.current_direction]  # 获取当前方向的目标点
			current_point = 0  # 初始化当前点位置
			threshold = 60  # 触发转弯的阈值

# 根据不同行驶方向选择参考点
			if self.current_direction == "Clockwise_outer_ring":
				current_point = points['max_x0']
				if abs(current_point - target_point) > threshold:
					self.flag_turn = 1  # 设置右转标志
					self.straight_count = 0

elif self.current_direction == "Counterclockwise_inner_ring":
				current_point = points['min_x1']
				if abs(current_point - target_point) > threshold:
					self.flag_turn = 2  # 设置左转标志
					self.straight_count = 0

elif self.current_direction == "Counterclockwise_outer_ring":
				current_point = points['min_x0']
				if abs(current_point - target_point) > threshold:
					self.flag_turn = 2  # 设置右转标志
					self.straight_count = 0

elif self.current_direction == "Clockwise_inner_ring":
				current_point = points['max_x1']
				if abs(current_point - target_point) > threshold:
					self.flag_turn = 1  # 设置左转标志
					self.straight_count = 0

# 更新PID控制器设定点并计算输出
			self.pid.SetPoint = target_point
			self.pid.update(current_point)
			return self.pid.output  # 返回PID计算的转向控制量

def _generate_motor_command(self, speed_diff):
			"""根据转向控制量生成电机控制命令"""
			# 根据是否转弯选择不同的最大转向速度差
			turn_max = Config.TURN_MAX if self.flag_turn else Config.STRAIGHT_MAX

# 计算左右轮速度
			speed_L = self.current_speed - max(-turn_max, min(turn_max, speed_diff))
			speed_R = self.current_speed + max(-turn_max, min(turn_max, speed_diff))

# 对速度进行限幅处理，确保在有效范围内
			speed_L = np.clip(speed_L, -4000, 4000)
			speed_R = np.clip(speed_R, -4000, 4000)

return int(speed_L), int(speed_R)  # 返回整数类型的左右轮速度

# =============================== 主程序 ===============================
	def main():
		global voice_, Control_Motor  # 声明全局变量

# 系统初始化流程
		try:
			# 获取当前脚本路径
			pwd = os.path.dirname(os.path.abspath(__file__))
			print(f"当前应用路径: {pwd}")

# 初始化语音系统
			voice_ = Voice()

# 初始化TPU加速的LaneNet模型
			tpu_lanenet = _init_tpu("lanenet", pwd)

# 初始化摄像头
			cap = _init_camera()

# 初始化车道跟踪器
			lane_follower = LaneFollower(Config.PID_P, Config.PID_I, Config.PID_D)

# 初始化电机控制
			Control_Motor = Motor(0, 0, 0, 0)

# 云台初始化
			Control_Motor.servo_ptz(0, 0)

# 语音提示系统初始化成功
			voice_.send(111)

# 进入主控制循环
			_main_loop(cap, tpu_lanenet, lane_follower)

except Exception as e:
			print(f"系统异常: {str(e)}")
			voice_.send(250)  # 播放异常提示音
		finally:
			_cleanup_resources(cap, tpu_lanenet)  # 清理系统资源

def _init_tpu(algorithm, pwd):
		"""初始化TPU加速的深度学习模型"""
		try:
			tpu = TPUFactory(algorithm=algorithm)  # 创建TPU工厂实例
		except Exception as e:
			voice_.send(242)  # 播放库导入异常提示音
			print(f"导入库异常, error: {e}")
			time.sleep(3)
			sys.exit(1)

if not tpu.link():  # 尝试连接TPU
			voice_.send(242)  # 播放TPU连接失败提示音
			print("TPU连接失败")
			time.sleep(3)
			sys.exit(1)

model_path = os.path.join(os.path.join(pwd, "model"), algorithm)  # 构建模型路径
		tpu.model_init(model_path, host=True)  # 初始化模型

return tpu  # 返回初始化后的TPU实例

def _init_camera():
		"""初始化摄像头设备"""
		try:
			return VideoCapture()  # 创建摄像头捕获实例
		except Exception as e:
			voice_.send(248)  # 播放摄像头异常提示音
			print(f"摄像头初始化失败: {str(e)}")
			time.sleep(5)
			sys.exit(1)

def _main_loop(cap, tpu_lanenet, lane_follower):
		"""系统主控制循环，处理图像、车道跟踪和车辆控制"""
		while True:  # 无限循环，直到退出条件满足
			try:
				# 从摄像头读取图像帧
				frame = cap.read()
				if frame is None:  # 处理读取失败的情况
					voice_.send(248)  # 播放摄像头异常提示音
					time.sleep(1)
					continue

# 执行车道跟踪，获取电机控制指令
				speed_L, speed_R = lane_follower.follow_lane(frame, tpu_lanenet)
				Control_Motor.GS_run(speed_L, speed_R, speed_L, speed_R)  # 控制电机运行

# 检查是否有退出指令
				if has_digit_input() == 'q':
					break

except KeyboardInterrupt:  # 处理键盘中断
				break

def _cleanup_resources(cap, tpu_lanenet):
		"""系统资源清理函数，确保安全退出"""
		voice_.send(4)  # 播放结束提示音
		Control_Motor.close()  # 关闭电机控制
		tpu_lanenet.release()  # 释放LaneNet模型资源
		voice_.close()  # 关闭语音系统
		cap.release()  # 释放摄像头资源
		print("系统安全关闭")

if __name__ == "__main__":
		main()  # 程序入口

[【附件】lanenet_model.zip](/media/attachment/2025/05/lanenet_model.zip)