diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 8ccfe99..8537e92 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -413,8 +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: - # 对180度旋转，这里完全不用补偿，直接使用原始角度 - time_compensation = 1.0 + # 对180度旋转，增加补偿系数，使旋转时间更长 + time_compensation = 1.4 # 增加40%的时间 # 调整实际目标角度，针对不同角度应用不同的缩放比例 actual_angle_deg = angle_deg @@ -433,7 +433,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 设定角速度（rad/s），可根据实际调整 # 减小基础角速度，增加精度 if angle_deg >= 170: - base_w = 0.6 # 大角度旋转时使用更小的基础角速度，提高精度 + base_w = 0.5 # 大角度旋转时使用更小的基础角速度，提高精度 else: base_w = 0.8 # 原来是0.6 @@ -524,8 +524,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 对小角度提前开始减速 if angle_deg <= 70: slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速 - elif angle_deg >= 170: # 针对大角度(如180度)采用更早的减速时机 - slowdown_threshold = 0.6 # 当达到目标角度的60%时开始减速 + elif angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机 + slowdown_threshold = 0.7 # 当达到目标角度的70%时才开始减速 else: slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%） @@ -546,11 +546,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 发送固定时间的旋转命令 start_time = time.time() - # 前半段使用全速 - half_time = rotation_time * 0.6 + # 前段使用全速，延长前段旋转时间 + first_segment_time = rotation_time * 0.7 # 延长前段时间 elapsed_time = 0 - while elapsed_time < half_time and time.time() - start_time < rotation_time + 1: # 加1秒保护 + while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2: # 加2秒保护 elapsed_time = time.time() - start_time # 计算已旋转角度（仅用于打印） @@ -576,12 +576,12 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc time.sleep(0.05) - # 后半段减速 + # 后段减速，减少减速幅度，保持更高速度 if observe: print(f"进入减速阶段") - # 减速到50%，保持前进速度和角速度的比例关系 - reduced_w = w * 0.5 + # 减速到70%，保持前进速度和角速度的比例关系 + reduced_w = w * 0.7 # 原来是0.5 reduced_v = abs(reduced_w * effective_r) # 维持圆弧关系 msg.mode = 11 @@ -592,8 +592,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc msg.life_count += 1 ctrl.Send_cmd(msg) - # 继续旋转直到总时间结束 - while time.time() - start_time < rotation_time: + # 继续旋转直到总时间结束，增加总旋转时间 + total_rotation_time = rotation_time * 1.2 # 增加20%的总旋转时间 + while time.time() - start_time < total_rotation_time: # 计算已旋转角度（仅用于打印） if observe and time.time() % 0.5 < 0.02: current_yaw = ctrl.odo_msg.rpy[2] @@ -603,7 +604,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc 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") + print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {time.time() - start_time:.2f}s/{total_rotation_time:.2f}s") # 检查QR码扫描结果（如果启用） if scan_qrcode: @@ -753,7 +754,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 针对不同角度使用不同的微调阈值 adjustment_threshold = 3.0 # 默认微调阈值 if angle_deg >= 170: - adjustment_threshold = 5.0 # 大角度旋转使用更大的微调阈值 + adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确 if angle_error > adjustment_threshold: # 如果误差超过阈值 if observe: @@ -766,21 +767,28 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc if angle_deg <= 70: compensation_factor = 0.7 # 小角度使用更小的补偿因子 elif angle_deg >= 170: # 单独处理180度附近的大角度 - compensation_factor = 0.5 # 大角度使用更小的补偿因子，防止过度校正 + # 动态调整补偿因子，误差越大，补偿系数越小 + if angle_error > 60: # 如果误差超过60度 + compensation_factor = 0.7 # 使用更大的补偿因子 + elif angle_error > 30: # 如果误差超过30度 + compensation_factor = 0.6 + else: + compensation_factor = 0.5 # 小误差使用更小的补偿因子，防止过度校正 else: compensation_factor = 0.85 # 一般角度只旋转计算值的85% # 对于特别大的误差进行多次微调 - if angle_deg >= 170 and angle_error > 20: - # 先进行粗略校正，只校正70% - first_adjust_angle = adjust_angle * 0.7 + if angle_deg >= 170 and angle_error > 30: # 降低阈值到30度，更容易触发多阶段校正 + # 先进行粗略校正 + first_adjust_ratio = 0.8 # 第一阶段校正更大比例 + first_adjust_angle = adjust_angle * first_adjust_ratio if observe: - print(f"误差较大，进行两阶段校正，第一阶段调整: {first_adjust_angle:.2f}度") + print(f"误差较大，进行多阶段校正，第一阶段调整: {first_adjust_angle:.2f}度，占总误差的{first_adjust_ratio*100:.0f}%") turn_success = turn_degree(ctrl, msg, first_adjust_angle, absolute=False) # 等待稳定后再次检查角度 - time.sleep(0.8) + time.sleep(1.0) # 增加等待时间 # 计算剩余误差 current_yaw = ctrl.odo_msg.rpy[2] @@ -798,12 +806,39 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc print(f"第一阶段校正后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度") # 对剩余误差进行更精确的校正 - if abs(remaining_error) > 2.0: - second_adjust_angle = remaining_error * 0.5 # 只校正一半 + if abs(remaining_error) > 5.0: # 修改阈值，更容易触发二阶段校正 + second_adjust_ratio = 0.7 # 第二阶段校正比例 + second_adjust_angle = remaining_error * second_adjust_ratio if observe: - print(f"进行第二阶段微调: {second_adjust_angle:.2f}度") + print(f"进行第二阶段微调: {second_adjust_angle:.2f}度，占剩余误差的{second_adjust_ratio*100:.0f}%") turn_success = turn_degree(ctrl, msg, second_adjust_angle, absolute=False) + + # 等待稳定后再次检查角度 + time.sleep(0.8) + + # 计算剩余误差 + current_yaw = ctrl.odo_msg.rpy[2] + current_angle_turned = current_yaw - start_yaw + # 角度归一化处理 + while current_angle_turned > math.pi: + current_angle_turned -= 2 * math.pi + while current_angle_turned < -math.pi: + current_angle_turned += 2 * math.pi + + current_angle_deg = math.degrees(current_angle_turned) + final_remaining_error = target_angle_deg - current_angle_deg + + if observe: + print(f"第二阶段校正后的角度: {current_angle_deg:.2f}度, 最终剩余误差: {final_remaining_error:.2f}度") + + # 如果仍有较大误差，进行第三阶段微调 + if abs(final_remaining_error) > 10.0: + third_adjust_angle = final_remaining_error * 0.5 + if observe: + print(f"进行第三阶段微调: {third_adjust_angle:.2f}度") + + turn_success = turn_degree(ctrl, msg, third_adjust_angle, absolute=False) else: adjust_angle *= compensation_factor