当前位置：首页 > news >正文

Jetson NX上实现5米高ArUco码动态定位

news 2026/4/23 20:27:45

针对 Jetson NX 平台在 5 米高度下对移动 Aruco 码进行无人机识别、追踪与定位的需求，核心在于构建一个集高速视觉处理、鲁棒性位姿解算、实时坐标转换与飞控闭环于一体的系统。

1. 系统架构与工作流程

模块	技术选型/实现	关键要点
视觉感知	OpenCV Aruco 模块	优化检测参数以适应高空及移动场景。
位姿解算	`cv2.aruco.estimatePoseSingleMarkers`	获取标记相对于相机的 6 自由度位姿（距离、角度）。
坐标转换	ROS TF / 自定义变换	将相机坐标系下的位姿转换至无人机机体坐标系及世界坐标系。
追踪控制	PID 控制器 / 航点跟踪	根据解算的位姿偏差生成速度或位置指令。
飞控闭环	MAVLink / ROS 节点	将控制指令发送至 PX4/Pixhawk 等飞控。
传感器融合	(可选) 红外/超声波定高	结合红外高度数据可提升定位精度，尤其在降落阶段。

工作流程遵循：图像捕获 → Aruco 检测与 ID 识别 → 姿态估计 → 坐标变换 → 控制律计算 → 指令下发的闭环。

2. Jetson NX 端核心实现步骤

2.1 环境配置与 Aruco 检测优化

首先确保 Jetson NX 已安装 OpenCV（包含contrib模块以支持 Aruco）。针对 5 米高度和移动场景，检测参数需调整。

import cv2 import numpy as np # 初始化摄像头（根据实际接口调整，如CSI或USB） cap = cv2.VideoCapture(0) cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280) # 建议高分辨率，利于远距离识别 cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720) # 定义 Aruco 字典和参数 aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250) aruco_params = cv2.aruco.DetectorParameters_create() # 关键优化参数：适应高空、移动可能带来的图像模糊和部分遮挡 aruco_params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX # 亚像素级角点优化 aruco_params.adaptiveThreshWinSizeMin = 3 # 降低自适应阈值窗口最小尺寸，适应光照变化 aruco_params.adaptiveThreshWinSizeMax = 23 # 调整窗口最大尺寸 aruco_params.adaptiveThreshConstant = 7 # 调整阈值常数 # 可适当减小 minMarkerPerimeterRate，以检测更小的标记（对应更远距离） # aruco_params.minMarkerPerimeterRate = 0.03 # 默认0.03，可根据实测调整 # 相机内参和畸变系数（必须通过相机标定获取） camera_matrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float32) dist_coeffs = np.array([k1, k2, p1, p2, k3], dtype=np.float32) # 径向和切向畸变系数

2.2 位姿解算与坐标变换

检测到标记后，计算其相对于相机的位置和姿态，并转换到无人机机体坐标系。

while True: ret, frame = cap.read() if not ret: break # 1. 检测 Aruco 码 corners, ids, rejected = cv2.aruco.detectMarkers(frame, aruco_dict, parameters=aruco_params) if ids is not None: # 2. 绘制检测结果（可视化，非必须） cv2.aruco.drawDetectedMarkers(frame, corners, ids) # 3. 姿态估计 (rvec: 旋转向量, tvec: 平移向量) rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(corners, marker_length, camera_matrix, dist_coeffs) # marker_length 是 Aruco 码的实际物理边长（单位：米），必须准确测量。 for i in range(len(ids)): # 4. 绘制坐标系轴（可视化） cv2.drawFrameAxes(frame, camera_matrix, dist_coeffs, rvecs[i], tvecs[i], 0.05) # 5. 提取当前标记的位姿 tvec = tvecs[i][0] # 平移向量 [x, y, z]，z通常为前方深度 rvec = rvecs[i][0] # 旋转向量 # 6. 坐标变换：从相机坐标系到机体坐标系 # 假设相机安装在无人机下方，镜头朝下，且与机体固连。 # 定义从相机系(C)到机体系(B)的变换矩阵 T_B_C。 # 这是一个固定的安装偏移。例如，相机在机体正下方0.05米，无旋转。 # 旋转矩阵 R_B_C (假设无旋转，为单位阵) R_B_C = np.eye(3) # 平移向量 t_B_C (相机在机体坐标系中的位置，单位：米) t_B_C = np.array([0.0, 0.0, -0.05]) # 相机在机体下方5cm # 将相机系下的点变换到机体系 # 首先将旋转向量转换为旋转矩阵 R_C_M, _ = cv2.Rodrigues(rvec) # 标记(M)在相机系(C)下的旋转 t_C_M = tvec.reshape(3, 1) # 标记(M)在相机系(C)下的平移 # 机体系下标记的位置: t_B_M = R_B_C * t_C_M + t_B_C t_B_M = np.dot(R_B_C, t_C_M) + t_B_C.reshape(3, 1) # 机体系下标记的姿态: R_B_M = R_B_C * R_C_M R_B_M = np.dot(R_B_C, R_C_M) # 此时，t_B_M 表示 Aruco 码中心在无人机机体坐标系下的三维位置。 # 例如，t_B_M[2][0] 表示标记在机体正下方的相对高度（通常为负值）。 # 这个位置信息可直接用于控制。 # 7. 输出或发布位姿信息 (例如，通过ROS topic) # position_in_body = t_B_M.flatten().tolist() # 或者转换为欧拉角以供飞控使用 # euler_angles = cv2.Rodrigues(R_B_M)[0] # 注意：返回的是旋转向量，需再转换为欧拉角 # 显示图像 cv2.imshow('Aruco Detection', frame) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows()

2.3 追踪控制逻辑示例

基于解算出的机体坐标系下的相对位置（t_B_M），设计一个简单的 PID 控制器，让无人机向标记中心移动。

# 简化的 PID 位置控制器示例 (仅水平方向X,Y) class SimplePID: def __init__(self, kp, ki, kd): self.kp = kp self.ki = ki self.kd = kd self.prev_error = 0 self.integral = 0 def compute(self, error, dt): self.integral += error * dt derivative = (error - self.prev_error) / dt if dt > 0 else 0 output = self.kp * error + self.ki * self.integral + self.kd * derivative self.prev_error = error return output # 假设期望位置是机体坐标系原点 (即无人机中心与标记中心重合) # 因此误差 error_x = 0 - t_B_M[0][0], error_y = 0 - t_B_M[1][0] pid_x = SimplePID(kp=0.5, ki=0.01, kd=0.05) pid_y = SimplePID(kp=0.5, ki=0.01, kd=0.05) # 在主循环中，计算控制量 dt = 0.1 # 假设控制周期为0.1秒 error_x = -t_B_M[0][0] # 标记在机体坐标系中的X偏移 error_y = -t_B_M[1][0] # 标记在机体坐标系中的Y偏移 vel_x_cmd = pid_x.compute(error_x, dt) vel_y_cmd = pid_y.compute(error_y, dt) # 将 vel_x_cmd, vel_y_cmd 通过 MAVLink 或 ROS 发送给飞控，控制无人机水平速度。 # 高度通常由另一独立控制器维持（如定高模式或基于 t_B_M[2][0] 的控制）。

2.4 集成与部署建议

性能：Jetson NX 算力强大，可流畅运行上述流程。若帧率不足，可降低图像分辨率或使用 GPU 加速的 OpenCV 版本。
鲁棒性：
- 移动识别：通过优化aruco_params并保证足够帧率（如 >15fps）来跟踪移动目标。
- 高度补偿：5 米高度下，标记在图像中像素尺寸小。需确保标记物理尺寸足够大（例如边长为 0.5 米以上），并精确标定相机焦距。
- 丢标处理：实现状态机。当连续多帧（如 5 帧）未检测到标记时，切换至悬停或搜索模式，避免失控。
与导航堆栈集成：若使用 ROS，可将解算出的位姿通过/tf发布，构建map->odom->base_link的坐标树，供move_base等导航模块使用。
传感器融合：为提升定位精度，尤其在垂直方向，可将 Aruco 解算的高度与红外/超声波传感器数据融合。可使用扩展卡尔曼滤波器（EKF）进行融合。
标记布置：若为地面移动目标，可参考 AGV 项目经验，每 3-4 米布置一个不同 ID 的标记，高度在 1.5-2 米，便于无人机在较大范围内持续定位。