From 59961b99052311649458a75a6051b1288915b895 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Fri, 16 May 2025 16:09:08 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20arc=5Fturn=5Faround=5Fhori?= =?UTF-8?q?=5Fline=20=E5=87=BD=E6=95=B0=EF=BC=8C=E8=B0=83=E6=95=B4?= =?UTF-8?q?=E6=97=8B=E8=BD=AC=E6=97=B6=E9=97=B4=E5=92=8C=E5=87=8F=E9=80=9F?= =?UTF-8?q?=E7=AD=96=E7=95=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 减少前段旋转时间比例，从0.7降至0.65，降低过度旋转风险 - 调整总旋转时间，从1.2降至1.15，确保旋转过程更平稳 - 更新大角度旋转的减速系数，分别从0.7、0.4、0.2降至0.65、0.35、0.15，增强控制精度 - 修改小角度和大角度的补偿因子，提升校正的保守性 - 精简多阶段校正逻辑，优化大误差情况下的调整策略，确保最终角度的准确性 --- base_move/move_base_hori_line.py | 107 +++++++++++++------------------ 1 file changed, 44 insertions(+), 63 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index baf250f..ebf3003 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -539,8 +539,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 发送固定时间的旋转命令 start_time = time.time() - # 前段使用全速，延长前段旋转时间 - first_segment_time = rotation_time * 0.7 # 延长前段时间 + # 前段使用全速，但减少前段旋转时间比例，减少过度旋转 + first_segment_time = rotation_time * 0.65 # 原来是0.7，减少到0.65 elapsed_time = 0 while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2: # 加2秒保护 @@ -585,8 +585,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc msg.life_count += 1 ctrl.Send_cmd(msg) - # 继续旋转直到总时间结束，增加总旋转时间 - total_rotation_time = rotation_time * 1.2 # 增加20%的总旋转时间 + # 继续旋转直到总时间结束，减少总旋转时间 + total_rotation_time = rotation_time * 1.15 # 原来是1.2，减少到1.15 while time.time() - start_time < total_rotation_time: # 计算已旋转角度（仅用于打印） if observe and time.time() % 0.5 < 0.02: @@ -649,11 +649,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 实现多阶段减速，特别针对大角度旋转 if angle_deg >= 170: if completion_ratio < 0.7: # 第一阶段：60%-70% - speed_factor = 0.7 + speed_factor = 0.65 # 原来是0.7，减少到0.65 elif completion_ratio < 0.8: # 第二阶段：70%-80% - speed_factor = 0.4 + speed_factor = 0.35 # 原来是0.4，减少到0.35 else: # 第三阶段：80%以上 - speed_factor = 0.2 + speed_factor = 0.15 # 原来是0.2，减少到0.15 else: # 标准减速逻辑保持不变 # 剩余比例作为速度系数 @@ -758,59 +758,35 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 增加补偿系数，因为实际旋转常会超出理论计算值 if angle_deg <= 70: - compensation_factor = 0.7 # 小角度使用更小的补偿因子 + compensation_factor = 0.55 # 比之前更保守，从0.65降至0.55 elif angle_deg >= 170: # 单独处理180度附近的大角度 - # 动态调整补偿因子，误差越大，补偿系数越小 + # 动态调整补偿因子，更保守处理 if angle_error > 60: # 如果误差超过60度 - compensation_factor = 0.7 # 使用更大的补偿因子 + compensation_factor = 0.4 # 从0.5降至0.4 elif angle_error > 30: # 如果误差超过30度 - compensation_factor = 0.6 + compensation_factor = 0.35 # 从0.4降至0.35 else: - compensation_factor = 0.5 # 小误差使用更小的补偿因子，防止过度校正 + compensation_factor = 0.25 # 从0.3降至0.25，更保守 else: - compensation_factor = 0.85 # 一般角度只旋转计算值的85% + compensation_factor = 0.6 # 从0.7降至0.6 - # 对于特别大的误差进行多次微调 - 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}度，占总误差的{first_adjust_ratio*100:.0f}%") - - turn_success = turn_degree(ctrl, msg, first_adjust_angle, absolute=False) - - # 等待稳定后再次检查角度 - time.sleep(1.0) # 增加等待时间 - - # 计算剩余误差 - 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) - remaining_error = target_angle_deg - current_angle_deg + # 对于特别大的误差进行更保守的单次校准 + if angle_deg >= 170 and angle_error > 30: + # 大误差直接一步校准，使用更保守的补偿系数 + adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15% + adjust_angle = adjust_angle * adjusted_compensation if observe: - print(f"第一阶段校正后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度") + print(f"误差较大({angle_error:.2f}度)，使用更保守的补偿系数{adjusted_compensation:.2f}进行一次性校准: {adjust_angle:.2f}度") - # 对剩余误差进行更精确的校正 - 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}度，占剩余误差的{second_adjust_ratio*100:.0f}%") - - turn_success = turn_degree(ctrl, msg, second_adjust_angle, absolute=False) - - # 等待稳定后再次检查角度 - time.sleep(0.8) - - # 计算剩余误差 + # 使用turn_degree函数进行微调 + turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False) + + # 等待稳定 + time.sleep(1.0) + + # 再次检查剩余误差(仅用于显示) + if observe: current_yaw = ctrl.odo_msg.rpy[2] current_angle_turned = current_yaw - start_yaw # 角度归一化处理 @@ -820,18 +796,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc 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) + remaining_error = target_angle_deg - current_angle_deg + print(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度") else: adjust_angle *= compensation_factor @@ -840,6 +806,21 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 使用turn_degree函数进行微调 turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False) + + # 等待稳定(仅用于显示) + if observe: + 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) + remaining_error = target_angle_deg - current_angle_deg + print(f"校准后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度") if observe: print(f"微调结果: {'成功' if turn_success else '失败'}")