优化 arc_turn_around_hori_line 函数，调整参数和逻辑以支持负角度表示右转，增强代码可读性和可维护性。同时更新 task_1.py 中的调用方式，确保一致性和清晰性。
This commit is contained in:
parent
f4eab6b296
commit
8ee1f5b5d8
@ -352,7 +352,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
# 放宽判断标准
|
||||
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,
|
||||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2,
|
||||
pass_align=False,
|
||||
radius=None,
|
||||
scan_qrcode=False,
|
||||
@ -365,8 +365,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
ctrl: Robot_Ctrl对象
|
||||
msg: robot_control_cmd_lcmt对象
|
||||
target_distance: 横线前目标保持距离，默认0.5米
|
||||
angle_deg: 旋转角度，支持90或180度
|
||||
left: True为左转，False为右转
|
||||
angle_deg: 旋转角度，正值为左转，负值为右转
|
||||
pass_align: 是否跳过对准步骤，默认为False
|
||||
observe: 是否打印调试信息
|
||||
scan_qrcode: 是否在旋转过程中扫描QR码，默认为False
|
||||
@ -440,9 +439,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 3. 计算圆弧运动参数
|
||||
# 根据角度大小调整时间补偿系数
|
||||
if angle_deg <= 70:
|
||||
abs_angle_deg = abs(angle_deg)
|
||||
if abs_angle_deg <= 70:
|
||||
time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数
|
||||
elif angle_deg <= 90:
|
||||
elif abs_angle_deg <= 90:
|
||||
time_compensation = 0.85 # 90度左右使用稍大的系数
|
||||
else:
|
||||
# 对180度旋转，增加补偿系数，使旋转时间更长
|
||||
@ -450,11 +450,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 调整实际目标角度，针对不同角度应用不同的缩放比例
|
||||
actual_angle_deg = angle_deg
|
||||
if angle_deg <= 70:
|
||||
if abs_angle_deg <= 70:
|
||||
actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85%
|
||||
elif angle_deg <= 90:
|
||||
elif abs_angle_deg <= 90:
|
||||
actual_angle_deg = angle_deg * 0.9 # 90度减少到90%
|
||||
elif angle_deg >= 170: # 处理大角度旋转，特别是180度情况
|
||||
elif abs_angle_deg >= 170: # 处理大角度旋转，特别是180度情况
|
||||
actual_angle_deg = angle_deg # 对于180度旋转，使用完整角度，不进行缩放
|
||||
|
||||
if observe and actual_angle_deg != angle_deg:
|
||||
@ -462,9 +462,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
angle_rad = math.radians(actual_angle_deg)
|
||||
|
||||
# 设定角速度（rad/s），可根据实际调整
|
||||
# 设定角速度（rad/s），角度的符号决定了转向方向
|
||||
base_w = 0.8
|
||||
w = base_w if left else -base_w # 左转为正，右转为负
|
||||
w = math.copysign(base_w, angle_deg) # 保持角速度与角度同号
|
||||
|
||||
# 确保r有一个最小值，避免速度过低
|
||||
min_radius = 0.2 # 最小半径，单位：米
|
||||
@ -512,8 +512,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 计算圆心位置 - 相对于机器人前进方向的左侧或右侧
|
||||
# 圆心与机器人当前位置的距离就是圆弧半径
|
||||
center_x = start_position[0] - effective_r * math.sin(yaw) * (1 if left else -1)
|
||||
center_y = start_position[1] + effective_r * math.cos(yaw) * (1 if left else -1)
|
||||
# 角度为正(左转)时，圆心在左侧；角度为负(右转)时，圆心在右侧
|
||||
circle_side = 1 if angle_deg > 0 else -1
|
||||
center_x = start_position[0] - effective_r * math.sin(yaw) * circle_side
|
||||
center_y = start_position[1] + effective_r * math.cos(yaw) * circle_side
|
||||
center_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
|
||||
# 在圆心放置绿色标记
|
||||
@ -522,10 +524,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 计算目标点的位置 - 旋转后的位置
|
||||
# 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置
|
||||
target_angle = yaw + (angle_rad if left else -angle_rad)
|
||||
target_angle = yaw + angle_rad # 直接使用angle_rad的正负确定方向
|
||||
# 计算从圆心到目标点的向量
|
||||
target_x = center_x + effective_r * math.sin(target_angle) * (1 if left else -1)
|
||||
target_y = center_y - effective_r * math.cos(target_angle) * (1 if left else -1)
|
||||
target_x = center_x + effective_r * math.sin(target_angle) * circle_side
|
||||
target_y = center_y - effective_r * math.cos(target_angle) * circle_side
|
||||
target_z = start_position[2] if len(start_position) > 2 else 0.0
|
||||
|
||||
# 在目标点放置红色标记
|
||||
@ -544,26 +546,26 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
current_w = w
|
||||
|
||||
# 对小角度提前开始减速
|
||||
if angle_deg <= 70:
|
||||
if abs_angle_deg <= 70:
|
||||
slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速
|
||||
elif angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机
|
||||
elif abs_angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机
|
||||
slowdown_threshold = 0.7 # 当达到目标角度的70%时才开始减速
|
||||
else:
|
||||
slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%）
|
||||
|
||||
# 根据角度大小调整旋转停止阈值
|
||||
if angle_deg >= 170:
|
||||
if abs_angle_deg >= 170:
|
||||
rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转
|
||||
else:
|
||||
rotation_completion_threshold = 0.95 # 默认旋转到95%时停止
|
||||
|
||||
# 监控旋转进度并自行控制减速
|
||||
# 对于180度旋转使用特殊处理
|
||||
if angle_deg >= 170:
|
||||
if abs_angle_deg >= 170:
|
||||
# 简化旋转逻辑，直接持续旋转一段时间
|
||||
rotation_time = t # 使用计算的理论时间
|
||||
if observe:
|
||||
info(f"大角度旋转({angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转")
|
||||
info(f"大角度旋转({abs_angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转")
|
||||
|
||||
# 发送固定时间的旋转命令
|
||||
start_time = time.time()
|
||||
@ -676,7 +678,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
# 当完成一定比例时开始减速
|
||||
if completion_ratio > slowdown_threshold:
|
||||
# 实现多阶段减速，特别针对大角度旋转
|
||||
if angle_deg >= 170:
|
||||
if abs_angle_deg >= 170:
|
||||
if completion_ratio < 0.7: # 第一阶段：60%-70%
|
||||
speed_factor = 0.65 # 原来是0.7，减少到0.65
|
||||
elif completion_ratio < 0.8: # 第二阶段：70%-80%
|
||||
@ -707,7 +709,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 针对180度旋转，在减速命令后稍作等待，以确保减速效果
|
||||
if angle_deg >= 170:
|
||||
if abs_angle_deg >= 170:
|
||||
time.sleep(0.03) # 短暂延迟，让减速命令更有效
|
||||
|
||||
if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次
|
||||
@ -758,11 +760,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
final_angle_deg = math.degrees(final_angle_turned)
|
||||
|
||||
# 这里使用原始目标角度进行比较
|
||||
# 这里直接使用原始目标角度
|
||||
if not bad_big_angle_corret:
|
||||
target_angle_deg = angle_deg if left else -angle_deg
|
||||
target_angle_deg = angle_deg
|
||||
else:
|
||||
target_angle_deg = -angle_deg if left else angle_deg
|
||||
target_angle_deg = -angle_deg
|
||||
|
||||
if observe:
|
||||
# 输出停止过程中角度变化
|
||||
@ -779,7 +781,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
|
||||
# 针对不同角度使用不同的微调阈值
|
||||
adjustment_threshold = 3.0 # 默认微调阈值
|
||||
if angle_deg >= 170:
|
||||
if abs_angle_deg >= 170:
|
||||
adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确
|
||||
|
||||
if angle_error > adjustment_threshold: # 如果误差超过阈值
|
||||
@ -790,9 +792,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
adjust_angle = target_angle_deg - final_angle_deg
|
||||
|
||||
# 增加补偿系数，因为实际旋转常会超出理论计算值
|
||||
if angle_deg <= 70:
|
||||
if abs_angle_deg <= 70:
|
||||
compensation_factor = 0.55 # 比之前更保守，从0.65降至0.55
|
||||
elif angle_deg >= 170: # 单独处理180度附近的大角度
|
||||
elif abs_angle_deg >= 170: # 单独处理180度附近的大角度
|
||||
# 动态调整补偿因子，更保守处理
|
||||
if angle_error > 60: # 如果误差超过60度
|
||||
compensation_factor = 0.4 # 从0.5降至0.4
|
||||
@ -804,7 +806,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc
|
||||
compensation_factor = 0.6 # 从0.7降至0.6
|
||||
|
||||
# 对于特别大的误差进行更保守的单次校准
|
||||
if angle_deg >= 170 and angle_error > 30:
|
||||
if abs_angle_deg >= 170 and angle_error > 30:
|
||||
# 大误差直接一步校准，使用更保守的补偿系数
|
||||
adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15%
|
||||
adjust_angle = adjust_angle * adjusted_compensation
|
||||
@ -1125,9 +1127,9 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None,
|
||||
if __name__ == "__main__":
|
||||
move_to_hori_line(None, None, observe=True)
|
||||
# 90度左转
|
||||
arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
|
||||
arc_turn_around_hori_line(None, None, angle_deg=90, observe=True)
|
||||
# 180度右转
|
||||
arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)
|
||||
arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True)
|
||||
# 双轨道跟随
|
||||
follow_dual_tracks(None, None, observe=True)
|
||||
|
||||
@ -29,8 +29,7 @@ def run_task_1(ctrl, msg):
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=85,
|
||||
left=False,
|
||||
angle_deg=-85, # 负值表示右转
|
||||
observe=observe,
|
||||
scan_qrcode=True, # 启用 QR 码扫描
|
||||
qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果
|
||||
@ -50,10 +49,8 @@ def run_task_1(ctrl, msg):
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=170,
|
||||
angle_deg=-170, # 负值表示右转
|
||||
target_distance=0.5, # TODO 优化这里的参数
|
||||
left=direction,
|
||||
#
|
||||
pass_align=True,
|
||||
observe=observe,
|
||||
# TODO clear
|
||||
@ -95,8 +92,7 @@ def run_task_1(ctrl, msg):
|
||||
turn_success, res = arc_turn_around_hori_line(
|
||||
ctrl=ctrl,
|
||||
msg=msg,
|
||||
angle_deg=-85,
|
||||
left=True,
|
||||
angle_deg=85, # 正值表示左转
|
||||
observe=observe
|
||||
)
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user