diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 4f07eea..0f56d18 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -468,7 +468,12 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left pre_stop_yaw = ctrl.odo_msg.rpy[2] # 使用专门的旋转停止函数替代原来的强制停止 - ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=5, delay=0.1, final_steps=2) + if observe: + print(f"使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]") + print(f'{math.degrees(ctrl.odo_msg.rpy[2]):.2f}') + ctrl.base_msg.stop() + if observe: + print(f'{math.degrees(ctrl.odo_msg.rpy[2])}:.2f') # 停下来后的最终角度 final_yaw = ctrl.odo_msg.rpy[2] @@ -520,260 +525,6 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left return True -def arc_turn_precise(ctrl, msg, radius, angle_deg, left=True, observe=False): - """ - 实现更精确的圆弧转弯控制，特别改进了停止阶段的处理 - - 参数: - ctrl: Robot_Ctrl对象 - msg: robot_control_cmd_lcmt对象 - radius: 圆弧半径(米) - angle_deg: 旋转角度(度) - left: True为左转，False为右转 - observe: 是否打印调试信息 - 返回: - bool: 是否成功完成动作 - """ - # 1. 参数准备 - angle_rad = math.radians(angle_deg) - # 设定基础角速度，为更好的控制降低了角速度 - base_w = 0.6 # 降低基础角速度提高精度 - - # 设定方向 - w = base_w if left else -base_w # 左转为正，右转为负 - v = abs(w * radius) # 线速度与角速度和半径成正比 - - # 计算理论时间 - t = abs(angle_rad / w) - - # 应用时间补偿系数 - time_compensation = 0.92 - t *= time_compensation - - if observe: - print(f"圆弧参数: 半径={radius:.3f}米, 角度={angle_deg}度, 方向={'左' if left else '右'}") - print(f"速度参数: 角速度={w:.3f}rad/s, 线速度={v:.3f}m/s") - print(f"时间参数: 理论时间={abs(angle_rad / w):.2f}s, 实际时间={t:.2f}s") - - # 2. 启动转向 - msg.mode = 11 - msg.gait_id = 26 - msg.vel_des = [v, 0, w] - msg.duration = int((t + 2) * 1000) # 余量2秒 - msg.step_height = [0.06, 0.06] - msg.life_count += 1 - ctrl.Send_cmd(msg) - - # 3. 记录初始状态 - start_yaw = ctrl.odo_msg.rpy[2] - start_position = list(ctrl.odo_msg.xyz) - start_time = time.time() - - # 4. 设置控制参数 - target_angle = angle_rad if left else -angle_rad - distance_moved = 0 - angle_turned = 0 - timeout = t + 2 # 超时保护 - current_v = v - current_w = w - - # 分阶段控制参数 - # 提前减速区间设置更大，从70%开始减速 - slowdown_threshold = 0.7 - # 慢速区间设置更大，从90%开始进入超慢速 - precision_threshold = 0.9 - - # 5. 实时监控和控制 - while abs(angle_turned) < abs(angle_rad) * 0.98 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 - - # 计算完成比例 - completion_ratio = abs(angle_turned) / abs(angle_rad) - - # 分阶段减速控制 - if completion_ratio > precision_threshold: - # 超低速阶段（精确控制） - speed_factor = 0.1 # 使用固定的极低速度 - - # 应用超低速 - 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 = 150 # 更短的更新周期 - msg.step_height = [0.06, 0.06] - msg.life_count += 1 - ctrl.Send_cmd(msg) - - if observe and time.time() % 0.5 < 0.02: - print(f"精确阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s") - - elif completion_ratio > slowdown_threshold: - # 一般减速阶段 - speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) - speed_factor = max(0.2, speed_factor * 0.8) # 整体降低一点速度 - - # 应用减速 - 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) - - if observe and time.time() % 0.5 < 0.02: - 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: - print(f"匀速阶段 - 已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度") - - # 更短的循环检查间隔 - time.sleep(0.03) - - # 6. 使用改进的平滑停止方法 - if observe: - print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]") - - # 记录停止前的角度 - pre_stop_yaw = ctrl.odo_msg.rpy[2] - - # 使用专门的旋转停止函数 - # 增加步数和降低延迟以获得更平滑的停止 - ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=7, delay=0.08, final_steps=3) - - # 7. 停止后的角度分析 - final_yaw = ctrl.odo_msg.rpy[2] - final_angle_turned = final_yaw - start_yaw - - # 角度归一化处理 - while final_angle_turned > math.pi: - final_angle_turned -= 2 * math.pi - while final_angle_turned < -math.pi: - final_angle_turned += 2 * math.pi - - final_angle_deg = math.degrees(final_angle_turned) - target_angle_deg = angle_deg if left else -angle_deg - - if observe: - # 输出停止过程中角度变化 - stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw) - while stop_angle_diff > 180: - stop_angle_diff -= 360 - while stop_angle_diff < -180: - stop_angle_diff += 360 - print(f"停止过程中旋转了: {stop_angle_diff:.2f}度") - print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度") - - # 8. 微调处理 - angle_error = abs(final_angle_deg - target_angle_deg) - - # 如果误差超过阈值，进行微调 - if angle_error > 2.0: # 降低误差阈值到2度 - if observe: - print(f"角度误差: {angle_error:.2f}度，进行微调") - - # 计算微调角度 - adjust_angle = target_angle_deg - final_angle_deg - - # 应用补偿系数 - compensation_factor = 0.85 - adjust_angle *= compensation_factor - - if observe: - print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度") - - # 使用turn_degree进行精确微调 - turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False) - - if observe: - print(f"微调结果: {'成功' if turn_success else '失败'}") - else: - if observe: - print(f"角度误差在允许范围内: {angle_error:.2f}度，无需微调") - - # 9. 最终位置和角度 - final_position = ctrl.odo_msg.xyz - final_yaw = ctrl.odo_msg.rpy[2] - - if observe: - dx = final_position[0] - start_position[0] - dy = final_position[1] - start_position[1] - total_distance = math.sqrt(dx*dx + dy*dy) - - # 计算理论圆弧长度 - arc_length = abs(radius * angle_rad) - print(f"位移统计: 直线距离={total_distance:.3f}米, 圆弧长度={arc_length:.3f}米") - print(f"精确圆弧转弯完成") - - return True - -def arc_turn_around_hori_line_precise(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False): - """ - 使用精确圆弧转弯方法，对准前方横线，然后以计算出的距离为半径，做一个向左或向右的圆弧旋转。 - - 参数: - ctrl: Robot_Ctrl对象 - msg: robot_control_cmd_lcmt对象 - target_distance: 横线前目标保持距离，默认0.2米 - angle_deg: 旋转角度，支持任意角度 - left: True为左转，False为右转 - observe: 是否打印调试信息 - 返回: - bool: 是否成功完成动作 - """ - # 1. 对准横线 - print("校准到横向线水平") - aligned = align_to_horizontal_line(ctrl, msg, observe=observe) - if not aligned: - print("无法校准到横向线水平，停止动作") - return False - - # 2. 检测横线并计算距离 - image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) - if edge_point is None or edge_info is None: - print("无法检测到横向线，停止动作") - return False - - camera_height = 0.355 # 单位: 米 - r = calculate_distance_to_line(edge_info, camera_height, observe=observe) - - # 减去目标距离得到圆弧半径 - r -= target_distance - - if r is None or r < 0.1: # 半径太小无法执行圆弧 - print(f"计算的半径太小: {r if r is not None else 'None'}，无法执行圆弧转弯") - return False - - if observe: - print(f"当前距离: {r+target_distance:.3f}米，目标距离: {target_distance:.3f}米") - print(f"将使用半径: {r:.3f}米 执行圆弧转弯") - - # 3. 使用精确圆弧转弯函数执行 - return arc_turn_precise(ctrl, msg, r, angle_deg, left, observe) - # 用法示例 if __name__ == "__main__": move_to_hori_line(None, None, observe=True) @@ -781,32 +532,3 @@ if __name__ == "__main__": arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True) # 180度右转 arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True) - # 使用精确版本做90度左转 - arc_turn_around_hori_line_precise(None, None, angle_deg=90, left=True, observe=True) - -""" -代码改进说明： - -1. 添加了专用的平滑旋转停止函数 stop_turn_smooth (在 utils/base_msg.py 中): - - 分两个阶段减速：先减小角速度，再减小线速度 - - 使用平方递减使角速度更快地降低 - - 发送多次零速度命令确保完全停止 - -2. 改进了原始的 arc_turn_around_hori_line 函数: - - 使用平滑停止替代强制停止 - - 添加停止过程中角度变化的监控 - - 改进了角度误差计算和微调逻辑 - -3. 新增了 arc_turn_precise 函数，实现更精确的转弯控制: - - 速度更低，控制更精细 - - 分三个阶段控制：匀速阶段、减速阶段、精确阶段 - - 更频繁的控制更新和监控 - - 更完善的停止策略 - - 更严格的角度误差要求 - -4. 新增 arc_turn_around_hori_line_precise 结合横线检测和精确转弯功能: - - 保持原有的横线检测和校准功能 - - 使用改进的精确转弯控制 - -这些改进有效解决了转弯后快速切换到零速度导致的角度偏移问题，提高了转弯的精度和稳定性。 -""" diff --git a/task_1/task_1.py b/task_1/task_1.py index d96aa2b..05a2fbd 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -1,5 +1,4 @@ import time -import cv2 import sys import os @@ -10,15 +9,21 @@ from base_move.move_base_hori_line import move_to_hori_line, arc_turn_around_hor observe = True def run_task_1(ctrl, msg): - print('Running task 1...') + print('🐶 Running task 1...') # v2 - arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=False, observe=observe) + print('😺 task 1 - 1') + arc_turn_around_hori_line(ctrl, msg, angle_deg=70, left=False, observe=observe) + print('😺 task 1 - 2') move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) - arc_turn_around_hori_line(ctrl, msg, angle_deg=180, left=True, observe=observe) + return + print('😺 task 1 - 3') + arc_turn_around_hori_line(ctrl, msg, angle_deg=160, left=True, observe=observe) + + print('😺 task 1 - 4') move_to_hori_line(ctrl, msg, observe=observe) diff --git a/utils/base_msg.py b/utils/base_msg.py index 495242b..c1b2e75 100644 --- a/utils/base_msg.py +++ b/utils/base_msg.py @@ -79,7 +79,6 @@ class BaseMsg: # 等待最后的命令完成 time.sleep(delay) - def stop_turn_smooth(self, current_vel=None, steps=5, delay=0.1, final_steps=2): """ 专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度