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

优化 arc_turn_around_hori_line 函数，调整减速和旋转策略

Browse Source 
- 将大角度减速阈值从0.7调整为0.8，旋转完成阈值从0.85调整为0.92
- 增加前段旋转时间比例，从0.65提高到0.75，确保更平稳的旋转
- 将减速比例从0.7提高到0.8，增强前进速度与角速度的比例关系
- 调整总旋转时间，从1.15增加到1.25，确保旋转过程的稳定性
- 更新大角度的速度系数，提升控制精度，确保最终角度的准确性
This commit is contained in:
Havoc 2025-05-18 12:25:19 +08:00
parent f467122e25
commit 44c83bc1d4
1 changed files with 18 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
base_move/move_base_hori_line.py
View File

@ -531,13 +531,13 @@ 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.7 # 当达到目标角度的70%时才开始减速
slowdown_threshold = 0.8 # 当达到目标角度的70%时才开始减速，改为80%
else:
slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%）
# 根据角度大小调整旋转停止阈值
if angle_deg >= 170:
rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转
rotation_completion_threshold = 0.92 # 大角度旋转提前停止，从0.85改为0.92
else:
rotation_completion_threshold = 0.95 # 默认旋转到95%时停止
@ -553,7 +553,7 @@ 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.65 # 原来是0.7，减少到0.65
first_segment_time = rotation_time * 0.75 # 原来是0.65，增加到0.75
elapsed_time = 0
while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2: # 加2秒保护
@ -586,8 +586,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
if observe:
info(f"进入减速阶段", "旋转")
# 减速到70%，保持前进速度和角速度的比例关系
reduced_w = w * 0.7 # 原来是0.5
# 减速到80%，保持前进速度和角速度的比例关系
reduced_w = w * 0.8 # 原来是0.7，提高到0.8
reduced_v = abs(reduced_w * effective_r) # 维持圆弧关系
msg.mode = 11
@ -598,8 +598,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.15 # 原来是1.2，减少到1.15
# 继续旋转直到总时间结束，增加总旋转时间
total_rotation_time = rotation_time * 1.25 # 原来是1.15，增加到1.25
while time.time() - start_time < total_rotation_time:
# 计算已旋转角度（仅用于打印）
if observe and time.time() % 0.5 < 0.02:
@ -662,11 +662,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.65 # 原来是0.7，减少到0.65
speed_factor = 0.75 # 原来是0.65，提高到0.75
elif completion_ratio < 0.8: # 第二阶段：70%-80%
speed_factor = 0.35 # 原来是0.4，减少到0.35
speed_factor = 0.45 # 原来是0.35，提高到0.45
else: # 第三阶段：80%以上
speed_factor = 0.15 # 原来是0.2，减少到0.15
speed_factor = 0.25 # 原来是0.15，提高到0.25
else:
# 标准减速逻辑保持不变
# 剩余比例作为速度系数
@ -764,7 +764,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 = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确
adjustment_threshold = 5.0 # 大角度旋转使用更大的微调阈值，从10.0改为5.0
if angle_error > adjustment_threshold: # 如果误差超过阈值
if observe:
@ -779,22 +779,22 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
elif angle_deg >= 170: # 单独处理180度附近的大角度
# 动态调整补偿因子，更保守处理
if angle_error > 60: # 如果误差超过60度
compensation_factor = 0.4 # 从0.5降至0.4
compensation_factor = 0.45 # 从0.4升至0.45
elif angle_error > 30: # 如果误差超过30度
compensation_factor = 0.35 # 从0.4降至0.35
compensation_factor = 0.4 # 从0.35升至0.4
else:
compensation_factor = 0.25 # 从0.3降至0.25，更保守
compensation_factor = 0.35 # 从0.25升至0.35，更积极
else:
compensation_factor = 0.6 # 从0.7降至0.6
# 对于特别大的误差进行更保守的单次校准
# 对于特别大的误差进行更积极的单次校准
if angle_deg >= 170 and angle_error > 30:
# 大误差直接一步校准，使用更保守的补偿系数
adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15%
# 大误差直接一步校准，使用更积极的补偿系数
adjusted_compensation = compensation_factor * 0.95 # 在已有补偿基础上只降低5%
adjust_angle = adjust_angle * adjusted_compensation
if observe:
info(f"误差较大({angle_error:.2f}度)，使用更保守的补偿系数{adjusted_compensation:.2f}进行一次性校准: {adjust_angle:.2f}度", "角度")
info(f"误差较大({angle_error:.2f}度)，使用更积极的补偿系数{adjusted_compensation:.2f}进行一次性校准: {adjust_angle:.2f}度", "角度")
# 使用turn_degree函数进行微调
turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)