From 5cfdb9473998aae0980e576bc1e08822e356b096 Mon Sep 17 00:00:00 2001 From: Havoc <2993167370@qq.com> Date: Thu, 15 May 2025 22:58:18 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=20arc=5Fturn=5Faround=5Fhori?= =?UTF-8?q?=5Fline=20=E5=87=BD=E6=95=B0=EF=BC=8C=E8=B0=83=E6=95=B4?= =?UTF-8?q?=E5=8F=82=E6=95=B0=E5=92=8C=E9=80=BB=E8=BE=91=E4=BB=A5=E6=8F=90?= =?UTF-8?q?=E9=AB=98=E6=97=8B=E8=BD=AC=E7=B2=BE=E5=BA=A6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 修改函数参数顺序，增强可读性 - 根据角度大小调整时间补偿系数和减速阈值，确保不同角度下的运动表现更佳 - 更新停止机制，确保机器人在旋转后平稳停止 - 增加观察输出信息，便于调试和分析旋转过程中的状态 --- base_move/move_base_hori_line.py | 57 ++++++++---- utils/base_msg.py | 147 +------------------------------ 2 files changed, 42 insertions(+), 162 deletions(-) diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 0f56d18..eaa8aae 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -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: diff --git a/utils/base_msg.py b/utils/base_msg.py index c1b2e75..3e93298 100644 --- a/utils/base_msg.py +++ b/utils/base_msg.py @@ -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) \ No newline at end of file + \ No newline at end of file