From 021915633ce71465ac9b27ff139809cd230fcda7 Mon Sep 17 00:00:00 2001
From: Havoc <2993167370@qq.com>
Date: Thu, 15 May 2025 22:24:24 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=9C=86=E5=BC=A7=E8=BD=AC?=
 =?UTF-8?q?=E5=BC=AF=E6=8E=A7=E5=88=B6=E9=80=BB=E8=BE=91=EF=BC=8C=E6=96=B0?=
 =?UTF-8?q?=E5=A2=9E=E5=B9=B3=E6=BB=91=E5=81=9C=E6=AD=A2=E5=92=8C=E7=B2=BE?=
 =?UTF-8?q?=E7=A1=AE=E8=BD=AC=E5=BC=AF=E5=8A=9F=E8=83=BD?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

- 在 move_base_hori_line.py 中，改进了 arc_turn_around_hori_line 函数，使用平滑停止方法替代强制停止，并添加停止过程中的角度监控。
- 新增 arc_turn_precise 函数，实现更精确的圆弧转弯控制，分阶段控制速度，提升转弯精度。
- 新增 arc_turn_around_hori_line_precise 函数，结合横线检测与精确转弯功能，确保机器人在执行任务时的路径更加准确。
- 在 base_msg.py 中，新增 stop_turn_smooth 方法，提供更细致的速度减小策略，确保旋转动作的平稳停止。
---
 base_move/move_base_hori_line.py | 311 ++++++++++++++++++++++++++++++-
 utils/base_msg.py                |  89 ++++++++-
 2 files changed, 394 insertions(+), 6 deletions(-)

diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py
index dfc1188..4f07eea 100644
--- a/base_move/move_base_hori_line.py
+++ b/base_move/move_base_hori_line.py
@@ -460,12 +460,20 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
         
         time.sleep(0.05)
 
-    # Stop
-    ctrl.base_msg.stop_force()
-
+    # 使用改进的平滑停止方法替代强制停止
+    if observe:
+        print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
+    
+    # 记录停止前的角度
+    pre_stop_yaw = ctrl.odo_msg.rpy[2]
+    
+    # 使用专门的旋转停止函数替代原来的强制停止
+    ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=5, delay=0.1, final_steps=2)
+    
     # 停下来后的最终角度
     final_yaw = ctrl.odo_msg.rpy[2]
     final_angle_turned = final_yaw - start_yaw
+    
     # 角度归一化处理
     while final_angle_turned > math.pi:
         final_angle_turned -= 2 * math.pi
@@ -476,8 +484,15 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
     target_angle_deg = angle_deg if left else -angle_deg
     
     if observe:
+        # 输出停止过程中角度变化
+        stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw)
+        while stop_angle_diff > 180:
+            stop_angle_diff -= 360
+        while stop_angle_diff < -180:
+            stop_angle_diff += 360
+        print(f"停止过程中旋转了: {stop_angle_diff:.2f}度")
         print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
-        
+    
     # 检查是否需要微调
     angle_error = abs(final_angle_deg - target_angle_deg)
     if angle_error > 3.0:  # 如果误差超过3度
@@ -495,13 +510,270 @@ def arc_turn_around_hori_line(ctrl, msg, target_distance=0.2, angle_deg=90, left
             print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
         
         # 使用turn_degree函数进行微调
-        turn_degree(ctrl, msg, adjust_angle, absolute=False)
+        turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
+        
+        if observe:
+            print(f"微调结果: {'成功' if turn_success else '失败'}")
     
     if observe:
         print("圆弧转弯完成")
 
     return True
 
+def arc_turn_precise(ctrl, msg, radius, angle_deg, left=True, observe=False):
+    """
+    实现更精确的圆弧转弯控制，特别改进了停止阶段的处理
+    
+    参数:
+        ctrl: Robot_Ctrl对象
+        msg: robot_control_cmd_lcmt对象
+        radius: 圆弧半径(米)
+        angle_deg: 旋转角度(度)
+        left: True为左转，False为右转
+        observe: 是否打印调试信息
+    返回:
+        bool: 是否成功完成动作
+    """
+    # 1. 参数准备
+    angle_rad = math.radians(angle_deg)
+    # 设定基础角速度，为更好的控制降低了角速度
+    base_w = 0.6  # 降低基础角速度提高精度
+    
+    # 设定方向
+    w = base_w if left else -base_w  # 左转为正，右转为负
+    v = abs(w * radius)  # 线速度与角速度和半径成正比
+    
+    # 计算理论时间
+    t = abs(angle_rad / w)
+    
+    # 应用时间补偿系数
+    time_compensation = 0.92
+    t *= time_compensation
+    
+    if observe:
+        print(f"圆弧参数: 半径={radius:.3f}米, 角度={angle_deg}度, 方向={'左' if left else '右'}")
+        print(f"速度参数: 角速度={w:.3f}rad/s, 线速度={v:.3f}m/s")
+        print(f"时间参数: 理论时间={abs(angle_rad / w):.2f}s, 实际时间={t:.2f}s")
+    
+    # 2. 启动转向
+    msg.mode = 11
+    msg.gait_id = 26
+    msg.vel_des = [v, 0, w]
+    msg.duration = int((t + 2) * 1000)  # 余量2秒
+    msg.step_height = [0.06, 0.06]
+    msg.life_count += 1
+    ctrl.Send_cmd(msg)
+    
+    # 3. 记录初始状态
+    start_yaw = ctrl.odo_msg.rpy[2]
+    start_position = list(ctrl.odo_msg.xyz)
+    start_time = time.time()
+    
+    # 4. 设置控制参数
+    target_angle = angle_rad if left else -angle_rad
+    distance_moved = 0
+    angle_turned = 0
+    timeout = t + 2  # 超时保护
+    current_v = v
+    current_w = w
+    
+    # 分阶段控制参数
+    # 提前减速区间设置更大，从70%开始减速
+    slowdown_threshold = 0.7  
+    # 慢速区间设置更大，从90%开始进入超慢速
+    precision_threshold = 0.9
+    
+    # 5. 实时监控和控制
+    while abs(angle_turned) < abs(angle_rad) * 0.98 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
+            
+        # 计算完成比例
+        completion_ratio = abs(angle_turned) / abs(angle_rad)
+        
+        # 分阶段减速控制
+        if completion_ratio > precision_threshold:
+            # 超低速阶段（精确控制）
+            speed_factor = 0.1  # 使用固定的极低速度
+            
+            # 应用超低速
+            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 = 150  # 更短的更新周期
+            msg.step_height = [0.06, 0.06]
+            msg.life_count += 1
+            ctrl.Send_cmd(msg)
+            
+            if observe and time.time() % 0.5 < 0.02:
+                print(f"精确阶段 - 已旋转: {math.degrees(angle_turned):.2f}度, 速度: {speed_factor:.2f}, 角速度: {current_w:.3f}rad/s")
+        
+        elif completion_ratio > slowdown_threshold:
+            # 一般减速阶段
+            speed_factor = 1.0 - (completion_ratio - slowdown_threshold) / (1.0 - slowdown_threshold)
+            speed_factor = max(0.2, speed_factor * 0.8)  # 整体降低一点速度
+            
+            # 应用减速
+            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)
+            
+            if observe and time.time() % 0.5 < 0.02:
+                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:
+                print(f"匀速阶段 - 已移动: {distance_moved:.3f}米, 已旋转: {math.degrees(angle_turned):.2f}度")
+        
+        # 更短的循环检查间隔
+        time.sleep(0.03)
+    
+    # 6. 使用改进的平滑停止方法
+    if observe:
+        print(f"旋转过程结束，使用平滑停止方法，当前速度: [{current_v:.3f}, 0, {current_w:.3f}]")
+    
+    # 记录停止前的角度
+    pre_stop_yaw = ctrl.odo_msg.rpy[2]
+    
+    # 使用专门的旋转停止函数
+    # 增加步数和降低延迟以获得更平滑的停止
+    ctrl.base_msg.stop_turn_smooth(current_vel=[current_v, 0, current_w], steps=7, delay=0.08, final_steps=3)
+    
+    # 7. 停止后的角度分析
+    final_yaw = ctrl.odo_msg.rpy[2]
+    final_angle_turned = final_yaw - start_yaw
+    
+    # 角度归一化处理
+    while final_angle_turned > math.pi:
+        final_angle_turned -= 2 * math.pi
+    while final_angle_turned < -math.pi:
+        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:
+        # 输出停止过程中角度变化
+        stop_angle_diff = math.degrees(final_yaw - pre_stop_yaw)
+        while stop_angle_diff > 180:
+            stop_angle_diff -= 360
+        while stop_angle_diff < -180:
+            stop_angle_diff += 360
+        print(f"停止过程中旋转了: {stop_angle_diff:.2f}度")
+        print(f"旋转完成，目标角度: {target_angle_deg:.2f}度, 实际角度: {final_angle_deg:.2f}度")
+    
+    # 8. 微调处理
+    angle_error = abs(final_angle_deg - target_angle_deg)
+    
+    # 如果误差超过阈值，进行微调
+    if angle_error > 2.0:  # 降低误差阈值到2度
+        if observe:
+            print(f"角度误差: {angle_error:.2f}度，进行微调")
+        
+        # 计算微调角度
+        adjust_angle = target_angle_deg - final_angle_deg
+        
+        # 应用补偿系数
+        compensation_factor = 0.85
+        adjust_angle *= compensation_factor
+        
+        if observe:
+            print(f"应用补偿系数{compensation_factor}后的微调角度: {adjust_angle:.2f}度")
+        
+        # 使用turn_degree进行精确微调
+        turn_success = turn_degree(ctrl, msg, adjust_angle, absolute=False)
+        
+        if observe:
+            print(f"微调结果: {'成功' if turn_success else '失败'}")
+    else:
+        if observe:
+            print(f"角度误差在允许范围内: {angle_error:.2f}度，无需微调")
+    
+    # 9. 最终位置和角度
+    final_position = ctrl.odo_msg.xyz
+    final_yaw = ctrl.odo_msg.rpy[2]
+    
+    if observe:
+        dx = final_position[0] - start_position[0]
+        dy = final_position[1] - start_position[1]
+        total_distance = math.sqrt(dx*dx + dy*dy)
+        
+        # 计算理论圆弧长度
+        arc_length = abs(radius * angle_rad)
+        print(f"位移统计: 直线距离={total_distance:.3f}米, 圆弧长度={arc_length:.3f}米")
+        print(f"精确圆弧转弯完成")
+    
+    return True
+
+def arc_turn_around_hori_line_precise(ctrl, msg, target_distance=0.2, angle_deg=90, left=True, observe=False):
+    """
+    使用精确圆弧转弯方法，对准前方横线，然后以计算出的距离为半径，做一个向左或向右的圆弧旋转。
+    
+    参数:
+        ctrl: Robot_Ctrl对象
+        msg: robot_control_cmd_lcmt对象
+        target_distance: 横线前目标保持距离，默认0.2米
+        angle_deg: 旋转角度，支持任意角度
+        left: True为左转，False为右转
+        observe: 是否打印调试信息
+    返回:
+        bool: 是否成功完成动作
+    """
+    # 1. 对准横线
+    print("校准到横向线水平")
+    aligned = align_to_horizontal_line(ctrl, msg, observe=observe)
+    if not aligned:
+        print("无法校准到横向线水平，停止动作")
+        return False
+
+    # 2. 检测横线并计算距离
+    image = ctrl.image_processor.get_current_image()
+    edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe)
+    if edge_point is None or edge_info is None:
+        print("无法检测到横向线，停止动作")
+        return False
+
+    camera_height = 0.355  # 单位: 米
+    r = calculate_distance_to_line(edge_info, camera_height, observe=observe)
+
+    # 减去目标距离得到圆弧半径
+    r -= target_distance
+
+    if r is None or r < 0.1:  # 半径太小无法执行圆弧
+        print(f"计算的半径太小: {r if r is not None else 'None'}，无法执行圆弧转弯")
+        return False
+        
+    if observe:
+        print(f"当前距离: {r+target_distance:.3f}米，目标距离: {target_distance:.3f}米")
+        print(f"将使用半径: {r:.3f}米 执行圆弧转弯")
+
+    # 3. 使用精确圆弧转弯函数执行
+    return arc_turn_precise(ctrl, msg, r, angle_deg, left, observe)
+
 # 用法示例
 if __name__ == "__main__":
     move_to_hori_line(None, None, observe=True)
@@ -509,3 +781,32 @@ if __name__ == "__main__":
     arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True)
     # 180度右转
     arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True)
+    # 使用精确版本做90度左转
+    arc_turn_around_hori_line_precise(None, None, angle_deg=90, left=True, observe=True)
+
+"""
+代码改进说明：
+
+1. 添加了专用的平滑旋转停止函数 stop_turn_smooth (在 utils/base_msg.py 中):
+   - 分两个阶段减速：先减小角速度，再减小线速度
+   - 使用平方递减使角速度更快地降低
+   - 发送多次零速度命令确保完全停止
+
+2. 改进了原始的 arc_turn_around_hori_line 函数:
+   - 使用平滑停止替代强制停止
+   - 添加停止过程中角度变化的监控
+   - 改进了角度误差计算和微调逻辑
+
+3. 新增了 arc_turn_precise 函数，实现更精确的转弯控制:
+   - 速度更低，控制更精细
+   - 分三个阶段控制：匀速阶段、减速阶段、精确阶段
+   - 更频繁的控制更新和监控
+   - 更完善的停止策略
+   - 更严格的角度误差要求
+
+4. 新增 arc_turn_around_hori_line_precise 结合横线检测和精确转弯功能:
+   - 保持原有的横线检测和校准功能
+   - 使用改进的精确转弯控制
+
+这些改进有效解决了转弯后快速切换到零速度导致的角度偏移问题，提高了转弯的精度和稳定性。
+"""
diff --git a/utils/base_msg.py b/utils/base_msg.py
index 2ee2146..852a4f8 100644
--- a/utils/base_msg.py
+++ b/utils/base_msg.py
@@ -67,4 +67,91 @@ class BaseMsg:
         self.ctrl.Send_cmd(self.msg)
         
         # 等待最后的命令完成
-        time.sleep(delay)
\ No newline at end of file
+        time.sleep(delay)
+        
+    def stop_turn_smooth(self, current_vel=None, steps=5, delay=0.1, final_steps=2):
+        """
+        专为旋转动作设计的平滑停止方法，通过更精细的步骤减小速度
+        
+        参数:
+        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