前面我們安裝好了機械臂，接下來我們需要校準工作臺座標，這個步驟非常重要。

影象座標系到工作臺座標系

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卡中，執行，抓取效果不錯，記錄在此