优化 arc_turn_around_hori_line 函数，调整参数和逻辑以提高旋转精度
- 修改函数参数顺序，增强可读性 - 根据角度大小调整时间补偿系数和减速阈值，确保不同角度下的运动表现更佳 - 更新停止机制，确保机器人在旋转后平稳停止 - 增加观察输出信息，便于调试和分析旋转过程中的状态
This commit is contained in:
parent
f484dab840
commit
5cfdb94739
@ -322,7 +322,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False):
|
||||
# 如果没有提供图像处理器或图像验证失败，则使用里程计数据判断
|
||||
return abs(distance_moved - abs(distance_to_move)) < 0.1 # 如果误差小于10厘米，则认为成功
|
||||
|
||||
def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False):
|
||||
def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, observe=False):
|
||||
"""
|
||||
对准前方横线，然后以计算出来的距离为半径，做一个向左或向右的圆弧旋转。
|
||||
参数:
|
||||
@ -363,9 +363,27 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
print(f"当前距离: {r:.3f}米")
|
||||
|
||||
# 3. 计算圆弧运动参数
|
||||
angle_rad = math.radians(angle_deg)
|
||||
# 根据角度大小调整时间补偿系数
|
||||
if angle_deg <= 70:
|
||||
time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数
|
||||
elif angle_deg <= 90:
|
||||
time_compensation = 0.85 # 90度左右使用稍大的系数
|
||||
else:
|
||||
time_compensation = 0.92 # 大角度保持原有补偿
|
||||
|
||||
# 调整实际目标角度，针对不同角度应用不同的缩放比例
|
||||
actual_angle_deg = angle_deg
|
||||
if angle_deg <= 70:
|
||||
actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85%
|
||||
elif angle_deg <= 90:
|
||||
actual_angle_deg = angle_deg * 0.9 # 90度减少到90%
|
||||
|
||||
if observe and actual_angle_deg != angle_deg:
|
||||
print(f"应用角度补偿: 目标{angle_deg}度 -> 实际指令{actual_angle_deg:.1f}度")
|
||||
|
||||
angle_rad = math.radians(actual_angle_deg)
|
||||
|
||||
# 设定角速度（rad/s），可根据实际调整
|
||||
|
||||
# 减小基础角速度，增加精度
|
||||
base_w = 0.8 # 原来是0.6
|
||||
|
||||
@ -373,8 +391,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
v = abs(w * r) # 线速度，正负号与角速度方向一致
|
||||
t = abs(angle_rad / w) # 运动时间，取绝对值保证为正
|
||||
|
||||
# 应用时间补偿系数，因为实际旋转常会比理论计算多
|
||||
time_compensation = 0.92 # 时间减少到92%
|
||||
# 应用时间补偿系数
|
||||
t *= time_compensation
|
||||
|
||||
if observe:
|
||||
@ -385,7 +402,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
msg.mode = 11
|
||||
msg.gait_id = 26
|
||||
msg.vel_des = [v, 0, w] # [前进速度, 侧向速度, 角速度]
|
||||
msg.duration = int((t + 2) * 1000) # 加2秒余量
|
||||
msg.duration = 0 # wait next
|
||||
msg.step_height = [0.06, 0.06]
|
||||
msg.life_count += 1
|
||||
|
||||
@ -410,7 +427,11 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
|
||||
# 旋转精度监控
|
||||
target_angle = angle_rad if left else -angle_rad # 目标角度（弧度）
|
||||
slowdown_threshold = 0.8 # 当达到目标角度的80%时开始减速（原来是85%）
|
||||
# 对小角度提前开始减速
|
||||
if angle_deg <= 70:
|
||||
slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速
|
||||
else:
|
||||
slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%）
|
||||
|
||||
# 监控旋转进度并自行控制减速
|
||||
while abs(angle_turned) < abs(angle_rad) * 0.95 and time.time() - start_time < timeout:
|
||||
@ -437,7 +458,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
# 剩余比例作为速度系数
|
||||
speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
|
||||
# 确保速度系数不会太小，但可以更慢一些
|
||||
speed_factor = max(0.2, speed_factor) # 原来是0.3
|
||||
speed_factor = max(0.15, speed_factor) # 原来是0.2
|
||||
|
||||
# 应用减速
|
||||
current_v = v * speed_factor
|
||||
@ -460,20 +481,15 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
|
||||
time.sleep(0.05)
|
||||
|
||||
# 使用改进的平滑停止方法替代强制停止
|
||||
if observe:
|
||||
print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
|
||||
print(f"旋转过程结束，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
|
||||
ctrl.base_msg.stop()
|
||||
|
||||
# 记录停止前的角度
|
||||
pre_stop_yaw = ctrl.odo_msg.rpy[2]
|
||||
|
||||
# 使用专门的旋转停止函数替代原来的强制停止
|
||||
if observe:
|
||||
print(f"使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
|
||||
print(f'{math.degrees(ctrl.odo_msg.rpy[2]):.2f}')
|
||||
ctrl.base_msg.stop()
|
||||
if observe:
|
||||
print(f'{math.degrees(ctrl.odo_msg.rpy[2])}:.2f')
|
||||
# 等待机器人完全停止
|
||||
time.sleep(0.5)
|
||||
|
||||
# 停下来后的最终角度
|
||||
final_yaw = ctrl.odo_msg.rpy[2]
|
||||
@ -486,6 +502,7 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
final_angle_turned += 2 * math.pi
|
||||
|
||||
final_angle_deg = math.degrees(final_angle_turned)
|
||||
# 这里使用原始目标角度进行比较
|
||||
target_angle_deg = angle_deg if left else -angle_deg
|
||||
|
||||
if observe:
|
||||
@ -508,7 +525,11 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
|
||||
adjust_angle = target_angle_deg - final_angle_deg
|
||||
|
||||
# 增加补偿系数，因为实际旋转常会超出理论计算值
|
||||
compensation_factor = 0.85 # 只旋转计算值的85%
|
||||
if angle_deg <= 70:
|
||||
compensation_factor = 0.7 # 小角度使用更小的补偿因子
|
||||
else:
|
||||
compensation_factor = 0.85 # 只旋转计算值的85%
|
||||
|
||||
adjust_angle *= compensation_factor
|
||||
|
||||
if observe:
|
||||
|
||||
@ -13,154 +13,13 @@ class BaseMsg:
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
self.ctrl.Wait_finish(0, 0)
|
||||
|
||||
def stop(self, wait_time=0):
|
||||
def stop(self, wait_time=1):
|
||||
self.msg.mode = 11
|
||||
self.msg.gait_id = 26
|
||||
self.msg.vel_des = [0, 0, 0]
|
||||
self.msg.duration = 10
|
||||
self.msg.duration = wait_time * 1000
|
||||
self.msg.life_count += 1
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
if wait_time:
|
||||
time.sleep(wait_time)
|
||||
|
||||
def stop_smooth(self, current_vel=None, steps=3, delay=0.2):
|
||||
"""
|
||||
平滑停止移动，通过逐步减小速度而不是强制停止
|
||||
|
||||
参数:
|
||||
current_vel: 当前速度列表 [x, y, z]，如果为None则使用当前消息中的速度
|
||||
steps: 减速步骤数，默认为3步
|
||||
delay: 每步之间的延迟时间(秒)，默认为0.2秒
|
||||
"""
|
||||
|
||||
# 如果没有提供当前速度，使用消息中的当前速度
|
||||
if current_vel is None:
|
||||
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0]
|
||||
|
||||
# 保存当前消息的一些参数
|
||||
current_mode = self.msg.mode
|
||||
current_gait_id = self.msg.gait_id
|
||||
|
||||
# 检查是否有速度需要减小
|
||||
if all(abs(v) < 0.01 for v in current_vel):
|
||||
# 速度已经很小，直接停止
|
||||
self.stop_force()
|
||||
return
|
||||
|
||||
# 逐步减小速度
|
||||
for i in range(1, steps + 1):
|
||||
# 计算这一步的减速比例
|
||||
ratio = (steps - i) / steps
|
||||
|
||||
# 计算新的速度
|
||||
new_vel = [v * ratio for v in current_vel]
|
||||
|
||||
# 更新消息
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = new_vel
|
||||
self.msg.duration = int(delay * 1000) # 毫秒
|
||||
self.msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待这一步完成
|
||||
time.sleep(delay)
|
||||
|
||||
# 最后发送一个零速度命令，确保完全停止
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = [0, 0, 0]
|
||||
self.msg.duration = int(delay * 1000)
|
||||
self.msg.life_count += 1
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待最后的命令完成
|
||||
time.sleep(delay)
|
||||
|
||||
"""
|
||||
专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度
|
||||
|
||||
参数:
|
||||
current_vel: 当前速度列表 [x, y, w]，如果为None则使用当前消息中的速度
|
||||
steps: 减速步骤数，默认为5步（比普通停止更多步骤）
|
||||
delay: 每步之间的延迟时间(秒)，默认为0.1秒（更快的响应）
|
||||
final_steps: 最后完全停止前的额外零速度指令次数，增强稳定性
|
||||
"""
|
||||
|
||||
# 如果没有提供当前速度，使用消息中的当前速度
|
||||
if current_vel is None:
|
||||
current_vel = self.msg.vel_des.copy() if hasattr(self.msg, 'vel_des') else [0, 0, 0]
|
||||
|
||||
# 保存当前消息的一些参数
|
||||
current_mode = self.msg.mode
|
||||
current_gait_id = self.msg.gait_id
|
||||
|
||||
# 检查是否有速度需要减小
|
||||
if all(abs(v) < 0.01 for v in current_vel):
|
||||
# 速度已经很小，直接停止
|
||||
self.stop_force()
|
||||
return
|
||||
|
||||
# 1. 先针对角速度进行单独减速，确保旋转先停稳
|
||||
if len(current_vel) >= 3 and abs(current_vel[2]) > 0.05: # 角速度
|
||||
# 首先只减小角速度，保持线速度
|
||||
for i in range(1, steps + 1):
|
||||
# 计算这一步的减速比例 - 角速度减速更快
|
||||
ratio = (steps - i) / steps
|
||||
ratio_w = ratio * ratio # 角速度使用平方递减，减速更快
|
||||
|
||||
# 计算新的速度，只改变角速度
|
||||
new_vel = current_vel.copy()
|
||||
new_vel[2] = current_vel[2] * ratio_w
|
||||
|
||||
# 更新消息
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = new_vel
|
||||
self.msg.duration = int(delay * 1000) # 毫秒
|
||||
self.msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待这一步完成
|
||||
time.sleep(delay)
|
||||
|
||||
# 2. 再减小整体速度
|
||||
current_vel_no_w = current_vel.copy()
|
||||
if len(current_vel) >= 3:
|
||||
current_vel_no_w[2] = 0 # 角速度已减为0
|
||||
|
||||
for i in range(1, steps + 1):
|
||||
# 计算这一步的减速比例
|
||||
ratio = (steps - i) / steps
|
||||
|
||||
# 计算新的速度
|
||||
new_vel = [v * ratio for v in current_vel_no_w]
|
||||
if len(current_vel) >= 3:
|
||||
new_vel[2] = 0 # 确保角速度为0
|
||||
|
||||
# 更新消息
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = new_vel
|
||||
self.msg.duration = int(delay * 1000) # 毫秒
|
||||
self.msg.life_count += 1
|
||||
|
||||
# 发送命令
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
|
||||
# 等待这一步完成
|
||||
time.sleep(delay)
|
||||
|
||||
# 3. 发送多次零速度命令，确保完全停止
|
||||
for _ in range(final_steps):
|
||||
self.msg.mode = current_mode
|
||||
self.msg.gait_id = current_gait_id
|
||||
self.msg.vel_des = [0, 0, 0]
|
||||
self.msg.duration = int(delay * 1000)
|
||||
self.msg.life_count += 1
|
||||
self.ctrl.Send_cmd(self.msg)
|
||||
time.sleep(delay)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user