✨ 在 move_base_hori_line.py 中优化了 align_to_horizontal_line 函数，简化了代码逻辑，调整了旋转角度的计算和限制，增强了校准过程的稳定性。同时，改进了日志记录信息，提升了调试的可读性和准确性。
This commit is contained in:
parent
f77b7edb13
commit
6b15e9be90
@ -31,12 +31,11 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
attempts = 0
|
||||
aligned = False
|
||||
image = ctrl.image_processor.get_current_image()
|
||||
last_angle_to_rotate = 0
|
||||
accumulated_angle = 0 # 添加累积角度跟踪
|
||||
max_total_rotation = 45 # 防止过度累积旋转
|
||||
angle_accuracy_threshold = 2.0 # 改为更宽松的阈值
|
||||
accumulated_angle = 0
|
||||
max_total_rotation = 45
|
||||
angle_accuracy_threshold = 2.0
|
||||
|
||||
# 记录初始位置，用于可能的回退
|
||||
# 记录初始位置
|
||||
initial_yaw = ctrl.odo_msg.rpy[2]
|
||||
|
||||
while attempts < max_attempts and not aligned:
|
||||
@ -50,15 +49,14 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
|
||||
# 尝试小幅度摇头寻找横线
|
||||
if attempts < max_attempts - 1:
|
||||
small_angle = 5 * (1 if attempts % 2 == 0 else -1) # 左右交替摇头
|
||||
small_angle = 5 * (1 if attempts % 2 == 0 else -1)
|
||||
info(f"尝试摇头 {small_angle}度 寻找横线", "校准")
|
||||
turn_degree(ctrl, msg, small_angle, absolute=False)
|
||||
time.sleep(0.5) # 等待稳定
|
||||
time.sleep(0.5)
|
||||
|
||||
attempts += 1
|
||||
continue
|
||||
|
||||
# 获取检测到的斜率和其他信息
|
||||
slope = edge_info["slope"]
|
||||
is_horizontal = edge_info["is_horizontal"]
|
||||
|
||||
@ -66,80 +64,54 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
debug(f"检测到横向线，斜率: {slope:.6f}", "检测")
|
||||
info(f"是否足够水平: {is_horizontal}", "检测")
|
||||
|
||||
# 如果已经水平，则无需旋转
|
||||
if is_horizontal:
|
||||
success("横向线已经水平，无需校准", "完成")
|
||||
return True
|
||||
|
||||
# 计算需要旋转的角度
|
||||
# 斜率 = tan(θ)，因此 θ = arctan(斜率)
|
||||
angle_rad = math.atan(slope)
|
||||
angle_deg = math.degrees(angle_rad)
|
||||
angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||||
|
||||
# 调整角度方向
|
||||
# 正的斜率意味着线条从左到右上升，需要逆时针旋转校正
|
||||
# 负的斜率意味着线条从左到右下降，需要顺时针旋转校正
|
||||
# 注意旋转方向: 顺时针为负角度，逆时针为正角度
|
||||
raw_angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||||
# 设置最小旋转阈值
|
||||
if abs(angle_to_rotate) < 0.5: # 降低最小旋转阈值
|
||||
angle_to_rotate = math.copysign(0.5, angle_to_rotate)
|
||||
|
||||
# 动态调整补偿系数：对于反复校准同一方向的情况，增加补偿
|
||||
if last_angle_to_rotate * raw_angle_to_rotate > 0: # 如果方向一致
|
||||
# 原来的角度太小，需要更大的补偿
|
||||
compensation_factor = 1.3 + min(0.3, attempts * 0.1) # 随着尝试次数增加补偿
|
||||
else:
|
||||
# 方向不一致，可能过度补偿，降低系数
|
||||
compensation_factor = 0.8
|
||||
# 限制单次旋转角度
|
||||
angle_to_rotate = max(-15, min(15, angle_to_rotate))
|
||||
|
||||
# 应用补偿
|
||||
angle_to_rotate = raw_angle_to_rotate * compensation_factor
|
||||
|
||||
# 避免角度过小导致效果不明显
|
||||
if abs(angle_to_rotate) < 2.0:
|
||||
# 对小角度增加最小旋转阈值
|
||||
angle_to_rotate = math.copysign(2.0, angle_to_rotate)
|
||||
|
||||
# 限制单次旋转角度，避免过度旋转
|
||||
angle_to_rotate = max(-20, min(20, angle_to_rotate))
|
||||
|
||||
# 检查累积旋转是否过大
|
||||
# 检查累积旋转
|
||||
if abs(accumulated_angle + angle_to_rotate) > max_total_rotation:
|
||||
warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告")
|
||||
# 如果过大，只旋转一半的角度
|
||||
angle_to_rotate = angle_to_rotate / 2
|
||||
|
||||
if observe:
|
||||
info(f"原始角度: {raw_angle_to_rotate:.2f}度, 补偿后: {angle_to_rotate:.2f}度", "角度")
|
||||
info(f"计算旋转角度: {angle_to_rotate:.2f}度", "角度")
|
||||
info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积")
|
||||
|
||||
# 使用turn_degree函数执行旋转，增加精度参数
|
||||
# 执行旋转
|
||||
turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True)
|
||||
|
||||
# 等待稳定
|
||||
time.sleep(0.3)
|
||||
|
||||
# 旋转后实际改变的角度
|
||||
# 计算实际旋转角度
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
actual_rotation = math.degrees(current_yaw - initial_yaw - math.radians(accumulated_angle))
|
||||
|
||||
# 规范化到 -180 到 180 度范围
|
||||
# 规范化角度
|
||||
while actual_rotation > 180:
|
||||
actual_rotation -= 360
|
||||
while actual_rotation < -180:
|
||||
actual_rotation += 360
|
||||
|
||||
# 更新累积角度
|
||||
accumulated_angle += actual_rotation
|
||||
|
||||
if observe:
|
||||
info(f"请求旋转: {angle_to_rotate:.2f}度, 实际旋转: {actual_rotation:.2f}度", "旋转")
|
||||
info(f"旋转结果: {'成功' if turn_success else '失败'}", "成功" if turn_success else "失败")
|
||||
|
||||
# 增加尝试次数
|
||||
attempts += 1
|
||||
last_angle_to_rotate = angle_to_rotate
|
||||
|
||||
# 判断成功条件：基于当前检测到的斜率，而不仅是运动控制的结果
|
||||
# 重新获取图像和检测横线
|
||||
# 检查校准结果
|
||||
edge_point_after, edge_info_after = detect_horizontal_track_edge(
|
||||
ctrl.image_processor.get_current_image(),
|
||||
observe=observe,
|
||||
@ -154,28 +126,25 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
if observe:
|
||||
info(f"校准后斜率: {current_slope:.6f}, 角度: {current_angle_deg:.2f}度", "检测")
|
||||
|
||||
# 使用更宽松的阈值判断成功
|
||||
if abs(current_angle_deg) < angle_accuracy_threshold:
|
||||
success(f"校准成功，当前角度: {current_angle_deg:.2f}度", "完成")
|
||||
aligned = True
|
||||
else:
|
||||
warning(f"校准后角度仍超出阈值: {abs(current_angle_deg):.2f}° > {angle_accuracy_threshold}°", "警告")
|
||||
|
||||
# 如果到达最大尝试次数且未对齐，但最后一次接近成功，也返回成功
|
||||
# 最后一次尝试的妥协处理
|
||||
if attempts == max_attempts and not aligned and edge_info_after:
|
||||
current_slope = edge_info_after["slope"]
|
||||
current_angle_deg = math.degrees(math.atan(current_slope))
|
||||
|
||||
# 如果接近水平，放宽标准接受这个结果
|
||||
if abs(current_angle_deg) < angle_accuracy_threshold * 1.5: # 允许1.5倍阈值
|
||||
if abs(current_angle_deg) < angle_accuracy_threshold * 1.5:
|
||||
warning(f"达到最大尝试次数，接受近似结果，当前角度: {current_angle_deg:.2f}度", "妥协")
|
||||
aligned = True
|
||||
|
||||
# 如果无法对齐但已累积大量旋转，尝试恢复到最佳状态
|
||||
# 恢复最佳状态的处理
|
||||
if not aligned and abs(accumulated_angle) > 15:
|
||||
warning(f"校准失败，累积旋转{accumulated_angle:.2f}度，尝试恢复到最佳状态", "恢复")
|
||||
|
||||
# 重新检测一次
|
||||
best_edge_point, best_edge_info = detect_horizontal_track_edge(
|
||||
ctrl.image_processor.get_current_image(),
|
||||
observe=observe,
|
||||
@ -186,7 +155,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
best_slope = best_edge_info["slope"]
|
||||
best_angle_deg = math.degrees(math.atan(best_slope))
|
||||
|
||||
# 如果当前状态已经是最好的，不进行调整
|
||||
if abs(best_angle_deg) < angle_accuracy_threshold * 1.5:
|
||||
info(f"当前状态已经接近水平，角度: {best_angle_deg:.2f}度", "保持")
|
||||
return True
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user