# 一.二值化/提取图像
# 1. BGR → HSV 转换
hsv = cv2.cvtColor(origin_img, cv2.COLOR_BGR2HSV)- OpenCV 读进来的图像是 BGR 格式。
- HSV 色彩空间更适合颜色识别，特别是在光照变化下。
# 2. 设置颜色阈值（HSV）
lower_color = np.array([H_min, S_min, V_min])#H色调,S饱和度,V亮度
higher_color = np.array([H_max, S_max, V_max])这一步是关键 —— 用于提取你关心的颜色区域（如黄色车道线或白色车道线）。
H_min, S_min, V_min = 20, 100, 100
H_max, S_max, V_max = 30, 255, 255# 3. 二值化
# 3.1 cv2.inRange(python)
binary_img = cv2.inRange(hsv, lower_color, higher_color)- 把在范围内的像素设为白（255），其他设为黑（0）。
- 得到的
binary_img是一张 二值图像。
# 3.2 cv::threshold(cpp) OTSU/TRIANGLE算法
cv::threshold(grayFrame, binary, threshold, 255, cv::THRESH_BINARY);
智能车用到的另一种二值化函数
// Otsu 自动阈值二值化
cv::threshold(grayFrame, binary, 0, 255, cv::THRESH_BINARY | cv::THRESH_OTSU);这行代码使用 Otsu算法 来自动选择最优阈值，将灰度图 grayFrame 转换为二值图 binary
cv::threshold(gray, binary, 0, 255, cv::THRESH_BINARY | cv::THRESH_TRIANGLE);| 参数位置 | 含义 |
|---|---|
grayFrame |
输入图像，必须是单通道（灰度图） |
binary |
输出图像（二值化后的图像） |
0 |
阈值值设为 0，仅作为占位符（因为 Otsu 会自动计算最优值） |
255 |
最大值，即大于阈值的像素值将设置为 255 |
# cv::THRESH_BINARY
- 表示基本的二值化操作。
- 如果像素值 > 阈值 → 设置为
maxval（通常是 255） - 否则 → 设置为 0
# cv::THRESH_OTSU
- 含义是：使用 Otsu 算法自动选择全局最优阈值，而不用你手动指定阈值值。
- 注意：你在
threshold()函数里传的阈值参数（第三个参数）将被忽略，OpenCV 会自动计算。
# 优势:
1.自动计算阈值,不需要人工设定阈值（如 128），它根据图像统计特性来自动选择；
2.对双峰直方图效果很好,如果图像前景和背景对比明显（有两个峰值），能找到最优分界线；
3.鲁棒性强,对于背景均匀、光照平稳的图像，效果非常好。
# cv::THRESH_TRIANGLE
cv::THRESH_TRIANGLE 是 OpenCV 中另一种 自动阈值算法，可以用来代替 Otsu 算法，尤其在图像直方图是单峰分布时效果更好。
# 优势:
1.图像直方图 单峰,比如光照均匀的手写文本图像、亮度集中于一侧；
2.目标与背景 对比不明显，比 Otsu 更敏感，可以提取一些低对比目标；
3.Otsu 效果不佳时的替代方案，不依赖“双峰直方图”，更通用。
# 常见二值化
(OpenCV 支持多种二值化方式，主要体现在 thresholdType 参数上)
| 模式 | 宏定义 | 效果说明 |
|---|---|---|
| 1️⃣ 普通二值化 | cv::THRESH_BINARY |
像素值 > 阈值 → maxval，否则 0 |
| 2️⃣ 反向二值化 | cv::THRESH_BINARY_INV |
像素值 > 阈值 → 0，否则 maxval |
| 3️⃣ 截断阈值 | cv::THRESH_TRUNC |
像素值 > 阈值 → 设为阈值，否则保持原值 |
| 4️⃣ 零阈值 | cv::THRESH_TOZERO |
像素值 > 阈值 → 保持原值，否则设为 0 |
| 5️⃣ 反向零阈值 | cv::THRESH_TOZERO_INV |
像素值 > 阈值 → 0，否则保持原值 |
| 6️⃣ Otsu 自动阈值 | cv::THRESH_OTSU |
自动找全局最优阈值（可与前几种组合） |
| 7️⃣ Triangle 自动阈值 | cv::THRESH_TRIANGLE |
另一种自动找阈值的方法，适用于峰值不明显的图像 |
# 3.3自适应阈值 adaptiveThreshold(cpp)
cv::adaptiveThreshold(gray, binary, 255, cv::ADAPTIVE_THRESH_MEAN_C, cv::THRESH_BINARY, 11, -5);| 参数位置 | 含义 |
|---|---|
gray |
输入图像（灰度图） |
binary |
输出图像（二值图） |
255 |
最大值，即大于阈值的像素被设为 255 |
cv::ADAPTIVE_THRESH_MEAN_C |
阈值类型，这里是使用局部区域的平均值 |
cv::THRESH_BINARY |
二值化类型：像素值 > 局部阈值 → 255，反之为 0 |
11 |
邻域大小（必须为奇数），即 11×11 像素区域 |
-5 |
常数 C，从局部均值中减去这个偏移值作为最终阈值。可正可负 |
# 工作原理
将整张图像分成小块，对每个区域单独计算局部阈值（如均值或高斯加权均值），更适合光照不均、图像局部变化大的情况。
# 3.4. 总结对比
| 方法 | 自动阈值？ | 适用场景 | 优缺点 |
|---|---|---|---|
threshold()（手动） |
❌ | 简单图像，用户可设定阈值 | 简单，但需要经验 |
threshold() + OTSU |
✅ | 背景均匀、前后景对比明显 | 自动化、全局处理 |
threshold() + TRIANGLE |
✅ | 直方图单峰 | 自动化，适用于特殊图像 |
adaptiveThreshold() |
✅（局部） | 光照不均、复杂背景 | 更强鲁棒性，但慢一些 |
# 4. 形态学操作：腐蚀 + 膨胀(高斯模糊滤波)
# 0.1高斯模糊
1.高斯模糊是加权模糊，越靠近中心权重越大，更自然。
2.比均值滤波更保留边缘信息，更适合图像处理中的前处理。
3.降低图像细节、去除高频噪声、平滑图像边缘
# 0 .2Canny边缘检测
| 参数名 | 说明 |
|---|---|
image |
输入图像（必须是灰度图） |
threshold1 |
较小的阈值（边缘开始跟踪的起点） |
threshold2 |
较大的阈值（强边缘判定的阈值） |
1.如果某像素的梯度 ≥ 150：认为是“强边缘”;
2.如果介于 50 到 150：认为是“弱边缘”，但如果连着强边缘，就也保留;
3.如果 < 50：抛弃.
# 参数调试建议：
- 开始用
(50, 150)比较稳健； - 若图像亮度较暗或边缘不明显，可尝试
(30, 100)； - 边缘太多太杂，可提高阈值，如
(100, 200)；
# # 高斯模糊滤波
# gs_img = cv2.GaussianBlur(binary_img, (5, 5), 0)
# # Canny 边缘检测算法
# edges = cv2.Canny(blurred, 50, 150) #cv2.Canny(image, threshold1, threshold2)
# 创建腐蚀膨胀卷积核，3*3矩形对二值化图像进行先膨胀再腐蚀(矩形,大小)
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) #必须这样写
#腐蚀
erode_img = cv2.erode(binary_img, kernel, iterations=1)
#膨胀
dilate_img = cv2.dilate(erode_img, kernel, iterations=2)# 1.腐蚀：去除小噪点
-
腐蚀会让白色区域变小，黑色部分“侵蚀”白色边缘
-
用于去除小的白色噪点或孤立白斑，断开小连接。
-
iterations=1表示腐蚀一次。# 2.膨胀：填补断裂区域
-
膨胀会让白色区域变大，用于填补裂缝、连接断裂部分。
-
这里用了
iterations=2，相当于膨胀两次，比腐蚀多，是为了弥补腐蚀“吃掉”的区域，同时增强线条。顺序是：先腐蚀再膨胀，称为开运算，用于去噪。
def line_preprocess(origin_img):
"""
车道线预处理函数
将BGR图像转换为HSV空间，应用颜色阈值，进行形态学操作
返回二值化后的车道线图像
"""
# opencv读取就是BGR 转HSV
hsv = cv2.cvtColor(origin_img, cv2.COLOR_BGR2HSV)
# blur_frame = cv2.medianBlur(img, 7) #中值滤波,7是卷积核
# hsv_image = cv2.cvtColor(blur_frame, cv2.COLOR_BGR2HSV)
# h = hsv[:,:,0]
# s = hsv[:,:,1]
# v = hsv[:,:,2]
# ev = cv2.equalizeHist(v)
# new_hsv = np.stack((h,s,ev),axis=2)
# 把背景滤掉，里面的数字是需要留下的颜色的HSV的色域，需要自己找出来
lower_color = np.array([H_min, S_min, V_min])
higher_color = np.array([H_max, S_max, V_max])
# 滤去背景留下车道，二值化，把大于大的小于小的滤去
binary_img = cv2.inRange(hsv, lower_color, higher_color)
# # 高斯模糊滤波
# gs_img = cv2.GaussianBlur(binary_img, (5, 5), 0)
# 创建腐蚀膨胀核，3*3矩形对二值化图像进行先膨胀再腐蚀
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
# 腐蚀
erode_img = cv2.erode(binary_img, kernel,iterations=1)
# 膨胀
dilate_img = cv2.dilate(erode_img, kernel,iterations=2)
# 获取边界
# line_img=cv2.Canny(erode_img)
# cv2.imshow("frame",dilate_img)
# 返回获取的车道图像
return dilate_img """
交通元素预处理函数
针对特定交通元素（如红绿灯、人行道等）进行预处理和轮廓检测
返回元素的最大轮廓面积
"""
blur_frame = cv2.medianBlur(img, 7) #中值滤波
hsv_image = cv2.cvtColor(blur_frame, cv2.COLOR_BGR2HSV)
binary_img = cv2.inRange(hsv_image, HSV_Threshold[element]['L'], HSV_Threshold[element]['H'])
#...同上
# 轮廓检测,只对二值图像有效
contours, h = cv2.findContours(dilate_img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
# 这就是获取轮廓的数量，也就是目标的数量
len_cont = len(contours)
element_area = 0
if len(contours) > 0:
max = max(contours, key=cv2.contourArea)
contours_img = cv2.drawContours(img, max, -1, (0, 255, 0), 3)#-1 表示画整个轮廓（而不是某个点）,3 是线宽
cv2.imshow(element,contours_img)
element_area = cv2.contourArea(max)
# 返回元素的最大轮廓面积
return element_area# 参数解释：
cv2.findContours(image, mode, method)image: 输入的二值图像（0 和 255）mode: 提取模式cv2.RETR_EXTERNAL: 只提取最外层轮廓（常用于你当前的目标提取）cv2.RETR_TREE: 提取所有轮廓，并构建轮廓层级结构
method: 轮廓近似方法cv2.CHAIN_APPROX_NONE: 保留所有边界点（适合精确形状分析）cv2.CHAIN_APPROX_SIMPLE: 只保留拐点（更节省内存）
# 返回值：
contours, hierarchy = cv2.findContours(...)contours: 是一个轮廓列表，每个轮廓是一个点集np.array（形状[n, 1, 2]）h或hierarchy: 层级信息（在RETR_TREE模式下更有意义）
# 二.霍夫变换
# 将BGR图像转换为HSV颜色空间
hsv = cv2.cvtColor(origin_img, cv2.COLOR_BGR2HSV)
# 定义颜色阈值范围
lower_color = np.array([H_min, S_min, V_min])
higher_color = np.array([H_max, S_max, V_max])
# 应用颜色阈值，创建二值图像
binary_img = cv2.inRange(hsv, lower_color, higher_color)
# 创建形态学操作核
kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
# 先进行腐蚀操作去除噪声
erode_img = cv2.erode(binary_img, kernel, iterations=1)
# 再进行膨胀操作连接断开的线段
dilate_img = cv2.dilate(erode_img, kernel, iterations=2)def HoughLines(line_roi):
"""
霍夫变换检测直线
处理车道线图像，筛选左右车道线，进行最小二乘拟合
返回左右车道线的端点坐标和拟合参数
"""
#cv2.HoughLinesP()：基于概率霍夫变换检测直线段。
#rho, theta, threshold 等是参数
#返回结果是 lines = [[[x1,y1,x2,y2]], ...]
lines = cv2.HoughLinesP(line_roi, rho, theta,threshold,minLineLength=min_line_lenght, maxLineGap=max_line_gap)
#定义输出结果容器
left_results, right_results, left_fit, right_fit = [], [], [], []
#斜率与位置筛选左右线段
if lines is not None:
left_lines, right_lines = [], []
for line in lines:
for x1, y1, x2, y2 in line:
if y1 != y2:
k = float(x1 - x2) / (y1 - y2) # 计算斜率
# 斜率筛选
if slope_max > abs(k) > slope_min:
if k > 0: # 右车道线
# 特殊场景下的筛选条件
if (k > 0.2 and clean_right_flag == False) or (k > 0.2 and change_flag == False and change_flag_2 == False):
continue
# 位置筛选
if (x1 > x2 and x1 < 120) or (x2 > x1 and x2 < 120):
continue
right_lines.append(line)
elif k < 0: # 左车道线
# 特殊场景下的筛选条件
if k > -1.4 and change_flag == False and change_flag_2 == False:
continue
# 位置筛选
if (x1 > x2 and x2 > 120) or (x2 > x1 and x1 > 120):
continue
left_lines.append(line)
# 左车道线处理
if len(left_lines) > 0:
clean_lines(left_lines, 0.1) # 清理异常线段
left_points = [(x1, y1) for line in left_lines for x1, y1, x2, y2 in line]
left_points += [(x2, y2) for line in left_lines for x1, y1, x2, y2 in line]
ymax, ymin = find_points(left_lines)
left_results, left_fit = least_squares_fit(left_points, ymin, ymax)
# 右车道线处理
if len(right_lines) > 0:
clean_lines(right_lines, 0.1)
right_points = [(x1, y1) for line in right_lines for x1, y1, x2, y2 in line]
right_points += [(x2, y2) for line in right_lines for x1, y1, x2, y2 in line]
ymax, ymin = find_points(right_lines)
right_results, right_fit = least_squares_fit(right_points, ymin, ymax)
return left_results, right_results, left_fit, right_fitdef least_squares_fit(point_list, ymin, ymax):
"""最小二乘法拟合车道线"""
x = [p[0] for p in point_list]
y = [p[1] for p in point_list]
fit = np.polyfit(y, x, 1) # 一阶多项式拟合
fit_fn = np.poly1d(fit) # 创建拟合函数
xmin = int(fit_fn(ymin)) # 计算y最小处的x值
xmax = int(fit_fn(ymax)) # 计算y最大处的x值
return [(xmin, ymin), (xmax, ymax)], fitdef least_squares_fit(point_list, ymin, ymax):
"""最小二乘法拟合车道线"""
x = [p[0] for p in point_list]
y = [p[1] for p in point_list]
fit = np.polyfit(y, x, 1) # 一阶多项式拟合
fit_fn = np.poly1d(fit) # 创建拟合函数
xmin = int(fit_fn(ymin)) # 计算y最小处的x值
xmax = int(fit_fn(ymax)) # 计算y最大处的x值
return [(xmin, ymin), (xmax, ymax)], fitdef how_to_turn(shape, left_results, right_results, left_fit, right_fit):
"""
计算转向偏差
基于检测到的车道线计算车辆中心与道路中心的偏差
返回转向控制量（error）
"""
height, width = shape[0], shape[1]
middle_line = width / 2
global road_width, start_cnt, turn_left_flag, turn_right_flag, change_flag_3, change_flag_4
# 双车道线都存在的情况
if left_results and right_results:
# 初始化阶段计算平均路宽
if start_cnt < 5 and EN == 1:
start_cnt += 1
road_width += (np.mean([right_results[1][0], right_results[0][0]]) -
np.mean([left_results[1][0], left_results[0][0]]))
if start_cnt == 5:
road_width /= 5
# 计算道路中心线
X = (left_fit[0] * height/2 + left_fit[1] +
right_fit[0] * height/2 + right_fit[1]) / 2
# 使用顶部点修正中心线
if abs(left_results[1][1] - right_results[1][1]) < 20:
X1 = (left_results[1][0] + right_results[1][0]) / 2
X = (X + X1) / 2
middle_line = X
rho = -(middle_line - width / 2) # 计算偏差
# 只有右车道线（需要左转）
elif not left_results and right_results:
if start_cnt != 5: # 未初始化时使用默认路宽
road_width = 170 if (not turn_flag and side_walk_flag) else 140
# 外推车道线底部位置
X_top = right_results[0][0]
X_bottom = (height - right_results[0][1]) * (right_results[0][0] - right_results[1][0]) / (
right_results[0][1] - right_results[1][1]) + right_results[0][0]
X = (X_top + X_bottom) / 2
middle_line = X - road_width / 2
rho = width / 2 - middle_line # 计算偏差
rho = abs(rho)
# 变道场景的特殊处理
if not change_flag_3 and change_flag_4:
rho = 45
# 只有左车道线（需要右转）
elif not right_results and left_results:
if start_cnt != 5:
road_width = 170 if (not turn_flag and side_walk_flag) else 140
# 外推车道线底部位置
X_top = left_results[0][0]
X_bottom = (height - left_results[0][1]) * (left_results[0][0] - left_results[1][0]) / (
left_results[0][1] - left_results[1][1]) + left_results[0][0]
X = (X_top + X_bottom) / 2
middle_line = X + road_width / 2
rho = width / 2 - middle_line
rho = abs(rho)
rho = -rho # 负值表示右转
# 无线道线的情况
else:
rho = 0
# 基于转弯状态的特殊处理
if not turn_left_flag and turn_flag:
rho = 100 # 强制左转
elif not turn_right_flag and turn_flag:
rho = -100 # 强制右转
# 危险区域的特殊转向逻辑
if not danger_flag and danger_flag_2:
rho = 100
elif not danger_flag and danger_flag_3 and not danger_flag_2:
rho = 0
return rhodef speed_control(cls_id, area):
"""
速度控制状态机
基于YOLO检测结果和当前状态标志调整车速
返回计算出的速度值
"""
# 全局状态标志声明（略，详见完整代码）
global side_walk_flag, speed_limit_flag, speed_flag, turn_left_flag, turn_right_flag
global stop_time, change_time, change_flag, change_flag_2, change_flag_3, change_flag_4
global turn_flag, turn_flag_2, red_light_flag, red_light_area, green_light_flag, green_light_area
global danger_flag, danger_flag_2, danger_flag_3, danger_time, speed_4_flag, yolo_flag
global traffic_flag, traffic_flag_2, speed, clean_right_flag, clean_time, clean_flag
global delayed_stop_flag, delayed_stop_time, doll_flag, last_stop_time
# 状态转换逻辑
# 左转标志检测
if turn_left_flag and turn_right_flag and int(cls_id) == 4 and area > 650:
turn_left_flag = False
turn_time[0] = time.time()
# 右转标志检测
elif turn_right_flag and turn_left_flag and int(cls_id) == 5 and area > 650:
turn_right_flag = False
turn_time[0] = time.time()
# 人行道标志检测
elif side_walk_flag and int(cls_id) == 1 and not turn_flag and area >= 1000:
side_walk_flag = False
stop_time[0] = time.time()
side_time[0] = time.time()
# 限速标志检测
elif speed_limit_flag and int(cls_id) == 3 and not side_walk_flag and area > 780:
speed_limit_flag = False
clean_right_flag = False # 清除右边线
speed_4_flag = True
clean_flag = False
clean_time[0] = time.time()
# 解除限速标志检测
elif speed_flag and int(cls_id) == 2 and not speed_limit_flag and area > 850:
speed_flag = False
# 红绿灯状态处理
elif traffic_flag and not traffic_flag_2:
# 红灯检测
if not speed_flag and red_light_flag and (red_light_area > green_light_area+100 and red_light_area > 450):
red_light_flag = False
delayed_stop_time = time.time() + 0.45 # 设置延迟停车时间
traffic_time[0] = time.time()
# 绿灯检测
elif not red_light_flag and green_light_flag and (green_light_area > red_light_area+100 and green_light_area > 350):
green_light_flag = False
traffic_flag = False
yolo_flag = True
# 超时处理
elif traffic_time[1] - traffic_time[0] > 5:
traffic_flag = False
yolo_flag = True
# 变道标志检测
elif not traffic_flag and change_flag and int(cls_id) == 6 and area > 370:
change_flag = False
change_time[0] = time.time()
# 危险标志检测
elif not change_flag and danger_flag and int(cls_id) == 0 and area > 370:
danger_flag = False
danger_time[0] = time.time()
# 速度控制逻辑
if stop_time[0] != -1: # 停车状态
speed = 0.0
stop_time[1] = time.time()
if stop_time[0] == -1 or stop_time[1] - stop_time[0] > 2: # 非停车状态
# 根据不同场景设置速度
if speed_4_flag: # 限速区域
speed = 0.28
if not speed_flag:
speed_4_flag = False
# 其他场景的速度设置（略，详见完整代码）
# ...
stop_time = [-1, -1] # 重置停车状态
# 状态超时处理
# 转弯超时
if turn_time[0] != 0 and (not turn_left_flag or not turn_right_flag):
turn_time[1] = time.time()
if turn_time[1] - turn_time[0] > 1.2:
turn_flag = False
if turn_time[1] - turn_time[0] > 4.2:
turn_flag_2 = False
# 变道超时
if change_time[0] != 0 and not change_flag:
change_time[1] = time.time()
if change_time[1] - change_time[0] > 1.7:
change_flag_2 = True
if change_time[1] - change_time[0] > 3.8:
change_flag_3 = False
if change_time[1] - change_time[0] > 4.5:
change_flag_4 = False
# 其他状态超时处理（略）
# ...
return speeddef speed_control(cls_id, area):
"""
速度控制状态机
基于YOLO检测结果和当前状态标志调整车速
返回计算出的速度值
"""
# 全局状态标志声明（略，详见完整代码）
global side_walk_flag, speed_limit_flag, speed_flag, turn_left_flag, turn_right_flag
global stop_time, change_time, change_flag, change_flag_2, change_flag_3, change_flag_4
global turn_flag, turn_flag_2, red_light_flag, red_light_area, green_light_flag, green_light_area
global danger_flag, danger_flag_2, danger_flag_3, danger_time, speed_4_flag, yolo_flag
global traffic_flag, traffic_flag_2, speed, clean_right_flag, clean_time, clean_flag
global delayed_stop_flag, delayed_stop_time, doll_flag, last_stop_time
# 状态转换逻辑
# 左转标志检测
if turn_left_flag and turn_right_flag and int(cls_id) == 4 and area > 650:
turn_left_flag = False
turn_time[0] = time.time()
# 右转标志检测
elif turn_right_flag and turn_left_flag and int(cls_id) == 5 and area > 650:
turn_right_flag = False
turn_time[0] = time.time()
# 人行道标志检测
elif side_walk_flag and int(cls_id) == 1 and not turn_flag and area >= 1000:
side_walk_flag = False
stop_time[0] = time.time()
side_time[0] = time.time()
# 限速标志检测
elif speed_limit_flag and int(cls_id) == 3 and not side_walk_flag and area > 780:
speed_limit_flag = False
clean_right_flag = False # 清除右边线
speed_4_flag = True
clean_flag = False
clean_time[0] = time.time()
# 解除限速标志检测
elif speed_flag and int(cls_id) == 2 and not speed_limit_flag and area > 850:
speed_flag = False
# 红绿灯状态处理
elif traffic_flag and not traffic_flag_2:
# 红灯检测
if not speed_flag and red_light_flag and (red_light_area > green_light_area+100 and red_light_area > 450):
red_light_flag = False
delayed_stop_time = time.time() + 0.45 # 设置延迟停车时间
traffic_time[0] = time.time()
# 绿灯检测
elif not red_light_flag and green_light_flag and (green_light_area > red_light_area+100 and green_light_area > 350):
green_light_flag = False
traffic_flag = False
yolo_flag = True
# 超时处理
elif traffic_time[1] - traffic_time[0] > 5:
traffic_flag = False
yolo_flag = True
# 变道标志检测
elif not traffic_flag and change_flag and int(cls_id) == 6 and area > 370:
change_flag = False
change_time[0] = time.time()
# 危险标志检测
elif not change_flag and danger_flag and int(cls_id) == 0 and area > 370:
danger_flag = False
danger_time[0] = time.time()
# 速度控制逻辑
if stop_time[0] != -1: # 停车状态
speed = 0.0
stop_time[1] = time.time()
if stop_time[0] == -1 or stop_time[1] - stop_time[0] > 2: # 非停车状态
# 根据不同场景设置速度
if speed_4_flag: # 限速区域
speed = 0.28
if not speed_flag:
speed_4_flag = False
# 其他场景的速度设置（略，详见完整代码）
# ...
stop_time = [-1, -1] # 重置停车状态
# 状态超时处理
# 转弯超时
if turn_time[0] != 0 and (not turn_left_flag or not turn_right_flag):
turn_time[1] = time.time()
if turn_time[1] - turn_time[0] > 1.2:
turn_flag = False
if turn_time[1] - turn_time[0] > 4.2:
turn_flag_2 = False
# 变道超时
if change_time[0] != 0 and not change_flag:
change_time[1] = time.time()
if change_time[1] - change_time[0] > 1.7:
change_flag_2 = True
if change_time[1] - change_time[0] > 3.8:
change_flag_3 = False
if change_time[1] - change_time[0] > 4.5:
change_flag_4 = False
# 其他状态超时处理（略）
# ...
return speeddef speed_control(cls_id, area):
"""
速度控制状态机
基于YOLO检测结果和当前状态标志调整车速
返回计算出的速度值
"""
# 全局状态标志声明（略，详见完整代码）
global side_walk_flag, speed_limit_flag, speed_flag, turn_left_flag, turn_right_flag
global stop_time, change_time, change_flag, change_flag_2, change_flag_3, change_flag_4
global turn_flag, turn_flag_2, red_light_flag, red_light_area, green_light_flag, green_light_area
global danger_flag, danger_flag_2, danger_flag_3, danger_time, speed_4_flag, yolo_flag
global traffic_flag, traffic_flag_2, speed, clean_right_flag, clean_time, clean_flag
global delayed_stop_flag, delayed_stop_time, doll_flag, last_stop_time
# 状态转换逻辑
# 左转标志检测
if turn_left_flag and turn_right_flag and int(cls_id) == 4 and area > 650:
turn_left_flag = False
turn_time[0] = time.time()
# 右转标志检测
elif turn_right_flag and turn_left_flag and int(cls_id) == 5 and area > 650:
turn_right_flag = False
turn_time[0] = time.time()
# 人行道标志检测
elif side_walk_flag and int(cls_id) == 1 and not turn_flag and area >= 1000:
side_walk_flag = False
stop_time[0] = time.time()
side_time[0] = time.time()
# 限速标志检测
elif speed_limit_flag and int(cls_id) == 3 and not side_walk_flag and area > 780:
speed_limit_flag = False
clean_right_flag = False # 清除右边线
speed_4_flag = True
clean_flag = False
clean_time[0] = time.time()
# 解除限速标志检测
elif speed_flag and int(cls_id) == 2 and not speed_limit_flag and area > 850:
speed_flag = False
# 红绿灯状态处理
elif traffic_flag and not traffic_flag_2:
# 红灯检测
if not speed_flag and red_light_flag and (red_light_area > green_light_area+100 and red_light_area > 450):
red_light_flag = False
delayed_stop_time = time.time() + 0.45 # 设置延迟停车时间
traffic_time[0] = time.time()
# 绿灯检测
elif not red_light_flag and green_light_flag and (green_light_area > red_light_area+100 and green_light_area > 350):
green_light_flag = False
traffic_flag = False
yolo_flag = True
# 超时处理
elif traffic_time[1] - traffic_time[0] > 5:
traffic_flag = False
yolo_flag = True
# 变道标志检测
elif not traffic_flag and change_flag and int(cls_id) == 6 and area > 370:
change_flag = False
change_time[0] = time.time()
# 危险标志检测
elif not change_flag and danger_flag and int(cls_id) == 0 and area > 370:
danger_flag = False
danger_time[0] = time.time()
# 速度控制逻辑
if stop_time[0] != -1: # 停车状态
speed = 0.0
stop_time[1] = time.time()
if stop_time[0] == -1 or stop_time[1] - stop_time[0] > 2: # 非停车状态
# 根据不同场景设置速度
if speed_4_flag: # 限速区域
speed = 0.28
if not speed_flag:
speed_4_flag = False
# 其他场景的速度设置（略，详见完整代码）
# ...
stop_time = [-1, -1] # 重置停车状态
# 状态超时处理
# 转弯超时
if turn_time[0] != 0 and (not turn_left_flag or not turn_right_flag):
turn_time[1] = time.time()
if turn_time[1] - turn_time[0] > 1.2:
turn_flag = False
if turn_time[1] - turn_time[0] > 4.2:
turn_flag_2 = False
# 变道超时
if change_time[0] != 0 and not change_flag:
change_time[1] = time.time()
if change_time[1] - change_time[0] > 1.7:
change_flag_2 = True
if change_time[1] - change_time[0] > 3.8:
change_flag_3 = False
if change_time[1] - change_time[0] > 4.5:
change_flag_4 = False
# 其他状态超时处理（略）
# ...
return speed