优化计算到横向线的距离函数，增加相机倾斜角度参数，添加观察模式以打印中间计算值，改进代码结构和注释，更新图像读取方式。

base_move/move_base_hori_line.py

@@ -1,7 +1,11 @@
 import math
 import time
 import cv2
-import numpy as np
+
+import sys
+import os
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
 from utils.detect_track import detect_horizontal_track_edge
 from base_move.turn_degree import turn_degree
 
@@ -116,41 +120,117 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3):
     
     return aligned
 
-def calculate_distance_to_line(edge_info, camera_height):
+def calculate_distance_to_line(edge_info, camera_height, camera_tilt_angle_deg=0, observe=False):
     """
-    根据边缘信息和相机高度计算到横向线的实际距离
+    根据相机参数和图像中横线位置计算相机到横线的实际距离
+    
+    几何模型说明:
+    1. 相机位于高度camera_height处，向下倾斜camera_tilt_angle_deg度
+    2. 图像底部对应相机视场的下边缘，横线在图像中的位置通过像素坐标确定
+    3. 计算相机视线到横线的角度，然后使用三角函数计算实际距离
     
     参数:
     edge_info: 边缘信息字典，包含distance_to_bottom等信息
     camera_height: 相机高度(米)
+    camera_tilt_angle_deg: 相机向下倾斜的角度（度）
+    observe: 是否打印中间计算值
     
     返回:
-    float: 到横向线的估计距离(米)
+    float: 到横向线的X轴水平距离(米)
     """
     if edge_info is None or "distance_to_bottom" not in edge_info:
         return None
     
-    # 获取交点到图像底部的像素距离
+    # 1. 获取图像中交点到底部的距离(像素)
     distance_in_pixels = edge_info["distance_to_bottom"]
+    if observe:
+        print(f"图像中交点到底部的像素距离: {distance_in_pixels}")
     
-    # 计算比例系数 (需要根据实际场景标定)
+    # 2. 获取相机参数
-    # 这里使用一个简化的算法，假设像素距离与实际距离成反比；实际应用中应该进行相机标定和测试以获得更准确的参数
+    horizontal_fov_rad = 1.46608  # 水平视场角（弧度）约84度
+    image_height_px = 1080       # 图像高度（像素）
+    image_width_px = 1920        # 图像宽度（像素）
     
-    # 假设FOV (视场角)为84度 (1.46608弧度), 图像高度为1080像素
+    # 3. 计算垂直视场角
-    fov_vertical = 1.46608 / 16 * 9  # 假设16:9的图像比例计算垂直FOV
+    aspect_ratio = image_width_px / image_height_px  # 宽高比
-    image_height = 1080  # 从robot.xacro中获取
+    vertical_fov_rad = horizontal_fov_rad / aspect_ratio  # 垂直视场角(弧度)
+    vertical_fov_deg = math.degrees(vertical_fov_rad)  # 垂直视场角(度)
     
-    # 计算每个像素对应的角度
+    if observe:
-    angle_per_pixel = fov_vertical / image_height
+        print(f"相机参数: 水平FOV={math.degrees(horizontal_fov_rad):.1f}°, 垂直FOV={vertical_fov_deg:.1f}°")
+        print(f"图像尺寸: {image_width_px}x{image_height_px}, 宽高比: {aspect_ratio:.2f}")
     
-    # 计算交点对应的角度
+    # 4. 直接计算视线角度
-    angle = angle_per_pixel * distance_in_pixels
     
-    # 使用相机高度和角度计算距离 (使用正切函数)
+    # 计算图像底部到相机视场中心的角度
-    # 距离 = 相机高度 / tan(角度)
+    half_vfov_rad = vertical_fov_rad / 2
-    distance = camera_height / math.tan(angle)
     
-    return max(0.0, distance)  # 确保距离是正数
+    # 计算图像底部到横线的角度比例
+    # 比例 = 底部到横线的像素距离 / 图像总高度
+    pixel_ratio = distance_in_pixels / image_height_px
+    
+    # 计算从图像底部到横线的角度
+    bottom_to_line_angle_rad = pixel_ratio * vertical_fov_rad
+    
+    # 计算从相机视场中心到横线的角度
+    # 负值表示横线在视场中心以下，正值表示在中心以上
+    center_to_line_angle_rad = bottom_to_line_angle_rad - half_vfov_rad
+    
+    # 考虑相机倾斜角度
+    # 相机向下倾斜为正值，此时视场中心相对水平线向下
+    camera_tilt_rad = math.radians(camera_tilt_angle_deg)
+    
+    # 计算横线相对于水平面的视线角度
+    # 负值表示视线向下看到横线，正值表示视线向上看到横线
+    view_angle_rad = center_to_line_angle_rad - camera_tilt_rad
+    
+    if observe:
+        print(f"视场角度关系:")
+        print(f"  - 图像底部到横线角度: {math.degrees(bottom_to_line_angle_rad):.2f}°")
+        print(f"  - 视场中心到横线角度: {math.degrees(center_to_line_angle_rad):.2f}°")
+        print(f"  - 相机倾斜角度: {camera_tilt_angle_deg}°")
+        print(f"  - 最终视线角度: {math.degrees(view_angle_rad):.2f}° ({'向下' if view_angle_rad < 0 else '向上'})")
+    
+    # 5. 防止除零错误或异常值
+    # 确保视线角度不接近于0(水平视线无法确定地面交点)
+    min_angle_rad = 0.01  # 约0.57度
+    if abs(view_angle_rad) < min_angle_rad:
+        if observe:
+            print(f"视线角度过小({math.degrees(view_angle_rad):.2f}°)，使用最小角度: {math.degrees(min_angle_rad):.2f}°")
+        view_angle_rad = -min_angle_rad  # 设为向下的最小角度
+    
+    # 6. 计算水平距离
+    # 仅当视线向下时计算地面距离
+    if view_angle_rad < 0:  # 视线向下
+        # 基本几何关系: 水平距离 = 高度 / tan(视线向下的角度)
+        # 注意角度为负，所以需要取负
+        ground_distance = camera_height / math.tan(-view_angle_rad)
+        
+        if observe:
+            print(f"计算公式: 距离 = 相机高度({camera_height}米) / tan(|视线角度|({abs(math.degrees(view_angle_rad)):.2f}°))")
+            print(f"计算结果: 距离 = {ground_distance:.3f}米")
+    else:  # 视线平行或向上，无法确定地面交点
+        if observe:
+            print(f"视线向上或水平，无法计算地面距离")
+        return 0.5  # 返回一个默认值
+    
+    # 7. 应用校正和限制
+    # 可选的校正因子(通过实验校准)
+    correction_factor = 1.0
+    distance = ground_distance * correction_factor
+    
+    # 设置合理的范围限制
+    min_distance = 0.1  # 最小距离(米)
+    
+    # 限制结果在合理范围内
+    final_distance = max(min_distance, distance)
+    
+    if observe and final_distance != distance:
+        print(f"应用范围限制: 原始距离 {distance:.3f}米 -> 最终距离 {final_distance:.3f}米")
+    elif observe:
+        print(f"最终距离: {final_distance:.3f}米")
+    
+    return final_distance
 
 def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     """
@@ -165,16 +245,16 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     返回:
     bool: 是否成功到达目标位置
     """
-    # 首先校准到水平
+    # # 首先校准到水平
-    aligned = align_to_horizontal_line(ctrl, msg, observe=False)
+    # aligned = align_to_horizontal_line(ctrl, msg, observe=False)
     
-    if not aligned:
+    # if not aligned:
-        print("无法校准到横向线水平，停止移动")
+    #     print("无法校准到横向线水平，停止移动")
-        return False
+    #     return False
     
     # 检测横向线
-    cv2.imwrite("current_image.jpg", ctrl.image_processor.get_current_image())
+    image = cv2.imread("current_image.jpg")  # ctrl.image_processor.get_current_image()
-    edge_point, edge_info = detect_horizontal_track_edge(ctrl.image_processor.get_current_image(), observe=False)
+    edge_point, edge_info = detect_horizontal_track_edge(image, observe=False)
     
     if edge_point is None or edge_info is None:
         print("无法检测到横向线，停止移动")
@@ -184,7 +264,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
     camera_height = 0.355  # 单位: 米 # INFO from TF-tree
     
     # 计算当前距离
-    current_distance = calculate_distance_to_line(edge_info, camera_height)
+    current_distance = calculate_distance_to_line(edge_info, camera_height, observe=observe)
     
     if current_distance is None:
         print("无法计算到横向线的距离，停止移动")
@@ -200,6 +280,8 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
         print("已经达到目标距离，无需移动")
         return True
     
+    return True
+    
     # 设置移动命令
     msg.mode = 11  # Locomotion模式
     msg.gait_id = 26  # 自变频步态
@@ -264,19 +346,4 @@ def move_to_hori_line(ctrl, msg, target_distance=0.1, observe=False):
 
 # 用法示例
 if __name__ == "__main__":
-    # 这里需要替换为实际的控制器和消息对象
+    move_to_hori_line(None, None, observe=True)
-    # ctrl = Robot_Ctrl()
-    # msg = robot_control_cmd_lcmt()
-    # image_processor = ImageProcessor()
-    
-    # 使用示例图像路径
-    image_path = "path/to/image.jpg"
-    
-    # 基本用法
-    # align_to_horizontal_line(ctrl, msg, image_path, observe=True)
-    # move_to_hori_line(ctrl, msg, image_path, target_distance=0.1, observe=True)
-    
-    # 使用里程计和图像处理器进行更精确的定位
-    # 从图像处理器获取当前图像
-    # current_image = image_processor.get_image()
-    # move_to_hori_line(ctrl, msg, current_image, target_distance=0.1, observe=True, image_processor=image_processor)