Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

新增go_straight_until_bar函数以控制机器人沿直线行走至检测到栏杆；优化follow_left_side_track函数的PID参数和稳定性要求，增强响应速度和稳定性；调整detect_left_side_track函数的搜索区域和评分逻辑，以提高线段检测的准确性和灵活性。

Browse Source
This commit is contained in:
Havoc 2025-05-25 19:55:35 +08:00
parent 009b8e616a
commit e77d5655ae
8 changed files with 109 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
base_move/go_straight.py
View File

@ -196,6 +196,12 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False):
return go_success return go_success
def go_straight_until_bar(ctrl, msg, distance, speed=0.5, observe=False):
"""
控制机器人沿直线行走指定距离，直到检测到栏杆
"""
pass
# 用法示例 # 用法示例
if __name__ == "__main__": if __name__ == "__main__":
# 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象 # 这里是示例代码，实际使用时需要提供合适的ctrl和msg对象

71
base_move/move_base_hori_line.py
View File

@ -1224,13 +1224,13 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False): def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=30, observe=False):
""" """
控制机器狗向左侧移动并靠近左侧的黄色轨迹线 控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进
参数: 参数:
ctrl: Robot_Ctrl 对象，包含里程计信息 ctrl: Robot_Ctrl 对象，包含里程计信息
msg: robot_control_cmd_lcmt 对象，用于发送命令 msg: robot_control_cmd_lcmt 对象，用于发送命令
target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距） target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距）
speed: 移动速度(米/秒)，默认为0.3米/秒 speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
max_time: 最大执行时间(秒)，默认为30秒 max_time: 最大执行时间(秒)，默认为30秒
observe: 是否输出中间状态信息和可视化结果，默认为False observe: 是否输出中间状态信息和可视化结果，默认为False
@ -1256,22 +1256,22 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
if hasattr(ctrl, 'place_marker'): if hasattr(ctrl, 'place_marker'):
ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True) ctrl.place_marker(start_position[0], start_position[1], start_position[2] if len(start_position) > 2 else 0.0, 'green', observe=True)
# PID控制参数 - 侧向移动的PID参数 # PID控制参数 - 侧向移动的PID参数，根据近距离跟踪需求调整
kp_side = 0.002 # 比例系数 kp_side = 0.0025 # 比例系数
ki_side = 0.0001 # 积分系数 ki_side = 0.0001 # 积分系数
kd_side = 0.0005 # 微分系数 kd_side = 0.001 # 微分系数
# 记录目标到达状态和稳定计数 # 记录目标到达状态和稳定计数
target_reached = False target_reached = False
stable_count = 0 stable_count = 0
required_stable_count = 10 # 需要连续稳定的检测次数 required_stable_count = 8 # 需要连续稳定的检测次数
# PID控制变量 # PID控制变量
previous_error = 0 previous_error = 0
integral = 0 integral = 0
# 最大侧向速度限制 # 最大侧向速度限制
max_side_velocity = 0.3 # m/s max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度
# 检测成功计数器 # 检测成功计数器
detection_success_count = 0 detection_success_count = 0
@ -1284,6 +1284,9 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
filter_size = 5 filter_size = 5
distance_queue = [] distance_queue = []
# 动态调整目标距离的阈值
target_reached_threshold = 25 # 在目标距离±25像素范围内视为达到目标
# 开始跟踪循环 # 开始跟踪循环
while time.time() - start_time < max_time: while time.time() - start_time < max_time:
# 获取当前图像 # 获取当前图像
@ -1321,7 +1324,7 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
error = target_distance - filtered_distance error = target_distance - filtered_distance
# 检查是否达到目标位置 # 检查是否达到目标位置
if abs(error) < 20: # 误差小于20像素视为达到目标 if abs(error) < target_reached_threshold: # 误差小于阈值视为达到目标
stable_count += 1 stable_count += 1
if stable_count >= required_stable_count: if stable_count >= required_stable_count:
if not target_reached: if not target_reached:
@ -1348,38 +1351,41 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
# 计算侧向速度 # 计算侧向速度
side_velocity = p_control + i_control + d_control side_velocity = p_control + i_control + d_control
# 应用非线性控制，加快大距离靠近，减缓小距离微调
if abs(error) > 100:
# 大误差时，增强响应
side_velocity *= 1.2
elif abs(error) < 40:
# 小误差时，减弱响应，增加稳定性
side_velocity *= 0.7
# 限制侧向速度范围 # 限制侧向速度范围
side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity)) side_velocity = max(-max_side_velocity, min(max_side_velocity, side_velocity))
# 根据目标状态调整速度 # 如果已达到目标，进一步降低侧向速度以减少振荡
forward_speed = speed
if target_reached: if target_reached:
# 如果已达到目标，降低侧向速度以减少振荡 side_velocity *= 0.4
side_velocity *= 0.5
# 降低前进速度让侧向移动更平稳
forward_speed *= 0.7
if observe and time.time() % 0.5 < 0.02: if observe and time.time() % 0.5 < 0.02:
debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制") debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制")
debug(f"控制指令: 前进={forward_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "速度") debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度")
# 设置速度命令 - [前进速度, 侧向速度, 角速度] # 设置速度命令 - [前进速度, 侧向速度, 角速度]
# 侧向速度为正表示向左移动，为负表示向右移动 # 侧向速度为正表示向左移动，为负表示向右移动
msg.vel_des = [forward_speed, side_velocity, 0] # 前进速度保持为0，只进行侧向移动
msg.vel_des = [0, side_velocity, 0]
# 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行 # 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行
# 斜率大于0表示向右倾斜，需要顺时针旋转(负角速度)
# 斜率小于0表示向左倾斜，需要逆时针旋转(正角速度)
if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正 if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正
slope = track_info["slope"] slope = track_info["slope"]
# 计算旋转角度 - 垂直线的ideal_angle应该是0 # 计算旋转角度 - 垂直线的ideal_angle应该是0
angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0 angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0
# 将角度校正应用为小的角速度 # 将角度校正应用为小的角速度
angular_velocity = angle_correction * 0.2 # 角速度控制系数 angular_velocity = angle_correction * 0.25
# 限制角速度范围 # 限制角速度范围
angular_velocity = max(-0.2, min(0.2, angular_velocity)) angular_velocity = max(-0.25, min(0.25, angular_velocity))
if abs(angular_velocity) > 0.02: # 只在需要明显校正时应用 if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用
msg.vel_des[2] = angular_velocity msg.vel_des[2] = angular_velocity
if observe and time.time() % 0.5 < 0.02: if observe and time.time() % 0.5 < 0.02:
debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正") debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正")
@ -1389,9 +1395,6 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
# 如果之前有有效检测结果，使用上一次的控制值但降低强度 # 如果之前有有效检测结果，使用上一次的控制值但降低强度
if last_valid_track_info is not None and len(distance_queue) > 0: if last_valid_track_info is not None and len(distance_queue) > 0:
# 使用上一次的距离，但降低侧向速度
reduced_speed = speed * 0.5 # 降低前进速度
# 如果上次距离已经接近目标，维持当前位置 # 如果上次距离已经接近目标，维持当前位置
last_error = target_distance - distance_queue[-1] last_error = target_distance - distance_queue[-1]
if abs(last_error) < 50: # 如果接近目标距离 if abs(last_error) < 50: # 如果接近目标距离
@ -1400,19 +1403,19 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
info("接近目标位置，保持当前侧向位置", "保持") info("接近目标位置，保持当前侧向位置", "保持")
else: else:
# 向目标方向缓慢移动 # 向目标方向缓慢移动
side_velocity = 0.05 * (1 if last_error > 0 else -1) side_velocity = 0.08 * (1 if last_error > 0 else -1)
if observe: if observe:
info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整") info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
msg.vel_des = [reduced_speed, side_velocity, 0] msg.vel_des = [0, side_velocity, 0]
if observe: if observe:
warning(f"使用降低强度的控制: 前进={reduced_speed:.2f}m/s, 侧向={side_velocity:.2f}m/s", "恢复") warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复")
else: else:
# 如果从未检测到轨迹线，降低速度直接向前 # 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线
reduced_speed = speed * 0.3 default_side_velocity = 0.1
msg.vel_des = [reduced_speed, 0, 0] msg.vel_des = [0, default_side_velocity, 0]
if observe: if observe:
warning(f"无法检测到左侧轨迹线，降低速度前进: {reduced_speed:.2f}m/s", "警告") warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
# 发送命令 # 发送命令
msg.life_count += 1 msg.life_count += 1
@ -1438,11 +1441,11 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
if observe: if observe:
info("开始平滑停止", "停止") info("开始平滑停止", "停止")
# 先降低速度再停止，实现平滑停止 # 平滑停止 - 由于没有前进速度，只需要停止侧向移动
slowdown_steps = 5 slowdown_steps = 3
for i in range(slowdown_steps, 0, -1): for i in range(slowdown_steps, 0, -1):
slowdown_factor = i / slowdown_steps slowdown_factor = i / slowdown_steps
msg.vel_des = [speed * slowdown_factor, 0, 0] msg.vel_des = [0, 0, 0] # 完全停止所有移动
msg.life_count += 1 msg.life_count += 1
ctrl.Send_cmd(msg) ctrl.Send_cmd(msg)
time.sleep(0.1) time.sleep(0.1)

37
logs/robot_2025-05-25.log Normal file
View File

@ -0,0 +1,37 @@
2025-05-25 19:50:01 | DEBUG | utils.log_helper - 🐞 步骤1: 创建黄色掩码
2025-05-25 19:50:02 | DEBUG | utils.log_helper - 🐞 步骤1.5: 底部区域掩码
2025-05-25 19:50:03 | DEBUG | utils.log_helper - 🐞 步骤2: 边缘检测
2025-05-25 19:50:04 | DEBUG | utils.log_helper - 🐞 步骤3: 检测到 65 条直线
2025-05-25 19:50:05 | DEBUG | utils.log_helper - 🐞 步骤4: 找到 8 条垂直线
2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 保存双轨迹线检测结果图像到: logs/image/dual_track_20250525_195007_428255.jpg
2025-05-25 19:50:07 | INFO | utils.log_helper - ℹ️ 双轨迹线检测结果: {'timestamp': '20250525_195007_428255', 'center_point': (834, 1080), 'deviation': -126, 'left_track_mid_x': 397.0, 'right_track_mid_x': 1351.5, 'track_width': 954.5, 'center_slope': -2.8529411764705883}
2025-05-25 19:50:15 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
2025-05-25 19:50:16 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
2025-05-25 19:50:17 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
2025-05-25 19:50:18 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
2025-05-25 19:50:19 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
2025-05-25 19:50:20 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
2025-05-25 19:50:21 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195022_290308.jpg
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195022_290308.jpg
2025-05-25 19:50:22 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195022_290308', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
2025-05-25 19:52:13 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
2025-05-25 19:52:14 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
2025-05-25 19:52:15 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
2025-05-25 19:52:16 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
2025-05-25 19:52:17 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 74 条直线
2025-05-25 19:52:18 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线
2025-05-25 19:52:19 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195220_395525.jpg
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195220_395525.jpg
2025-05-25 19:52:20 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195220_395525', 'tracking_point': (549, 1071), 'ground_intersection': (543, 1080), 'distance_to_left': 584.5, 'slope': -1.619718309859155, 'line_mid_x': 584.5}
2025-05-25 19:52:26 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
2025-05-25 19:52:27 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
2025-05-25 19:52:28 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
2025-05-25 19:52:29 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
2025-05-25 19:52:31 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 79 条直线
2025-05-25 19:52:32 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
2025-05-25 19:52:33 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_195234_036434.jpg
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_195234_036434.jpg
2025-05-25 19:52:34 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_195234_036434', 'tracking_point': (133, 1077), 'ground_intersection': (128, 1080), 'distance_to_left': 246.0, 'slope': -0.7035398230088495, 'line_mid_x': 246.0}

BIN
res/path/test/left_track_results/result_image_20250513_162556.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 46 KiB

After

Width:  |  Height:  |  Size: 47 KiB

BIN
res/path/test/left_track_results/result_image_20250525_113607.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 64 KiB

BIN
res/path/test/left_track_results/result_image_20250525_113635.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 63 KiB

BIN
res/path/test/result_image_20250514_024347.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 59 KiB

53
utils/detect_track.py
View File

@ -1248,11 +1248,10 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
# 计算图像中间和左侧区域的范围 # 计算图像中间和左侧区域的范围
center_x = width // 2 center_x = width // 2
# 主要关注视野的左半部分 # 主要关注视野的左半部分，但稍微扩大一点以确保捕捉到左侧线
left_region_width = center_x left_region_width = int(center_x * 1.2) # 扩大左侧搜索区域
left_region_height = height
left_bound = 0 left_bound = 0
right_bound = center_x right_bound = min(width, left_region_width)
bottom_bound = height bottom_bound = height
top_bound = 0 top_bound = 0
@ -1268,17 +1267,17 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
# 转换到HSV颜色空间以便更容易提取黄色 # 转换到HSV颜色空间以便更容易提取黄色
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
# 黄色的HSV范围 - 扩大范围以更好地捕捉不同光照条件下的黄色 # 黄色的HSV范围 - 进一步扩大范围以适应不同光照条件
lower_yellow = np.array([15, 80, 80]) # 更宽松的黄色下限 lower_yellow = np.array([12, 70, 70]) # 更宽松的黄色下限
upper_yellow = np.array([35, 255, 255]) # 更宽松的黄色上限 upper_yellow = np.array([38, 255, 255]) # 更宽松的黄色上限
# 创建黄色的掩码 # 创建黄色的掩码
mask = cv2.inRange(hsv, lower_yellow, upper_yellow) mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
# 形态学操作以改善掩码 # 形态学操作以改善掩码
kernel = np.ones((5, 5), np.uint8) # 增大kernel尺寸 kernel = np.ones((5, 5), np.uint8)
mask = cv2.dilate(mask, kernel, iterations=1) mask = cv2.dilate(mask, kernel, iterations=1)
mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1) # 添加腐蚀操作去除噪点 mask = cv2.erode(mask, np.ones((3, 3), np.uint8), iterations=1)
if observe: if observe:
debug("步骤2: 创建黄色掩码", "处理") debug("步骤2: 创建黄色掩码", "处理")
@ -1301,9 +1300,9 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
cv2.imshow("边缘检测", edges) cv2.imshow("边缘检测", edges)
cv2.waitKey(delay) cv2.waitKey(delay)
# 霍夫变换检测直线 # 霍夫变换检测直线 - 降低minLineLength以更好地检测近距离的线段
lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=25, lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=20,
minLineLength=height*0.15, maxLineGap=50) # 调整参数以检测更长的线段 minLineLength=height*0.1, maxLineGap=60) # 调整参数以检测更短的线段
if lines is None or len(lines) == 0: if lines is None or len(lines) == 0:
error("未检测到直线", "失败") error("未检测到直线", "失败")
@ -1318,7 +1317,7 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
cv2.imshow("检测到的直线", lines_img) cv2.imshow("检测到的直线", lines_img)
cv2.waitKey(delay) cv2.waitKey(delay)
# 筛选左侧区域内的近似垂直线 # 筛选左侧区域内的近似垂直线，放宽条件以捕获更多可能的线
left_vertical_lines = [] left_vertical_lines = []
for line in lines: for line in lines:
x1, y1, x2, y2 = line[0] x1, y1, x2, y2 = line[0]
@ -1333,8 +1332,8 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
else: else:
slope = (y2 - y1) / (x2 - x1) slope = (y2 - y1) / (x2 - x1)
# 筛选接近垂直的线 (斜率较大) # 放宽垂直线的斜率范围，以适应近距离时线的倾斜
if abs(slope) > 0.7: # 设置较宽松的垂直线斜率阈值 if abs(slope) > 0.5: # 降低垂直线斜率阈值
line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2) line_length = np.sqrt((x2-x1)**2 + (y2-y1)**2)
# 计算线的中点坐标 # 计算线的中点坐标
mid_x = (x1 + x2) / 2 mid_x = (x1 + x2) / 2
@ -1360,15 +1359,24 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
cv2.imshow("左侧垂直线", left_lines_img) cv2.imshow("左侧垂直线", left_lines_img)
cv2.waitKey(delay) cv2.waitKey(delay)
# 按线段长度和位置进行评分，优先选择更长且更靠近图像左边的线 # 修改评分函数，优先选择最左侧的线
def score_left_line(line_info): def score_left_line(line_info):
_, mid_x, _, _, length = line_info _, mid_x, mid_y, slope, length = line_info
# 线段越长分数越高
# 线段越长分数越高，但降低权重
length_score = min(1.0, length / (height * 0.3)) length_score = min(1.0, length / (height * 0.3))
# 越靠近左边分数越高
# 越靠近左边分数越高，增加权重
position_score = 1.0 - (mid_x / center_x) position_score = 1.0 - (mid_x / center_x)
# 综合评分
return length_score * 0.7 + position_score * 0.3 # 优先选择在图像下半部分的线段
height_score = min(1.0, mid_y / (height * 0.6))
# 斜率得分，垂直度越高越好
slope_score = min(1.0, abs(slope) / 20) if abs(slope) < 100 else 1.0
# 综合评分，加大位置权重
return length_score * 0.2 + position_score * 0.5 + height_score * 0.2 + slope_score * 0.1
# 对线段进行评分并排序 # 对线段进行评分并排序
left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True) left_vertical_lines = sorted(left_vertical_lines, key=score_left_line, reverse=True)
@ -1387,9 +1395,6 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
tracking_point = (x2, y2) if y2 > y1 else (x1, y1) tracking_point = (x2, y2) if y2 > y1 else (x1, y1)
# 计算线与地面的交点 # 计算线与地面的交点
# 使用线段的方程: (y - y1) = slope * (x - x1)
# 地面对应图像底部: y = height
# 解这个方程得到交点的x坐标
if abs(slope) < 0.01: # 几乎垂直 if abs(slope) < 0.01: # 几乎垂直
ground_intersection_x = x1 ground_intersection_x = x1
else: else: