本發(fā)明涉及機(jī)器人領(lǐng)域，具體涉及一種基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)。

背景技術(shù)：

縱觀空間機(jī)械臂國(guó)內(nèi)外的發(fā)展現(xiàn)狀，關(guān)節(jié)作為機(jī)械臂的核心，一般均采用模塊化設(shè)計(jì)理念，中空走線的方式，實(shí)現(xiàn)機(jī)械、控制、電氣的高度集成。此外，由于空間在軌服務(wù)任務(wù)的多樣性和復(fù)雜性，促進(jìn)了可重構(gòu)機(jī)械臂的發(fā)展，傳統(tǒng)的機(jī)械臂都是基于某一功能研制，模塊與模塊之間無法更換，只能完成單一的在軌任務(wù)，成本較高，周期長(zhǎng)，采用可重構(gòu)的機(jī)械臂通過變換構(gòu)型來完成不同的工作是一種現(xiàn)實(shí)的方法。可重構(gòu)機(jī)械臂在模塊化機(jī)械臂的基礎(chǔ)上發(fā)展而來，通過模塊與模塊之間的重新組合，來完成不同的工作任務(wù)。可重構(gòu)機(jī)械臂各關(guān)節(jié)之間，機(jī)械臂之間可以根據(jù)實(shí)際環(huán)境和操作要求實(shí)現(xiàn)快速重構(gòu)。成本較高，周期長(zhǎng)；且傳統(tǒng)的空間機(jī)械臂內(nèi)部電纜線繁多，各部件之間的接口關(guān)系復(fù)雜不易更換，導(dǎo)致空間機(jī)械臂的可靠性、安全性和可維護(hù)性極差。再一方面，采用zigbee、藍(lán)牙等技術(shù)進(jìn)行機(jī)械臂通信控制，其帶寬低，傳輸速率較慢。

綜上，因此該技術(shù)有必要進(jìn)行改進(jìn)。

技術(shù)實(shí)現(xiàn)要素：

為了解決上述技術(shù)問題，本發(fā)明的目的是提供一種能解決傳統(tǒng)空間機(jī)械臂功能單一、模塊與模塊之間無法更換、內(nèi)部繁雜冗余的電纜走線的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂。

本發(fā)明所采用的技術(shù)方案是：

本發(fā)明提供一種基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，包括關(guān)節(jié)、可重構(gòu)接口和臂桿，所述關(guān)節(jié)與可重構(gòu)接口之間通過可重構(gòu)接口主動(dòng)端的止口固定連接，所述臂桿與可重構(gòu)接口之間通過可重構(gòu)接口被動(dòng)端的止口固定連接；所述系統(tǒng)還包括智能終端、WIFI通信模塊、關(guān)節(jié)伺服控制器、傳感器信號(hào)采集模塊、電機(jī)驅(qū)動(dòng)模塊和關(guān)節(jié)電機(jī)，所述智能終端通過WIFI通信模塊與關(guān)節(jié)伺服控制器連接；所述關(guān)節(jié)伺服控制器的輸出端與所述電機(jī)驅(qū)動(dòng)模塊連接，所述電機(jī)驅(qū)動(dòng)模塊的輸出端與關(guān)節(jié)電機(jī)連接；所述傳感器信號(hào)采集模塊的輸出端與所述關(guān)節(jié)伺服控制器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述系統(tǒng)還包括外部存儲(chǔ)器，所述關(guān)節(jié)伺服控制器的輸出端與外部存儲(chǔ)器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述系統(tǒng)還包括電源模塊，所述電源模塊的輸出端與外部存儲(chǔ)器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述傳感器信號(hào)采集模塊包括電流采樣、編碼器信號(hào)采樣。

進(jìn)一步地，所述傳感器信號(hào)采集模塊還包括六維力傳感器、關(guān)節(jié)力矩傳感器、溫度傳感器。

進(jìn)一步地，所述關(guān)節(jié)伺服控制器采用 ARM 作為處理器。

進(jìn)一步地，所述智能終端包括PC。

進(jìn)一步地，所述機(jī)械臂通過所述六維力傳感器和關(guān)節(jié)力矩傳感器做柔順控制。

本發(fā)明的有益效果是：本發(fā)明提供的基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，其通過機(jī)械臂模塊之間的重構(gòu)和無線通信的應(yīng)用，從而豐富機(jī)械臂的功能以及布局布線的簡(jiǎn)潔。采用可重構(gòu)的機(jī)械臂通過變換構(gòu)型來完成不同的工作；本發(fā)明采用無線通信，有利于控制系統(tǒng)的快速重構(gòu)，采用WIFI作為機(jī)械臂內(nèi)無線通信的方式，可以實(shí)現(xiàn)數(shù)據(jù)穩(wěn)定的發(fā)送。使用WIFI輸出圖像和數(shù)據(jù)速率均能滿足要求，WIFI的安全機(jī)制通過加以改造可以獲得很高的安全級(jí)別；且采用可重構(gòu)空間機(jī)械臂的快換接口，用于實(shí)現(xiàn)模塊之間的快速更換，用于重組構(gòu)型。機(jī)械臂的可重構(gòu)接口不僅能夠?qū)崿F(xiàn)模塊之間的機(jī)械連接，通過可重構(gòu)接口中心的航空插頭，可實(shí)現(xiàn)模塊之間的電連接。

附圖說明

下面結(jié)合附圖對(duì)本發(fā)明的具體實(shí)施方式作進(jìn)一步說明：

圖1是本發(fā)明的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的第一實(shí)施例的機(jī)械臂無線控制系統(tǒng)示意圖；

圖2是本發(fā)明的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的第二實(shí)施例的機(jī)械臂結(jié)構(gòu)示意圖；

圖3是本發(fā)明的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的第三實(shí)施例的關(guān)節(jié)控制原理圖；

圖4是本發(fā)明的機(jī)械臂關(guān)節(jié)的第四實(shí)施例的控制軟件流程圖；

圖5是本發(fā)明的第五實(shí)施例的剛性臂控制器系統(tǒng)框圖；

圖6是本發(fā)明的第六實(shí)施例的機(jī)械臂無線控制系統(tǒng)示意圖；

圖7是本發(fā)明的第七實(shí)施例的電機(jī)控制示意圖。

具體實(shí)施方式

需要說明的是，在不沖突的情況下，本申請(qǐng)中的實(shí)施例及實(shí)施例中的特征可以相互組合。

本發(fā)明提供一種基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，包括關(guān)節(jié)、可重構(gòu)接口和臂桿，所述關(guān)節(jié)與可重構(gòu)接口之間通過可重構(gòu)接口主動(dòng)端的止口固定連接，所述臂桿與可重構(gòu)接口之間通過可重構(gòu)接口被動(dòng)端的止口固定連接；所述系統(tǒng)還包括智能終端、WIFI通信模塊、關(guān)節(jié)伺服控制器、傳感器信號(hào)采集模塊、電機(jī)驅(qū)動(dòng)模塊和關(guān)節(jié)電機(jī)，所述智能終端通過WIFI通信模塊與關(guān)節(jié)伺服控制器連接；所述關(guān)節(jié)伺服控制器的輸出端與所述電機(jī)驅(qū)動(dòng)模塊連接，所述電機(jī)驅(qū)動(dòng)模塊的輸出端與關(guān)節(jié)電機(jī)連接；所述傳感器信號(hào)采集模塊的輸出端與所述關(guān)節(jié)伺服控制器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述系統(tǒng)還包括外部存儲(chǔ)器，所述關(guān)節(jié)伺服控制器的輸出端與外部存儲(chǔ)器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述系統(tǒng)還包括電源模塊，所述電源模塊的輸出端與外部存儲(chǔ)器的輸入端連接。

作為該技術(shù)方案的改進(jìn)，所述傳感器信號(hào)采集模塊包括電流采樣、編碼器信號(hào)采樣。

進(jìn)一步地，所述傳感器信號(hào)采集模塊還包括六維力傳感器、關(guān)節(jié)力矩傳感器、溫度傳感器。

進(jìn)一步地，所述關(guān)節(jié)伺服控制器采用 ARM 作為處理器。

進(jìn)一步地，所述智能終端包括PC。

進(jìn)一步地，所述機(jī)械臂通過所述六維力傳感器和關(guān)節(jié)力矩傳感器做柔順控制。

參照?qǐng)D1、圖３，是本發(fā)明的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的機(jī)械臂無線控制系統(tǒng)示意圖。一種基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，所述系統(tǒng)包括中央控制器、WIFI通信模塊、關(guān)節(jié)伺服控制器、傳感器信號(hào)采集模塊、電機(jī)驅(qū)動(dòng)模塊和關(guān)節(jié)電機(jī)，所述中央控制器通過WIFI通信模塊與關(guān)節(jié)伺服控制器連接；所述關(guān)節(jié)伺服控制器的輸出端與所述電機(jī)驅(qū)動(dòng)模塊連接，所述電機(jī)驅(qū)動(dòng)模塊的輸出端與關(guān)節(jié)電機(jī)連接；所述傳感器信號(hào)采集模塊的輸出端與所述關(guān)節(jié)伺服控制器的輸入端連接。所述系統(tǒng)還包括外部存儲(chǔ)器，所述關(guān)節(jié)伺服控制器的輸出端與外部存儲(chǔ)器的輸入端連接。所述系統(tǒng)還包括電源模塊，所述電源模塊的輸出端與外部存儲(chǔ)器的輸入端連接。所述傳感器信號(hào)采集模塊包括電流采樣、編碼器信號(hào)采樣。所述傳感器信號(hào)采集模塊還包括六維力傳感器、關(guān)節(jié)力矩傳感器、溫度傳感器。所述關(guān)節(jié)伺服控制器采用 ARM 作為處理器。

上層為中央控制器，下層為關(guān)節(jié)伺服控制器。關(guān)節(jié)控制系統(tǒng)集成在關(guān)節(jié)內(nèi)部，基于 ARM3 開發(fā)的電機(jī)矢量控制器，用于控制每個(gè)關(guān)節(jié)的位置、速度和力矩。機(jī)械臂中央控制器也是基于 ARM3 開發(fā)，包含機(jī)械臂軌跡規(guī)劃算法、軌跡生成等，接收末端位置和姿態(tài)傳感器發(fā)送的位置和姿態(tài)數(shù)據(jù)，不斷地進(jìn)行關(guān)節(jié)角度結(jié)算，換算成響應(yīng)的位置指令發(fā)送給關(guān)節(jié)伺服控制器。機(jī)械臂通過末端六維力傳感器和關(guān)節(jié)力矩傳感器做柔順控制。 PC 機(jī)上的 WIFI 調(diào)試界面用于和關(guān)節(jié)伺服控制器通信。電氣系統(tǒng)包含采樣電路、電源系統(tǒng)等。電源系統(tǒng)將基座提供的 28V 電壓轉(zhuǎn)換為關(guān)節(jié)、末段工具需要的不同的額定電壓。傳感器信號(hào)采集電路包括電流采樣、編碼器信號(hào)采樣等，用來采集傳感器數(shù)據(jù)。機(jī)械臂關(guān)節(jié)控制器位置環(huán)采用三閉環(huán)控制，使用關(guān)節(jié)端編碼器作為位置環(huán)的反饋信號(hào)。控制板尺寸適合關(guān)節(jié)機(jī)械尺寸，通過螺柱固定在關(guān)節(jié)內(nèi)部，實(shí)現(xiàn)單關(guān)節(jié)的高度集成。中央控制器和關(guān)節(jié)伺服控制器之間采用 WIFI 進(jìn)行通信。

關(guān)節(jié)伺服控制器按照上層的控制指令驅(qū)動(dòng)關(guān)節(jié)。這些控制指令主要有位置、速度、力矩和制動(dòng)信號(hào)。電機(jī)采用全閉環(huán)矢量控制，關(guān)節(jié)在三環(huán)控制的基礎(chǔ)上級(jí)聯(lián)一個(gè)力矩環(huán)。

關(guān)節(jié)伺服控制器采用 ARM 作為處理器，與 PC 通過 WiFi 進(jìn)行通信。WIFI 芯片與 ARM 的接口為 SDIO 接口，PC 有 WIFI 的可編輯調(diào)試界面，可向 ARM 發(fā)送控制指令。

機(jī)械臂的可重構(gòu)快換接口根據(jù)套筒在接口上的位置不同，可重構(gòu)接口對(duì)應(yīng)兩種狀態(tài)，分別為自鎖狀態(tài)和解鎖狀態(tài)。自鎖狀態(tài)下，主動(dòng)端和被動(dòng)端通過鋼珠鎖緊，承受軸向力，鍵的配合承受周向力，實(shí)現(xiàn)可靠的連接；在解鎖狀態(tài)下，被動(dòng)端可從主動(dòng)端拔出，實(shí)現(xiàn)分離。主動(dòng)接口和被動(dòng)接口的航空插頭通過預(yù)緊的彈簧連接在一起，實(shí)現(xiàn)可靠的電能和信號(hào)傳遞。

電氣系統(tǒng)包含采樣電路、電源系統(tǒng)等。電源系統(tǒng)將基座提供的28V電壓轉(zhuǎn)換為關(guān)節(jié)、末段工具需要的不同的額定電壓。傳感器信號(hào)采集電路包括電流采樣、編碼器信號(hào)采樣等，用來采集傳感器數(shù)據(jù)。機(jī)械臂關(guān)節(jié)控制器位置環(huán)采用三閉環(huán)控制，使用關(guān)節(jié)端編碼器作為位置環(huán)的反饋信號(hào)。控制板尺寸適合關(guān)節(jié)機(jī)械尺寸，通過螺柱固定在關(guān)節(jié)內(nèi)部，實(shí)現(xiàn)單關(guān)節(jié)的高度集成。中央控制器和關(guān)節(jié)伺服控制器之間采用WIFI 進(jìn)行通信。

無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂包括：關(guān)節(jié)1、可重構(gòu)化接口2、臂桿3。關(guān)節(jié)1與可重構(gòu)化接口2之間通過可重構(gòu)接口主動(dòng)端的止口配合，并通過螺釘擰緊。臂桿3與可重構(gòu)化接口2之間通過可重構(gòu)接口被動(dòng)端的止口配合，并通過螺釘擰緊。可重構(gòu)化接口2中裝有航空插頭，在完成機(jī)械連接的同時(shí)完成電連接。

所述的無線化通信方式采用WIFI作為機(jī)械臂內(nèi)無線通信的方式，關(guān)節(jié)控制板使用WIFI8686無線通信模塊與上位機(jī)進(jìn)行通信，該模塊傳輸協(xié)議為802.11b/g，芯片，支持SPI和SDIO通信，使用SDIO時(shí)，數(shù)據(jù)傳輸速率達(dá)到54Mbps。可重構(gòu)空間機(jī)械臂的上層與下層控制器之間采用無線通信傳輸數(shù)據(jù)，關(guān)節(jié)伺服控制器采集的位置信息、電流信息等、機(jī)械臂視覺場(chǎng)采集的圖片信息均通過無線通信傳輸?shù)街醒肟刂破髦小Ｖ醒肟刂破鞅O(jiān)測(cè)機(jī)械臂的運(yùn)行狀態(tài)，通過估計(jì)規(guī)劃程序給伺服控制器發(fā)送指令。

所述的一種可重構(gòu)空間機(jī)械臂的快換接口，包括航空插頭、半圓頭內(nèi)六角螺釘、主動(dòng)連接件、軸用擋圈、定位套筒、平面渦卷彈簧、被動(dòng)連接件、鋼珠、半圓頭內(nèi)六角螺釘、拉伸彈簧和航空插座組成。所述可重構(gòu)機(jī)械臂的快換接口，采用鋼珠鎖緊的方案，軸向的拉力和壓力均由鋼珠承受。扭矩由連接桿上的鍵承受。通過控制套筒的轉(zhuǎn)動(dòng)位置，可重構(gòu)接口對(duì)應(yīng)兩種狀態(tài)，分別為自鎖狀態(tài)和解鎖狀態(tài)。自鎖狀態(tài)下，主動(dòng)端和被動(dòng)端通過鋼珠鎖緊，實(shí)現(xiàn)可靠的連接；在解鎖狀態(tài)下，被動(dòng)端可從主動(dòng)端拔出，實(shí)現(xiàn)分離。

所述的模塊化設(shè)計(jì)是將機(jī)械臂的關(guān)節(jié)進(jìn)行模塊化，關(guān)節(jié)是由動(dòng)塊部件、靜塊部件、驅(qū)動(dòng)部件組成，其內(nèi)部集成了豐富的傳感器，有電機(jī)矢量控制的增量式編碼器、位置環(huán)控制的絕對(duì)式編碼器、力矩控制的電流傳感器。位置環(huán)用于實(shí)現(xiàn)關(guān)節(jié)的位置控制，給定關(guān)節(jié)位置指令，關(guān)節(jié)就會(huì)到達(dá)指定位置。關(guān)節(jié)采用中空直流無刷電機(jī)串聯(lián)諧波減速器的方式作為主要驅(qū)動(dòng)和傳動(dòng)部分，電機(jī)帶動(dòng)轉(zhuǎn)軸輸入經(jīng)諧波減速器經(jīng)動(dòng)塊部件進(jìn)行輸出，關(guān)節(jié)與可重構(gòu)化接口之間通過可重構(gòu)接口主動(dòng)端的止口配合，并通過螺釘擰緊。

所述的機(jī)械臂具有7個(gè)自由度，兩端分別為兩兩垂直的關(guān)節(jié)，可沿肘部折疊，中間三個(gè)關(guān)節(jié)平行，肩部采用roll-yaw-pitch配置，肘部為pitch軸，腕部采用Pitch-Yaw-Roll配置。

所述的重構(gòu)是關(guān)節(jié)與臂桿之間的重構(gòu)，此外該重構(gòu)還可以用于關(guān)節(jié)與關(guān)節(jié)之間以及臂桿與臂桿之間的重構(gòu)。

關(guān)節(jié)伺服控制器按照上層的控制指令驅(qū)動(dòng)關(guān)節(jié)。這些控制指令主要有位置、速度、力矩和制動(dòng)信號(hào)。電機(jī)采用全閉環(huán)矢量控制，關(guān)節(jié)在三環(huán)控制的基礎(chǔ)上級(jí)聯(lián)一個(gè)力矩環(huán)。關(guān)節(jié)伺服控制器用 ARM 作為處理器，與 PC 通過 WiFi 進(jìn)行通信。WIFI 芯片與 ARM 的接口為 SDIO 接口，PC 有 WIFI 的可編輯調(diào)試界面，可向 ARM 發(fā)送控制指令。使用PC機(jī)的 WIFI 調(diào)試界面，通過 WIFI 給關(guān)節(jié)伺服控制器發(fā)送指令。通過配置HTML 文件，可以開發(fā)出關(guān)節(jié)調(diào)試的簡(jiǎn)單界面。 WIFI 人機(jī)包括 WIFI 參數(shù)配置、關(guān)節(jié)選擇，關(guān)節(jié)不同模式選擇和指令下達(dá)控制器復(fù)位等狀態(tài)欄。在位置模式下輸入相應(yīng)的脈沖個(gè)數(shù)指令，點(diǎn)擊發(fā)送就會(huì)將數(shù)據(jù)傳給對(duì)應(yīng)的關(guān)節(jié)控制器，關(guān)節(jié)控制器控制電機(jī)做出相應(yīng)運(yùn)動(dòng)。 通過配上位機(jī)軟件方便了對(duì)機(jī)械臂關(guān)節(jié)的調(diào)試。

參照?qǐng)D2，是本發(fā)明的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的第二實(shí)施例的機(jī)械臂結(jié)構(gòu)示意圖。一種基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，包括關(guān)節(jié)１、可重構(gòu)接口２和臂桿３，所述關(guān)節(jié)１與可重構(gòu)接口２之間通過可重構(gòu)接口主動(dòng)端的止口配合，并通過螺釘擰緊。所述臂桿３與可重構(gòu)接口２之間通過可重構(gòu)接口被動(dòng)端的止口配合，并通過螺釘擰緊。可重構(gòu)化接口２中裝有航空插頭，在完成機(jī)械連接的同時(shí)完成電連接，其內(nèi)部采用鋼珠進(jìn)行鎖緊，軸向的拉力和壓力均由鋼珠承受。

所述機(jī)械臂具有7個(gè)自由度，兩端分別為兩兩垂直的關(guān)節(jié)，可沿肘部折疊，中間三個(gè)關(guān)節(jié)相互平行，肩部采用roll-yaw-pitch（橫滾-偏航-俯仰）配置，肘部為pitch軸，腕部采用Pitch-Yaw-Roll（俯仰-偏航-橫滾）配置。本方案肩部、肘部散的關(guān)節(jié)兩兩正交，可以實(shí)現(xiàn)機(jī)械臂全方位無奇異的運(yùn)動(dòng)和遙控操作下機(jī)械臂姿態(tài)的獨(dú)立控制；中間三個(gè)關(guān)節(jié)相互平行，運(yùn)動(dòng)學(xué)存在解析解。

參照?qǐng)D4，是本發(fā)明的機(jī)械臂關(guān)節(jié)的第四實(shí)施例的控制軟件流程圖。根據(jù)無線化可重構(gòu)關(guān)節(jié)的控制要求和控制方法，設(shè)計(jì)了關(guān)節(jié)的控制軟件總體架構(gòu)。在進(jìn)入主函數(shù)之前，先將所用到的串口、IO、中斷、 AD 等進(jìn)行初始化操作，然后等待定時(shí)器中斷；在中斷子程序中讀取編碼器的位置、編碼器計(jì)數(shù)清零、讀取力矩信號(hào)、力矩環(huán) PID 計(jì)算、讀取位置信號(hào)、位置環(huán) PID 計(jì)算、速度信號(hào)采樣、速度環(huán) PID 計(jì)算、電流信號(hào)采樣、電流環(huán) PID 計(jì)算。此時(shí)判斷是否檢測(cè)到 3 次 Z相脈沖，如果計(jì)數(shù)器未計(jì)數(shù)到 3，則執(zhí)行霍爾換向程序，進(jìn)行方波控制； 如果計(jì)數(shù)器大于3 時(shí)，開始執(zhí)行 SVPWM 程序，進(jìn)行矢量控制。方波控制和矢量控制產(chǎn)生PWM 波控制開關(guān)管通斷驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)。其中，速度環(huán)和電流環(huán)使用電機(jī)輸入端增量式編碼器和電流采樣 AD 實(shí)現(xiàn)，位置環(huán)和力矩環(huán)使用關(guān)節(jié)輸出端絕對(duì)值編碼器和力矩傳感器實(shí)現(xiàn)。

參照?qǐng)D5，是本發(fā)明的第五實(shí)施例的剛性臂控制器系統(tǒng)框圖。剛性臂控制系統(tǒng)是由中央控制器和底層伺服控制器組成,中央控制器與底層伺服器通過WiFi通信，中央控制器使用PC機(jī)或是STM32ZET6，內(nèi)部集成了剛性臂的軌跡規(guī)劃、柔順控制、干擾角動(dòng)量計(jì)算等算法。底層控制器包含關(guān)節(jié)伺服控制器、末端工具控制器、手眼視覺處理器等，它們通過航空插頭進(jìn)行電源連接。

參照?qǐng)D6，是本發(fā)明的第六實(shí)施例的機(jī)械臂無線控制系統(tǒng)示意圖。多個(gè)機(jī)械臂協(xié)同操作時(shí)，每個(gè)機(jī)械臂都可以隨時(shí)加入WiFi網(wǎng)絡(luò)，并將機(jī)械臂的D—H參數(shù)發(fā)送給協(xié)調(diào)控制器，通過向協(xié)調(diào)控制器發(fā)送請(qǐng)求，協(xié)調(diào)控制器收到請(qǐng)求，做出應(yīng)答。另一方面，多臂協(xié)同控制器和單臂中央控制器均采用冗余備份設(shè)計(jì)方案，主份和備份使用相同的電路，主份CPU-微控制器A和備份CPU-微控制器B都有自己的最小系統(tǒng)，共用一片雙端口RAM，主份CPU-微控制器A在運(yùn)行過程中，將控制器運(yùn)行的關(guān)鍵數(shù)據(jù)及運(yùn)行狀態(tài)信息存儲(chǔ)在雙端口RAM中，當(dāng)系統(tǒng)仲裁模塊檢查到主份CPU-微控制器A出故障，從而讓備份CPU-微控制器B啟動(dòng)的時(shí)候，備份CPU-微控制器B將先讀取雙端口RAM中存儲(chǔ)的系統(tǒng)關(guān)鍵信息，接著系統(tǒng)的狀態(tài)繼續(xù)運(yùn)行，保證整個(gè)多臂控制柜系統(tǒng)的正常工作。

參照?qǐng)D7，是本發(fā)明的第七實(shí)施例的電機(jī)控制示意圖。電機(jī)轉(zhuǎn)子初始定位方法：電機(jī)初始使用方波啟動(dòng)，檢測(cè)編碼器Z相脈沖。當(dāng)檢測(cè)到脈沖信號(hào)時(shí)，轉(zhuǎn)子角度賦初值：θ=0°。切換到矢量控制，觀察電機(jī)是否轉(zhuǎn)動(dòng)。若電機(jī)沒有轉(zhuǎn)動(dòng)，讓?duì)染鶆虻脑黾咏嵌圈痢．?dāng)電機(jī)開始旋轉(zhuǎn)時(shí)，輸出當(dāng)前轉(zhuǎn)子角度θ對(duì)應(yīng)的的速度Vθ和下一個(gè)速度V（θ+α）。判斷Vθ< V（θ+α），是否成立，若條件成立，將繼續(xù)增大角度。當(dāng)Vθ< V（θ+α）時(shí)，判斷|Vθ-V（θ+α）|是否小于給定誤差ε，若條件不成立，則通過二分法進(jìn)一步縮小θ的值，直到條件成立。此時(shí)的角度θ即為轉(zhuǎn)子的初始角度。

本發(fā)明提供的無線通信可重構(gòu)人機(jī)協(xié)作機(jī)械臂的工作原理為：機(jī)械臂的可重構(gòu)與模塊化密不可分，可重構(gòu)首先要模塊化，在此基礎(chǔ)上設(shè)計(jì)可重構(gòu)接口來實(shí)現(xiàn)機(jī)械臂的重構(gòu)。關(guān)節(jié)是機(jī)械臂的核心模塊，是機(jī)械系統(tǒng)、控制系統(tǒng)和電氣系統(tǒng)高度集成的驅(qū)動(dòng)和傳動(dòng)單元。采用中空直流無刷電機(jī)串聯(lián)諧波減速器的方式作為主要驅(qū)動(dòng)和傳動(dòng)部分；絕對(duì)值編碼器用于關(guān)節(jié)的位置檢測(cè)，關(guān)節(jié)內(nèi)部還包括力矩傳感器，用于關(guān)節(jié)的柔順控制；制動(dòng)器，用于關(guān)節(jié)的緊急制動(dòng)。

根據(jù)無線化可重構(gòu)關(guān)節(jié)的控制要求和控制方法，設(shè)計(jì)了關(guān)節(jié)的控制軟件總體架構(gòu)。在進(jìn)入主函數(shù)之前，先將所用到的串口、IO、中斷、 AD 等進(jìn)行初始化操作。然后等待定時(shí)器中斷。在中斷子程序中讀取編碼器的位置、編碼器計(jì)數(shù)清零、讀取力矩信號(hào)、力矩環(huán) PID 計(jì)算、讀取位置信號(hào)、位置環(huán) PID 計(jì)算、速度信號(hào)采樣、速度環(huán) PID 計(jì)算、電流信號(hào)采樣、電流環(huán) PID 計(jì)算。此時(shí)判斷是否檢測(cè)到 3 次 Z相脈沖，如果計(jì)數(shù)器未計(jì)數(shù)到 3，則執(zhí)行霍爾換向程序，進(jìn)行方波控制； 如果計(jì)數(shù)器大于 3 時(shí)，開始執(zhí)行 SVPWM 程序，進(jìn)行矢量控制。方波控制和矢量控制產(chǎn)生PWM 波控制開關(guān)管通斷驅(qū)動(dòng)關(guān)節(jié)轉(zhuǎn)動(dòng)。其中， 速度環(huán)和電流環(huán)使用電機(jī)輸入端增量式編碼器和電流采樣 AD 實(shí)現(xiàn)，位置環(huán)和力矩環(huán)使用關(guān)節(jié)輸出端絕對(duì)值編碼器和力矩傳感器實(shí)現(xiàn)。

多個(gè)機(jī)械臂協(xié)同操作時(shí)，每個(gè)機(jī)械臂都可以隨時(shí)加入WiFi網(wǎng)絡(luò)，并將機(jī)械臂的D—H參數(shù)發(fā)送給協(xié)調(diào)控制器。通過向協(xié)調(diào)控制器發(fā)送請(qǐng)求，協(xié)調(diào)控制器收到請(qǐng)求，做出應(yīng)答。即插即用技術(shù)，有利于機(jī)械臂快速投入使用和多機(jī)械臂之間協(xié)調(diào)操作。

多臂協(xié)同控制器和單臂中央控制器均采用冗余備份設(shè)計(jì)方案，主份和備份使用相同的電路，主份CPU-微控制器A和備份CPU-微控制器B都有自己的最小系統(tǒng)，共用一片雙端口RAM，主份CPU-微控制器A在運(yùn)行過程中，將控制器運(yùn)行的關(guān)鍵數(shù)據(jù)及運(yùn)行狀態(tài)信息存儲(chǔ)在雙端口RAM中，當(dāng)系統(tǒng)仲裁模塊檢查到主份CPU-微控制器A出故障，從而讓備份CPU-微控制器B啟動(dòng)的時(shí)候，備份CPU-微控制器B將先讀取雙端口RAM中存儲(chǔ)的系統(tǒng)關(guān)鍵信息，接著系統(tǒng)的狀態(tài)繼續(xù)運(yùn)行，保證整個(gè)多臂控制柜系統(tǒng)的正常工作。

本發(fā)明提供的基于WIFI的可重構(gòu)人機(jī)協(xié)作機(jī)械臂系統(tǒng)，其通過機(jī)械臂模塊之間的重構(gòu)和無線通信的應(yīng)用，從而豐富機(jī)械臂的功能以及布局布線的簡(jiǎn)潔。采用可重構(gòu)的機(jī)械臂通過變換構(gòu)型來完成不同的工作；本發(fā)明采用無線通信，有利于控制系統(tǒng)的快速重構(gòu)，采用WIFI作為機(jī)械臂內(nèi)無線通信的方式，可以實(shí)現(xiàn)數(shù)據(jù)穩(wěn)定的發(fā)送。使用WIFI輸出圖像和數(shù)據(jù)速率均能滿足要求，WIFI的安全機(jī)制通過加以改造可以獲得很高的安全級(jí)別；且采用可重構(gòu)空間機(jī)械臂的快換接口，用于實(shí)現(xiàn)模塊之間的快速更換，用于重組構(gòu)型。機(jī)械臂的可重構(gòu)接口不僅能夠?qū)崿F(xiàn)模塊之間的機(jī)械連接，通過可重構(gòu)接口中心的航空插頭，可實(shí)現(xiàn)模塊之間的電連接。

以上是對(duì)本發(fā)明的較佳實(shí)施進(jìn)行了具體說明，但本發(fā)明創(chuàng)造并不限于所述實(shí)施例，熟悉本領(lǐng)域的技術(shù)人員在不違背本發(fā)明精神的前提下還可做出種種的等同變形或替換，這些等同的變形或替換均包含在本申請(qǐng)權(quán)利要求所限定的范圍內(nèi)。