基于ArUco Board的无人机相机位姿估计精度异常问题问询

我正在使用ArUco Board获取无人机相机的位置与姿态，将ArUco Board设为参考原点。当无人机绕自身偏航轴旋转，偏航角达到-10°后，位置估计误差（尤其y轴）显著增大。我已收集x、y轴位置估计数据并绘图，可见偏航角小于-10°时y轴误差骤增。我尝试了所有角点优化方法（aruco.CORNER_REFINE_NONE、aruco.CORNER_REFINE_SUBPIX、aruco.CORNER_REFINE_CONTOUR、aruco.CORNER_REFINE_APRILTAG），但结果无改善。现咨询：该现象产生的原因是什么？如何提升位姿估计精度并避免该问题？

相关实现代码如下：

from djitellopy import tello
import time
from time import sleep
import numpy as np
import cv2
from cv2 import aruco
import sys, math

telloDroneEnabled = 1   # Aruco Board detection Laptop Camera or Tello Drone Camera
                        # telloDroneEnabled = 1 : Aruco Detection with Tello Drone Camera
                        # telloDroneEnabled = 0 : Aruco Detection with Laptop Camera

w, h = 640, 480

#---------------- ARUCO MARKER ---------------#
# Create vectors we'll be using for rotations and translations for postures
rvec, tvec = None, None
R_ct = 0
R_tc = 0

corners = 0

#--- Get the camera calibration path
calib_path  = ""
if telloDroneEnabled == 0:
    cameraMatrix   = np.loadtxt(calib_path+'cameraMatrix_asusWebcam.txt', delimiter=',')
    distCoeffs = np.loadtxt(calib_path+'cameraDistortion_asusWebcam.txt', delimiter=',')
elif telloDroneEnabled == 1:
    cameraMatrix = np.loadtxt(calib_path + 'cameraMatrix_telloCamera.txt', delimiter=',')
    distCoeffs = np.loadtxt(calib_path + 'cameraDistortion_telloCamera.txt', delimiter=',')

#--- 180 deg rotation matrix around the x axis
R_flip  = np.zeros((3,3), dtype=np.float32)
R_flip[0,0] = 1.0
R_flip[1,1] =-1.0
R_flip[2,2] =-1.0

#--- Define the aruco dictionary
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_1000)

# Create grid board object we're using in our stream
board = aruco.GridBoard_create(
            markersX=4,
            markersY=6,
            markerLength=0.06,
            markerSeparation=0.01,
            dictionary=ARUCO_DICT)

landingPadPosXY = [0.0] * 2
landingPadDetected = 0
landingPadLostCNTR = 0
landingPadDistanceXY = 0.0
newPadPositionReady = 0
rangefinder = 0
thresholdQR = 5

battery = 0

#------------------------------------------------------------------------------
#------- ROTATIONS https://www.learnopencv.com/rotation-matrix-to-euler-angles/
#------------------------------------------------------------------------------
# Checks if a matrix is a valid rotation matrix.
def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-2


# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R):
    assert (isRotationMatrix(R))

    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])

    singular = sy < 1e-2

    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])
        y = math.atan2(-R[2, 0], sy)
        z = math.atan2(R[1, 0], R[0, 0])
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0

    return np.array([x, y, z])

if telloDroneEnabled == 0:
    #--- Capture the videocamera (this may also be a video or a picture)
    cap = cv2.VideoCapture(0)
    # -- Set the camera size as the one it was calibrated with
    cap.set(cv2.CAP_PROP_FRAME_WIDTH, w)
    cap.set(cv2.CAP_PROP_FRAME_HEIGHT, h)
    # cap.set(cv2.CAP_PROP_FPS, 40)
elif telloDroneEnabled == 1:
    me = tello.Tello()
    me.connect()
    print(me.get_battery())
    me.streamon()

#-- Font for the text in the image
font = cv2.FONT_HERSHEY_PLAIN

prev_frame_time = 0

while True:
    # -- Read the camera frame
    if telloDroneEnabled == 0:
        ret, frame = cap.read()
    elif telloDroneEnabled == 1:
        frame = me.get_frame_read().frame
        frame = cv2.resize(frame, (w, h))

    # -- Convert in gray scale
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)  # -- remember, OpenCV stores color images in Blue, Green, Red

    # Detect Aruco markers
    corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, ARUCO_DICT, parameters=ARUCO_PARAMETERS)

    # Refine detected markers
    # Eliminates markers not part of our board, adds missing markers to the board
    corners, ids, rejectedImgPoints, recoveredIds = aruco.refineDetectedMarkers(
        image=gray,
        board=board,
        detectedCorners=corners,
        detectedIds=ids,
        rejectedCorners=rejectedImgPoints,
        cameraMatrix=cameraMatrix,
        distCoeffs=distCoeffs)

    # Outline all of the markers detected in our image
    frame = aruco.drawDetectedMarkers(frame, corners, ids, borderColor=(0, 255, 0))

    # Require 1 markers before drawing axis
    if ids is not None and len(ids) > 0:
        # Estimate the posture of the gridboard, which is a construction of 3D space based on the 2D video
        pose, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board, cameraMatrix, distCoeffs, rvec, tvec)

        if pose:
            # Draw the camera posture calculated from the gridboard
            #tvec[2] = tvec[2] * (-1)
            #frame = aruco.drawAxis(frame, cameraMatrix, distCoeffs, rvec, tvec, 0.3)
            frame = cv2.drawFrameAxes(frame, cameraMatrix, distCoeffs, rvec, tvec, 0.2)

            tvec[0] *= 95
            tvec[1] *= 95
            tvec[2] *= 95

            # -- Print the tag position in camera frame
            # str_position = "MARKER Position x=%4.0f  y=%4.0f  z=%4.0f"%(tvec[0], tvec[1], tvec[2])
            # cv2.putText(frame, str_position, (0, 360), font, 1.3, (0, 255, 0), 2, cv2.LINE_AA)

            # -- Obtain the rotation matrix tag->camera
            R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
            R_tc = R_ct.T

            # -- Get the attitude in terms of euler 321 (Needs to be flipped first)
            roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(R_flip*R_tc)

            #-- Print the marker's attitude respect to camera frame
            # str_attitude = "MARKER Attitude r=%4.0f  p=%4.0f  y=%4.0f"%(math.degrees(roll_marker),math.degrees(pitch_marker),
            #                     math.degrees(yaw_marker))
            # cv2.putText(frame, str_attitude, (0, 380), font, 1, (0, 255, 0), 2, cv2.LINE_AA)

            # -- Now get Position and attitude f the camera respect to the marker
            pos_camera = -R_tc * np.matrix(tvec)
            str_position = "CAMERA Position x=%4.0f  y=%4.0f"%(pos_camera[2], pos_camera[0])
            cv2.putText(frame, str_position, (0, 430), font, 1.4, (0, 255, 0), 2, cv2.LINE_AA)

            # -- Get the attitude of the camera respect to the frame
            pitch_camera, yaw_camera, roll_camera = rotationMatrixToEulerAngles(R_flip * R_tc)
            str_attitude = "CAMERA Attitude roll=%4.1f  pitch=%4.1f  yaw=%4.1f"%(math.degrees(roll_camera),math.degrees(pitch_camera),
                                math.degrees(yaw_camera))
            cv2.putText(frame, str_attitude, (0, 460), font, 1.4, (0, 255, 0), 2, cv2.LINE_AA)


    # calculate the FPS and display on frame
    new_frame_time = time.time()
    if telloDroneEnabled == 0:
        fps = 1 / (new_frame_time - prev_frame_time)
        prev_frame_time = new_frame_time
        cv2.putText(frame, "FPS" + str(int(fps)), (10, 40), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)
    elif telloDroneEnabled == 1:
        cv2.putText(frame, "Battery:%" + str(battery), (10, 40), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)
    # cv2.putText(frame, "Land OK: " + str(int(landingPadDetected)), (10, 60), font, 1.3, (100, 255, 0), 2, cv2.LINE_AA)

    # --- Display the frame
    cv2.imshow('frame', frame)
    if telloDroneEnabled == 1:
        battery = me.get_battery()

    #         time.sleep(0.5)

    # --- use 'q' to quit
    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'):
        if telloDroneEnabled == 0:
            cap.release()
        cv2.destroyAllWindows()
        break

可能的原因 #

• 相机标定精度不足：Tello相机的标定参数仅在小视角下有效，偏航后相机进入大畸变区域，原有畸变校正无法抵消视角偏移带来的误差，导致位姿求解精度下降。
• 观测几何退化：偏航角超过-10°后，ArUco板在相机视野中出现裁切或长宽比严重失真，位姿求解的线性方程组条件数变差，解的稳定性大幅降低，尤其y轴对观测角度变化更敏感。
• 坐标变换逻辑错误：代码中pos_camera = -R_tc * np.matrix(tvec)的轴系映射可能存在偏差，偏航角变化时该偏差被放大，集中体现在y轴误差上。
• 图像质量下降：无人机偏航运动时可能出现视频流帧率波动或运动模糊，虽然尝试了角点优化，但基础图像质量不足仍会影响检测精度。

解决方法 #

1. 重新优化相机标定 #

• 采集覆盖全视角（包括大偏航角）的标定图像（至少20组），确保标定参数在所有工作视角下有效。
• 确认标定图像分辨率与运行时的640x480一致，避免因分辨率缩放导致标定参数失效。

2. 改善ArUco板观测条件 #

• 增大ArUco板尺寸或调整无人机与板的距离，确保偏航角范围内板始终完整出现在相机视野中。
• 更换标记数量更多的ArUco板，提升位姿求解时的几何约束强度。

3. 修正坐标变换逻辑 #

• 验证轴系定义：确认ArUco板参考轴系与无人机轴系的一致性，检查R_flip矩阵的翻转逻辑是否符合实际需求。
• 静态测试验证：固定无人机，移动ArUco板，确认坐标输出是否与实际位移匹配，排查转换公式错误。

4. 位姿估计后处理 #

• 加入滑动窗口均值滤波或卡尔曼滤波，平滑连续帧的位姿结果，抑制误差突变。
• 结合无人机IMU数据（若可用），在大偏航角时辅助修正位姿估计结果。

5. 调整ArUco检测参数 #

• 提高ARUCO_PARAMETERS中的minMarkerPerimeterRate阈值，过滤模糊或部分裁切的标记。
• 微调cornerRefinementWinSize参数，配合角点优化方法提升检测精度。

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