优化 move_to_hori_line 和 arc_turn_around_hori_line 函数，提升移动和旋转精度
- 增加移动超时时间，确保在复杂环境下的稳定性 - 放宽移动距离判断标准，提升成功率 - 针对180度旋转简化逻辑，直接使用原始角度，避免过度旋转 - 实现大角度旋转的特殊处理，优化减速策略，确保平稳性 - 增强调试信息输出，便于实时监控旋转和移动状态
This commit is contained in:
parent
9a53927cc5
commit
ae8de7bd28
@ -295,10 +295,10 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
# 使用里程计进行实时监控移动距离
|
||||
distance_moved = 0
|
||||
start_time = time.time()
|
||||
timeout = move_time + 1 # 超时时间设置为预计移动时间加1秒
|
||||
timeout = move_time + 2 # 增加超时时间为预计移动时间加2秒
|
||||
|
||||
# 监控移动距离，但不执行减速（改用stop_smooth）
|
||||
while distance_moved < abs(distance_to_move) * 0.95 and time.time() - start_time < timeout:
|
||||
# 监控移动距离，增加移动距离的系数
|
||||
while distance_moved < abs(distance_to_move) * 1.05 and time.time() - start_time < timeout:
|
||||
# 计算已移动距离
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
@ -321,8 +321,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
if hasattr(ctrl, 'place_marker'):
|
||||
ctrl.place_marker(end_position[0], end_position[1], end_position[2] if len(end_position) > 2 else 0.0, 'red', observe=True)
|
||||
|
||||
# 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
|
||||
return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功
|
||||
# 放宽判断标准
|
||||
return abs(distance_moved - abs(distance_to_move)) < 0.15 # 如果误差小于15厘米，则认为成功
|
||||
|
||||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2,
|
||||
pass_align=False,
|
||||
@ -413,7 +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:
|
||||
time_compensation = 0.75 # 大角度(如180度)减小补偿系数，防止过度旋转
|
||||
# 对180度旋转，这里完全不用补偿，直接使用原始角度
|
||||
time_compensation = 1.0
|
||||
|
||||
# 调整实际目标角度，针对不同角度应用不同的缩放比例
|
||||
actual_angle_deg = angle_deg
|
||||
@ -422,7 +423,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
elif angle_deg <= 90:
|
||||
actual_angle_deg = angle_deg * 0.9 # 90度减少到90%
|
||||
elif angle_deg >= 170: # 处理大角度旋转，特别是180度情况
|
||||
actual_angle_deg = angle_deg * 0.75 # 减少到75%，防止过度旋转
|
||||
actual_angle_deg = angle_deg # 对于180度旋转，使用完整角度，不进行缩放
|
||||
|
||||
if observe and actual_angle_deg != angle_deg:
|
||||
print(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度")
|
||||
@ -490,79 +491,161 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
rotation_completion_threshold = 0.95 # 默认旋转到95%时停止
|
||||
|
||||
# 监控旋转进度并自行控制减速
|
||||
while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout:
|
||||
# 计算已移动距离
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
dy = current_position[1] - start_position[1]
|
||||
distance_moved = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
# 计算已旋转角度
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
angle_turned = current_yaw - start_yaw
|
||||
# 角度归一化处理
|
||||
while angle_turned > math.pi:
|
||||
angle_turned -= 2 * math.pi
|
||||
while angle_turned < -math.pi:
|
||||
angle_turned += 2 * math.pi
|
||||
# 对于180度旋转使用特殊处理
|
||||
if angle_deg >= 170:
|
||||
# 简化旋转逻辑，直接持续旋转一段时间
|
||||
rotation_time = t # 使用计算的理论时间
|
||||
if observe:
|
||||
print(f"大角度旋转({angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒")
|
||||
|
||||
# 检查QR码扫描结果（如果启用）
|
||||
if scan_qrcode:
|
||||
current_time = time.time()
|
||||
if current_time - last_qr_check_time >= qr_check_interval:
|
||||
qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
|
||||
if qr_data and scan_time > start_time: # 确保是在旋转开始后的扫描结果
|
||||
qr_result = qr_data
|
||||
print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
|
||||
last_qr_check_time = current_time
|
||||
|
||||
# 计算角度完成比例
|
||||
completion_ratio = abs(angle_turned) / abs(angle_rad)
|
||||
# 发送固定时间的旋转命令
|
||||
start_time = time.time()
|
||||
|
||||
# 当完成一定比例时开始减速
|
||||
if completion_ratio > slowdown_threshold:
|
||||
# 实现多阶段减速，特别针对大角度旋转
|
||||
if angle_deg >= 170:
|
||||
if completion_ratio < 0.7: # 第一阶段：60%-70%
|
||||
speed_factor = 0.7
|
||||
elif completion_ratio < 0.8: # 第二阶段：70%-80%
|
||||
speed_factor = 0.4
|
||||
else: # 第三阶段：80%以上
|
||||
speed_factor = 0.2
|
||||
# 前半段使用全速
|
||||
half_time = rotation_time * 0.6
|
||||
elapsed_time = 0
|
||||
|
||||
while elapsed_time < half_time and time.time() - start_time < rotation_time + 1: # 加1秒保护
|
||||
elapsed_time = time.time() - start_time
|
||||
|
||||
# 计算已旋转角度（仅用于打印）
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
angle_turned = current_yaw - start_yaw
|
||||
# 角度归一化处理
|
||||
while angle_turned > math.pi:
|
||||
angle_turned -= 2 * math.pi
|
||||
while angle_turned < -math.pi:
|
||||
angle_turned += 2 * math.pi
|
||||
print(f"全速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 已用时间: {elapsed_time:.2f}s/{rotation_time:.2f}s")
|
||||
|
||||
# 检查QR码扫描结果（如果启用）
|
||||
if scan_qrcode:
|
||||
current_time = time.time()
|
||||
if current_time - last_qr_check_time >= qr_check_interval:
|
||||
qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
|
||||
if qr_data and scan_time > start_time:
|
||||
qr_result = qr_data
|
||||
print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
|
||||
last_qr_check_time = current_time
|
||||
|
||||
time.sleep(0.05)
|
||||
|
||||
# 后半段减速
|
||||
if observe:
|
||||
print(f"进入减速阶段")
|
||||
|
||||
# 减速到50%
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [v * 0.5, 0, w * 0.5]
|
||||
msg.duration = 0
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 继续旋转直到总时间结束
|
||||
while time.time() - start_time < rotation_time:
|
||||
# 计算已旋转角度（仅用于打印）
|
||||
if observe and time.time() % 0.5 < 0.02:
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
angle_turned = current_yaw - start_yaw
|
||||
# 角度归一化处理
|
||||
while angle_turned > math.pi:
|
||||
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")
|
||||
|
||||
# 检查QR码扫描结果（如果启用）
|
||||
if scan_qrcode:
|
||||
current_time = time.time()
|
||||
if current_time - last_qr_check_time >= qr_check_interval:
|
||||
qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
|
||||
if qr_data and scan_time > start_time:
|
||||
qr_result = qr_data
|
||||
print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
|
||||
last_qr_check_time = current_time
|
||||
|
||||
time.sleep(0.05)
|
||||
|
||||
# 停止
|
||||
ctrl.base_msg.stop()
|
||||
else:
|
||||
# 非180度的常规旋转逻辑保持不变
|
||||
while abs(angle_turned) < abs(angle_rad) * rotation_completion_threshold and time.time() - start_time < timeout:
|
||||
# 计算已移动距离
|
||||
current_position = ctrl.odo_msg.xyz
|
||||
dx = current_position[0] - start_position[0]
|
||||
dy = current_position[1] - start_position[1]
|
||||
distance_moved = math.sqrt(dx*dx + dy*dy)
|
||||
|
||||
# 计算已旋转角度
|
||||
current_yaw = ctrl.odo_msg.rpy[2]
|
||||
angle_turned = current_yaw - start_yaw
|
||||
# 角度归一化处理
|
||||
while angle_turned > math.pi:
|
||||
angle_turned -= 2 * math.pi
|
||||
while angle_turned < -math.pi:
|
||||
angle_turned += 2 * math.pi
|
||||
|
||||
# 检查QR码扫描结果（如果启用）
|
||||
if scan_qrcode:
|
||||
current_time = time.time()
|
||||
if current_time - last_qr_check_time >= qr_check_interval:
|
||||
qr_data, scan_time = ctrl.image_processor.get_last_qr_result()
|
||||
if qr_data and scan_time > start_time:
|
||||
qr_result = qr_data
|
||||
print(f"🔍 在旋转过程中扫描到QR码: {qr_data}")
|
||||
last_qr_check_time = current_time
|
||||
|
||||
# 计算角度完成比例
|
||||
completion_ratio = abs(angle_turned) / abs(angle_rad)
|
||||
|
||||
# 当完成一定比例时开始减速
|
||||
if completion_ratio > slowdown_threshold:
|
||||
# 实现多阶段减速，特别针对大角度旋转
|
||||
if angle_deg >= 170:
|
||||
if completion_ratio < 0.7: # 第一阶段：60%-70%
|
||||
speed_factor = 0.7
|
||||
elif completion_ratio < 0.8: # 第二阶段：70%-80%
|
||||
speed_factor = 0.4
|
||||
else: # 第三阶段：80%以上
|
||||
speed_factor = 0.2
|
||||
else:
|
||||
# 标准减速逻辑保持不变
|
||||
# 剩余比例作为速度系数
|
||||
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
|
||||
|
||||
# 根据角度调整最小速度因子
|
||||
min_speed_factor = 0.15 # 默认最小速度因子
|
||||
# 确保速度系数不会太小，但可以更慢一些
|
||||
speed_factor = max(min_speed_factor, speed_factor)
|
||||
|
||||
# 应用减速
|
||||
current_v = v * speed_factor
|
||||
current_w = w * speed_factor
|
||||
|
||||
# 更新速度命令
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [current_v, 0, current_w]
|
||||
msg.duration = 200 # 短时间更新
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 针对180度旋转，在减速命令后稍作等待，以确保减速效果
|
||||
if angle_deg >= 170:
|
||||
time.sleep(0.03) # 短暂延迟，让减速命令更有效
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s")
|
||||
else:
|
||||
# 标准减速逻辑保持不变
|
||||
# 剩余比例作为速度系数
|
||||
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
|
||||
|
||||
# 根据角度调整最小速度因子
|
||||
min_speed_factor = 0.15 # 默认最小速度因子
|
||||
# 确保速度系数不会太小，但可以更慢一些
|
||||
speed_factor = max(min_speed_factor, speed_factor)
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
|
||||
|
||||
# 应用减速
|
||||
current_v = v * speed_factor
|
||||
current_w = w * speed_factor
|
||||
|
||||
# 更新速度命令
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [current_v, 0, current_w]
|
||||
msg.duration = 200 # 短时间更新
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 针对180度旋转，在减速命令后稍作等待，以确保减速效果
|
||||
if angle_deg >= 170:
|
||||
time.sleep(0.03) # 短暂延迟，让减速命令更有效
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
print(f"减速阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度因子: {speed_factor:.2f}, 当前角速度: {current_w:.3f}rad/s")
|
||||
else:
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
print(f"已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
|
||||
|
||||
time.sleep(0.05)
|
||||
time.sleep(0.05)
|
||||
|
||||
if observe:
|
||||
print(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user