From 296a228c3b60687a977c4adb010cc9882d9faad1 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Thu, 15 May 2025 20:44:24 +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=E9=80=9F=E5=BA=A6=E5=92=8C=E5=87=8F=E9=80=9F?= =?UTF-8?q?=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 增加基础角速度至 0.8，提高旋转精度 - 应用时间补偿系数，减少实际运动时间至 92% - 调整减速阈值至目标角度的 80% - 修改速度系数下限至 0.2，允许更慢的减速 - 增加微调角度的补偿系数，确保最终角度更准确 --- base_move/move_base_hori_line.py | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 9d84fcf..8577feb 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -366,14 +366,20 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left angle_rad = math.radians(angle_deg) # 设定角速度（rad/s），可根据实际调整 - base_w = 0.6 + # 减小基础角速度，增加精度 + base_w = 0.8 # 原来是0.6 w = base_w if left else -base_w # 左转为正，右转为负 v = abs(w * r) # 线速度，正负号与角速度方向一致 t = abs(angle_rad / w) # 运动时间，取绝对值保证为正 - + + # 应用时间补偿系数，因为实际旋转常会比理论计算多 + time_compensation = 0.92 # 时间减少到92% + t *= time_compensation + if observe: - print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s, 运动时间: {t:.2f}s") + print(f"圆弧半径: {r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s") + print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s") # 4. 发送圆弧运动指令 msg.mode = 11 @@ -404,10 +410,10 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left # 旋转精度监控 target_angle = angle_rad if left else -angle_rad # 目标角度（弧度） - slowdown_threshold = 0.85 # 当达到目标角度的85%时开始减速 + slowdown_threshold = 0.8 # 当达到目标角度的80%时开始减速（原来是85%） # 监控旋转进度并自行控制减速 - while abs(angle_turned) < abs(angle_rad) * 0.98 and time.time() - start_time < timeout: + while abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout: # 计算已移动距离 current_position = ctrl.odo_msg.xyz dx = current_position[0] - start_position[0] @@ -430,8 +436,8 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left if completion_ratio > slowdown_threshold: # 剩余比例作为速度系数 speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold) - # 确保速度系数不会太小 - speed_factor = max(0.3, speed_factor) + # 确保速度系数不会太小，但可以更慢一些 + speed_factor = max(0.2, speed_factor) # 原来是0.3 # 应用减速 current_v = v * speed_factor @@ -487,6 +493,13 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left # 计算需要微调的角度 adjust_angle = target_angle_deg - final_angle_deg + # 增加补偿系数，因为实际旋转常会超出理论计算值 + compensation_factor = 0.85 # 只旋转计算值的85% + adjust_angle *= compensation_factor + + if observe: + print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度") + # 使用turn_degree函数进行微调 turn_degree(ctrl, msg, adjust_angle, absolute=False)