基于四臂協(xié)調(diào)控制的魔方還原系統(tǒng)設(shè)計與實現(xiàn)

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

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

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

中圖分類號：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

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

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

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

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

1.1系統(tǒng)功能

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

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

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

1.2系統(tǒng)模塊

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.3舞臺效果機構(gòu)設(shè)計

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

升降舞臺在裝置中的組合效果如圖6所示。

2.4控制電路設(shè)計

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

據(jù)此，本文在系統(tǒng)中規(guī)定了如下2個特定的旋轉(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個面。因此需要設(shè)計一套邏輯算法，配合4只U型機械手，能夠逐次掃描魔方的各個面，并且沒有重復(fù)。

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

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

首先對U型機械手的旋轉(zhuǎn)步驟做一個定義：U型機械手1次動作視為旋轉(zhuǎn)1步。

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

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

該掃描邏輯如圖9所示。

3.4魔方的座位還原算法

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

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

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

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

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

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

魔方共有6種旋轉(zhuǎn)操作，每種操作都有3種衍生的操作，即順時針旋轉(zhuǎn)90°、順時針旋轉(zhuǎn)180°和逆時針旋轉(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]的研究中簡單給出了機械的控制流程。在本文中將進(jìn)一步詳細(xì)闡述四機械臂控制的具體算法流程。

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

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

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

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

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

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

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

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

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

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

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

3.6攝像頭的顏色識別

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

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

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

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

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

3.7Thislethwaite Method算法簡介

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

4結(jié)語

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

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

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

[1]哈金才，李若雪，哈瑞.魔方的數(shù)學(xué)模型研究及其應(yīng)用[J].創(chuàng)新創(chuàng)業(yè)理論研究與實踐，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]李國軍，鐘志強，張毅寧.LEGO機器人魔方還原基本方法與過程[J].鞍山師范學(xué)院學(xué)報，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é)和計算機表現(xiàn)[M].東營：石油大學(xué)出版社，2003.

[5]喻騰飛. 魔方群的研究[D].合肥：中國科學(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)實現(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]田田，徐林，趙洪華，等.解魔方四爪機械手結(jié)構(gòu)設(shè)計與操作[J].機器人技術(shù)與應(yīng)用，2014（5）：38-39.

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

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

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

[12]李澤萱，滕旭陽，鄭藝彬，等.基于Arduino的兩臂解魔方機器人——算法設(shè)計[J].電腦知識與技術(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雙臂魔方機器人的設(shè)計[J].電子產(chǎn)品世界，2018（11）：51-53.

[14]左國玉，劉洪星，顧凌云，等.四面對稱結(jié)構(gòu)的解魔方機器人[J].實驗技術(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]董海陽，魏巍.類人四軸解魔方機器人的設(shè)計[J].電子技術(shù)與軟件工程，2013（8）：62.

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

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

[17]劉遠(yuǎn)法，周屹.基于Arduino單片機的解魔方機器人——控制部分[J].電腦知識與技術(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]張艷珠，古長軍，王凡迪，等.魔方機器人控制系統(tǒng)設(shè)計[J].沈陽理工大學(xué)學(xué)報，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ù)，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]黃銘，陸思良，孔凡讓.魔方還原機器人的視覺子系統(tǒng)設(shè)計與實現(xiàn)[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]張金龍，鄒裕龍，楊斌，等.基于二階段雙向搜索的解魔方機器人系統(tǒng)[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.