From ae8de7bd28d46b9025b492740fb0ac581ac62e1b Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Fri, 16 May 2025 14:25:41 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20move=5Fto=5Fhori=5Fline=20?= =?UTF-8?q?=E5=92=8C=20arc=5Fturn=5Faround=5Fhori=5Fline=20=E5=87=BD?= =?UTF-8?q?=E6=95=B0=EF=BC=8C=E6=8F=90=E5=8D=87=E7=A7=BB=E5=8A=A8=E5=92=8C?= =?UTF-8?q?=E6=97=8B=E8=BD=AC=E7=B2=BE=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 增加移动超时时间，确保在复杂环境下的稳定性 - 放宽移动距离判断标准，提升成功率 - 针对180度旋转简化逻辑，直接使用原始角度，避免过度旋转 - 实现大角度旋转的特殊处理，优化减速策略，确保平稳性 - 增强调试信息输出，便于实时监控旋转和移动状态 --- base_move/move_base_hori_line.py | 235 +++++++++++++++++++++---------- 1 file changed, 159 insertions(+), 76 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 6dafff4..62375f4 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -295,10 +295,10 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 使用里程计进行实时监控移动距离 distance_moved = 0 start_time = time.time() - timeout = move_time + 1 # 超时时间设置为预计移动时间加1秒 + timeout = move_time + 2 # 增加超时时间为预计移动时间加2秒 - # 监控移动距离，但不执行减速（改用stop_smooth） - while distance_moved < abs(distance_to_move) * 0.95 and time.time() - start_time < timeout: + # 监控移动距离，增加移动距离的系数 + while distance_moved < abs(distance_to_move) * 1.05 and time.time() - start_time < timeout: # 计算已移动距离 current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] @@ -321,8 +321,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): 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) - # 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断 - return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功 + # 放宽判断标准 + return abs(distance_moved - abs(distance_to_move)) < 0.15 # 如果误差小于15厘米，则认为成功 def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, pass_align=False, @@ -413,7 +413,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc elif angle_deg <= 90: time_compensation = 0.85 # 90度左右使用稍大的系数 else: - time_compensation = 0.75 # 大角度(如180度)减小补偿系数，防止过度旋转 + # 对180度旋转，这里完全不用补偿，直接使用原始角度 + time_compensation = 1.0 # 调整实际目标角度，针对不同角度应用不同的缩放比例 actual_angle_deg = angle_deg @@ -422,7 +423,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc elif angle_deg <= 90: actual_angle_deg = angle_deg * 0.9 # 90度减少到90% elif angle_deg >= 170: # 处理大角度旋转，特别是180度情况 - actual_angle_deg = angle_deg * 0.75 # 减少到75%，防止过度旋转 + actual_angle_deg = angle_deg # 对于180度旋转，使用完整角度，不进行缩放 if observe and actual_angle_deg != angle_deg: print(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度") @@ -490,79 +491,161 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc rotation_completion_threshold = 0.95 # 默认旋转到95%时停止 # 监控旋转进度并自行控制减速 - while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout: - # 计算已移动距离 - 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) - - # 计算已旋转角度 - current_yaw = ctrl.odo_msg.rpy[2] - angle_turned = current_yaw - start_yaw - # 角度归一化处理 - while angle_turned > math.pi: - angle_turned -= 2 * math.pi - while angle_turned < -math.pi: - angle_turned += 2 * math.pi + # 对于180度旋转使用特殊处理 + if angle_deg >= 170: + # 简化旋转逻辑，直接持续旋转一段时间 + rotation_time = t # 使用计算的理论时间 + if observe: + print(f"大角度旋转({angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒") - # 检查QR码扫描结果（如果启用） - if scan_qrcode: - current_time = time.time() - if current_time - last_qr_check_time >= qr_check_interval: - qr_data, scan_time = ctrl.image_processor.get_last_qr_result() - if qr_data and scan_time > start_time: # 确保是在旋转开始后的扫描结果 - qr_result = qr_data - print(f"🔍 在旋转过程中扫描到QR码: {qr_data}") - last_qr_check_time = current_time - - # 计算角度完成比例 - completion_ratio = abs(angle_turned) / abs(angle_rad) + # 发送固定时间的旋转命令 + start_time = time.time() - # 当完成一定比例时开始减速 - if completion_ratio > slowdown_threshold: - # 实现多阶段减速，特别针对大角度旋转 - if angle_deg >= 170: - if completion_ratio < 0.7: # 第一阶段：60%-70% - speed_factor = 0.7 - elif completion_ratio < 0.8: # 第二阶段：70%-80% - speed_factor = 0.4 - else: # 第三阶段：80%以上 - speed_factor = 0.2 + # 前半段使用全速 + half_time = rotation_time * 0.6 + elapsed_time = 0 + + while elapsed_time < half_time and time.time() - start_time < rotation_time + 1: # 加1秒保护 + elapsed_time = time.time() - start_time + + # 计算已旋转角度（仅用于打印） + if observe and time.time() % 0.5 < 0.02: + current_yaw = ctrl.odo_msg.rpy[2] + angle_turned = current_yaw - start_yaw + # 角度归一化处理 + while angle_turned > math.pi: + angle_turned -= 2 * math.pi + while angle_turned < -math.pi: + angle_turned += 2 * math.pi + print(f"全速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {elapsed_time:.2f}s/{rotation_time:.2f}s") + + # 检查QR码扫描结果（如果启用） + if scan_qrcode: + current_time = time.time() + if current_time - last_qr_check_time >= qr_check_interval: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data and scan_time > start_time: + qr_result = qr_data + print(f"🔍 在旋转过程中扫描到QR码: {qr_data}") + last_qr_check_time = current_time + + time.sleep(0.05) + + # 后半段减速 + if observe: + print(f"进入减速阶段") + + # 减速到50% + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [v * 0.5, 0, w * 0.5] + msg.duration = 0 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + # 继续旋转直到总时间结束 + while time.time() - start_time < rotation_time: + # 计算已旋转角度（仅用于打印） + if observe and time.time() % 0.5 < 0.02: + current_yaw = ctrl.odo_msg.rpy[2] + angle_turned = current_yaw - start_yaw + # 角度归一化处理 + while angle_turned > math.pi: + angle_turned -= 2 * math.pi + while angle_turned < -math.pi: + angle_turned += 2 * math.pi + print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {time.time() - start_time:.2f}s/{rotation_time:.2f}s") + + # 检查QR码扫描结果（如果启用） + if scan_qrcode: + current_time = time.time() + if current_time - last_qr_check_time >= qr_check_interval: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data and scan_time > start_time: + qr_result = qr_data + print(f"🔍 在旋转过程中扫描到QR码: {qr_data}") + last_qr_check_time = current_time + + time.sleep(0.05) + + # 停止 + ctrl.base_msg.stop() + else: + # 非180度的常规旋转逻辑保持不变 + while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout: + # 计算已移动距离 + 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) + + # 计算已旋转角度 + current_yaw = ctrl.odo_msg.rpy[2] + angle_turned = current_yaw - start_yaw + # 角度归一化处理 + while angle_turned > math.pi: + angle_turned -= 2 * math.pi + while angle_turned < -math.pi: + angle_turned += 2 * math.pi + + # 检查QR码扫描结果（如果启用） + if scan_qrcode: + current_time = time.time() + if current_time - last_qr_check_time >= qr_check_interval: + qr_data, scan_time = ctrl.image_processor.get_last_qr_result() + if qr_data and scan_time > start_time: + qr_result = qr_data + print(f"🔍 在旋转过程中扫描到QR码: {qr_data}") + last_qr_check_time = current_time + + # 计算角度完成比例 + completion_ratio = abs(angle_turned) / abs(angle_rad) + + # 当完成一定比例时开始减速 + if completion_ratio > slowdown_threshold: + # 实现多阶段减速，特别针对大角度旋转 + if angle_deg >= 170: + if completion_ratio < 0.7: # 第一阶段：60%-70% + speed_factor = 0.7 + elif completion_ratio < 0.8: # 第二阶段：70%-80% + speed_factor = 0.4 + else: # 第三阶段：80%以上 + speed_factor = 0.2 + else: + # 标准减速逻辑保持不变 + # 剩余比例作为速度系数 + speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) + + # 根据角度调整最小速度因子 + min_speed_factor = 0.15 # 默认最小速度因子 + # 确保速度系数不会太小，但可以更慢一些 + speed_factor = max(min_speed_factor, speed_factor) + + # 应用减速 + current_v = v * speed_factor + current_w = w * speed_factor + + # 更新速度命令 + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [current_v, 0, current_w] + msg.duration = 200 # 短时间更新 + msg.step_height = [0.06, 0.06] + msg.life_count += 1 + ctrl.Send_cmd(msg) + + # 针对180度旋转，在减速命令后稍作等待，以确保减速效果 + if angle_deg >= 170: + time.sleep(0.03) # 短暂延迟，让减速命令更有效 + + if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 + print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s") else: - # 标准减速逻辑保持不变 - # 剩余比例作为速度系数 - speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) - - # 根据角度调整最小速度因子 - min_speed_factor = 0.15 # 默认最小速度因子 - # 确保速度系数不会太小，但可以更慢一些 - speed_factor = max(min_speed_factor, speed_factor) + if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 + print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度") - # 应用减速 - current_v = v * speed_factor - current_w = w * speed_factor - - # 更新速度命令 - msg.mode = 11 - msg.gait_id = 26 - msg.vel_des = [current_v, 0, current_w] - msg.duration = 200 # 短时间更新 - msg.step_height = [0.06, 0.06] - msg.life_count += 1 - ctrl.Send_cmd(msg) - - # 针对180度旋转，在减速命令后稍作等待，以确保减速效果 - if angle_deg >= 170: - time.sleep(0.03) # 短暂延迟，让减速命令更有效 - - if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 - print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s") - else: - if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 - print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度") - - time.sleep(0.05) + time.sleep(0.05) if observe: print(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")