前面我们安装好了机械臂，接下来我们需要校准工作台坐标，这个步骤非常重要。

图像坐标系到工作台坐标系

AprilTag坐标系

每个AprilTag都有自己的一套坐标系。

遵守右手法则， x轴指向正前方, 向右旋转90度就是y轴， z轴垂直于平面朝上。

AprilTag在平面中， 位姿只有一个旋转角度。 按照右手法则， 从x旋转到y方向为正方向， 此时 。旋转角度的取值范围是：

因为AprilTag相对于工作台足够大，因此可以忽略摄像头畸变带来的误差。又因为摄像头在工作台中心的正上方， 因此使用小孔成像模型来近似。

我用了一个变焦镜头代替原装摄像头。

下面开始校准　

校准程序如下

'''
AprilTag识别，把AprilTag中心的坐标转换成工作台坐标系,同时也检测AprilTag的旋转角度
'''

import sensor, image, time, math
import utime

# 调试模式
#    is_debug=True 更多日志输出
is_debug = True

# 相机初始化部分
sensor.reset() # 感光芯片重启
sensor.set_pixformat(sensor.RGB565) #　设置图像像素格式为RGB565
sensor.set_framesize(sensor.QQVGA) # 低分辨率　QQVGA: 160 x 120

sensor.set_auto_gain(False) # 必须关闭自动增益
sensor.set_auto_whitebal(False) # 必须关闭自动白平衡
sensor.set_hmirror(True) # 水平方向翻转
sensor.set_vflip(True) # 垂直方向翻转

sensor.skip_frames(time = 2000)

clock = time.clock()

# OpenMV AprilTag识别函数, 支持同时识别6种Family家族的Tag.
# 返回对象信息包含Tag Family的名称, Tag ID
tag_families = 0
# 通过或位运算, 来决定是否识别某一种Family的Tag
tag_families |= image.TAG16H5 #  这里只用到了TAG16H5家族的TAG16H5

# 定义一些常量
IMG_WIDTH = 160 # 图像的宽度
IMG_HEIGHT = 120 # 图像的高度

OFFSET_X = -0.03 # ｘ方向上的偏移量
OFFSET_Y = -0.05 # y方向上的偏移量
FX = 0.11069547011997175 # x轴方向上的焦距
FY = 0.16256159237060022 # ｙ轴方向上的焦距

def image2workplace(cx, cy):
    '''将AprilTag的图像坐标系转换到工作台坐标系下'''
    x, y = cy, cx# 交换cx与cy
    x = FX * ( 0.5 - x / IMG_HEIGHT  + OFFSET_X)
    y = FY * ( 0.5 - y / IMG_WIDTH   + OFFSET_Y)

    return x, y

def calc_tag_offset(tag_radius):
    '''
    [-pi/4, pi/4]
    '''
    tag_degree = math.degrees(tag_radius) # 将弧度转换为角度
    current_axes = int(tag_degree / 90)
    next_axes = (current_axes + 1)
    ref_degree1 = current_axes * 90
    ref_degree2 = next_axes * 90

    if tag_degree - ref_degree1  < ref_degree2 - tag_degree:
        # CW -pi/4  -> 0
        offset =  -(tag_degree - ref_degree1)
    else:
        # CCW 0 -> pi/4
        offset = ref_degree2 - tag_degree
    return math.radians(offset)

while(True):
    clock.tick()
    img = sensor.snapshot()
    # 检测画面中的AprilTag
    for tag in img.find_apriltags(families=tag_families):
        # 在画面中绘制AprilTag所在的矩形
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
        # 绘制AprilTag的中心坐标区域
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        tag_id = tag.id() # 获取TAG的ID
        tag_radius = tag.rotation() # 获取TAG的旋转角度 单位是弧度
                                    # 取值范围 0-2\pi
        tag_degree = math.degrees(tag_radius) # 将弧度转换为角度

        print('alpha: {}'.format(calc_tag_offset(tag_radius)))
        # 图像坐标系 转换成工作台坐标系
        x, y = image2workplace(tag.cx(), tag.cy())

        if is_debug:
            # 打印日志
            print("Tag ID %d, rotation %f (radius)  = %f (degrees)" % (tag_id, tag_radius, tag_degree))
            print("Tag cx: {} cy:{}".format(tag.cx(), tag.cy()))
            print("Workspace: x:{}  y:{}".format(x, y))

    # utime.sleep_ms((200))
    print(clock.fps())

首先将FX, FY 都设置为１，将AprilTag物块放在工作台原点，并摆正

然后调整OFFSET_X, OFFSET_Y.

OFFSET_X =-0.03# ｘ方向上的偏移量(百分比)
OFFSET_Y =-0.05# y方向上的偏移量（百分比）
FX =1# x轴方向上的焦距
FY =1# ｙ轴方向上的焦距

观察打印出来的ｘｙ, 调整OFFSET_X与OFFSET_Y，使其x y变为０.

print("Workspace: x:{} y:{}".format(x, y))

调整完之后，将AprilTag物块移动到工作台的x=0.06m, y=-0.08m 处。(当然你也可以选择其他点)

FX, FY 是比例系数, 调整FX, FY, 使"Workspace: x:{} y:{}" 的输出为"Workspace: x: 0.06 y: -0.08".

上面都做好之后，需要注意机械臂基座标和工作台坐标的关系。机械臂基座标为最低固定的地方（我是这样取得），

然后就可以进行测试，将程序放入SD卡中，运行，抓取效果不错，记录在此