Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化 arc_turn_around_hori_line 函数，调整大角度旋转的补偿和减速逻辑

Browse Source 
- 修改大角度旋转时的时间补偿系数，防止过度旋转
- 针对大角度（如180度）调整减速阈值和停止条件，提升旋转精度
- 实现多阶段减速策略，确保在大角度旋转时的平稳性
- 增加微调逻辑，针对大角度误差进行两阶段校正，提升最终角度的准确性
- 更新调试信息输出，便于跟踪旋转过程中的状态变化
This commit is contained in:
Havoc 2025-05-16 14:09:52 +08:00
parent 790fe1fe0f
commit 9a53927cc5
2 changed files with 89 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

107
base_move/move_base_hori_line.py
View File

@ -413,7 +413,7 @@ 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.92 # 大角度保持原有补偿
time_compensation = 0.75 # 大角度(如180度)减小补偿系数，防止过度旋转
# 调整实际目标角度，针对不同角度应用不同的缩放比例
actual_angle_deg = angle_deg
@ -421,6 +421,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85%
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%，防止过度旋转
if observe and actual_angle_deg != angle_deg:
print(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度")
@ -429,7 +431,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
# 设定角速度（rad/s），可根据实际调整
# 减小基础角速度，增加精度
base_w = 0.8 # 原来是0.6
if angle_deg >= 170:
base_w = 0.6 # 大角度旋转时使用更小的基础角速度，提高精度
else:
base_w = 0.8 # 原来是0.6
w = base_w if left else -base_w # 左转为正，右转为负
v = abs(w * r) # 线速度，正负号与角速度方向一致
@ -470,16 +475,22 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
current_v = v
current_w = w
# 旋转精度监控
target_angle = angle_rad if left else -angle_rad # 目标角度（弧度）
# 对小角度提前开始减速
if angle_deg <= 70:
slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速
elif angle_deg >= 170: # 针对大角度(如180度)采用更早的减速时机
slowdown_threshold = 0.6 # 当达到目标角度的60%时开始减速
else:
slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%）
# 根据角度大小调整旋转停止阈值
if angle_deg >= 170:
rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转
else:
rotation_completion_threshold = 0.95 # 默认旋转到95%时停止
# 监控旋转进度并自行控制减速
while abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout:
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]
@ -510,10 +521,23 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
# 当完成一定比例时开始减速
if completion_ratio > slowdown_threshold:
# 剩余比例作为速度系数
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
# 确保速度系数不会太小，但可以更慢一些
speed_factor = max(0.15, speed_factor) # 原来是0.2
# 实现多阶段减速，特别针对大角度旋转
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
@ -528,6 +552,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
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:
@ -575,7 +603,13 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
# 检查是否需要微调
angle_error = abs(final_angle_deg - target_angle_deg)
if angle_error > 3.0: # 如果误差超过3度
# 针对不同角度使用不同的微调阈值
adjustment_threshold = 3.0 # 默认微调阈值
if angle_deg >= 170:
adjustment_threshold = 5.0 # 大角度旋转使用更大的微调阈值
if angle_error > adjustment_threshold: # 如果误差超过阈值
if observe:
print(f"角度误差: {angle_error:.2f}度，进行微调")
@ -585,16 +619,53 @@ 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 # 大角度使用更小的补偿因子，防止过度校正
else:
compensation_factor = 0.85 # 只旋转计算值的85%
compensation_factor = 0.85 # 一般角度只旋转计算值的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 angle_deg >= 170 and angle_error > 20:
# 先进行粗略校正，只校正70%
first_adjust_angle = adjust_angle * 0.7
if observe:
print(f"误差较大，进行两阶段校正，第一阶段调整: {first_adjust_angle:.2f}度")
turn_success = turn_degree(ctrl, msg, first_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)
remaining_error = target_angle_deg - current_angle_deg
if observe:
print(f"第一阶段校正后的角度: {current_angle_deg:.2f}度, 剩余误差: {remaining_error:.2f}度")
# 对剩余误差进行更精确的校正
if abs(remaining_error) > 2.0:
second_adjust_angle = remaining_error * 0.5 # 只校正一半
if observe:
print(f"进行第二阶段微调: {second_adjust_angle:.2f}度")
turn_success = turn_degree(ctrl, msg, second_adjust_angle, absolute=False)
else:
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 '失败'}")