郝崇清 過仕安 焦敏 馬海港 于清超 趙宇洋

摘要：為了解決舵機(jī)本身旋轉(zhuǎn)角度的限制、滑軌機(jī)械自由度的限制，以及在四機(jī)械臂的機(jī)械限制下能夠協(xié)調(diào)控制旋轉(zhuǎn)魔方6個(gè)平面的問題，針對(duì)魔方結(jié)構(gòu)建立易于控制的位置狀態(tài)模型，通過切換魔方位置狀態(tài)控制其所有平面。建立舵機(jī)及機(jī)械臂的狀態(tài)模型，進(jìn)而針對(duì)建立的模型設(shè)計(jì)四機(jī)械臂協(xié)調(diào)控制算法，有序執(zhí)行魔方的各個(gè)旋轉(zhuǎn)操作。通過設(shè)計(jì)控制電路、舞臺(tái)演示模塊、滑軌機(jī)械臂、U型機(jī)械手等模塊完成系統(tǒng)硬件搭建，并根據(jù)四機(jī)械臂協(xié)調(diào)控制算法設(shè)計(jì)了完整的控制程序。結(jié)果表明，系統(tǒng)通過四機(jī)械臂協(xié)調(diào)控制算法能夠在現(xiàn)有機(jī)械結(jié)構(gòu)的限制下流暢地將一個(gè)任意打亂、任意放置的魔方還原，實(shí)現(xiàn)教學(xué)演示與科普展示的推廣使用。系統(tǒng)中的顏色識(shí)別和機(jī)械手的協(xié)同控制部分為其他分揀裝配提供了新的思路，對(duì)無人自動(dòng)展示裝置等設(shè)計(jì)也具有一定的參考價(jià)值。

關(guān)鍵詞：機(jī)器人控制;魔方還原;協(xié)調(diào)控制;控制模型;U型機(jī)械手

中圖分類號(hào)：TP249文獻(xiàn)標(biāo)志碼：Adoi： 10.7535/hbgykj.2019yx04009

Abstract：In order to solve the limitation of the rotation angle of the servo， the limitation of the degree of freedom of the mechanism of the slide rail， and the problem of coordinated control of the six planes of the Rubik′s cube under the mechanical constraints of the four robot arms， an easy-to-control position state model is established for the Rubik′s cube structure， and the function going to all the planes of the Rubik′s Cube is realized by switching position states. The state model of the servo and the robot arm is established， and then the four-armed coordinated rotation control algorithm is designed to perform the rotation operations of the Rubik′s cube in an orderly manner. The system hardware is built by designing the control circuit， the stage demonstration module， the slide rail arm， the U-shaped robot and other modules， and the complete control program is designed according to the four-arm coordination control algorithm. The results show that the system can smoothly restore an arbitrary chaotic and arbitrarily placed Rubik′s cube under the restriction of the existing mechanical structure through the four-arm coordinated control algorithm， and realize the promotion of teaching demonstration and popular science display. ?The part of color recognition and coordinated control of the robot in the system provide new ideas for other sorting assemblies， and also have certain reference value for the design of unmanned automatic display devices.

Keywords：robot control; Rubik′s Cube restoration; coordinated control; control model; U-shaped robot

魔方還原是一項(xiàng)備受人們青睞的智力運(yùn)動(dòng)。三階魔方（Rubik′s Cube）最早于1974年由匈牙利布達(dá)佩斯建筑學(xué)院的厄爾諾·魯比克（Ern Rubik）教授發(fā)明，是目前最主流的魔方種類[1]。魔方的造型十分奇巧，共有6個(gè)中心塊、8個(gè)角塊和12個(gè)邊塊，所有塊構(gòu)成魔方整體，整面旋轉(zhuǎn)不會(huì)影響其他面的位置，魔方整體亦不會(huì)散[2]。魔方表面附有顏色貼紙，常見顏色貼紙是：白對(duì)黃，藍(lán)對(duì)綠，紅對(duì)橙。魔方的初始狀態(tài)一般是每一個(gè)面9個(gè)方塊同色。通過旋轉(zhuǎn)不同的面，可以將魔方顏色打亂。通過這種旋轉(zhuǎn)方式，三階魔方可以具有4.3×1019種不同的狀態(tài)。常見的智力運(yùn)動(dòng)就是將一個(gè)打亂的魔方還原回各個(gè)面同色，為此自從魯比克教授發(fā)明魔方以來，對(duì)魔方還原的探索就從未停止過。目前人為手動(dòng)還原的常見方法有層先法、角先法、棱先法、橋式方法、CFOP等方法[3]。對(duì)于計(jì)算機(jī)這樣的方式往往由于步數(shù)過多可能并不合適，目前大部分算法都會(huì)涉及到群論知識(shí)，群論對(duì)于魔方的表示有很大關(guān)系[4-5]。梁小龍[6]探討了主流算法中更適用于計(jì)算機(jī)還原的算法，其中Thistlethwaite Method算法更為合適。魔方最少還原步數(shù)被稱之為“上帝之?dāng)?shù)”，經(jīng)證明最多不超過20步即可還原一個(gè)被任意打亂的魔方[7]。

第4期郝崇清，等：基于四臂協(xié)調(diào)控制的魔方還原系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)河北工業(yè)科技第36卷為減少機(jī)器還原冗余較多的還原步驟，在本文中采用四機(jī)械臂設(shè)計(jì)的魔方還原系統(tǒng)。但四機(jī)械臂設(shè)計(jì)存在著一定的限制，在四機(jī)械臂的情況下需控制魔方的6個(gè)平面。同時(shí)，舵機(jī)的旋轉(zhuǎn)角度與機(jī)械臂的自由度同樣存在著一定的限制。目前針對(duì)四機(jī)械臂魔方旋轉(zhuǎn)協(xié)調(diào)控制算法的相關(guān)文獻(xiàn)報(bào)道較少。因此，針對(duì)現(xiàn)有系統(tǒng)機(jī)械上的控制限制，本文在文獻(xiàn)[8]的基礎(chǔ)上提出了四機(jī)械臂協(xié)調(diào)旋轉(zhuǎn)控制算法，建立相關(guān)的控制模型，用于四臂協(xié)調(diào)配合有序執(zhí)行魔方的還原步驟。

為強(qiáng)化系統(tǒng)演示效果，本文在文獻(xiàn)[9]的基礎(chǔ)上提出微觀化舞臺(tái)理念，引入電氣控制元素，以適應(yīng)科普推廣、教學(xué)演示的功能需求。

1系統(tǒng)結(jié)構(gòu)框架

1.1系統(tǒng)功能

魔方演示還原系統(tǒng)主要包含以下2個(gè)方面的功能。

基礎(chǔ)功能是將一個(gè)任意打亂放置的魔方還原到每一個(gè)面9個(gè)方塊同色的初始狀態(tài)。這將涉及以下功能的實(shí)現(xiàn)：攝像頭對(duì)魔方顏色的讀取;上位機(jī)對(duì)魔方顏色的判斷;上位機(jī)與單片機(jī)的相互通信;單片機(jī)對(duì)機(jī)械部分的控制;機(jī)械部分對(duì)魔方的旋轉(zhuǎn)控制;硬件部分的電路控制。

特色功能是桌面化舞臺(tái)效果演示。本文提出桌面化舞臺(tái)效果的理念，將室外大型舞臺(tái)所使用的燈光、升降臺(tái)、音響等元素整合并微觀化處理，打造桌面化的舞美效果，融入到演示系統(tǒng)之中，以強(qiáng)化本文系統(tǒng)的演示效果。這將涉及到以下功能的實(shí)現(xiàn)：升降舞臺(tái)的設(shè)計(jì)與控制;燈光、音響的控制; 舞臺(tái)元素的相互配合與流程控制。

1.2系統(tǒng)模塊

本設(shè)計(jì)是具有教學(xué)演示功能的自動(dòng)魔方還原系統(tǒng)。系統(tǒng)共由電源與電路模塊、單片機(jī)控制模塊、舞臺(tái)演示模塊、攝像頭掃描模塊、魔方還原機(jī)械模塊、上位機(jī)算法處理模塊等組成，如圖1所示。

電源與電路模塊：主要連接系統(tǒng)的控制器與各個(gè)硬件外設(shè)，負(fù)責(zé)供電與通信;

單片機(jī)控制模塊：用于控制系統(tǒng)中的硬件外設(shè)，執(zhí)行相應(yīng)的功能流程，并負(fù)責(zé)與上位機(jī)的通信交流;

舞臺(tái)演示模塊：本系統(tǒng)中硬件外設(shè)的一部分，用于加強(qiáng)系統(tǒng)功能的演示效果;

攝像頭掃描模塊：本系統(tǒng)中硬件外設(shè)的一部分，與上位機(jī)連接通信，用于識(shí)別魔方顏色，獲取魔方信息;

魔方還原機(jī)械模塊：本系統(tǒng)中硬件外設(shè)的一部分，用于執(zhí)行魔方旋轉(zhuǎn)的相關(guān)操作;

上位機(jī)算法處理模塊：用于處理魔方還原的相關(guān)算法，并負(fù)責(zé)與單片機(jī)通信，將執(zhí)行步驟傳回單片機(jī)。

最終完成的系統(tǒng)實(shí)物圖如圖2所示。

2系統(tǒng)硬件部分設(shè)計(jì)

2.1主體結(jié)構(gòu)設(shè)計(jì)

裝置的結(jié)構(gòu)主體由20×20 mm型號(hào)的鋁型材搭建而成，型材連接處皆有型材角件連接，堅(jiān)固可靠。底座為3 mm厚的201不銹鋼鋼板底座，底板有1 cm邊緣彎折以增強(qiáng)鋼板整體強(qiáng)度。鋁型材與鋼板之間連接處放有橡膠墊，以避免鋁型材與鋼板之間直接摩擦。間架結(jié)構(gòu)整體中心位于底座，上方機(jī)械結(jié)構(gòu)轉(zhuǎn)動(dòng)時(shí)保證裝置整體穩(wěn)定不晃動(dòng)。

采用型材制作間架結(jié)構(gòu)能夠使裝置主體有較好的強(qiáng)度和穩(wěn)定性;同時(shí)又具有較好的可擴(kuò)展性，通過滑塊螺母可以使其他硬件外設(shè)直接連接在型材上，十分方便。

2.2旋轉(zhuǎn)執(zhí)行機(jī)構(gòu)設(shè)計(jì)

采用了帶有孔位、便于安裝的滑軌。該滑軌與鋁型材通過滑塊螺母固定。為了滑軌能夠前后移動(dòng)，在型材的另一側(cè)放置舵機(jī)，通過構(gòu)造曲柄滑塊結(jié)構(gòu)使滑軌能夠前后移動(dòng)。為了能使舵機(jī)能夠固定在型材的側(cè)面，在本裝置中設(shè)計(jì)了特殊的舵機(jī)支架用于固定舵機(jī)。

使用球頭拉桿將舵機(jī)與滑軌連接構(gòu)成曲柄滑塊機(jī)構(gòu)[10]。球頭拉桿的使用可以減少機(jī)械裝置間拉動(dòng)的摩擦，使得裝置的耐用性更好。連接示意圖如圖3所示。

從圖5可知，在γ = 1左右的地方（即γ = 57°左右），滑軌能達(dá)到的距離最大，約83.64 cm;在γ = 2.1左右的地方（即γ= 120°左右），滑軌能夠回到末端。在γ =1～2.26（即γ=57°～130°），整個(gè)滑軌的移動(dòng)隨舵機(jī)旋轉(zhuǎn)角度變化近似是線性的，也就是說當(dāng)舵機(jī)旋轉(zhuǎn)時(shí)滑軌滑動(dòng)的反應(yīng)會(huì)比較靈敏，并且在實(shí)際情況中，滑軌無需回到末端，移動(dòng)的距離也并不長(zhǎng)，這樣的反應(yīng)已經(jīng)足夠在本系統(tǒng)中使用。因此，這樣的曲柄滑塊機(jī)構(gòu)在本系統(tǒng)中是可行的。

2.3舞臺(tái)效果機(jī)構(gòu)設(shè)計(jì)

舞臺(tái)效果機(jī)構(gòu)由直線推桿電機(jī)、自行設(shè)計(jì)的魔方托架、LED彩燈、揚(yáng)聲器等組成。直線推桿電機(jī)搭配魔方托架構(gòu)成升降舞臺(tái);LED彩燈打造燈光效果;揚(yáng)聲器構(gòu)成音響元素。

升降舞臺(tái)在裝置中的組合效果如圖6所示。

2.4控制電路設(shè)計(jì)

文獻(xiàn)[11]中闡述了其各模塊控制供電的思路。出于對(duì)美觀性和功能的考慮，本系統(tǒng)電路連接均采用“控制芯片-光電耦合模塊/繼電器模塊-硬件外設(shè)”的連接模式，外電路共用一個(gè)電源的外設(shè)共地，每個(gè)電源之間為獨(dú)立共地，控制電路信號(hào)端均與單片機(jī)共地，使用的光電耦合模塊/繼電器作用是對(duì)控制信號(hào)電氣隔離，保護(hù)控制電路，防止外電路的回流燒壞控制芯片，同時(shí)防止信號(hào)干擾。

橋模塊的控制輸入信號(hào)電壓要求（3.3～7.2 V），單片機(jī)輸出端信號(hào)高電平為3.3 V，偶爾會(huì)造成信號(hào)遺失，所以在控制芯片和橋模塊之間設(shè)計(jì)了一個(gè)電壓放大電路，把控制信號(hào)升壓到5 V。外設(shè)硬件電路連接框圖見圖7。

3系統(tǒng)軟件部分設(shè)計(jì)

3.1U型機(jī)械手的狀態(tài)模型建立及其控制

文獻(xiàn)[12]—[13]的設(shè)計(jì)中均采用的雙臂解決方案，文獻(xiàn)[14]—[16]的設(shè)計(jì)研究中則采用的是四臂解決方案。出于對(duì)還原步驟冗余的減少，本文采用對(duì)稱的四臂解決方案。

在本系統(tǒng)中，需要設(shè)計(jì)U型機(jī)械手的狀態(tài)與控制模型，以便在程序中表示和控制機(jī)械手。建立模型的要求有：1）因?yàn)椴捎玫亩鏅C(jī)不是連續(xù)多周旋轉(zhuǎn)的舵機(jī)，需要記錄U型機(jī)械手的位置;2）需要設(shè)計(jì)的舵機(jī)旋轉(zhuǎn)函數(shù)能夠同時(shí)改變已經(jīng)記錄的舵機(jī)位置。

根據(jù)上述要求建立其狀態(tài)模型，本文對(duì)機(jī)械手位置做出如下規(guī)定。

1）角度記錄規(guī)定

因?yàn)閁型機(jī)械手僅有-180°，-90°，0°，90°和180°等5種狀態(tài)，因此規(guī)定用帶符號(hào)的整數(shù)表示在不同狀態(tài)下記錄的U型機(jī)械手狀態(tài)。U型機(jī)械手的角度記錄規(guī)定見表1。

這樣記錄的好處是方便角度加減計(jì)算，以便在旋轉(zhuǎn)函數(shù)中同時(shí)改變已經(jīng)記錄的狀態(tài)。

2）狀態(tài)記錄規(guī)定

這里的狀態(tài)是指U型機(jī)械手處于水平狀態(tài)還是豎直狀態(tài)。U型機(jī)械手的狀態(tài)記錄規(guī)定見表2。

記錄U型機(jī)械手狀態(tài)的目的是方便程序后續(xù)步驟檢測(cè)機(jī)械沖突，防止U型機(jī)械手直接相撞.至此，U型機(jī)械手的狀態(tài)模型建立完畢。

在程序中，需要給出的指令是某只U型機(jī)械手順時(shí)針或者逆時(shí)針旋轉(zhuǎn)一個(gè)度數(shù)，以完成旋轉(zhuǎn)魔方的相關(guān)操作。同時(shí)，需要改變上面記錄好的狀態(tài)模型里的參數(shù)。

程序執(zhí)行的流程如下：

控制函數(shù)中傳入需要執(zhí)行的指令，根據(jù)傳入的旋轉(zhuǎn)指令計(jì)算出狀態(tài)模型里的2個(gè)新的參數(shù)。底層的舵機(jī)旋轉(zhuǎn)函數(shù)能夠讓舵機(jī)旋轉(zhuǎn)到指定度數(shù)。將計(jì)算好的新的角度參數(shù)傳入底層舵機(jī)旋轉(zhuǎn)函數(shù)，執(zhí)行旋轉(zhuǎn)功能。最后，用計(jì)算好的新的狀態(tài)模型參數(shù)覆蓋原有的模型參數(shù)，完成旋轉(zhuǎn)命令的執(zhí)行。

滑軌狀態(tài)記錄規(guī)定見表3。

滑軌僅有2種狀態(tài)，即在前方與在后方，發(fā)出的指令也只有前進(jìn)和后退2種。程序執(zhí)行的流程如下：滑軌控制函數(shù)根據(jù)傳入的指令計(jì)算出新的狀態(tài)，交給底層函數(shù)執(zhí)行，將原有的狀態(tài)數(shù)據(jù)覆蓋，完成執(zhí)行。

滑軌的狀態(tài)模型的存在意義是：滑軌在前方與在后方代表著當(dāng)前U型機(jī)械手是否在抓握魔方。魔方要想在空中停留，一定至少需要2只機(jī)械手同時(shí)夾持魔方;當(dāng)U型機(jī)械手本身需要調(diào)整角度時(shí)，不能抓握魔方。因此，通過檢測(cè)滑軌的狀態(tài)模型參數(shù)是否合適就可以確保是否與上述兩條規(guī)則沖突。

3.2魔方的狀態(tài)模型建立

魔方根據(jù)自身的位置和不同的旋轉(zhuǎn)方式與旋轉(zhuǎn)次數(shù)擁有著難以計(jì)數(shù)的狀態(tài)數(shù)量。因此，為了方便控制，有必要對(duì)魔方的狀態(tài)做出規(guī)定，建立魔方的狀態(tài)模型。建立模型的要求有：1）通過模型能夠推算出魔方所處的位置狀態(tài);2）盡可能規(guī)范減少魔方的狀態(tài)數(shù)量以方便程序書寫。

魔方的旋轉(zhuǎn)操作都是以整個(gè)平面為單位，1次旋轉(zhuǎn)1個(gè)平面。根據(jù)魔方的結(jié)構(gòu)可知，無論旋轉(zhuǎn)哪一個(gè)平面，中心顏色塊都不會(huì)移動(dòng)。因此，可以通過中心塊顏色來規(guī)范魔方座位，從而能夠獲取魔方的位置信息，進(jìn)而規(guī)范魔方操作。

在魔方的求解算法中，規(guī)定好一種座位能夠大大減少代碼的書寫量，因此需要規(guī)定一個(gè)魔方的求解標(biāo)準(zhǔn)座位。同時(shí)，由于4只U型機(jī)械手的結(jié)構(gòu)限制，只能同時(shí)轉(zhuǎn)動(dòng)4個(gè)平面，因此還需要規(guī)定特定的旋轉(zhuǎn)操作座位。

在本系統(tǒng)中規(guī)定的標(biāo)準(zhǔn)座位如下：當(dāng)正視于魔方的時(shí)候，綠色中心塊所在平面沖向前面，白色中心塊所在平面沖向上面。實(shí)際上，在市面上買到的魔方都有標(biāo)準(zhǔn)配色，當(dāng)掌握2個(gè)不同面顏色的位置時(shí)，其余平面中心塊的顏色也就能自然推出。

接下來，本文再規(guī)定特定的旋轉(zhuǎn)操作座位。僅依據(jù)中心塊顏色來區(qū)分魔方的位置狀態(tài)，實(shí)際中魔方共由24種不同的位置狀態(tài)。要對(duì)每一種狀態(tài)寫出不同狀態(tài)下平面的操作，代碼書寫量會(huì)十分繁重且復(fù)雜。因此，需要減少操控的魔方位置狀態(tài)以方便代碼書寫。

根據(jù)現(xiàn)有的機(jī)械結(jié)構(gòu)，當(dāng)魔方處于初始的標(biāo)準(zhǔn)座位時(shí)，僅能操控旋轉(zhuǎn)前面（F）、后面（B）、左面（L）、右面（R），上面（U）和下面（D）不能操控。若想旋轉(zhuǎn)上面（U）和下面（D），需要將魔方向左側(cè)翻轉(zhuǎn)一次就能操控，此時(shí)左面（L）和右面（R）不能旋轉(zhuǎn)。因此，僅需要2種狀態(tài)就可以操控到所有的平面（實(shí)際上，隨意一種翻轉(zhuǎn)方法都可以操控到2個(gè)之前不能旋轉(zhuǎn)的平面，而其中2個(gè)之前可以旋轉(zhuǎn)的平面又不能操控到）。

據(jù)此，本文在系統(tǒng)中規(guī)定了如下2個(gè)特定的旋轉(zhuǎn)操作座位，如圖8所示。

根據(jù)這種旋轉(zhuǎn)操作座位的設(shè)置，系統(tǒng)只需讓魔方在這2種位置狀態(tài)之間來回切換，就可以操控到所有的平面。通過這種方式，將24種位置狀態(tài)縮小到了2種位置狀態(tài)，極大地提高了代碼書寫的便利性。至此，魔方的狀態(tài)模型建立完畢。

3.3魔方的座位掃描算法

在本系統(tǒng)中，需要攝像頭完整地逐次掃描魔方的6個(gè)面。因此需要設(shè)計(jì)一套邏輯算法，配合4只U型機(jī)械手，能夠逐次掃描魔方的各個(gè)面，并且沒有重復(fù)。

在設(shè)計(jì)中，需要注意的問題有：1）每一面都要掃描到，不能有遺漏;2）避免重復(fù)掃描。

先不考慮4只U型機(jī)械手的配合問題，只考慮各個(gè)面被掃描的先后順序。

首先對(duì)U型機(jī)械手的旋轉(zhuǎn)步驟做一個(gè)定義：U型機(jī)械手1次動(dòng)作視為旋轉(zhuǎn)1步。

也就是說，不論U型機(jī)械手是旋轉(zhuǎn)90°，180°，270°還是360°，只要U型機(jī)械手動(dòng)作1次，即視為旋轉(zhuǎn)1步，與其旋轉(zhuǎn)的角度和方向無關(guān)。這樣的規(guī)定是因?yàn)槊看涡D(zhuǎn)不同的度數(shù)時(shí)間差非常短;而來回切換不同的U型機(jī)械手涉及到多機(jī)構(gòu)移動(dòng)，會(huì)花費(fèi)大量時(shí)間，相比之下僅因?yàn)樾D(zhuǎn)度數(shù)不同的時(shí)間差可以忽略。

為了減少U型機(jī)械手的旋轉(zhuǎn)步驟，在每對(duì)U型機(jī)械手夾持1次魔方的情況下掃描2個(gè)面。因此本文設(shè)計(jì)出了固定順序的掃描邏輯。在這種邏輯下，可以保證魔方的每一個(gè)平面都能被掃描到，不會(huì)重復(fù)。

該掃描邏輯如圖9所示。

3.4魔方的座位還原算法

在本系統(tǒng)的設(shè)計(jì)中，要求魔方以任意位置放置復(fù)原。因此，對(duì)于任意放置的魔方要先將其還原回標(biāo)準(zhǔn)座位，才能繼續(xù)配合程序執(zhí)行還原過程。如何通過U型機(jī)械手配合攝像頭使魔方回歸到標(biāo)準(zhǔn)座位成為要先解決的問題。

在設(shè)計(jì)過程中，本文的設(shè)計(jì)思路還是力圖通過減少魔方位置狀態(tài)數(shù)來一步步縮小范圍，最終還原回標(biāo)準(zhǔn)座位?；谶@一思路，設(shè)計(jì)出了如下還原算法，流程圖如圖10所示。

在規(guī)定標(biāo)準(zhǔn)座位時(shí)提到，只需要知道2個(gè)面的準(zhǔn)確位置，就可以根據(jù)標(biāo)準(zhǔn)配色推算出魔方位置狀態(tài)。因此，只需先規(guī)定好一個(gè)固定平面，就能將位置狀態(tài)數(shù)量從24種縮小到4種，此時(shí)再固定方向地掃描一面就很容易獲取當(dāng)前精確的位置狀態(tài)，并根據(jù)當(dāng)前狀態(tài)設(shè)計(jì)還原至標(biāo)準(zhǔn)座位的步驟。

依托3.3小節(jié)設(shè)計(jì)好的魔方座位掃描算法，逐面不重復(fù)地掃描魔方每一面，直至尋找到綠色中心塊面（掃描綠色中心塊面的原因是綠色處于前面，這樣便于編寫還原步驟的代碼）。此時(shí)將魔方向下翻轉(zhuǎn)90°，可能出現(xiàn)的顏色僅有4種：白色、橙色、黃色、紅色。此時(shí)只需針對(duì)不同的顏色情況編寫不同的還原步驟即可。

3.5四臂協(xié)調(diào)控制算法

在介紹四臂協(xié)調(diào)控制算法前，先簡(jiǎn)要介紹一下魔方的旋轉(zhuǎn)操作。

魔方共有6種旋轉(zhuǎn)操作，每種操作都有3種衍生的操作，即順時(shí)針旋轉(zhuǎn)90°、順時(shí)針旋轉(zhuǎn)180°和逆時(shí)針旋轉(zhuǎn)90°。6種操作分別是前側(cè)旋轉(zhuǎn)（F，F(xiàn)2，F(xiàn)’）、后側(cè)旋轉(zhuǎn)（B，B2，B’）、上側(cè)旋轉(zhuǎn)（U，U2，U’）、下側(cè)旋轉(zhuǎn)（D，D2，D’）、左側(cè)旋轉(zhuǎn)（L，L2，L’）和右側(cè)旋轉(zhuǎn)（R，R2，R’）。

在劉遠(yuǎn)法等[17]以及張艷珠等[18]的研究中簡(jiǎn)單給出了機(jī)械的控制流程。在本文中將進(jìn)一步詳細(xì)闡述四機(jī)械臂控制的具體算法流程。

旋轉(zhuǎn)控制是執(zhí)行還原命令的核心，設(shè)計(jì)出穩(wěn)定的旋轉(zhuǎn)邏輯能夠連貫實(shí)現(xiàn)魔方還原步驟。在設(shè)計(jì)中，需要注意的問題有：1）注意4只U型機(jī)械手和4只滑軌的相互配合，避免沖突;2）收到不同的指令能夠執(zhí)行相應(yīng)的步驟;3）模塊化設(shè)計(jì)，方便接收到指令直接調(diào)用。

接下來介紹本系統(tǒng)中設(shè)計(jì)的旋轉(zhuǎn)控制算法。在每一個(gè)步驟命令被調(diào)用時(shí)，都要經(jīng)過下面3個(gè)流程：檢測(cè)沖突、調(diào)整狀態(tài)、執(zhí)行步驟。

在執(zhí)行旋轉(zhuǎn)步驟之前，都要做好必要的沖突檢測(cè)和狀態(tài)調(diào)整，否則極易出現(xiàn)物理上U型機(jī)械手撞擊或者與魔方?jīng)_撞等現(xiàn)象。若沖突檢測(cè)結(jié)果為沒有沖突，則無需調(diào)整狀態(tài)。接下來介紹的每一個(gè)旋轉(zhuǎn)邏輯函數(shù)都會(huì)從這3個(gè)方面介紹。

在介紹這些設(shè)計(jì)的邏輯之前，先簡(jiǎn)單介紹下本文設(shè)計(jì)的函數(shù)。根據(jù)命令需要設(shè)計(jì)了3類函數(shù)：初始化豎直夾持魔方函數(shù)、改變魔方座位狀態(tài)函數(shù)、單一的魔方面旋轉(zhuǎn)函數(shù)、魔方整體旋轉(zhuǎn)函數(shù)。在后續(xù)的代碼優(yōu)化中，還設(shè)計(jì)了2個(gè)魔方面同時(shí)旋轉(zhuǎn)的函數(shù)。

這里以典型的初始化豎直夾持魔方函數(shù)、單一的魔方面旋轉(zhuǎn)函數(shù)為例列出流程圖及其詳細(xì)介紹，其余函數(shù)設(shè)計(jì)思路與之類似。

初始化豎直夾持魔方函數(shù)的功能是讓一對(duì)U型機(jī)械手豎直地夾持魔方。所謂豎直地夾持，就是讓程序中U型機(jī)械手狀態(tài)模型里的狀態(tài)記錄為0。設(shè)計(jì)這個(gè)函數(shù)的目的是為了更方便地解決后續(xù)步驟里的沖突問題。當(dāng)其中一對(duì)U型機(jī)械手豎直地夾持魔方時(shí)，另一對(duì)機(jī)械手無論是豎直狀態(tài)還是水平狀態(tài)，都可以直接夾持魔方，不會(huì)沖突。另外，當(dāng)需要調(diào)整魔方位置狀態(tài)時(shí)，也可以利用此函數(shù)為調(diào)整做準(zhǔn)備。

初始化豎直夾持魔方函數(shù)設(shè)計(jì)的主要思路是：首先看需要初始化的U型機(jī)械手是否已經(jīng)處于目標(biāo)狀態(tài)，即豎直地夾持魔方。若不在此狀態(tài)，則需另外一對(duì)U型機(jī)械手先夾持魔方，再撤回需要調(diào)整的U型機(jī)械手，然后調(diào)整好再豎直夾持，完成邏輯。

由于函數(shù)本身就是一個(gè)調(diào)整函數(shù)，不執(zhí)行旋轉(zhuǎn)魔方的功能，因此只包含沖突檢測(cè)和狀態(tài)調(diào)整2個(gè)環(huán)節(jié)。函數(shù)執(zhí)行的流程如圖11所示。

魔方單一平面旋轉(zhuǎn)函數(shù)是執(zhí)行魔方旋轉(zhuǎn)功能的核心函數(shù)。在主函數(shù)中，通過讀取不同的步驟調(diào)用不同的旋轉(zhuǎn)操作函數(shù)來執(zhí)行旋轉(zhuǎn)操作。各個(gè)操作旋轉(zhuǎn)函數(shù)的執(zhí)行思路大致相似。

在本系統(tǒng)中，共由4只U型機(jī)械手。在目前規(guī)定的兩個(gè)旋轉(zhuǎn)座位中，座位狀態(tài)‘0’可以旋轉(zhuǎn)左面和右面，座位狀態(tài)‘1’可以旋轉(zhuǎn)上面和下面。前面和后面的旋轉(zhuǎn)兩個(gè)狀態(tài)下都可以操作，無需考慮魔方座位狀態(tài)。

魔方單一平面旋轉(zhuǎn)函數(shù)的執(zhí)行流程如圖12所示。

3.6攝像頭的顏色識(shí)別

梁煒等[19]探討了幾種顏色識(shí)別的空間模型及其算法;黃銘等[20]探討了如何將RGB空間顏色識(shí)別應(yīng)用于魔方顏色識(shí)別中;張金龍等[21]探討了如何將HSV空間顏色識(shí)別應(yīng)用于魔方顏色識(shí)別中。本系統(tǒng)結(jié)合MATLAB，采用RGB的空間顏色識(shí)別算法。

系統(tǒng)上位機(jī)MATLAB調(diào)用預(yù)連接好的攝像頭執(zhí)行拍照指令獲取圖像。根據(jù)獲取的圖像在MATLAB中生成圖像的RGB三維矩陣。根據(jù)預(yù)設(shè)的圖片位置取出相應(yīng)色塊矩陣的RGB值;根據(jù)預(yù)設(shè)的標(biāo)準(zhǔn)色塊的RGB值對(duì)每一個(gè)像素點(diǎn)進(jìn)行比對(duì)，比對(duì)的方式如下：

對(duì)魔方一個(gè)平面內(nèi)某一色塊采集到的全部像素判定，根據(jù)統(tǒng)計(jì)出的眾數(shù)值確定魔方該色塊的顏色，即識(shí)別完成。

關(guān)于魔方6種顏色的基準(zhǔn)值取值，是在實(shí)驗(yàn)室環(huán)境下取預(yù)設(shè)位置的所用像素，對(duì)其求RGB 3個(gè)分量的平均值作為該顏色的基準(zhǔn)RGB值。

圖13為采用的RGB顏色識(shí)別模型。通過掃描魔方的各個(gè)標(biāo)準(zhǔn)顏色平面獲取各個(gè)顏色標(biāo)準(zhǔn)值;在圖中當(dāng)前待識(shí)別的顏色在空間距離上與橙色的標(biāo)準(zhǔn)值最為接近，在當(dāng)前模型下將該顏色判定為橙色。

3.7Thislethwaite Method算法簡(jiǎn)介

本系統(tǒng)中使用Thislethwaite Method算法作為魔方的還原解法。它與日常所使用的層先法等入門算法不同，打破了人們視覺上按照逐層還原的常規(guī)思路，將魔方顏色區(qū)分不同的優(yōu)先級(jí)，將不同優(yōu)先級(jí)的顏色相對(duì)還原后再整體還原魔方。該算法以群論作為理論基礎(chǔ)，將魔方的各個(gè)旋轉(zhuǎn)操作作為一個(gè)集合中的元素，該集合稱為魔方群。通過群論中的置換運(yùn)算獲得子群，對(duì)魔方群逐步降階，降階后的魔方群中元素減少，代表魔方的混亂程度越小，魔方的狀態(tài)數(shù)量減少。當(dāng)魔方群降階到單位子群時(shí)，魔方復(fù)原完成[6]。

4結(jié)語

在魔方還原系統(tǒng)的設(shè)計(jì)中建立了魔方的狀態(tài)模型與U型機(jī)械手、滑軌機(jī)械臂的狀態(tài)模型，在四機(jī)械臂及舵機(jī)旋轉(zhuǎn)角度等條件限制下設(shè)計(jì)了四機(jī)械臂協(xié)調(diào)旋轉(zhuǎn)控制算法，完整地設(shè)計(jì)了魔方還原系統(tǒng)的各個(gè)組成部分，結(jié)合算法最終實(shí)現(xiàn)了結(jié)合四臂協(xié)調(diào)控制的魔方還原系統(tǒng)設(shè)計(jì)。使用該算法，系統(tǒng)能夠流暢地將一個(gè)任意打亂、任意放置的魔方還原，證明了算法的可行性。配合本文提出的微觀化舞臺(tái)理念，能夠強(qiáng)化系統(tǒng)的演示效果，使得本系統(tǒng)適用于教學(xué)演示、科普展覽等場(chǎng)景的應(yīng)用。

本研究通過魔方與U型機(jī)械手的模型建立與算法設(shè)計(jì)實(shí)現(xiàn)了四臂協(xié)調(diào)控制復(fù)原魔方，給出了詳細(xì)的實(shí)現(xiàn)方案。但設(shè)計(jì)的座位掃描算法、旋轉(zhuǎn)控制算法并非是四臂協(xié)調(diào)的最優(yōu)控制，對(duì)此仍有較大的探討空間。

參考文獻(xiàn)/References：

[1]哈金才，李若雪，哈瑞.魔方的數(shù)學(xué)模型研究及其應(yīng)用[J].創(chuàng)新創(chuàng)業(yè)理論研究與實(shí)踐，2018（19）：83-86.

HA Jincai， LI Ruoxue， HA Rui. Research on mathematical model of Rubik′s Cube and its application[J]. The Theory and Practice of Innovation and Entrepreneurship， 2018（19）：83-86.

[2]李世春.魔方里的科學(xué)和文化[M].北京：高等教育出版社，2015.

[3]李國(guó)軍，鐘志強(qiáng)，張毅寧.LEGO機(jī)器人魔方還原基本方法與過程[J].鞍山師范學(xué)院學(xué)報(bào)，2014，16（2）：71-74.

LI Guojun， ZHONG Zhiqiang， ZHANG Yining. The basic method and process of Lego robot working Rubik′s Cube work[J]. Journal of Anshan Normal University， 2014， 16（2）：71-74.

[4]李世春.魔方的科學(xué)和計(jì)算機(jī)表現(xiàn)[M].東營(yíng)：石油大學(xué)出版社，2003.

[5]喻騰飛. 魔方群的研究[D].合肥：中國(guó)科學(xué)技術(shù)大學(xué)，2014.

YU Tengfei. Research of Rubik’s Cube Group[D]. Hefei： University of Science and Technology of China， 2014.

[6] 梁小龍. 解魔方算法的研究和系統(tǒng)實(shí)現(xiàn)[D].沈陽：東北大學(xué)，2013.

LIANG Xiaolong. Research and Realization of Rubik’s Cube Algorithm[D]. Shenyang： Northeastern University， 2013.

[7]盧昌海.魔方與“上帝之?dāng)?shù)”[J].金融博覽，2014（11）：26-27.

[8]田田，徐林，趙洪華，等.解魔方四爪機(jī)械手結(jié)構(gòu)設(shè)計(jì)與操作[J].機(jī)器人技術(shù)與應(yīng)用，2014（5）：38-39.

[9]麥陵璧.微型舞臺(tái)[J].教育導(dǎo)刊，1997（sup1）：42.

[10]李亞南，王增勝，孔令云.基于MathCAD的曲柄滑塊機(jī)構(gòu)運(yùn)動(dòng)可視化研究[J].現(xiàn)代商貿(mào)工業(yè)，2018，39（31）：191-192.

[11]古長(zhǎng)軍，張艷珠，鄧宣金，等.魔方機(jī)器人硬件系統(tǒng)設(shè)計(jì)[J].電子制作，2015（13）：81.

[12]李澤萱，滕旭陽，鄭藝彬，等.基于Arduino的兩臂解魔方機(jī)器人——算法設(shè)計(jì)[J].電腦知識(shí)與技術(shù)，2018，14（17）：248-250.

LI Zexuan， TENG Xuyang， ZHENG Yibin， et al. The robot can restore the Rubik′s Cube based on arduino microcontroller： Algorithm design[J]. Computer Knowledge and Technology， 2018， 14（17）：248-250.

[13]高達(dá).基于STM32雙臂魔方機(jī)器人的設(shè)計(jì)[J].電子產(chǎn)品世界，2018（11）：51-53.

[14]左國(guó)玉，劉洪星，顧凌云，等.四面對(duì)稱結(jié)構(gòu)的解魔方機(jī)器人[J].實(shí)驗(yàn)技術(shù)與管理，2018，35（6）：83-86.

ZUO Guoyu， LIU Hongxing， GU Lingyun， et al. Rubik’s Cube solution robot with four-side symmetrical structure[J]. Experimental Technology and Management， 2018，35（6）：83-86.

[15]董海陽，魏巍.類人四軸解魔方機(jī)器人的設(shè)計(jì)[J].電子技術(shù)與軟件工程，2013（8）：62.

[16]向臘. 魔方機(jī)器人控制系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D].長(zhǎng)沙：湖南大學(xué)，2016.

XIANG La. Design and Realization of Rubik’s Cube Robot Control System[D]. Changsha： Hunan University， 2016.

[17]劉遠(yuǎn)法，周屹.基于Arduino單片機(jī)的解魔方機(jī)器人——控制部分[J].電腦知識(shí)與技術(shù)，2016，12（7）：171-173.

LIU Yuanfa， ZHOU Yi. The robot can restore a Rubik′s Cube based on Arduino microcontroller： The control module[J]. Computer Knowledge and Technology， 2016，12（7）：171-173.

[18]張艷珠，古長(zhǎng)軍，王凡迪，等.魔方機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].沈陽理工大學(xué)學(xué)報(bào)，2016，35（3）：41-46.

ZHANG Yanzhu， GU Changjun， WANG Fandi， et al. Control system design of magic cube robot[J]. Journal of Shenyang Ligong University， 2016，35（3）：41-46.

[19]梁煒，申彥春，張銀蒲，等.一種基于機(jī)器視覺的顏色識(shí)別算法的研究[J].電視技術(shù)，2011，35（23）：129-131.

LIANG Wei， SHEN Yanchun， ZHANG Yinpu，et al. Method for color recognition based on machine vision[J]. Video Engineering， 2011，35（23）：129-131.

[20]黃銘，陸思良，孔凡讓.魔方還原機(jī)器人的視覺子系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J].機(jī)械與電子，2013（5）：60-64.

HUANG Ming， LU Siliang， KONG Fanrang. Design and implementation of vision subsystem of Rubik’s Cube robot[J]. Machinery & Electronics， 2013（5）：60-64.

[21]張金龍，鄒裕龍，楊斌，等.基于二階段雙向搜索的解魔方機(jī)器人系統(tǒng)[J].計(jì)算機(jī)與現(xiàn)代化，2019（2）：82-87.

ZHANG Jinlong， ZOU Yulong， YANG Bin， et al. Solving cube robot system based on two-stage bidirectional search[J]. Computer and Modernization， 2019（2）：82-87.