优化 arc_turn_around_hori_line 函数，调整大角度旋转的时间补偿和减速策略
- 增加180度旋转的时间补偿系数，确保旋转时间更长 - 调整大角度旋转的基础角速度和减速阈值，提升旋转精度 - 实现多阶段校正逻辑，针对较大误差进行分阶段微调，确保最终角度的准确性 - 更新调试信息，便于实时监控旋转过程中的状态变化
This commit is contained in:
parent
19d583e4d8
commit
95b426bf29
@ -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
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user