优化follow_left_side_track函数，调整最大时间参数至30秒，增强轨迹跟随的稳定性和响应速度；改进检测逻辑，确保在未检测到轨迹线时能够继续使用上一次控制命令；更新速度计算方式，确保更平滑的调整和停止过程。
This commit is contained in:
parent
612256c525
commit
76c00337ed
@ -1222,7 +1222,7 @@ 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=540, speed=0.3, max_time=3, observe=False):
|
||||
def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=30, observe=False):
|
||||
"""
|
||||
控制机器狗向左侧移动并靠近左侧的黄色轨迹线，只进行侧向移动，不进行前进
|
||||
|
||||
@ -1248,255 +1248,90 @@ def follow_left_side_track(ctrl, msg, target_distance=540, speed=0.3, max_time=3
|
||||
# 记录起始时间
|
||||
start_time = time.time()
|
||||
|
||||
# 记录起始位置
|
||||
start_position = list(ctrl.odo_msg.xyz)
|
||||
# 记录初始检测
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||||
|
||||
if track_info is None:
|
||||
error("无法检测到左侧轨迹线，无法开始跟踪", "失败")
|
||||
return False
|
||||
|
||||
initial_distance = track_info["distance_to_left"]
|
||||
if observe:
|
||||
debug(f"起始位置: {start_position}", "位置")
|
||||
# 在起点放置绿色标记
|
||||
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)
|
||||
info(f"初始距离: {initial_distance:.1f}px, 目标距离: {target_distance}px", "初始")
|
||||
|
||||
# 最大侧向速度限制与动态调整
|
||||
max_side_velocity = speed
|
||||
min_side_velocity = 0.05 # 最小侧向速度，确保能够缓慢移动
|
||||
# 最大允许误差(像素)
|
||||
error_threshold = 30
|
||||
|
||||
# 检测成功计数器
|
||||
detection_success_count = 0
|
||||
detection_total_count = 0
|
||||
|
||||
# 保存上一次有效的检测结果，用于检测失败时的平滑过渡
|
||||
last_valid_track_info = None
|
||||
|
||||
# 滤波队列 - 增大滤波窗口以提高稳定性
|
||||
filter_size = 7
|
||||
distance_queue = []
|
||||
|
||||
# 设置差值阈值 - 当距离与目标的差值小于此阈值时，认为已达到目标
|
||||
distance_threshold = 25 # 像素误差阈值
|
||||
|
||||
# 记录目标到达状态和稳定计数
|
||||
target_reached = False
|
||||
stable_count = 0
|
||||
required_stable_count = 8 # 连续多帧满足条件才算稳定到达目标
|
||||
# 简单比例控制参数
|
||||
kp = 0.0005
|
||||
|
||||
# 开始跟踪循环
|
||||
while time.time() - start_time < max_time:
|
||||
# 获取当前图像
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
|
||||
# 检测左侧轨迹线
|
||||
detection_total_count += 1
|
||||
track_info, tracking_point = detect_left_side_track(image, observe=observe, delay=500 if observe else 0, save_log=True)
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||||
|
||||
if track_info is not None:
|
||||
detection_success_count += 1
|
||||
|
||||
# 保存有效的轨迹信息
|
||||
last_valid_track_info = track_info
|
||||
|
||||
# 获取当前与左侧线的距离
|
||||
current_distance = track_info["distance_to_left"]
|
||||
|
||||
# 添加到滤波队列
|
||||
distance_queue.append(current_distance)
|
||||
if len(distance_queue) > filter_size:
|
||||
distance_queue.pop(0)
|
||||
|
||||
# 计算滤波后的距离值 - 使用中值滤波，对抗异常值
|
||||
if len(distance_queue) >= 3:
|
||||
# 中值滤波更能抵抗异常值
|
||||
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"左侧距离: {filtered_distance:.1f}px, 目标距离: {target_distance}px, 差值: {difference:.1f}px", "距离")
|
||||
|
||||
# 检查是否达到目标位置（差值小于阈值）
|
||||
if abs(difference) < distance_threshold:
|
||||
stable_count += 1
|
||||
if stable_count >= required_stable_count and not target_reached:
|
||||
target_reached = True
|
||||
if observe:
|
||||
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
|
||||
|
||||
# 计算速度系数 - 差值越大，速度越大，但有上限
|
||||
# 使用非线性映射，使速度变化更平滑
|
||||
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", "调整")
|
||||
|
||||
# 确保最小移动速度 - 当需要移动但计算值太小时
|
||||
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 observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"控制指令: 侧向={side_velocity:.2f}m/s", "速度")
|
||||
|
||||
# 设置速度命令 - [前进速度, 侧向速度, 角速度]
|
||||
# 侧向速度为正表示向左移动，为负表示向右移动
|
||||
# 前进速度保持为0，只进行侧向移动
|
||||
msg.vel_des = [0, side_velocity, 0]
|
||||
|
||||
# 使用轨迹线斜率进行小角度调整，确保机器人方向与轨迹线平行
|
||||
if not track_info["is_vertical"]: # 对于非垂直线，进行角度校正
|
||||
slope = track_info["slope"]
|
||||
# 计算旋转角度 - 垂直线的ideal_angle应该是0
|
||||
angle_correction = -math.atan(1/slope) if abs(slope) > 0.01 else 0
|
||||
# 将角度校正应用为小的角速度，并根据距离调整强度
|
||||
angle_factor = 1.0
|
||||
if target_reached:
|
||||
angle_factor = 0.5 # 接近目标时减弱角度校正
|
||||
angular_velocity = angle_correction * 0.2 * angle_factor
|
||||
# 限制角速度范围
|
||||
angular_velocity = max(-0.2, min(0.2, angular_velocity))
|
||||
|
||||
if abs(angular_velocity) > 0.01: # 只在需要明显校正时应用
|
||||
msg.vel_des[2] = angular_velocity
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
debug(f"应用角度校正，斜率: {slope:.2f}, 角速度: {angular_velocity:.3f}", "校正")
|
||||
|
||||
else:
|
||||
warning("未检测到左侧轨迹线", "警告")
|
||||
|
||||
# 如果之前有有效检测结果，使用上一次的控制值但降低强度
|
||||
if last_valid_track_info is not None and len(distance_queue) > 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("上次距离接近目标，保持当前位置", "保持")
|
||||
else:
|
||||
# 根据差值方向移动，但速度降低
|
||||
direction = 1 if last_difference > 0 else -1
|
||||
side_velocity = 0.08 * direction
|
||||
if observe:
|
||||
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", "恢复")
|
||||
else:
|
||||
# 如果从未检测到轨迹线，默认向左侧移动一点，尝试寻找
|
||||
default_side_velocity = 0.08
|
||||
msg.vel_des = [0, default_side_velocity, 0]
|
||||
if observe:
|
||||
warning(f"无法检测到左侧轨迹线，向左缓慢移动尝试寻找: {default_side_velocity:.2f}m/s", "警告")
|
||||
if track_info is None:
|
||||
warning("未检测到左侧轨迹线，继续使用上一次控制", "警告")
|
||||
# 继续执行最后一次的控制命令
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.1)
|
||||
continue
|
||||
|
||||
# 发送命令
|
||||
# 获取当前距离
|
||||
current_distance = track_info["distance_to_left"]
|
||||
|
||||
# 计算误差 (正值表示需要向左移动，负值表示需要向右移动)
|
||||
error = target_distance - current_distance
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
debug(f"当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px, 误差: {error:.1f}px", "跟踪")
|
||||
|
||||
# 检查是否已达到目标
|
||||
if abs(error) < error_threshold:
|
||||
success(f"已达到目标距离，当前距离: {current_distance:.1f}px, 目标距离: {target_distance}px", "完成")
|
||||
break
|
||||
|
||||
# 计算侧向速度 (简单比例控制)
|
||||
lateral_velocity = kp * error
|
||||
|
||||
# 限制侧向速度
|
||||
lateral_velocity = max(-speed, min(speed, lateral_velocity))
|
||||
|
||||
# 设置速度命令
|
||||
msg.vel_des = [0, lateral_velocity, 0] # [前进速度, 侧向速度, 角速度]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 短暂延时
|
||||
time.sleep(0.05)
|
||||
|
||||
# 计算已移动距离(仅用于记录)
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
dy = current_position[1] - start_position[1]
|
||||
distance_moved = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
if observe and time.time() % 2.0 < 0.02: # 每2秒左右打印一次
|
||||
info(f"已移动: {distance_moved:.3f}米, 已用时间: {time.time() - start_time:.1f}秒", "移动")
|
||||
# 显示检测成功率
|
||||
if detection_total_count > 0:
|
||||
detection_rate = (detection_success_count / detection_total_count) * 100
|
||||
info(f"左侧轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
|
||||
time.sleep(0.1)
|
||||
|
||||
# 平滑停止
|
||||
if observe:
|
||||
info("开始平滑停止", "停止")
|
||||
|
||||
# 更平滑的停止
|
||||
slowdown_steps = 4
|
||||
for i in range(slowdown_steps, 0, -1):
|
||||
slowdown_factor = i / slowdown_steps
|
||||
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.12) # 稍微延长每步的等待时间，使停止更平滑
|
||||
|
||||
# 最后完全停止
|
||||
# 停止移动
|
||||
msg.vel_des = [0, 0, 0]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.2)
|
||||
ctrl.base_msg.stop()
|
||||
|
||||
# 计算最终移动距离
|
||||
final_position = ctrl.odo_msg.xyz
|
||||
dx = final_position[0] - start_position[0]
|
||||
dy = final_position[1] - start_position[1]
|
||||
final_distance = math.sqrt(dx*dx + dy*dy)
|
||||
# 最终检查
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
track_info, _ = detect_left_side_track(image, observe=observe, save_log=True)
|
||||
|
||||
if observe:
|
||||
# 在终点放置红色标记
|
||||
end_position = list(final_position)
|
||||
if hasattr(ctrl, 'place_marker'):
|
||||
ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
|
||||
if track_info is not None:
|
||||
final_distance = track_info["distance_to_left"]
|
||||
distance_error = abs(final_distance - target_distance)
|
||||
final_success = distance_error < error_threshold
|
||||
|
||||
success(f"左侧轨迹跟随完成，总移动距离: {final_distance:.3f}米", "完成")
|
||||
if observe:
|
||||
if final_success:
|
||||
success(f"成功达到目标位置，最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "成功")
|
||||
else:
|
||||
warning(f"未能精确达到目标位置，最终距离: {final_distance:.1f}px, 目标: {target_distance}px", "警告")
|
||||
|
||||
# 显示检测成功率
|
||||
if detection_total_count > 0:
|
||||
detection_rate = (detection_success_count / detection_total_count) * 100
|
||||
info(f"左侧轨迹检测成功率: {detection_rate:.1f}% ({detection_success_count}/{detection_total_count})", "统计")
|
||||
|
||||
return target_reached
|
||||
|
||||
# 用法示例
|
||||
if __name__ == "__main__":
|
||||
move_to_hori_line(None, None, observe=True)
|
||||
# 90度左转
|
||||
arc_turn_around_hori_line(None, None, angle_deg=90, observe=True)
|
||||
# 180度右转
|
||||
arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True)
|
||||
# 双轨道跟随
|
||||
follow_dual_tracks(None, None, observe=True)
|
||||
# 左侧轨迹跟随
|
||||
follow_left_side_track(None, None, observe=True)
|
||||
|
||||
return final_success
|
||||
else:
|
||||
if observe:
|
||||
warning("无法检测最终位置的左侧轨迹线", "警告")
|
||||
return False
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user