优化follow_left_side_track函数，调整目标距离参数至540像素，增强稳定性和响应速度；更新滤波队列大小至7以提高稳定性；改进速度计算逻辑，确保更平滑的调整和停止过程；更新左侧轨迹线检测演示程序的输入路径。
This commit is contained in:
parent
e77d5655ae
commit
5fd630c27b
@ -1222,14 +1222,14 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
|
|||||||
|
|
||||||
return True
|
return True
|
||||||
|
|
||||||
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=540, 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: 目标与左侧线的像素距离，默认为540像素（考虑机器狗视角、高度的合适距离）
|
||||||
speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
|
speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
|
||||||
max_time: 最大执行时间(秒)，默认为30秒
|
max_time: 最大执行时间(秒)，默认为30秒
|
||||||
observe: 是否输出中间状态信息和可视化结果，默认为False
|
observe: 是否输出中间状态信息和可视化结果，默认为False
|
||||||
@ -1256,22 +1256,9 @@ 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参数，根据近距离跟踪需求调整
|
# 最大侧向速度限制与动态调整
|
||||||
kp_side = 0.0025 # 比例系数
|
max_side_velocity = speed
|
||||||
ki_side = 0.0001 # 积分系数
|
min_side_velocity = 0.05 # 最小侧向速度，确保能够缓慢移动
|
||||||
kd_side = 0.001 # 微分系数
|
|
||||||
|
|
||||||
# 记录目标到达状态和稳定计数
|
|
||||||
target_reached = False
|
|
||||||
stable_count = 0
|
|
||||||
required_stable_count = 8 # 需要连续稳定的检测次数
|
|
||||||
|
|
||||||
# PID控制变量
|
|
||||||
previous_error = 0
|
|
||||||
integral = 0
|
|
||||||
|
|
||||||
# 最大侧向速度限制
|
|
||||||
max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度
|
|
||||||
|
|
||||||
# 检测成功计数器
|
# 检测成功计数器
|
||||||
detection_success_count = 0
|
detection_success_count = 0
|
||||||
@ -1280,12 +1267,17 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
|||||||
# 保存上一次有效的检测结果，用于检测失败时的平滑过渡
|
# 保存上一次有效的检测结果，用于检测失败时的平滑过渡
|
||||||
last_valid_track_info = None
|
last_valid_track_info = None
|
||||||
|
|
||||||
# 滤波队列
|
# 滤波队列 - 增大滤波窗口以提高稳定性
|
||||||
filter_size = 5
|
filter_size = 7
|
||||||
distance_queue = []
|
distance_queue = []
|
||||||
|
|
||||||
# 动态调整目标距离的阈值
|
# 设置差值阈值 - 当距离与目标的差值小于此阈值时，认为已达到目标
|
||||||
target_reached_threshold = 25 # 在目标距离±25像素范围内视为达到目标
|
distance_threshold = 25 # 像素误差阈值
|
||||||
|
|
||||||
|
# 记录目标到达状态和稳定计数
|
||||||
|
target_reached = False
|
||||||
|
stable_count = 0
|
||||||
|
required_stable_count = 8 # 连续多帧满足条件才算稳定到达目标
|
||||||
|
|
||||||
# 开始跟踪循环
|
# 开始跟踪循环
|
||||||
while time.time() - start_time < max_time:
|
while time.time() - start_time < max_time:
|
||||||
@ -1310,64 +1302,69 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
|||||||
if len(distance_queue) > filter_size:
|
if len(distance_queue) > filter_size:
|
||||||
distance_queue.pop(0)
|
distance_queue.pop(0)
|
||||||
|
|
||||||
# 计算滤波后的距离值 (去除最大和最小值后的平均)
|
# 计算滤波后的距离值 - 使用中值滤波，对抗异常值
|
||||||
if len(distance_queue) >= 3:
|
if len(distance_queue) >= 3:
|
||||||
filtered_distances = sorted(distance_queue)[1:-1] if len(distance_queue) > 2 else distance_queue
|
# 中值滤波更能抵抗异常值
|
||||||
filtered_distance = sum(filtered_distances) / len(filtered_distances)
|
filtered_distance = sorted(distance_queue)[len(distance_queue) // 2]
|
||||||
else:
|
else:
|
||||||
filtered_distance = current_distance
|
filtered_distance = current_distance
|
||||||
|
|
||||||
|
# 直接计算当前距离与目标距离的差值
|
||||||
|
difference = target_distance - filtered_distance # 正值：需要向左移动；负值：需要向右移动
|
||||||
|
|
||||||
if observe and time.time() % 0.5 < 0.02:
|
if observe and time.time() % 0.5 < 0.02:
|
||||||
debug(f"原始左侧距离: {current_distance:.1f}px, 滤波后: {filtered_distance:.1f}px, 目标: {target_distance}px", "距离")
|
debug(f"左侧距离: {filtered_distance:.1f}px, 目标距离: {target_distance}px, 差值: {difference:.1f}px", "距离")
|
||||||
|
|
||||||
# 计算误差 - 正误差表示需要向左移动，负误差表示需要向右移动
|
# 检查是否达到目标位置（差值小于阈值）
|
||||||
error = target_distance - filtered_distance
|
if abs(difference) < distance_threshold:
|
||||||
|
|
||||||
# 检查是否达到目标位置
|
|
||||||
if abs(error) < target_reached_threshold: # 误差小于阈值视为达到目标
|
|
||||||
stable_count += 1
|
stable_count += 1
|
||||||
if stable_count >= required_stable_count:
|
if stable_count >= required_stable_count and not target_reached:
|
||||||
if not target_reached:
|
target_reached = True
|
||||||
target_reached = True
|
if observe:
|
||||||
if observe:
|
success(f"已达到目标位置，当前距离: {filtered_distance:.1f}px, 与目标差值: {abs(difference):.1f}px", "成功")
|
||||||
success(f"已达到目标位置，误差: {abs(error):.1f}px", "成功")
|
|
||||||
|
# 在目标位置附近时，使用非常小的速度进行微调
|
||||||
|
# 根据差值比例计算速度，使调整更平滑
|
||||||
|
adjustment_factor = difference / distance_threshold # 归一化到[-1,1]范围
|
||||||
|
side_velocity = min_side_velocity * adjustment_factor * 0.5 # 乘以0.5使调整更微小
|
||||||
|
|
||||||
|
# 如果计算出的速度非常小，直接设为0
|
||||||
|
if abs(side_velocity) < 0.01:
|
||||||
|
side_velocity = 0
|
||||||
|
|
||||||
|
if observe and time.time() % 0.5 < 0.02 and side_velocity != 0:
|
||||||
|
info(f"接近目标位置，微调: {side_velocity:.3f}m/s", "调整")
|
||||||
else:
|
else:
|
||||||
|
# 不在目标位置，根据差值计算速度
|
||||||
stable_count = 0
|
stable_count = 0
|
||||||
target_reached = False
|
target_reached = False
|
||||||
|
|
||||||
|
# 计算速度系数 - 差值越大，速度越大，但有上限
|
||||||
|
# 使用非线性映射，使速度变化更平滑
|
||||||
|
if abs(difference) > 200:
|
||||||
|
# 差值大于200像素时，使用最大速度
|
||||||
|
speed_factor = 1.0
|
||||||
|
else:
|
||||||
|
# 差值在0-200像素之间，速度从0增长到最大
|
||||||
|
speed_factor = min(1.0, abs(difference) / 200.0)
|
||||||
|
# 应用平方根函数使速度增长曲线更合理（开始缓慢，后面加速）
|
||||||
|
speed_factor = math.sqrt(speed_factor)
|
||||||
|
|
||||||
|
# 应用方向（差值为正向左移动，为负向右移动）
|
||||||
|
side_velocity = max_side_velocity * speed_factor * (1 if difference > 0 else -1)
|
||||||
|
|
||||||
|
if observe and time.time() % 0.5 < 0.02:
|
||||||
|
direction = "左" if side_velocity > 0 else "右"
|
||||||
|
info(f"距离目标较{direction}远({difference:.1f}px)，向{direction}移动: {side_velocity:.3f}m/s", "调整")
|
||||||
|
|
||||||
# 比例项
|
# 确保最小移动速度 - 当需要移动但计算值太小时
|
||||||
p_control = kp_side * error
|
if 0 < abs(side_velocity) < min_side_velocity:
|
||||||
|
side_velocity = min_side_velocity * (1 if side_velocity > 0 else -1)
|
||||||
# 积分项 (添加积分限制以防止积分饱和)
|
|
||||||
integral += error
|
|
||||||
integral = max(-500, min(500, integral)) # 限制积分范围
|
|
||||||
i_control = ki_side * integral
|
|
||||||
|
|
||||||
# 微分项
|
|
||||||
derivative = error - previous_error
|
|
||||||
d_control = kd_side * derivative
|
|
||||||
previous_error = error
|
|
||||||
|
|
||||||
# 计算侧向速度
|
|
||||||
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))
|
||||||
|
|
||||||
# 如果已达到目标，进一步降低侧向速度以减少振荡
|
|
||||||
if target_reached:
|
|
||||||
side_velocity *= 0.4
|
|
||||||
|
|
||||||
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"控制指令: 侧向={side_velocity:.2f}m/s", "速度")
|
debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度")
|
||||||
|
|
||||||
# 设置速度命令 - [前进速度, 侧向速度, 角速度]
|
# 设置速度命令 - [前进速度, 侧向速度, 角速度]
|
||||||
@ -1380,10 +1377,13 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
|||||||
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.25
|
angle_factor = 1.0
|
||||||
|
if target_reached:
|
||||||
|
angle_factor = 0.5 # 接近目标时减弱角度校正
|
||||||
|
angular_velocity = angle_correction * 0.2 * angle_factor
|
||||||
# 限制角速度范围
|
# 限制角速度范围
|
||||||
angular_velocity = max(-0.25, min(0.25, angular_velocity))
|
angular_velocity = max(-0.2, min(0.2, angular_velocity))
|
||||||
|
|
||||||
if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用
|
if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用
|
||||||
msg.vel_des[2] = angular_velocity
|
msg.vel_des[2] = angular_velocity
|
||||||
@ -1395,27 +1395,34 @@ 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:
|
||||||
# 如果上次距离已经接近目标，维持当前位置
|
# 获取上次的滤波距离
|
||||||
last_error = target_distance - distance_queue[-1]
|
last_filtered_distance = distance_queue[-1]
|
||||||
if abs(last_error) < 50: # 如果接近目标距离
|
|
||||||
side_velocity = 0 # 停止侧向移动
|
# 计算上次距离与目标的差值
|
||||||
|
last_difference = target_distance - last_filtered_distance
|
||||||
|
|
||||||
|
# 根据上次差值决定行为
|
||||||
|
if abs(last_difference) < distance_threshold:
|
||||||
|
# 上次接近目标，保持位置
|
||||||
|
side_velocity = 0
|
||||||
if observe:
|
if observe:
|
||||||
info("接近目标位置，保持当前侧向位置", "保持")
|
info("上次距离接近目标，保持当前位置", "保持")
|
||||||
else:
|
else:
|
||||||
# 向目标方向缓慢移动
|
# 根据差值方向移动，但速度降低
|
||||||
side_velocity = 0.08 * (1 if last_error > 0 else -1)
|
direction = 1 if last_difference > 0 else -1
|
||||||
|
side_velocity = 0.08 * direction
|
||||||
if observe:
|
if observe:
|
||||||
info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
|
info(f"根据上次距离差值({last_difference:.1f}px)，向{'左' if direction > 0 else '右'}缓慢移动", "调整")
|
||||||
|
|
||||||
msg.vel_des = [0, side_velocity, 0]
|
msg.vel_des = [0, side_velocity, 0]
|
||||||
if observe:
|
if observe:
|
||||||
warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复")
|
warning(f"使用上次距离估计控制: 侧向={side_velocity:.2f}m/s", "恢复")
|
||||||
else:
|
else:
|
||||||
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线
|
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试寻找
|
||||||
default_side_velocity = 0.1
|
default_side_velocity = 0.08
|
||||||
msg.vel_des = [0, default_side_velocity, 0]
|
msg.vel_des = [0, default_side_velocity, 0]
|
||||||
if observe:
|
if observe:
|
||||||
warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
|
warning(f"无法检测到左侧轨迹线，向左缓慢移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
|
||||||
|
|
||||||
# 发送命令
|
# 发送命令
|
||||||
msg.life_count += 1
|
msg.life_count += 1
|
||||||
@ -1441,14 +1448,21 @@ def follow_left_side_track(ctrl, msg, target_distance=150, speed=0.3, max_time=3
|
|||||||
if observe:
|
if observe:
|
||||||
info("开始平滑停止", "停止")
|
info("开始平滑停止", "停止")
|
||||||
|
|
||||||
# 平滑停止 - 由于没有前进速度，只需要停止侧向移动
|
# 更平滑的停止
|
||||||
slowdown_steps = 3
|
slowdown_steps = 4
|
||||||
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 = [0, 0, 0] # 完全停止所有移动
|
if i > 1:
|
||||||
|
# 前几步只降低速度
|
||||||
|
last_vel = msg.vel_des[1]
|
||||||
|
reduced_vel = last_vel * slowdown_factor
|
||||||
|
msg.vel_des = [0, reduced_vel, 0]
|
||||||
|
else:
|
||||||
|
# 最后一步完全停止
|
||||||
|
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.12) # 稍微延长每步的等待时间，使停止更平滑
|
||||||
|
|
||||||
# 最后完全停止
|
# 最后完全停止
|
||||||
ctrl.base_msg.stop()
|
ctrl.base_msg.stop()
|
||||||
|
|||||||
@ -35,3 +35,23 @@
|
|||||||
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/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 - ℹ️ 保存左侧轨迹线检测结果图像到: 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}
|
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}
|
||||||
|
2025-05-25 21:00:13 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||||
|
2025-05-25 21:00:15 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||||
|
2025-05-25 21:00:16 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||||
|
2025-05-25 21:00:17 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||||
|
2025-05-25 21:00:18 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
|
||||||
|
2025-05-25 21:00:19 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||||
|
2025-05-25 21:00:20 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||||
|
2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_210021_481763.jpg
|
||||||
|
2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_210021_481763.jpg
|
||||||
|
2025-05-25 21:00:21 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_210021_481763', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
|
||||||
|
2025-05-25 21:20:33 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||||
|
2025-05-25 21:20:34 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||||
|
2025-05-25 21:20:35 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||||
|
2025-05-25 21:20:36 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||||
|
2025-05-25 21:20:37 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
|
||||||
|
2025-05-25 21:20:38 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||||
|
2025-05-25 21:20:39 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||||
|
2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250525_212040_471018.jpg
|
||||||
|
2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250525_212040_471018.jpg
|
||||||
|
2025-05-25 21:20:40 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250525_212040_471018', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
|
||||||
|
|||||||
30
logs/robot_2025-05-26.log
Normal file
30
logs/robot_2025-05-26.log
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
2025-05-26 00:20:28 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||||
|
2025-05-26 00:20:29 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||||
|
2025-05-26 00:20:30 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||||
|
2025-05-26 00:20:31 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||||
|
2025-05-26 00:20:32 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
|
||||||
|
2025-05-26 00:20:33 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||||
|
2025-05-26 00:20:34 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||||
|
2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002035_779025.jpg
|
||||||
|
2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002035_779025.jpg
|
||||||
|
2025-05-26 00:20:35 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002035_779025', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
|
||||||
|
2025-05-26 00:21:27 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||||
|
2025-05-26 00:21:28 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||||
|
2025-05-26 00:21:29 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||||
|
2025-05-26 00:21:30 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||||
|
2025-05-26 00:21:31 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 36 条直线
|
||||||
|
2025-05-26 00:21:32 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 4 条垂直线
|
||||||
|
2025-05-26 00:21:33 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||||
|
2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002134_938675.jpg
|
||||||
|
2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002134_938675.jpg
|
||||||
|
2025-05-26 00:21:34 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002134_938675', 'tracking_point': (94, 1079), 'ground_intersection': (92, 1080), 'distance_to_left': 215.0, 'slope': -0.756198347107438, 'line_mid_x': 215.0}
|
||||||
|
2025-05-26 00:25:05 | DEBUG | utils.log_helper - 🐞 步骤1: 原始图像已加载
|
||||||
|
2025-05-26 00:25:06 | DEBUG | utils.log_helper - 🐞 步骤2: 创建黄色掩码
|
||||||
|
2025-05-26 00:25:07 | DEBUG | utils.log_helper - 🐞 步骤3: 左侧区域掩码
|
||||||
|
2025-05-26 00:25:08 | DEBUG | utils.log_helper - 🐞 步骤4: 边缘检测
|
||||||
|
2025-05-26 00:25:10 | DEBUG | utils.log_helper - 🐞 步骤5: 检测到 74 条直线
|
||||||
|
2025-05-26 00:25:11 | DEBUG | utils.log_helper - 🐞 步骤6: 左侧区域找到 2 条垂直线
|
||||||
|
2025-05-26 00:25:12 | DEBUG | utils.log_helper - 🐞 步骤7: 左侧最佳跟踪线和点
|
||||||
|
2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 保存原始图像到: logs/image/original_20250526_002513_050661.jpg
|
||||||
|
2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 保存左侧轨迹线检测结果图像到: logs/image/left_track_20250526_002513_050661.jpg
|
||||||
|
2025-05-26 00:25:13 | INFO | utils.log_helper - ℹ️ 左侧轨迹线检测结果: {'timestamp': '20250526_002513_050661', 'tracking_point': (549, 1071), 'ground_intersection': (543, 1080), 'distance_to_left': 584.5, 'slope': -1.619718309859155, 'line_mid_x': 584.5}
|
||||||
BIN
res/path/left/1.png
Normal file
BIN
res/path/left/1.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 48 KiB |
BIN
res/path/left/2.png
Normal file
BIN
res/path/left/2.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 49 KiB |
BIN
res/path/test/left_track_results/result_2.png
Normal file
BIN
res/path/test/left_track_results/result_2.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 64 KiB |
@ -196,7 +196,7 @@ def process_video(video_path, save_dir=None, show_steps=False):
|
|||||||
|
|
||||||
def main():
|
def main():
|
||||||
parser = argparse.ArgumentParser(description='左侧黄色轨迹线检测演示程序')
|
parser = argparse.ArgumentParser(description='左侧黄色轨迹线检测演示程序')
|
||||||
parser.add_argument('--input', type=str, default='res/path/image_20250513_162556.png', help='输入图像或视频的路径')
|
parser.add_argument('--input', type=str, default='res/path/left/2.png', help='输入图像或视频的路径')
|
||||||
parser.add_argument('--output', type=str, default='res/path/test/left_track_results/', help='输出结果的保存目录')
|
parser.add_argument('--output', type=str, default='res/path/test/left_track_results/', help='输出结果的保存目录')
|
||||||
parser.add_argument('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测')
|
parser.add_argument('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测')
|
||||||
parser.add_argument('--show', default=True, help='显示处理步骤')
|
parser.add_argument('--show', default=True, help='显示处理步骤')
|
||||||
|
|||||||
@ -1372,11 +1372,8 @@ def detect_left_side_track(image, observe=False, delay=1000, save_log=True):
|
|||||||
# 优先选择在图像下半部分的线段
|
# 优先选择在图像下半部分的线段
|
||||||
height_score = min(1.0, mid_y / (height * 0.6))
|
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
|
return length_score * 0.2 + position_score * 0.5 + height_score * 0.2
|
||||||
|
|
||||||
# 对线段进行评分并排序
|
# 对线段进行评分并排序
|
||||||
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)
|
||||||
|
|||||||
Loading…
x
Reference in New Issue
Block a user