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

优化follow_left_side_track函数，调整目标距离参数至540像素，增强稳定性和响应速度；更新滤波队列大小至7以提高稳定性；改进速度计算逻辑，确保更平滑的调整和停止过程；更新左侧轨迹线检测演示程序的输入路径。

Browse Source
This commit is contained in:
Havoc 2025-05-26 00:44:24 +08:00
parent e77d5655ae
commit 5fd630c27b
8 changed files with 149 additions and 88 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
base_move/move_base_hori_line.py
View File

@ -1222,14 +1222,14 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
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 对象，包含里程计信息
msg: robot_control_cmd_lcmt 对象，用于发送命令
target_distance: 目标与左侧线的像素距离，默认为150像素（保持一定间距）
target_distance: 目标与左侧线的像素距离，默认为540像素（考虑机器狗视角、高度的合适距离）
speed: 侧向移动的最大速度(米/秒)，默认为0.3米/秒
max_time: 最大执行时间(秒)，默认为30秒
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'):
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 # 比例系数
ki_side = 0.0001 # 积分系数
kd_side = 0.001 # 微分系数
# 记录目标到达状态和稳定计数
target_reached = False
stable_count = 0
required_stable_count = 8 # 需要连续稳定的检测次数
# PID控制变量
previous_error = 0
integral = 0
# 最大侧向速度限制
max_side_velocity = speed # 使用参数传入的速度作为最大侧向速度
# 最大侧向速度限制与动态调整
max_side_velocity = speed
min_side_velocity = 0.05 # 最小侧向速度，确保能够缓慢移动
# 检测成功计数器
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
# 滤波队列
filter_size = 5
# 滤波队列 - 增大滤波窗口以提高稳定性
filter_size = 7
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:
@ -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:
distance_queue.pop(0)
# 计算滤波后的距离值 (去除最大和最小值后的平均)
# 计算滤波后的距离值 - 使用中值滤波，对抗异常值
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:
filtered_distance = current_distance
# 直接计算当前距离与目标距离的差值
difference = target_distance - filtered_distance # 正值：需要向左移动；负值：需要向右移动
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(error) < target_reached_threshold: # 误差小于阈值视为达到目标
# 检查是否达到目标位置（差值小于阈值）
if abs(difference) < distance_threshold:
stable_count += 1
if stable_count >= required_stable_count:
if not target_reached:
if stable_count >= required_stable_count and not target_reached:
target_reached = True
if observe:
success(f"已达到目标位置，误差: {abs(error):.1f}px", "成功")
success(f"已达到目标位置，当前距离: {filtered_distance:.1f}px, 与目标差值: {abs(difference):.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:
# 不在目标位置，根据差值计算速度
stable_count = 0
target_reached = False
# 比例项
p_control = kp_side * error
# 计算速度系数 - 差值越大，速度越大，但有上限
# 使用非线性映射，使速度变化更平滑
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)
# 积分项 (添加积分限制以防止积分饱和)
integral += error
integral = max(-500, min(500, integral)) # 限制积分范围
i_control = ki_side * integral
# 应用方向（差值为正向左移动，为负向右移动）
side_velocity = max_side_velocity * speed_factor * (1 if difference > 0 else -1)
# 微分项
derivative = error - previous_error
d_control = kd_side * derivative
previous_error = error
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", "调整")
# 计算侧向速度
side_velocity = p_control + i_control + d_control
# 应用非线性控制，加快大距离靠近，减缓小距离微调
if abs(error) > 100:
# 大误差时，增强响应
side_velocity *= 1.2
elif abs(error) < 40:
# 小误差时，减弱响应，增加稳定性
side_velocity *= 0.7
# 确保最小移动速度 - 当需要移动但计算值太小时
if 0 < abs(side_velocity) < min_side_velocity:
side_velocity = min_side_velocity * (1 if side_velocity > 0 else -1)
# 限制侧向速度范围
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:
debug(f"误差: {error:.1f}px, P: {p_control:.4f}, I: {i_control:.4f}, D: {d_control:.4f}", "控制")
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"]
# 计算旋转角度 - 垂直线的ideal_angle应该是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: # 只在需要明显校正时应用
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:
# 如果上次距离已经接近目标，维持当前位置
last_error = target_distance - distance_queue[-1]
if abs(last_error) < 50: # 如果接近目标距离
side_velocity = 0 # 停止侧向移动
# 获取上次的滤波距离
last_filtered_distance = distance_queue[-1]
# 计算上次距离与目标的差值
last_difference = target_distance - last_filtered_distance
# 根据上次差值决定行为
if abs(last_difference) < distance_threshold:
# 上次接近目标，保持位置
side_velocity = 0
if observe:
info("接近目标位置，保持当前侧向位置", "保持")
info("上次距离接近目标，保持当前位置", "保持")
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:
info(f"距离目标较远，缓慢向{'左' if side_velocity > 0 else '右'}移动", "调整")
info(f"根据上次距离差值({last_difference:.1f}px)，向{'左' if direction > 0 else '右'}缓慢移动", "调整")
msg.vel_des = [0, side_velocity, 0]
if observe:
warning(f"使用降低强度的控制: 侧向={side_velocity:.2f}m/s", "恢复")
warning(f"使用上次距离估计控制: 侧向={side_velocity:.2f}m/s", "恢复")
else:
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试找到轨迹线
default_side_velocity = 0.1
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试寻找
default_side_velocity = 0.08
msg.vel_des = [0, default_side_velocity, 0]
if observe:
warning(f"无法检测到左侧轨迹线，向左移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
warning(f"无法检测到左侧轨迹线，向左缓慢移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
# 发送命令
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:
info("开始平滑停止", "停止")
# 平滑停止 - 由于没有前进速度，只需要停止侧向移动
slowdown_steps = 3
# 更平滑的停止
slowdown_steps = 4
for i in range(slowdown_steps, 0, -1):
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
ctrl.Send_cmd(msg)
time.sleep(0.1)
time.sleep(0.12) # 稍微延长每步的等待时间，使停止更平滑
# 最后完全停止
ctrl.base_msg.stop()

20
logs/robot_2025-05-25.log
View File

@ -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/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 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
View 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
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 48 KiB

BIN
res/path/left/2.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 49 KiB

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

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 64 KiB

2
test/task-path-track/left_track_demo.py
View File

@ -196,7 +196,7 @@ def process_video(video_path, save_dir=None, show_steps=False):
def main():
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('--type', type=str, choices=['image', 'video'], help='输入类型，不指定会自动检测')
parser.add_argument('--show', default=True, help='显示处理步骤')

5
utils/detect_track.py
View File

@ -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))
# 斜率得分，垂直度越高越好
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)