「開源分享」雙臂二指魔方機器人的還原控制

admin

魔方還原算法最先可以追溯到1980年捷克共和國的女大學生Jessica Fridrich發現的CFOP算法，今朝這個算法依然遍及利用于國際魔方速拧角逐中。而颠末几十年的成长，魔方還原的主流算法有：层先法、角先法、TM算法和二阶段算法。關于算法的好坏與否，重要看它的两個方面：魔方還原步数、魔方寻優時候。本文算法寻求的是在尽量短的時候內完成魔方的還原。雙臂二指魔方呆板人還原魔方的時候包含寻優的時候和呆板人拧魔方的時候，而雙臂二指魔方呆板人完成一次魔方扭转所用的時候是固定的，是以呆板人還原魔方的時候與還原指令的长度呈線性瓜葛，即還原指令长度越短，魔方還原所耗费的時候越少。按照相干資料显示，Kociemba 二阶段算法是最優的選擇。本章重要先容Kociemba 二阶段算法。

1.魔方状况阐發

魔方的状况阐發是實現魔方樂成求解的条件和根本。若是把魔方的12個棱块放于12個邊的位置，按照分列次序有12!種环境，除此以外，還要斟酌棱块的標的目的問題，有2種標的目的置于12個邊上，按照乘法道理可知有種环境，综上所述棱块的状况数一共有：

同理，魔方有8個角块，每一個角块朝向有3種，以是角块的状况数一共是：

但是，魔方清酸茶,的一些状况是不存在的。由于對付一個還原的魔方来讲，不存在零丁扭转一個棱块或角块的朝向的环境和零丁一對角块和一對棱块交换位置的环境[37]，以是應當在魔方所有状况的根本上去掉一個倍数：

此可以获得魔方的所有可能状况数：

由此获得：

魔方的所有可能状况有43252003274489856000種，阐明魔方還原的難度很是之大，想要经由過程胡乱的扭转荣幸将魔方還原是几近不成能的。是以各類各样的魔方還原算法相继而来，目古人們已将“天主之数”紧缩到20，即任何一種组合的魔方都能在20步之內還原。

2. Kociemba 二阶段算法

经上位機（電脑）視觉辨认处置完成以後，就获得一串包括魔方相對于位置與色彩的字符串，這串字符串就代表了這個魔方此時的状况信息，经由過程這串字符串，计较機也便可以正确的获得此時的魔方模子状况。今朝来讲，魔方呆板人利用较為遍及的算法是来自 Kociemba 的二阶段算法，如下简称 Kociemba 算法。Kociemba 算法是一種能在较短期內利用较短步数還原魔方的算法，凡是能在 1s 內获得均匀 20 步的解法，相比力于人工還原魔方的法子，如层先法、角先法，進阶的如 CFOP 法的動辄上百步還原步调，是很是具备上風的， 以是咱們也采纳的此算法。

咱們可以把魔方简略的看作一個立方體，對每一個面举行响應的標识表记標帜，六個面分咱們可以把魔方简略的看作一個立方體，對每一個面举行响應的標识表记標帜别離為 U（顶面）、F（前面）、 L（左面）、R（右面）、B（後面）、D（下面），對付每個面，又别離有顺時针扭转 90°，逆時针扭转 90°， 扭转 180°（顺時针逆時针都可）這三種操作，咱們可以把對應的操作记為 XY，此中 X 為面的代号，Y 為對 應操作代号（1 為顺時针扭转 90°，2 為扭转 180°，3陽痿要吃什麼, 逆時针扭转 90°），如 U3 就代表着是顶层逆時针扭转 90°。经 Kociemba 算法获得的解法就是多個上述操作代号相连起来，比方“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是经 Kociemba 算法得出来的完备的一個解魔方步调。

3.魔方滚動状况阐發

咱們可以把魔方简略的看作一個立方體，對每一個面举行响應的標识表记標帜，六個面别離為U(顶面)、F（前面）、L（左面）、R（右面）、B（後面）、D（下面），對付每個面，又别離有顺時针扭转90°，逆時针扭转90°，扭转180°（顺時针逆時针都可）這三種操作，咱們可以把對應的操作记為XY，此中X為面的代号，Y為對應操作代号（1為顺時针扭转90°，2為扭转180°，3逆時针扭转90°），如U3就代表着是顶层逆時针扭转90°。

4. kociemba算法成果

经Kociemba算法获得的解法就是多個上述操作代号相连起来，比方“D2 R1 D3 F2 B1 D1 R2 D2 R3 F2 D3 F2 U3 B2 L2 U2 D1 R2 U1”就是经Kociemba算法得出来的完备的一個解魔方步调。對付咱們的呆板人而言，如许的一個步调是不克不及直接應用的，對付雙臂二指呆板人而言，如圖，它能直接滚動的面只有D、B两個面，要想滚動其他的面，還必要举行一些其他的機器臂的操作转化，接下来會鄙人位機算法中具體先容。

5. 串口通信

经Kociemba算法获得的解法，必要颠末必定的通信手腕，将数据從上位機傳输给下位機举行後续的操作，故選擇简略而且不乱的usb串口通信，只必要将上位機和下位機经由過程一根usb線毗连起来，拔取和下位機相對于應的端標语和波特率，便可以向下位機傳输這一组数据了，下附python源代码，此中的txt即是经Kociemba算法获得的一串包括准确解法的字符串。

6. 串口数据接管及数据解码

下位機举行串口缓存檢測，一旦稀有据输入時，就按次序读取单個字符值，并转化為字符，轮回赋值给a数组。如许便可以获得上位機傳输過来的数据了。

但如许获得的字符串其實不能被下位機直接辨认，将获得的字符串以空格為界，将两個空格之間的字符的ASCLL码相加，使得魔方的每個滚動方法都有一個對應的十進制数字與之相對于應。

7. 機器臂節制及转化算法

咱們的雙臂二指呆板人包括两個舵機，别離節制摆布两只手手指的夹紧與铺開，此外還包括两個步進機電，用来别離節制摆布两個機器臂的滚動，咱們可以做以下界說：

L0左手手指伸開

L1左手手指夹紧

Ls左手顺時针扭转90°

Ln左手逆時针扭转90°

L180左手扭转180°（至因而顺時针仍是逆時针以後會有另行阐明）

舵機節制代码：

步進機電在滚動的時辰，按照手指的夹紧和伸開状况，有三種速率可以選擇，在分歧负载下選擇分歧的速率来举行滚動，此中lim值的感化鄙人文防环绕纠缠算法中會具體阐明：

R0、R一、Rs、Rn、R180则是右手响應動作。

如许，咱們便可以把魔方還原的解法经由過程必定的映照，转换為呆板人响應的機器步调，從而讓呆板人可以准确還原魔方。界說機器臂手指平行地面為归位，垂直地面為不归位，在举行一次滚動操作今後，一共會呈現三種状况，别離是1：左手归位、右手不归位2：左手不归位、右手归位3：摆布手都归位。如在摆布手臂都是初始状况時履行U3步调：R1→L180→R0→Rn，對付其他步调，一样存在响應的转化法子，下附表1。

表1 待滚動面滚動至操作面機器臂動作转

颠末如许的胡蘿蔔素泡沫面膜,操作以後，只是把待滚動面挪動到了機器爪的操作范畴以內，至因而顺時针仍是逆時针，用左手仍是右手，颠末步伐的進一步果断今後便可以举行下一步操作。

8. 坐標转换算法

固然，在雙手機器臂還原魔方的進程中，會触及到必定的坐標變更，如履行U3步调以後，後续的所有還原步调中：U→D、D→U、L→R、R→L，F和B面稳定，不難發明，只如果對U面举行操作，都是如许的坐標转换步调。固然，對付其他此外的5個面，在举行操作今後也會有必定的转换法则，下附表2。

表2  滚動進程中坐標转换比照表

9.步進機電節制

當步進機電加快渡過快時，會使魔方產生错位的危害，在這里，经由過程利用了Arduino自带的AccelStepper库函数，可以加倍简略便利的来驱動步進機電来到达想要的速率。

10.線的防环绕纠缠算法

因為呆板人選擇的是用舵機来節制手指的日本酵素,開合，在舵機和節制板之間必需要有一根線来傳输数据和供卡利百家樂,给電力，在魔方举行還原的進程中，若是不加以限定，這根線就會一點一點的环绕纠缠在機器臂上，為领會决這個問題，咱們做了以下優化事情：界說呆板人初始機器臂状况為“0”，當機器臂顺時针扭转90°後，咱們讓機器臂状况值加之90，逆時针扭转90°则減去90，在以前界說扭转180°的時辰，咱們没有界說扭转的標的目的，在這里，咱們可以按照状况值動态選擇顺時针仍是逆時针，详细来讲，就是在機器臂要举行扭转180°的時辰，機器臂状况值大于0時，選擇逆時针扭转180°，同時状况值削減180，状况值小于0時，選擇顺時针扭转180°，同時状况值加之180，如许的算法會使機器臂在扭转的進程中，機器臂的状况值始终是趋近于0也就是機器臂的状况始终向着初始状况挨近，如许便可以解决線的环绕纠缠問題。

在颠末如许的一系列算法处置今後，魔方呆板人便可以驱動機電，實現還原魔方的操作了。

经由過程 10 次的安稳運行測试，還原速率在 18s~25s 區間內，均匀還原速率為 22.9 s。

雙臂二指魔方呆板人的完备資料详见