aruco.estimatePoseSingleMarkers的rvecs、tvecs理解及无人机定位应用问题

架构老林

2026-9-25

问题核心原因梳理

你对cv2.aruco.estimatePoseSingleMarkers的基础功能理解没有完全错误，该接口返回的tvecs确实是ArUco码坐标系原点相对于相机坐标系的平移向量，但你踩了无人机视觉定位里最常见的坐标系对齐、转换顺序的坑，才会出现仅对准图像中心不位移的问题。

现有代码的核心错误

坐标系转换链路缺失相机外参：你直接把相机系下的坐标用无人机的RPY旋转矩阵转换，跳过了「相机坐标系 → 无人机机体坐标系」的外参转换步骤。相机在无人机上的安装位置、安装角度都是固定偏移量，必须先做这一步转换才能和机体坐标系对齐，否则坐标映射完全错误。
坐标轴交换逻辑无依据：你构造cam_pts时直接交换了tvec的X/Y轴还对Y轴取反，如果你没有提前校准相机安装朝向对应关系，这一步会直接把坐标顺序搞混，导致只有角度控制生效、位移控制失效。
旋转矩阵使用方向错误：你当前构造的rot_mat是「世界坐标系到机体坐标系」的旋转矩阵，要把机体系下的坐标转到世界系，需要乘以该矩阵的转置（旋转矩阵为正交矩阵，转置等于逆），直接相乘相当于做了反向转换，坐标值完全不对。
未使用姿态估计结果：你完全忽略了返回的rvecs旋转向量，无人机姿态不对会导致位姿估计出现投影误差，同时也无法保证对准ArUco码，进一步放大定位偏差。

具体修正方案

统一坐标系定义并校准相机外参
先明确所有坐标系的轴定义，通用约定如下：

OpenCV相机系：X轴沿图像向右，Y轴沿图像向下，Z轴沿镜头拍摄方向向前
无人机机体系（NED约定）：X轴沿机头向前，Y轴向机身右侧，Z轴向下
根据相机实际安装朝向，校准得到相机系到机体系的固定旋转矩阵R_cam2body和平移向量t_cam2body，如果相机是正装机头前方无安装偏角，仅需要做轴交换即可，不需要额外校准。

修正坐标转换流程

if id[0] == 20:
    aruco_len = 0.25
    rvecs, tvecs = cv2.aruco.estimatePoseSingleMarkers(corners[0], aruco_len, self.alg.camera_matrix, self.alg.camera_dist)
    # 原始tvec是码在相机系下的坐标 形状(3,1)
    t_cam = tvecs[0][0].reshape(3,1)
    # 第一步：相机系转机体系
    t_body = R_cam2body @ t_cam + t_cam2body
    # 第二步：构造正确的旋转矩阵 机体系转世界系
    roll  = self.roll
    pitch = self.pitch
    yaw   = self.yaw 
    # 构造世界到机体的旋转矩阵 和你现有代码逻辑一致
    R_world2body = np.array([
        [math.cos(roll)*math.cos(pitch),  (math.cos(roll)*math.sin(pitch)*math.sin(yaw))-(math.sin(roll)*math.cos(yaw)), math.cos(roll)*math.sin(pitch)*math.cos(yaw)+math.sin(roll)*math.sin(yaw)],
        [math.sin(roll)*math.cos(pitch),  (math.sin(roll)*math.sin(pitch)*math.sin(yaw))+(math.cos(pitch)*math.cos(yaw)), math.sin(roll)*math.sin(pitch)*math.cos(yaw)-math.cos(roll)*math.sin(yaw)],
        [              -math.sin(pitch),                                              math.cos(pitch)*math.sin(yaw),                                             math.cos(pitch)*math.cos(yaw)]
    ])
    # 机体转世界用转置（旋转矩阵正交，转置等于逆）
    R_body2world = R_world2body.T
    t_world = R_body2world @ t_body
    # 得到的t_world就是码在世界系下的相对偏移 直接用于飞行控制

补充姿态对准逻辑
将rvecs转为旋转矩阵，计算码和相机之间的姿态差，转换为机体的姿态控制量，保证无人机飞行过程中始终对准ArUco码，减少投影误差。
增加滤波平滑
对识别得到的tvecs和rvecs做一阶低通滤波或者卡尔曼滤波，避免识别抖动导致的控制震荡。

内容的提问来源于stack exchange，提问作者Wilson Lysford