方案設(shè)計(jì)

采用舵機(jī)作為魔方機(jī)器人的驅(qū)動(dòng)電機(jī)，從舵機(jī)的驅(qū)動(dòng)原理可知：舵機(jī)運(yùn)行的速度和控制器的主頻沒有關(guān)系，所以采用單片機(jī)和采用更高主頻的嵌入式處理器相比在控制效果上沒有什么差別。單片機(jī)編程過程簡(jiǎn)單，非常容易上手，而且不需要進(jìn)行操作系統(tǒng)的移植，非常適合對(duì)魔方機(jī)器人的舵機(jī)進(jìn)行控制。 2.復(fù)原時(shí)間是魔方機(jī)器人的一個(gè)非常重要，可以說是最為重要的一個(gè)參數(shù)，本文的軟件設(shè)計(jì)中涉及到了大量的算法，如 Kocemba 復(fù)原算法和 KNN 分類算法等，而控制器主頻對(duì)于算法運(yùn)行時(shí)間的長短起著決定性的作用。 所以在本文的方案設(shè)計(jì)中，我們把核心算法全部交給 Allwinner A20 運(yùn)行的 APP。

設(shè)計(jì)原理

1、Kociemba算法

Kociemba算法，又稱為二階段算法，是一個(gè)使用較短時(shí)間和較少次數(shù)還原魔方的算法。按照以前網(wǎng)上的說法，詳細(xì)的原理可以在網(wǎng)頁http://kociemba.org/cube上面找到。然而現(xiàn)在這一個(gè)網(wǎng)頁也是已經(jīng)不存在了，其他網(wǎng)站所介紹的也是非常的簡(jiǎn)略。幸好，在我的努力之下，還是找到一些零星的資料，主要是一些外文的論文和一些github項(xiàng)目。通過幾天的加班閱讀，我總算是理解了大部分的內(nèi)容。我把自己所理解的只是寫在這篇論文上面，希望能給和我一樣的魔方愛好者一些啟發(fā)，同時(shí)也算是一個(gè)拋磚引玉，希望能夠獲得讀者們更好的建議。

2、從魔方的狀態(tài)數(shù)說起

這篇文章討論的對(duì)象是三階魔方，原則上Kociemba算法可以變形為四階魔方的，但是這并不是本文要討論的內(nèi)容。首先我們先對(duì)魔方的元素進(jìn)行命名上面的約定，魔方由26個(gè)方塊組成，每個(gè)方塊由1到3個(gè)面是露在外面的，有1個(gè)面的我們稱為中心方塊，兩個(gè)面為邊，三個(gè)面的為角。通過旋轉(zhuǎn)，每個(gè)角有8中位置、3個(gè)方向，每個(gè)邊有12種位置、2個(gè)方向。我們假定魔方的中心方塊是不會(huì)移動(dòng)的，也就是說，只有非中間層的面可以旋轉(zhuǎn)，以后也是不考慮中間面的旋轉(zhuǎn)。所以會(huì)產(chǎn)生變化的只有邊和角的位置和方向，不考慮組合的合法性，魔方的總組合為：8!12!(38)*(212)種 但是魔方會(huì)包括非法的情況，這里要考慮到魔方的狀態(tài)集的問題。我們約定魔方的所有邊和角的位置和方向?yàn)槟Х降囊粋€(gè)狀態(tài)。魔方從一個(gè)狀態(tài)通過若干次的旋轉(zhuǎn)后，魔方的新狀態(tài)一定和原狀態(tài)在一個(gè)狀態(tài)集里面。

可以證明，魔方的狀態(tài)集分成12個(gè)，每一個(gè)的總數(shù)是一樣的，所以，實(shí)際上魔方的狀態(tài)數(shù)為：8!12!(38)*(212)/12種 這個(gè)數(shù)大概是4.3萬億億，具體大小不必細(xì)究，反正非常大。如果使用最暴力的算法，例如DFS、BFS等，絕對(duì)會(huì)耗盡計(jì)算機(jī)的資源，更高階的魔方就不用說了。所以，我們要對(duì)這些狀態(tài)進(jìn)行裁剪，Kociemba使用的就是因式分解的算法。打個(gè)比方，如果m=a*b，且a、b>2，那么令n=a+b，必有n 最后，我們將將魔方的面分為6個(gè)：T（上）、D（下）、F（前）、B（后）、L（左）、R（右）。面向X面，順時(shí)針旋轉(zhuǎn)X面90度表示為X、旋轉(zhuǎn)半周為X2、逆時(shí)針旋轉(zhuǎn)90度為X’。

3、魔方的特殊性質(zhì)

人們發(fā)現(xiàn)，魔方具有一些優(yōu)良的性質(zhì)，這些性質(zhì)可以幫助將狀態(tài)數(shù)分解： 1.指定角的方向后，對(duì)于所有的面，旋轉(zhuǎn)兩次并不會(huì)改變角的方向，會(huì)有相對(duì)的兩個(gè)面（例如前和后），旋轉(zhuǎn)一次不會(huì)改變角的方向。其他面旋轉(zhuǎn)一次，會(huì)按照特定的規(guī)律變換方向，如果我們把方向編碼為0、1、2，假如某一個(gè)面的四個(gè)角原始方向按照順時(shí)針是co0、co1、co2、co3，那么如果這個(gè)面的一次旋轉(zhuǎn)會(huì)改變四個(gè)角的方向的話，對(duì)角的兩個(gè)角會(huì)變成co0+1、co2+1另外兩個(gè)對(duì)角會(huì)變成co1-1、co3-1。 2.指定邊的方向后，只有兩個(gè)相對(duì)的面的旋轉(zhuǎn)會(huì)改變邊的方向，其余面的旋轉(zhuǎn)不會(huì)改變。 下面，我們假定一次旋轉(zhuǎn)上下面不會(huì)改變魔方的角的方向，一次旋轉(zhuǎn)左右面會(huì)改變魔方邊的方向。至于具體如何編碼，我會(huì)在下下面詳細(xì)闡述。

通過這兩種性質(zhì)，我們將魔方的狀態(tài)分成兩個(gè)狀態(tài)集G1、G2。其中G1狀態(tài)為魔方的原始狀態(tài)經(jīng)過若干步規(guī)定以內(nèi)的旋轉(zhuǎn)所組成的狀態(tài)集，這規(guī)定的旋轉(zhuǎn)包括：T、T’、D、D’、T2、D2、F2、B2、L2、R2。根據(jù)上面的假定，這些旋轉(zhuǎn)不會(huì)改變魔方8個(gè)角的方向，也不會(huì)改變12條邊的方向，而且原始狀態(tài)下中間的四條邊，在這個(gè)集合里面依然在中間層內(nèi)。

根據(jù)這一個(gè)結(jié)論，我們可以將魔方的總數(shù)減少為：4!*8!*8!/12 第一個(gè)4!表示中間四條邊的位置排列，第二個(gè)8!表示上下四條邊的位置排列，第三個(gè)8!表示8個(gè)角的位置排列。在使用因式分解，總數(shù)就變?yōu)椋?!+8!+8!，這個(gè)數(shù)少于10萬，并且在下面會(huì)看到，使用這一種裁剪方法會(huì)獲得較高的精度。 G2表示以G1的所有狀態(tài)為原始狀態(tài)，經(jīng)過若干次任意旋轉(zhuǎn)后的狀態(tài)集，在這個(gè)狀態(tài)集里面內(nèi)有什么優(yōu)良的的性質(zhì)可以利用，所以實(shí)際上使用了單純的裁剪手段。也是分成三組，第一組為編號(hào)為0到3的所有邊的方向和位置，第二組為編號(hào)為4到11的所有邊位置上面邊的方向，第三組為所有角的方向，所以總數(shù)為A(12,4)*(26)+28+3^8種。

4、搜索算法

Kociemba算法使用了搜索算法還原魔方。具體來說，就是先使用搜索算法轉(zhuǎn)換為G1狀態(tài)，在使用另一種搜索方式轉(zhuǎn)換為原始狀態(tài)。使用何種搜索算法對(duì)于還原魔方至關(guān)重要，搜索算法選得不好，例如使用BFS解決問題，會(huì)耗盡計(jì)算機(jī)的資源。經(jīng)過前輩的不斷探索，最終改良出IDA*。所謂IDA*，就是A*算法加上迭代加深的升級(jí)版。迭代加深就是可以保證搜索樹上面的任一個(gè)節(jié)點(diǎn)都可以獲得精確的啟發(fā)函數(shù)值，要實(shí)現(xiàn)迭代加深就必須先對(duì)搜索樹上的各個(gè)節(jié)點(diǎn)進(jìn)行預(yù)先的計(jì)算，也就是初始化。

具體到這里，就是在A*的啟發(fā)函數(shù)初始化的時(shí)候，制作一個(gè)映射表，然后通過魔方從原始狀態(tài)為起點(diǎn)進(jìn)行若干次操作，組成一個(gè)根節(jié)點(diǎn)為魔方的原始狀態(tài)的、具有一定深度的操作樹。遍歷操作樹上面的所有節(jié)點(diǎn)，也就是每一個(gè)狀態(tài)，從表上可以映射為一個(gè)啟發(fā)函數(shù)值，其值的的大小就為節(jié)點(diǎn)的深度。因?yàn)榇藭r(shí)的魔方的狀態(tài)就是魔方在原始狀態(tài)通過等于其深度的次數(shù)的操作結(jié)果，經(jīng)過相反的操作當(dāng)然可以還原魔方。

同時(shí)，可以知道，操作數(shù)上面必然會(huì)有狀態(tài)相同的不同節(jié)點(diǎn)，因?yàn)閱l(fā)函數(shù)的值應(yīng)該為最小值才可以保證精度，因此，我們?cè)诒闅v操作樹的時(shí)候，使用廣度優(yōu)先搜索，保證淺層的節(jié)點(diǎn)先被記錄，而深層的同狀態(tài)節(jié)點(diǎn)因?yàn)橛成湟呀?jīng)存在就會(huì)被忽略。 有了映射表，在調(diào)用啟發(fā)函數(shù)的時(shí)候，根據(jù)當(dāng)前的魔方狀態(tài)，經(jīng)過查表可以獲得啟發(fā)函數(shù)的值，使用A*算法判斷魔方的下一個(gè)判斷即可，同時(shí)因?yàn)榈由畹木壒?，?jié)點(diǎn)的判斷函數(shù)都是準(zhǔn)確的，不需要記錄對(duì)未搜索的可達(dá)節(jié)點(diǎn)。 那么，問題推到了映射表上面，如何設(shè)計(jì)高效的映射表？

5、映射表的設(shè)計(jì)

映射表只是一個(gè)概念，它代表從魔方的特定狀態(tài)向最小還原步數(shù)的映射。具體需要選擇什么實(shí)現(xiàn)，還需要看實(shí)際情況。實(shí)際需求可以總結(jié)為一下幾點(diǎn)： 1.映射表初始化后不會(huì)發(fā)生改動(dòng)，也就是說不需要?jiǎng)討B(tài)的改動(dòng)映射的內(nèi)容和規(guī)模。 2.映射需要一個(gè)比較高的效率。 3.映射的規(guī)模是已知的。 4.映射表應(yīng)該覆蓋所有的狀態(tài)編碼組合。 第2條決定了不應(yīng)該使用像std::map之類的樹型映射表，最佳方案應(yīng)該是數(shù)組。因?yàn)閿?shù)組查詢速度快，規(guī)模是固定的。只要我們找到一個(gè)哈希函數(shù)，將當(dāng)前魔方的狀態(tài)轉(zhuǎn)換為一個(gè)整數(shù)，就可以完成映射表了。

6、方塊編碼

我認(rèn)為，這一部分是算法的核心之一，之前的各種設(shè)計(jì)各種規(guī)律都是建立在這種編碼方案之上的。這說明一個(gè)好的建模方案可以大大地減少問題的難度。 首先，我要介紹角的編碼，容易知道，魔方有八個(gè)角，分別編碼為0至7，編碼與位置的對(duì)應(yīng)關(guān)系與問題的解決沒有關(guān)系。同理，魔方的12條邊編碼為0值11。 真正的問題在于邊和方塊的方向問題。我們知道，邊和角的方向沒有固定的參照物，因此，我們?cè)诰幋a的時(shí)候，通過規(guī)定某一些方向的方法規(guī)定方塊的方向。對(duì)于邊的方向，這比較簡(jiǎn)單，只要是0或者1即可。

對(duì)于角，情況就有點(diǎn)復(fù)雜，我在這里舉個(gè)例子，說明一下編碼的過程： 我們將T、F、R面交界處的角方塊，暫稱為方塊1，方塊1的原始位置為位置1。我們要求在位置1上面的各種角方塊面，都要按照T、F、R方向的順序表示，如果1號(hào)方塊在1號(hào)位置的順序剛好就是T、F、R，那么我們認(rèn)為此時(shí)1號(hào)方塊的方向就是0，除此之外，還有的可能的順序就是F、R、T和R、T、F，我們分別標(biāo)號(hào)為1和2。 由于T和D旋轉(zhuǎn)是不會(huì)改變方塊的方向，所以我們可以來一個(gè)T’旋轉(zhuǎn)，是的T、F、L面交界處的角方塊，我們暫稱為方塊2，到達(dá)位置1。此時(shí)從T、F、R面上面讀取方塊2，可以讀到T、L、F。

也就是所，方塊2在順序?yàn)門、L、F的時(shí)候是0號(hào)方向，我們模仿1號(hào)方塊，認(rèn)為順序L、F、T為1號(hào)方向，F(xiàn)、T、L為2號(hào)。同理可以知道T面的其他角。 由于F2旋轉(zhuǎn)是不活改變角的方向，所以我們進(jìn)行F2旋轉(zhuǎn)，此時(shí)方塊2旋轉(zhuǎn)至F、D、R的交接面處，暫稱為位置3。此時(shí)T、L、F面的方向?yàn)镈、R、F，所以我們認(rèn)為方塊3的0號(hào)方向順序?yàn)镈、R、F。同理可以知道D面的其他角。 通過這種建模方式，可以模仿上面說道的魔方的這些優(yōu)良性質(zhì)，保證搜索算法和映射表的成立。

硬件設(shè)計(jì)框圖

軟件設(shè)計(jì)框架

原理圖

工程設(shè)計(jì)圖紙

源代碼

import kociemba as kc


import os
import cv2
import numpy as np
from copy import deepcopy
import math


def imgcheck(frame_raw):
    hsv_table = [[[0, 10], [43, 255], [46, 255], '紅'],
                 [[156, 180], [43, 255], [46, 255], '紅'],
                 [[11, 20], [43, 255], [46, 255], '橙'],
                 [[20, 34], [43, 255], [46, 255], '黃'],
                 [[35, 80], [43, 255], [46, 255], '綠'],
                 [[80, 99], [43, 255], [46, 255], '青'],
                 [[100, 124], [43, 255], [46, 255], '藍(lán)'],
                 [[125, 155], [43, 255], [46, 255], '紫'],
                 [[0, 180], [0, 30], [166, 255], '白'],
                 [[0, 180], [0, 43], [46, 166], '灰'],
                 [[0, 180], [0, 255], [0, 46], '黑']]


    cube_list = []
    frame = frame_raw.copy()
    index = 0
    center = []
    candidates = []
    hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    for process_ind in range(2):
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
        #cv2.imshow("image", hsv)
        #cv2.waitKey(0)
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        blurred = cv2.GaussianBlur(gray, (3, 3), 0)
        canny = cv2.Canny(blurred, 20, 40)
        #cv2.imshow("image", canny)
        #cv2.waitKey(0)
        if process_ind == 0:
            kernel = np.ones((3, 3), np.uint8)
            dilated = cv2.dilate(canny, kernel, iterations=12)
        else:
            kernel = np.ones((6, 6), np.uint8)
            dilated = cv2.dilate(canny, kernel, iterations=3)
        if process_ind == 1 or process_ind == 0:
            cv2.imshow("image", dilated)
            cv2.waitKey(0)
        (contours, hierarchy) = cv2.findContours(dilated.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        hierarchy = hierarchy[0]
        pre_cX = 0
        pre_cY = 0
        area_arr = []
        for component in zip(contours, hierarchy):
            contour = component[0]
            peri = cv2.arcLength(contour, True)
            approx = cv2.approxPolyDP(contour, 0.1 * peri, True)
            area = cv2.contourArea(contour)
            corners = len(approx)
            # compute the center of the contour
            M = cv2.moments(contour)
            if M["m00"]:
                cX = int(M["m10"] / M["m00"])
                cY = int(M["m01"] / M["m00"])
            else:
                cX = None
                cY = None
            if cX is not None:
                if process_ind == 0:
                    tmp = {'area': area, 'contour': contour}
                    area_arr.append(tmp)
                elif 60000 > area > 1000:
                    tmp = {'index': index, 'cx': cX, 'cy': cY, 'contour': contour}
                    center.append(tmp)
                    index += 1
        if process_ind == 0:
            area_arr.sort(key=lambda k: (k.get('area', 0)), reverse=True)
            mx,my,mw,mh = cv2.boundingRect(area_arr[0].get('contour'))
            cv2.rectangle(frame, (mx,my), (mx+mw, my+mh), (0, 255, 0), 2)
            #cv2.imshow("image", frame)
            #cv2.waitKey(0)
            frame = frame[my-5:my+mh+5, mx-5:mx+mw+5]
            #cv2.imshow("image", frame)
            #cv2.waitKey(0)
            frame = cv2.resize(frame, (320, 320))
    #if index < 9:
    #    return


    print(str(index))
    '''
    center.sort(key=lambda k: (k.get('cx', 0)))
    center.sort(key=lambda k: (k.get('cy', 0)))
    '''
    center.sort(key=lambda k: (k.get('cy', 0)))
    row1 = center[0:3]
    row1.sort(key=lambda k: (k.get('cx', 0)))
    row2 = center[3:6]
    row2.sort(key=lambda k: (k.get('cx', 0)))
    row3 = center[6:9]
    row3.sort(key=lambda k: (k.get('cx', 0)))
    center.clear()
    center = row1 + row2 + row3
    for component in center:
        candidates.append(component.get('contour'))
        x,y,w,h = cv2.boundingRect(component.get('contour'))
        if abs(w - h) < 10:
            cv2.rectangle(frame, (x, y), (x+w, y+h), (0, 255, 0), 2)
            #cv2.imshow("image", frame)
            #cv2.waitKey(0)
            h_ = 0
            s_ = 0
            v_ = 0
            ss = w * h
            for i in range(w):
                for j in range(h):
                    h_ = h_ + hsv[y+j][x+i][0]
                    s_ = s_ + hsv[y+j][x+i][1]
                    v_ = v_ + hsv[y+j][x+i][2]
            h_ = h_ / ss
            s_ = s_ / ss
            v_ = v_ / ss
            print(str(h_) + ',' + str(s_) + ',' + str(v_))
            for k in hsv_table:
                if k[0][0] < h_ < k[0][1] and k[1][0] < s_ < k[1][1] and k[2][0] < v_ < k[2][1]:
                    # print(k[3])
                    cube_list.append(k[3])
                    break
    print(str(len(cube_list)))
    #if len(cube_list) == 9:
    print(cube_list)
    #cv2.drawContours(frame, candidates, -1, (0, 0, 255), 3)
    cv2.imshow("image", frame)
    cv2.waitKey(0)




if __name__ == "__main__":
    webcam = cv2.VideoCapture(0)


    if not webcam.isOpened():
        print("can't open the camera!!!")
    while True:
        ret, frame = webcam.read()
        rec_w = 200
        rec_h = 200
        rec_y = int((frame.shape[0] - rec_h)/2)
        rec_x = int((frame.shape[1] - rec_w) / 2)
        cv2.rectangle(frame, (rec_x, rec_y), (rec_x + rec_w, rec_y + rec_h), (0, 255, 0), 2)
        imgcheck(frame)
        cv2.imshow("video", frame)
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break


    webcam.release()
    cv2.destroyAllWindows()
編輯：黃飛

?

?