✨ 在 move_base_hori_line.py 中优化了 align_to_horizontal_line 函数，添加了累积角度跟踪和动态补偿机制，增强了校准过程的稳定性和准确性。同时在 turn_degree.py 中引入了精确模式，改进了旋转控制逻辑，提升了旋转的精度和稳定性，确保机器人在执行旋转时能够更好地适应不同的情况。
This commit is contained in:
parent
aac055c1e0
commit
f77b7edb13
@ -31,15 +31,32 @@ 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 # 改为更宽松的阈值
|
||||
|
||||
# 记录初始位置，用于可能的回退
|
||||
initial_yaw = ctrl.odo_msg.rpy[2]
|
||||
|
||||
while attempts < max_attempts and not aligned:
|
||||
section(f"尝试校准横线 {attempts+1}/{max_attempts}", "校准")
|
||||
|
||||
# 检测横向线边缘
|
||||
edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=observe, delay=1000 if observe else 0, save_log=True)
|
||||
|
||||
if edge_point is None or edge_info is None:
|
||||
error("未检测到横向线，无法进行校准", "失败")
|
||||
return False
|
||||
|
||||
# 尝试小幅度摇头寻找横线
|
||||
if attempts < max_attempts - 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) # 等待稳定
|
||||
|
||||
attempts += 1
|
||||
continue
|
||||
|
||||
# 获取检测到的斜率和其他信息
|
||||
slope = edge_info["slope"]
|
||||
@ -63,36 +80,116 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
|
||||
# 正的斜率意味着线条从左到右上升，需要逆时针旋转校正
|
||||
# 负的斜率意味着线条从左到右下降，需要顺时针旋转校正
|
||||
# 注意旋转方向: 顺时针为负角度，逆时针为正角度
|
||||
angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||||
raw_angle_to_rotate = -angle_deg # 取负值使旋转方向正确
|
||||
|
||||
# 动态调整补偿系数：对于反复校准同一方向的情况，增加补偿
|
||||
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 = 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"需要旋转的角度: {angle_to_rotate:.2f}度", "角度")
|
||||
info(f"原始角度: {raw_angle_to_rotate:.2f}度, 补偿后: {angle_to_rotate:.2f}度", "角度")
|
||||
info(f"当前累积旋转: {accumulated_angle:.2f}度", "累积")
|
||||
|
||||
# 执行旋转
|
||||
# 如果角度很小，增加一个小的偏移以确保旋转足够
|
||||
if abs(angle_to_rotate) < 3.0:
|
||||
angle_to_rotate *= 1.5 # 对小角度进行放大以确保效果
|
||||
# 使用turn_degree函数执行旋转，增加精度参数
|
||||
turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False, precision=True)
|
||||
|
||||
# 限制旋转角度，避免过度旋转
|
||||
angle_to_rotate = max(-30, min(30, angle_to_rotate))
|
||||
# 等待稳定
|
||||
time.sleep(0.3)
|
||||
|
||||
# 使用turn_degree函数执行旋转
|
||||
turn_success = turn_degree(ctrl, msg, angle_to_rotate, absolute=False)
|
||||
# 旋转后实际改变的角度
|
||||
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
|
||||
|
||||
# 在旋转后重新获取图像，这里需要调用获取图像的函数
|
||||
# 代码中没有提供获取实时图像的方法，假设每次外部会更新image参数
|
||||
# 判断成功条件：基于当前检测到的斜率，而不仅是运动控制的结果
|
||||
# 重新获取图像和检测横线
|
||||
edge_point_after, edge_info_after = detect_horizontal_track_edge(
|
||||
ctrl.image_processor.get_current_image(),
|
||||
observe=observe,
|
||||
delay=1000 if observe else 0,
|
||||
save_log=True
|
||||
)
|
||||
|
||||
# 检查是否已经对齐
|
||||
# 对于实际应用，应该在旋转后重新捕获图像并检测横向线
|
||||
# 这里简单地根据旋转是否成功和旋转角度是否足够小来判断
|
||||
if turn_success and abs(angle_to_rotate) < 5.0:
|
||||
aligned = True
|
||||
if edge_info_after and "slope" in edge_info_after:
|
||||
current_slope = edge_info_after["slope"]
|
||||
current_angle_deg = math.degrees(math.atan(current_slope))
|
||||
|
||||
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倍阈值
|
||||
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,
|
||||
save_log=True
|
||||
)
|
||||
|
||||
if best_edge_info and "slope" in best_edge_info:
|
||||
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
|
||||
|
||||
return aligned
|
||||
|
||||
|
||||
@ -9,7 +9,7 @@ from utils.log_helper import LogHelper, get_logger, section, info, debug, warnin
|
||||
# 创建本模块特定的日志记录器
|
||||
logger = get_logger("旋转控制")
|
||||
|
||||
def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False):
|
||||
"""
|
||||
结合里程计实现精确稳定的旋转指定角度
|
||||
|
||||
@ -18,6 +18,7 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
msg: robot_control_cmd_lcmt 对象，用于发送命令
|
||||
degree: 要旋转的角度，正值表示逆时针，负值表示顺时针，默认为90度
|
||||
absolute: 是否绝对角度，默认为 False
|
||||
precision: 是否启用高精度模式，该模式下会使用更慢的速度和更细致的微调，默认为 False
|
||||
|
||||
返回:
|
||||
Bool: 是否成功旋转到指定角度
|
||||
@ -41,7 +42,8 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
target_yaw += 2 * math.pi
|
||||
|
||||
# 定义允许的误差范围（弧度）
|
||||
limit = 0.04 # 约2.3度
|
||||
# 如果是精确模式，使用更小的误差阈值
|
||||
limit = 0.03 if precision else 0.04 # 约1.7度或2.3度
|
||||
|
||||
# 计算最短旋转方向和距离
|
||||
def circle_dist(target, location):
|
||||
@ -63,22 +65,77 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
# 主要转向
|
||||
const_int = 2470 # 转1.57弧度约需2470的duration值
|
||||
|
||||
# 精确模式下使用更小的旋转速度
|
||||
turn_speed = 0.3 if precision else 0.5
|
||||
|
||||
# 如果角度很小且使用精确模式，进一步降低速度
|
||||
if precision and abs(dist) < 0.2: # 约11.5度
|
||||
turn_speed = 0.2
|
||||
|
||||
# 设置转向命令
|
||||
msg.mode = 11 # Locomotion模式
|
||||
msg.gait_id = 26 # 自变频步态
|
||||
msg.vel_des = [0, 0, 0.5 if direction > 0 else -0.5] # 转向速度
|
||||
msg.duration = int(const_int * abs(dist))
|
||||
msg.vel_des = [0, 0, turn_speed if direction > 0 else -turn_speed] # 转向速度
|
||||
|
||||
# 精确模式下延长转向时间以保证稳定性
|
||||
duration_factor = 1.2 if precision else 1.0
|
||||
msg.duration = int(const_int * abs(dist) * duration_factor)
|
||||
msg.step_height = [0.06, 0.06] # 抬腿高度
|
||||
msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
ctrl.Send_cmd(msg)
|
||||
debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转")
|
||||
debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 速度={turn_speed}, 持续时间={msg.duration}", "旋转")
|
||||
|
||||
# 等待转向完成
|
||||
wait_time = 7 * abs(dist) / 1.57
|
||||
# 精确模式下增加等待时间
|
||||
wait_factor = 1.3 if precision else 1.0
|
||||
wait_time = 7 * abs(dist) / 1.57 * wait_factor
|
||||
debug(f"等待旋转完成: {wait_time:.2f}秒", "时间")
|
||||
time.sleep(wait_time)
|
||||
|
||||
# 精确模式下使用实时监控而不是固定等待时间
|
||||
if precision and abs(dist) > 0.1: # 对于较大角度
|
||||
start_time = time.time()
|
||||
last_yaw = current_yaw
|
||||
stable_count = 0
|
||||
timeout = wait_time + 3 # 增加超时保护
|
||||
|
||||
# 实时监控旋转进度
|
||||
while time.time() - start_time < timeout:
|
||||
time.sleep(0.1) # 频繁检查
|
||||
|
||||
current_yaw_now = ctrl.odo_msg.rpy[2]
|
||||
# 计算已旋转角度
|
||||
rotated = abs(current_yaw_now - last_yaw)
|
||||
if rotated > math.pi:
|
||||
rotated = 2 * math.pi - rotated
|
||||
|
||||
# 如果旋转速度很小（几乎停止）
|
||||
if rotated < 0.01: # 约0.6度
|
||||
stable_count += 1
|
||||
else:
|
||||
stable_count = 0
|
||||
|
||||
last_yaw = current_yaw_now
|
||||
|
||||
# 如果机器人稳定一段时间，认为旋转完成
|
||||
if stable_count >= 3:
|
||||
debug(f"检测到旋转已稳定，提前结束等待", "旋转")
|
||||
break
|
||||
|
||||
# 每0.5秒打印一次进度
|
||||
elapsed = time.time() - start_time
|
||||
if int(elapsed * 2) % 2 == 0:
|
||||
remaining_yaw = target_yaw - current_yaw_now
|
||||
# 标准化到 [-pi, pi]
|
||||
if remaining_yaw > math.pi:
|
||||
remaining_yaw -= 2 * math.pi
|
||||
if remaining_yaw < -math.pi:
|
||||
remaining_yaw += 2 * math.pi
|
||||
debug(f"旋转进度: {elapsed:.1f}s/{wait_time:.1f}s, 剩余角度: {math.degrees(remaining_yaw):.2f}°", "进度")
|
||||
else:
|
||||
# 非精确模式使用固定等待时间
|
||||
time.sleep(wait_time)
|
||||
|
||||
# 获取当前角度
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
@ -94,23 +151,35 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
# 精细调整（如果误差大于限制）
|
||||
if abs(remaining_dist) > limit:
|
||||
warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度")
|
||||
|
||||
# 进行微调
|
||||
const_int_tiny = 1200 # 微调使用较小的系数
|
||||
const_int_tiny = 1200 if not precision else 1000 # 精确模式下使用更小的系数
|
||||
|
||||
# 精确模式下使用更小的微调速度
|
||||
fine_turn_speed = 0.3 if precision else 0.5
|
||||
|
||||
# 如果剩余误差很小，进一步降低速度
|
||||
if abs(remaining_dist) < 0.1: # 约5.7度
|
||||
fine_turn_speed = 0.15
|
||||
|
||||
# 设置微调命令
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [0, 0, 0.5 if remaining_dist > 0 else -0.5]
|
||||
msg.duration = int(const_int_tiny * abs(remaining_dist))
|
||||
msg.vel_des = [0, 0, fine_turn_speed if remaining_dist > 0 else -fine_turn_speed]
|
||||
|
||||
# 精确模式下使用更长的微调时间
|
||||
duration_factor = 1.25 if precision else 1.0
|
||||
msg.duration = int(const_int_tiny * abs(remaining_dist) * duration_factor)
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
ctrl.Send_cmd(msg)
|
||||
debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 持续时间={msg.duration}", "旋转")
|
||||
debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 速度={fine_turn_speed}, 持续时间={msg.duration}", "旋转")
|
||||
|
||||
# 等待微调完成
|
||||
time.sleep(5)
|
||||
# 精确模式下等待更长时间
|
||||
wait_time = 5 if not precision else 7
|
||||
time.sleep(wait_time)
|
||||
|
||||
# 获取最终角度
|
||||
final_yaw = ctrl.odo_msg.rpy[2]
|
||||
@ -126,6 +195,51 @@ def turn_degree(ctrl, msg, degree=90, absolute=False):
|
||||
return True
|
||||
else:
|
||||
warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告")
|
||||
|
||||
# 在精确模式下，如果误差仍然很大，尝试第二次微调
|
||||
if precision and final_error > limit * 2:
|
||||
info("尝试第二次微调", "精确")
|
||||
|
||||
# 计算第二次微调角度
|
||||
second_adj = target_yaw - final_yaw
|
||||
if second_adj > math.pi:
|
||||
second_adj -= 2 * math.pi
|
||||
elif second_adj < -math.pi:
|
||||
second_adj += 2 * math.pi
|
||||
|
||||
# 降低微调速度
|
||||
very_fine_speed = 0.1
|
||||
|
||||
# 设置第二次微调命令
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [0, 0, very_fine_speed if second_adj > 0 else -very_fine_speed]
|
||||
msg.duration = int(800 * abs(second_adj)) # 使用更小的系数
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
ctrl.Send_cmd(msg)
|
||||
debug(f"第二次微调：旋转{math.degrees(second_adj):.2f}°", "精确")
|
||||
|
||||
# 等待第二次微调完成
|
||||
time.sleep(3)
|
||||
|
||||
# 获取最终结果
|
||||
final_final_yaw = ctrl.odo_msg.rpy[2]
|
||||
|
||||
final_final_error = abs(target_yaw - final_final_yaw)
|
||||
if final_final_error > math.pi:
|
||||
final_final_error = 2 * math.pi - final_final_error
|
||||
|
||||
if final_final_error <= limit:
|
||||
success(f"第二次微调成功，最终误差: {math.degrees(final_final_error):.2f}°", "成功")
|
||||
return True
|
||||
else:
|
||||
warning(f"两次微调后仍有误差: {math.degrees(final_final_error):.2f}°", "警告")
|
||||
# 如果误差已经比第一次小，算作改进
|
||||
return final_final_error < final_error
|
||||
|
||||
return False
|
||||
|
||||
success("旋转成功完成", "完成")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user