作者：孫尹同 ?黃祖杰

單位：廣西大學(xué) ?機(jī)械工程學(xué)院

指導(dǎo)老師：李俚

一、視覺識(shí)別

1.方案選擇

? ? ? ?根據(jù)硬件實(shí)際需求以及本團(tuán)隊(duì)能力需求，視覺顏色的識(shí)別是本項(xiàng)目的不可缺少的一部分，方案的選擇主要有一下三種類型：

(1)OpenMV

? ? ? ?簡(jiǎn)單的來說，它是一個(gè)可編程的攝像頭，通過MicroPython語言，可以實(shí)現(xiàn)需求的邏輯，而且攝像頭本身內(nèi)置了一些圖像處理算法，很容易使用。

? ? ??? OpenMV適合DIY相關(guān)的項(xiàng)目制作，比如追蹤小球的車，云臺(tái)，或者解魔方的機(jī)器人。對(duì)成本要求很高的嵌入式工業(yè)方案，比如流水線物品的分揀。OpenMV不適合復(fù)雜的算法：比如OCR識(shí)別，車牌識(shí)別，貓狗分類，深度學(xué)習(xí)之類的。

（2）K210

? ? ? ?K210是由一家叫做嘉楠公司在去年推出的一款MCU，其特色在于芯片架構(gòu)中包含了一個(gè)自研的神經(jīng)網(wǎng)絡(luò)硬件加速器KPU，可以高性能地進(jìn)行卷積神經(jīng)網(wǎng)絡(luò)運(yùn)算。在AI計(jì)算方面，K210的算力其實(shí)是相當(dāng)可觀的。根據(jù)嘉楠官方的描述，K210的KPU算力有0.8TOPS ，作為對(duì)比，擁有128個(gè)CUDA單元GPU的英偉達(dá)Jetson Nano的算力是0.47TFLOPS ；而最新的樹莓派4只有不到0.1TFLOPS 。

? ? ?? 當(dāng)然了，這個(gè)性能跟某些旗艦級(jí)別的SoC還是有差距的：A76級(jí)別的CPU本身就已經(jīng)很變態(tài)，更何況旗艦SoC上面都會(huì)搭載用于AI加速的硬件用于異構(gòu)運(yùn)算，比如高通的Hexagon DSP、蘋果的Neural Engine、華為的達(dá)芬奇架構(gòu)NPU等等，這些NPU在某些應(yīng)用下甚至能達(dá)到與數(shù)百W功耗的桌面級(jí)GPU接近的算力。

（3）OpenCV

? ?? ? OpenCV是一個(gè)用于圖像處理、分析、機(jī)器視覺方面的開源函數(shù)庫.

? ?? ? 該庫采用C及C++語言編寫，可以在windows, linux, mac OSX系統(tǒng)上面運(yùn)行。該庫的所有代碼都經(jīng)過優(yōu)化，計(jì)算效率很高，因?yàn)?，它更專注于設(shè)計(jì)成為一種用于實(shí)時(shí)系統(tǒng)的開源庫。opencv采用C語言進(jìn)行優(yōu)化，而且，在多核機(jī)器上面，其運(yùn)行速度會(huì)更快。它的一個(gè)目標(biāo)是提供友好的機(jī)器視覺接口函數(shù)，從而使得復(fù)雜的機(jī)器視覺產(chǎn)品可以加速面世。該庫包含了橫跨工業(yè)產(chǎn)品檢測(cè)、醫(yī)學(xué)圖像處理、安防、用戶界面、攝像頭標(biāo)定、三維成像、機(jī)器視覺等領(lǐng)域的超過500個(gè)接口函數(shù)。

? ?? ? 同時(shí)，由于計(jì)算機(jī)視覺與機(jī)器學(xué)習(xí)密不可分，該庫也包含了比較常用的一些機(jī)器學(xué)習(xí)算法?；蛟S，很多人知道，圖像識(shí)別、機(jī)器視覺在安防領(lǐng)域有所應(yīng)用。但，很少有人知道，在航拍圖片、街道圖片（例如google street view）中，要嚴(yán)重依賴于機(jī)器視覺的攝像頭標(biāo)定、圖像融合等技術(shù)。

? ? ? ?根據(jù)現(xiàn)實(shí)硬件要求，openmv以及K210雖然繼承了mirco python識(shí)別顏色的函數(shù)，但無法根據(jù)實(shí)際光照強(qiáng)度調(diào)整參數(shù)，并且無法外加更多的算法加強(qiáng)顏色識(shí)別，例如下圖為我團(tuán)隊(duì)的魔方識(shí)別區(qū)域。綜合考慮，本團(tuán)隊(duì)采用opencv進(jìn)行顏色識(shí)別，并且使用主流的KMeans聚類算法，對(duì)黑色進(jìn)行刪除后，得出占比最大的顏色即為識(shí)別顏色。

2.實(shí)現(xiàn)流程

? ? ?實(shí)現(xiàn)流程可分為圖片區(qū)域獲取、顏色閾值設(shè)置以及顏色閾值匹配三個(gè)部分。

2.1 圖像區(qū)域獲取

? ? ? ?在獲取圖像時(shí)，我團(tuán)隊(duì)使用的是四攝像頭四進(jìn)程進(jìn)行同步采樣，能夠更快的完成圖像取樣的動(dòng)作，并且避免死機(jī)的現(xiàn)象，四個(gè)進(jìn)程同時(shí)開啟后，對(duì)攝像頭捕獲的圖像進(jìn)行保存，在后面的顏色識(shí)別處理中只需讀取對(duì)應(yīng)路徑圖片即可進(jìn)行顏色識(shí)別。

2.2 圖像區(qū)域分割

? ? ??對(duì)圖像進(jìn)行獲取后，上下兩個(gè)攝像頭因?yàn)橥瑫r(shí)拍攝了魔方兩個(gè)面的圖片，需要進(jìn)行圖片切割以及旋轉(zhuǎn)，該部分先使用旋轉(zhuǎn)。

旋轉(zhuǎn)效果

旋轉(zhuǎn)后進(jìn)行透視變換，對(duì)周圍干擾環(huán)境進(jìn)行割除

透視變換效果

? ???? 這樣便可以獲得比較晚上的九色塊圖片，后續(xù)進(jìn)行九個(gè)色塊的聚類算法便可以得到對(duì)應(yīng)的顏色。

2.3 顏色閾值設(shè)置

? ? ? 在顏色設(shè)置中，為了方便調(diào)顏色閾值，我團(tuán)隊(duì)單獨(dú)寫了調(diào)閾值的程序，代碼及效果如下：

? ? ? 在顏色空間選取中，我們團(tuán)隊(duì)選擇了顏色閾值更加分離的RGB色彩空間，拋棄了還原人眼看見的HSV色彩空間，更容易進(jìn)行顏色的識(shí)別。

? ???? 在效果圖中，最左邊為輸出的顏色閾值范圍，左二為原圖，左三為顏色閾值范圍篩選后綠色的掩模圖，最右邊為設(shè)置顏色閾值的滑動(dòng)條。

2.4 顏色閾值匹配

? ? ? 根據(jù)上圖獲取到每個(gè)色塊的顏色閾值后，即可設(shè)置顏色識(shí)別閾值的范圍，下圖為6個(gè)色塊的RGB顏色閾值區(qū)域。

2.5 KMeans聚類算法

? ? ? ?本章節(jié)的核心為KMeans聚類算法，是識(shí)別出正確顏色的重要部分，相較于上面直接顏色閾值區(qū)域匹配，進(jìn)行KMeans聚類算法后能夠刪除黑色區(qū)域，從而減少機(jī)械爪子對(duì)色塊遮擋的干擾。

KMeans聚類算法的效果如下：

? ? ? ? 左一為各顏色區(qū)域的閾值范圍以及占比，中間為輸入圖片，右一為聚類分割后區(qū)分的三種顏色?？梢悦黠@看出對(duì)黑色區(qū)域篩選后，再根據(jù)占比最高，從而獲得正確的紅色閾值范圍。

2.6 編號(hào)輸出

? ??? ?編號(hào)輸出的方法包括六個(gè)面的序號(hào)排序，排序正確后再進(jìn)行每個(gè)面九個(gè)色塊的順序編號(hào)，便于應(yīng)對(duì)各種魔方擺放順序的情況

二、還原控制

? ? ? ? 魔方還原算法最早可以追溯到1980年捷克共和國的女大學(xué)生Jessica Fridrich發(fā)明的CFOP算法，目前這個(gè)算法仍然廣泛應(yīng)用于國際魔方速擰比賽中。而經(jīng)過幾十年的發(fā)展，魔方還原的主流算法有:層先法、角先法、TM算法和二階段算法。關(guān)于算法的優(yōu)劣與否，主要看它的兩個(gè)方面：魔方還原步數(shù)、魔方尋優(yōu)時(shí)間。本文算法追求的是在盡可能短的時(shí)間內(nèi)完成魔方的還原。雙臂二指魔方機(jī)器人還原魔方的時(shí)間包括尋優(yōu)的時(shí)間和機(jī)器人擰魔方的時(shí)間，而雙臂二指魔方機(jī)器人完成一次魔方旋轉(zhuǎn)所用的時(shí)間是固定的，因此機(jī)器人還原魔方的時(shí)間與還原指令的長度呈線性關(guān)系，即還原指令長度越短，魔方還原所花費(fèi)的時(shí)間越少。根據(jù)相關(guān)資料顯示，Kociemba 二階段算法是最優(yōu)的選擇。本章主要介紹Kociemba 二階段算法。

1.魔方狀態(tài)分析

? ? ?? 魔方的狀態(tài)分析是實(shí)現(xiàn)魔方成功求解的前提和基礎(chǔ)。如果把魔方的12個(gè)棱塊放于12個(gè)邊的位置，根據(jù)排列順序有12!種情況，除此之外，還要考慮棱塊的方向問題，有2種方向置于12個(gè)邊上，根據(jù)乘法原理可知有? 種情況，綜上所述棱塊的狀態(tài)數(shù)一共有：

?? (5-1)

????? 同理，魔方有8個(gè)角塊，每個(gè)角塊朝向有3種，所以角塊的狀態(tài)數(shù)一共是：

?? (5-2)

? ? ? ? 然而，魔方的一些狀態(tài)是不存在的。因?yàn)閷?duì)于一個(gè)還原的魔方來說，不存在單獨(dú)改變一個(gè)棱塊或角塊的朝向的情況以及單獨(dú)一對(duì)角塊和一對(duì)棱塊互換位置的情況[37]，所以應(yīng)該在魔方所有狀態(tài)的基礎(chǔ)上去掉一個(gè)倍數(shù)：

?? (5-3)

????? 此可以得到魔方的所有可能狀態(tài)數(shù)：

??? (5-4)

由此得到:

? ?? ? 魔方的所有可能狀態(tài)有43252003274489856000種，說明魔方還原的難度非常之大，想要通過胡亂的旋轉(zhuǎn)僥幸將魔方還原是幾乎不可能的。因此各種各樣的魔方還原算法接踵而來，目前人們已經(jīng)將“上帝之?dāng)?shù)”壓縮到20，即任何一種組合的魔方都能在20步以內(nèi)還原。

2. Kociemba 二階段算法

? ? ? ?經(jīng)上位機(jī)（電腦）視覺識(shí)別處理完成之后，就得到一串包含魔方相對(duì)位置與顏色的字符串，這串字符串就代表了這個(gè)魔方此時(shí)的狀態(tài)信息，通過這串字符串，計(jì)算機(jī)也就可以準(zhǔn)確的得到此時(shí)的魔方模型狀態(tài)。目前來說，魔方機(jī)器人使用較為廣泛的算法是來自 Kociemba 的二階段算法，以下簡(jiǎn)稱 Kociemba 算法。Kociemba 算法是一種能在較短時(shí)間內(nèi)使用較短步數(shù)還原魔方的算法，通常能在 1s 內(nèi)得到平均 20 步的解法，相比較于人工還原魔方的方法，如層先法、角先法，進(jìn)階的如 CFOP 法的動(dòng)輒上百步還原步驟，是非常具有優(yōu)勢(shì)的， 所以我們也采用的此算法。

? ?? ? 我們可以把魔方簡(jiǎn)單的看做一個(gè)立方體，對(duì)每個(gè)面進(jìn)行相應(yīng)的標(biāo)記，六個(gè)面分我們可以把魔方簡(jiǎn)單的看做一個(gè)立方體，對(duì)每個(gè)面進(jìn)行相應(yīng)的標(biāo)記分別為 U（頂面）、F（前面）、 L（左面）、R（右面）、B（后面）、D（下面），對(duì)于每一個(gè)面，又分別有順時(shí)針旋轉(zhuǎn) 90°，逆時(shí)針旋轉(zhuǎn) 90°， 旋轉(zhuǎn) 180°（順時(shí)針逆時(shí)針均可）這三種操作，我們可以把對(duì)應(yīng)的操作記為 XY，其中 X 為面的代號(hào)，Y 為對(duì) 應(yīng)操作代號(hào)（1 為順時(shí)針旋轉(zhuǎn) 90°，2 為旋轉(zhuǎn) 180°，3 逆時(shí)針旋轉(zhuǎn) 90°），如 U3 就代表著是頂層逆時(shí)針旋轉(zhuǎn) 90°。經(jīng) Kociemba 算法得到的解法就是多個(gè)上述操作代號(hào)相連起來，例如“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是經(jīng) Kociemba 算法得出來的完整的一個(gè)解魔方步驟。

3.魔方轉(zhuǎn)動(dòng)狀態(tài)分析

? ? ??? 我們可以把魔方簡(jiǎn)單的看做一個(gè)立方體，對(duì)每個(gè)面進(jìn)行相應(yīng)的標(biāo)記，六個(gè)面分別為U(頂面)、F（前面）、L（左面）、R（右面）、B（后面）、D（下面），對(duì)于每一個(gè)面，又分別有順時(shí)針旋轉(zhuǎn)90°，逆時(shí)針旋轉(zhuǎn)90°，旋轉(zhuǎn)180°（順時(shí)針逆時(shí)針均可）這三種操作，我們可以把對(duì)應(yīng)的操作記為XY，其中X為面的代號(hào)，Y為對(duì)應(yīng)操作代號(hào)（1為順時(shí)針旋轉(zhuǎn)90°，2為旋轉(zhuǎn)180°，3逆時(shí)針旋轉(zhuǎn)90°），如U3就代表著是頂層逆時(shí)針旋轉(zhuǎn)90°。

4. kociemba算法結(jié)果

? ? ? ?經(jīng)Kociemba算法得到的解法就是多個(gè)上述操作代號(hào)相連起來，例如“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是經(jīng)Kociemba算法得出來的完整的一個(gè)解魔方步驟。對(duì)于我們的機(jī)器人而言，這樣的一個(gè)步驟是不能直接運(yùn)用的，對(duì)于雙臂二指機(jī)器人而言，如圖，它能直接轉(zhuǎn)動(dòng)的面只有D、B兩個(gè)面，要想轉(zhuǎn)動(dòng)其他的面，還需要進(jìn)行一些其他的機(jī)械臂的操作轉(zhuǎn)化，接下來會(huì)在下位機(jī)算法中詳細(xì)介紹。

5. 串口通訊

?????? 經(jīng)Kociemba算法得到的解法，需要經(jīng)過一定的通訊手段，將數(shù)據(jù)從上位機(jī)傳輸給下位機(jī)進(jìn)行后續(xù)的操作，故選擇簡(jiǎn)單并且穩(wěn)定的usb串口通訊，只需要將上位機(jī)和下位機(jī)通過一根usb線連接起來，選取和下位機(jī)相對(duì)應(yīng)的端口號(hào)和波特率，就可以向下位機(jī)傳輸這一組數(shù)據(jù)了，下附python源代碼，其中的txt便是經(jīng)Kociemba算法得到的一串包含正確解法的字符串。

6. 串口數(shù)據(jù)接收及數(shù)據(jù)解碼

? ? ? 下位機(jī)進(jìn)行串口緩存檢測(cè)，一旦有數(shù)據(jù)輸入時(shí)，就按順序讀取單個(gè)字符值，并轉(zhuǎn)化為字符，循環(huán)賦值給a數(shù)組。這樣就可以得到上位機(jī)傳輸過來的數(shù)據(jù)了。

? ? ? ? 但這樣得到的字符串并不能被下位機(jī)直接識(shí)別，將得到的字符串以空格為界，將兩個(gè)空格之間的字符的ASCLL碼相加，使得魔方的每一個(gè)轉(zhuǎn)動(dòng)方式都有一個(gè)對(duì)應(yīng)的十進(jìn)制數(shù)字與之相對(duì)應(yīng)。

7. 機(jī)械臂控制及轉(zhuǎn)化算法

? ?? ? 我們的雙臂二指機(jī)器人包含兩個(gè)舵機(jī)，分別控制左右兩只手手指的夾緊與放開，另外還包含兩個(gè)步進(jìn)電機(jī)，用來分別控制左右兩個(gè)機(jī)械臂的轉(zhuǎn)動(dòng)，我們可以做如下定義：

? ? ? L0左手手指張開

? ? ? L1左手手指夾緊

? ? ? Ls左手順時(shí)針旋轉(zhuǎn)90°

? ? ? Ln左手逆時(shí)針旋轉(zhuǎn)90°

? ? ? L180左手旋轉(zhuǎn)180°（至于是順時(shí)針還是逆時(shí)針之后會(huì)有另行說明）

舵機(jī)控制代碼：

? ? ? ? 步進(jìn)電機(jī)在轉(zhuǎn)動(dòng)的時(shí)候，根據(jù)手指的夾緊和張開狀態(tài)，有三種速度可以選擇，在不同負(fù)載下選擇不同的速度來進(jìn)行轉(zhuǎn)動(dòng)，其中l(wèi)im值的作用在下文防纏繞算法中會(huì)詳細(xì)說明：

R0、R1、Rs、Rn、R180則是右手相應(yīng)動(dòng)作。

? ? ? ?這樣，我們就可以把魔方還原的解法通過一定的映射，轉(zhuǎn)換為機(jī)器人相應(yīng)的機(jī)械步驟，從而讓機(jī)器人可以正確還原魔方。定義機(jī)械臂手指平行地面為歸位，垂直地面為不歸位，在進(jìn)行一次轉(zhuǎn)動(dòng)操作以后，一共會(huì)出現(xiàn)三種狀態(tài)，分別是1：左手歸位、右手不歸位2：左手不歸位、右手歸位3：左右手都?xì)w位。如在左右手臂都是初始狀態(tài)時(shí)執(zhí)行U3步驟：R1→L180→R0→Rn，對(duì)于其他步驟，同樣存在相應(yīng)的轉(zhuǎn)化方法，下附表1。

? ? ? ? 經(jīng)過這樣的操作之后，只是把待轉(zhuǎn)動(dòng)面移動(dòng)到了機(jī)械爪的操作范圍之內(nèi)，至于是順時(shí)針還是逆時(shí)針，用左手還是右手，經(jīng)過程序的進(jìn)一步判斷以后就可以進(jìn)行下一步操作。

8. 坐標(biāo)轉(zhuǎn)換算法

? ? ? ?當(dāng)然，在雙手機(jī)械臂還原魔方的過程中，會(huì)涉及到一定的坐標(biāo)變換，如執(zhí)行U3步驟之后，后續(xù)的所有還原步驟中：U→D、D→U、L→R、R→L，F(xiàn)和B面不變，不難發(fā)現(xiàn)，只要是對(duì)U面進(jìn)行操作，都是這樣的坐標(biāo)轉(zhuǎn)換步驟。當(dāng)然，對(duì)于其他另外的5個(gè)面，在進(jìn)行操作以后也會(huì)有一定的轉(zhuǎn)換規(guī)則，下附表2。

9.步進(jìn)電機(jī)控制

? ? ?? 當(dāng)步進(jìn)電機(jī)加速度過快時(shí)，會(huì)使魔方發(fā)生錯(cuò)位的風(fēng)險(xiǎn)，在這里，通過使用了Arduino自帶的AccelStepper庫函數(shù)，可以更加簡(jiǎn)單方便的來驅(qū)動(dòng)步進(jìn)電機(jī)來達(dá)到想要的速度。

10.線的防纏繞算法

? ? ? ?由于機(jī)器人選擇的是用舵機(jī)來控制手指的開合，在舵機(jī)和控制板之間必須要有一根線來傳輸數(shù)據(jù)和提供電力，在魔方進(jìn)行還原的過程中，如果不加以限制，這根線就會(huì)一點(diǎn)一點(diǎn)的纏繞在機(jī)械臂上，為了解決這個(gè)問題，我們做了如下優(yōu)化工作：定義機(jī)器人初始機(jī)械臂狀態(tài)為“0”，當(dāng)機(jī)械臂順時(shí)針旋轉(zhuǎn)90°后，我們讓機(jī)械臂狀態(tài)值加上90，逆時(shí)針旋轉(zhuǎn)90°則減去90，在之前定義旋轉(zhuǎn)180°的時(shí)候，我們沒有定義旋轉(zhuǎn)的方向，在這里，我們可以根據(jù)狀態(tài)值動(dòng)態(tài)選擇順時(shí)針還是逆時(shí)針，具體來說，就是在機(jī)械臂要進(jìn)行旋轉(zhuǎn)180°的時(shí)候，機(jī)械臂狀態(tài)值大于0時(shí)，選擇逆時(shí)針旋轉(zhuǎn)180°，同時(shí)狀態(tài)值減少180，狀態(tài)值小于0時(shí)，選擇順時(shí)針旋轉(zhuǎn)180°，同時(shí)狀態(tài)值加上180，這樣的算法會(huì)使機(jī)械臂在旋轉(zhuǎn)的過程中，機(jī)械臂的狀態(tài)值始終是趨近于0也就是機(jī)械臂的狀態(tài)始終向著初始狀態(tài)靠近，這樣就可以解決線的纏繞問題。

? ? ? ?在經(jīng)過這樣的一系列算法處理以后，魔方機(jī)器人就可以驅(qū)動(dòng)電機(jī)，實(shí)現(xiàn)還原魔方的操作了。

? ? ? ?通過 10 次的平穩(wěn)運(yùn)行測(cè)試，還原速度在 18s~25s 區(qū)間內(nèi)，平均還原速度為 22.9 s。

---------------------------------------------完結(jié)-------------------------------------------------

雙臂二指魔方機(jī)器人的完整內(nèi)容詳見?https://www.robotway.com/h-col-159.html

想了解更多機(jī)器人開源項(xiàng)目請(qǐng)關(guān)注 機(jī)器譜網(wǎng)站https://www.robotway.com