Niryo Ned2六自由度机械臂X轴抓取偏差：基于末端俯仰角修正问询

我使用Niryo Ned2六自由度机械臂+Depth AI OAK-D RGB-D相机，基于YOLO实现物体检测与机械臂抓取，已完成以下功能：

• 检测物体及其相对相机的空间位置
• 将坐标转换至末端执行器基座坐标系
• 控制机械臂移动至目标点

当前遇到的问题：机械臂Y轴、Z轴移动正常，但X轴始终存在4-8厘米偏差，距离目标位置过近。怀疑因相机非垂直安装，AI计算的X坐标存在误差，询问是否可基于末端执行器的俯仰角计算X坐标。

相关代码如下：

def convert_to_arm_coordinates(self, object_pos_3d) -> np.array:
    # Input distance is in milimeters while Niryo works in meters
    object_pos_3d /= 1000.0

    # Rotation matrix to rotate the coordinates from the camera frame to the robot frame
    rotation_matrix = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, -1]])

    # Translation vector to translate the coordinates from the camera frame to the robot frame
    translation_vector = np.array([+0.004, -0.04, -0.085])

    # Transform the coordinates using the rotation matrix
    object_pos_robot = np.matmul(rotation_matrix, object_pos_3d + translation_vector)

    return object_pos_robot

def loop_detections(detections):
    for detection in detections:
        label, x_ia, y_ia, z_ia = get_position(detection)
        if int(x_ia) != 0 and int(y_ia) != 0 and int(z_ia) != 0:
            if label == "label_to_grab":
                print("[CAMERA] Grabing label {} ..".format(label), flush=True)

                # Read detected position of the object from RGB-D camera
                relative_pos = np.array([x_ia, y_ia, z_ia])

                # Convert to robot coordinates system
                world_pos = convert_to_arm_coordinates(relative_pos)

                # Shift position
                x, y, z, roll, pitch, yaw = robot.arm.get_pose().to_list()
                print("[Niryo] Current pose ({}, {}, {})".format(x, y, z), flush=True)
                x += world_pos[1]
                y += world_pos[1]
                z += world_pos[2]
                x = max(-0.5, min(0.35, x))
                y = max(-0.5, min(0.5, y))
                z = max(0.17, min(0.6, z))
                print("[DepthAI] {}: Detection ({}, {}, {}), World ({}, {}, {}), Shifted ({}, {}, {})".format(label, x_ia, y_ia, z_ia, world_pos[0], world_pos[1], world_pos[2], x, y, z), flush=True)

                # Reach the coordinates and pick the object
                robot.pick_place.pick_from_pose([x, y, z, roll, pitch, yaw])

                # Move back to stand-by position
                robot.arm.move_pose(robot.stand_by)

1. 先修复代码中的明显错误 #

观察loop_detections函数，发现X轴更新逻辑存在bug：

x += world_pos[1]
y += world_pos[1]

X轴和Y轴都累加了world_pos[1]，这会导致X轴位置计算完全错误，大概率是X轴偏差的主要原因。正确逻辑应根据坐标转换结果分别赋值：

x += world_pos[0]
y += world_pos[1]

先修正该错误，再测试X轴移动精度是否恢复。

2. 相机倾斜的坐标修正方案 #

若修正代码后仍有偏差，确实可以利用末端执行器的俯仰角修正相机倾斜带来的X坐标误差，核心思路是通过俯仰角计算投影偏移，修正检测到的3D坐标：

• 获取实时俯仰角：从robot.arm.get_pose()中提取的pitch值即为末端俯仰角（注意Niryo Ned2默认使用弧度单位）。
• 修正X轴检测坐标：相机倾斜时，AI检测到的X坐标会产生投影误差。假设相机安装在末端，俯仰角为pitch，可通过三角函数修正：

  # z_ia为相机到物体的深度，pitch为末端俯仰角（弧度）
  corrected_x_ia = x_ia - z_ia * np.tan(pitch)
  

  逻辑说明：相机向前倾斜（pitch为正）时，物体实际X位置比检测值更靠外，需减去投影偏移量；若向后倾斜则调整符号。
• 替换原坐标进行转换：将修正后的corrected_x_ia替代原x_ia，再执行坐标转换与机械臂移动逻辑。

3. 额外优化建议 #

• 重新标定手眼外参：当前手动设置的rotation_matrix和translation_vector可能存在误差，可使用Niryo官方标定流程或OpenCV手眼标定模块，获取精准的相机-机械臂外参矩阵，从根源解决坐标转换误差。
• 增加抓取前微调：若仍有小偏差，可在机械臂到达目标位置附近后，利用相机实时检测结果进行X轴小幅度微调，直到末端对准物体。

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