优化 arc_turn_around_hori_line 函数，调整旋转时间和减速策略
- 减少前段旋转时间比例，从0.7降至0.65，降低过度旋转风险 - 调整总旋转时间，从1.2降至1.15，确保旋转过程更平稳 - 更新大角度旋转的减速系数，分别从0.7、0.4、0.2降至0.65、0.35、0.15，增强控制精度 - 修改小角度和大角度的补偿因子，提升校正的保守性 - 精简多阶段校正逻辑，优化大误差情况下的调整策略，确保最终角度的准确性
This commit is contained in:
parent
91c9896ad0
commit
59961b9905
@ -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 '失败'}")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user