本發(fā)明涉及機(jī)械臂控制領(lǐng)域，具體涉及一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法與裝置。

背景技術(shù)：

冗余度機(jī)械臂具有自由度大于任務(wù)空間所需最少自由度的特性，廣泛應(yīng)用于裝備制造、產(chǎn)品加工、機(jī)器作業(yè)等國民經(jīng)濟(jì)生產(chǎn)活動中，相較于傳統(tǒng)的人工作業(yè)能大幅提高生產(chǎn)效率。

在醫(yī)療手術(shù)、汽車裝配等應(yīng)用領(lǐng)域中，通常需要使用由多個冗余度機(jī)械臂組成的多冗余度機(jī)械臂系統(tǒng)，因此如何對多冗余度機(jī)械臂系統(tǒng)進(jìn)行協(xié)同控制，保證多冗余度機(jī)械臂系統(tǒng)中各個冗余度機(jī)械臂能夠?qū)崿F(xiàn)重復(fù)運(yùn)動，成為人們必須考慮的問題。

目前，在廣闊的操作環(huán)境中，多冗余度機(jī)械臂系統(tǒng)中各個冗余度機(jī)械臂因距離以及通信負(fù)載的限制，通常不具有全局通信的能力，此外，控制中心也難以接入所有機(jī)械臂進(jìn)行直接通信。在大規(guī)模環(huán)境下，冗余度機(jī)械臂之間有可能因為數(shù)量較多導(dǎo)致通信負(fù)載過大而難以通信，進(jìn)而導(dǎo)致信息不能隨時、隨地進(jìn)行共享，嚴(yán)重阻礙多機(jī)械臂系統(tǒng)實現(xiàn)分布式協(xié)同重復(fù)運(yùn)動。

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

本發(fā)明提出一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法與裝置，旨在解決如何在通信受限的情況下實現(xiàn)多機(jī)械臂系統(tǒng)的分布式協(xié)同重復(fù)運(yùn)動的問題。

本發(fā)明實施例第一方面提供了一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法，所述多冗余度機(jī)械臂系統(tǒng)包括兩個以上的冗余度機(jī)械臂，所述兩個以上的冗余度機(jī)械臂在通信拓?fù)鋱D中連通；

所述控制方法包括：

確定所述兩個以上的冗余度機(jī)械臂中至少一個冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂；

獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息，所述相鄰冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂在通信拓?fù)鋱D中相鄰連接；

獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息，所述參考點(diǎn)軌跡信息由預(yù)設(shè)的目標(biāo)參考點(diǎn)和所述目標(biāo)參考點(diǎn)的期望軌跡確定；

根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；

在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；

根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。

本發(fā)明實施例第二方面提供了一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制裝置，所述多冗余度機(jī)械臂系統(tǒng)包括兩個以上的冗余度機(jī)械臂，所述兩個以上的冗余度機(jī)械臂在通信拓?fù)鋱D中連通；

所述控制裝置包括：

目標(biāo)機(jī)械臂確定模塊，用于確定所述兩個以上的冗余度機(jī)械臂中至少一個冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂；

信息獲取模塊，用于獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息，所述相鄰冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂在通信拓?fù)鋱D中相鄰連接；

參考點(diǎn)軌跡信息獲取模塊，用于獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息，所述參考點(diǎn)軌跡信息由預(yù)設(shè)的目標(biāo)參考點(diǎn)和所述目標(biāo)參考點(diǎn)的期望軌跡確定；

等式構(gòu)建模塊，用于根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；

控制信號確定模塊，用于在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；

機(jī)械臂控制模塊，用于根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。

在本發(fā)明實施例中，確定多冗余度機(jī)械臂系統(tǒng)中的目標(biāo)冗余度機(jī)械臂；獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息；獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息；根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。利用本發(fā)明實施例提出的協(xié)同控制方法，由于目標(biāo)冗余度機(jī)械臂只需與數(shù)量較少的相鄰冗余度機(jī)械臂進(jìn)行通信，大大減小了通信負(fù)載，從而可在通信受限的情況下實現(xiàn)多機(jī)械臂系統(tǒng)的分布式協(xié)同重復(fù)運(yùn)動。

附圖說明

圖1為本發(fā)明實施例中一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法一個實施例的流程圖；

圖2為圖1中步驟104的一個實施例的具體流程圖；

圖3為圖2中步驟1042的一個實施例的具體流程圖；

圖4為圖1中步驟105的一個實施例的具體流程圖；

圖5為本發(fā)明實施例中一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制裝置一個實施例的結(jié)構(gòu)圖。

具體實施方式

本發(fā)明提出一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法與裝置，旨在解決如何在通信受限的情況下實現(xiàn)多機(jī)械臂系統(tǒng)的分布式協(xié)同重復(fù)運(yùn)動的問題。

為使得本發(fā)明的發(fā)明目的、特征、優(yōu)點(diǎn)能夠更加的明顯和易懂，下面將結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，下面所描述的實施例僅僅是本發(fā)明一部分實施例，而非全部的實施例?；诒景l(fā)明中的實施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其它實施例，都屬于本發(fā)明保護(hù)的范圍。

圖1示出了本發(fā)明一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法的一個實施例的流程圖，所述多冗余度機(jī)械臂系統(tǒng)包括兩個以上的冗余度機(jī)械臂，所述兩個以上的冗余度機(jī)械臂在通信拓?fù)鋱D中連通；

如圖1所示，所述控制方法包括：

101、確定所述兩個以上的冗余度機(jī)械臂中至少一個冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂；

所述兩個以上的冗余度機(jī)械臂在工作空間中按需分布，確定其中的一個或多個冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂，使所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。

102、獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息，所述相鄰冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂在通信拓?fù)鋱D中相鄰連接；

在確定目標(biāo)冗余度機(jī)械臂后，獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息。所述相鄰冗余度機(jī)械臂指與所述目標(biāo)冗余度機(jī)械臂在通信拓?fù)鋱D中相鄰連接的冗余度機(jī)械臂，和目標(biāo)冗余度機(jī)械臂屬于同一多冗余度機(jī)械臂系統(tǒng)。由于所述多冗余度機(jī)械臂系統(tǒng)在通信拓?fù)鋱D中是連通的，因此對于某個目標(biāo)冗余度機(jī)械臂來說，其相鄰冗余度機(jī)械臂的個數(shù)為一個或以上。所述位置信息可以是冗余度機(jī)械臂的末端執(zhí)行器的位置信息，也可以是冗余度機(jī)械臂的其它部位的位置信息。同樣地，所述速度信息可以是冗余度機(jī)械臂的末端執(zhí)行器的速度信息，也可以是冗余度機(jī)械臂的其它部位的速度信息，目標(biāo)冗余度機(jī)械臂可以和其相鄰冗余度機(jī)械臂互相交換所述位置和速度信息。

103、獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息，所述參考點(diǎn)軌跡信息由預(yù)設(shè)的目標(biāo)參考點(diǎn)和所述目標(biāo)參考點(diǎn)的期望軌跡確定；

所述目標(biāo)參考點(diǎn)是預(yù)設(shè)的位于目標(biāo)冗余度機(jī)械臂工作空間中的位置參考點(diǎn)，所述參考點(diǎn)軌跡信息由所述目標(biāo)參考點(diǎn)的期望位置及其移動速度確定?？梢栽诙嗳哂喽葯C(jī)械臂系統(tǒng)的工作空間中設(shè)置一個控制中心，由所述控制中心將所述參考點(diǎn)軌跡信息發(fā)送給各個目標(biāo)冗余度機(jī)械臂，也可以利用相鄰冗余度機(jī)械臂或其它方式將所述參考點(diǎn)軌跡信息發(fā)送給各個目標(biāo)冗余度機(jī)械臂。

104、根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；

在獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息，以及所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息之后，根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式。

進(jìn)一步的，如圖2所示，步驟104具體可以包括：

1041、構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的權(quán)重矩陣，所述權(quán)重矩陣中的各個元素為所述目標(biāo)冗余度機(jī)械臂與所述兩個以上的冗余度機(jī)械臂中的各個機(jī)械臂之間分別對應(yīng)的第一連通權(quán)值；

所述權(quán)重矩陣的元素個數(shù)由所述多冗余度機(jī)械臂系統(tǒng)中的機(jī)械臂個數(shù)決定，所述第一連通權(quán)值代表所述目標(biāo)冗余度機(jī)械臂與所述兩個以上的冗余度機(jī)械臂中的各個機(jī)械臂之間的通信連接關(guān)系。比如定義權(quán)重矩陣其第ij個元素定義為通信拓?fù)鋱D上第i個冗余度機(jī)械臂與第j個冗余度機(jī)械臂之間的第一連通權(quán)值。

1042、根據(jù)所述參考點(diǎn)軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機(jī)械臂的第二連通權(quán)值；

所述第二連通權(quán)值代表所述目標(biāo)冗余度機(jī)械臂對于所述參考點(diǎn)軌跡信息的獲取狀態(tài)，可以用ρ_i來表示第i個冗余度機(jī)械臂的第二連通權(quán)值。

1043、根據(jù)所述位置和速度信息、所述參考點(diǎn)軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式。

在構(gòu)建權(quán)重矩陣以及確定第二連通權(quán)值后，可以根據(jù)所述位置和速度信息、所述參考點(diǎn)軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式。

更進(jìn)一步的，所述第一連通權(quán)值可以通過以下步驟確定：

(1)將所述目標(biāo)冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂自身之間的第一連通權(quán)值設(shè)置為1；

(2)將所述目標(biāo)冗余度機(jī)械臂與相鄰冗余度機(jī)械臂之間的第一連通權(quán)值設(shè)置為1；

(3)將所述目標(biāo)冗余度機(jī)械臂與所述兩個以上的冗余度機(jī)械臂中的非自身且非相鄰冗余度機(jī)械臂之間的第一連通權(quán)值設(shè)置為0。

對于上述步驟(1)，所述目標(biāo)冗余度機(jī)械臂與其自身之間具備通信連接關(guān)系，因此將所述第一連通權(quán)值設(shè)置為1。對于上述步驟(2)，所述目標(biāo)冗余度機(jī)械臂與相鄰冗余度機(jī)械臂之間可以直接交換信息，因此將所述第一連通權(quán)值設(shè)置為1。對于上述步驟(3)，所述目標(biāo)冗余度機(jī)械臂與非自身且非相鄰冗余度機(jī)械臂之間無需直接交換信息，因此將所述第一連通權(quán)值設(shè)置為0。

如圖3所示，步驟1042具體可以包括：

10421、判斷所述目標(biāo)冗余度機(jī)械臂是否獲取到所述參考點(diǎn)軌跡信息；

10422、若所述目標(biāo)冗余度機(jī)械臂獲取到所述參考點(diǎn)軌跡信息，則將所述第二連通權(quán)值設(shè)置為1；

10423、若所述目標(biāo)冗余度機(jī)械臂未獲取到所述參考點(diǎn)軌跡信息，則將所述第二連通權(quán)值設(shè)置為0。

根據(jù)所述參考點(diǎn)軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機(jī)械臂的第二連通權(quán)值，若所述目標(biāo)冗余度機(jī)械臂獲取到所述參考點(diǎn)軌跡信息，則將所述第二連通權(quán)值設(shè)置為1，否則將所述第二連通權(quán)值設(shè)置為0。

所述雅可比矩陣等式的通用表達(dá)式具體可以為：

其中，J_i表示目標(biāo)冗余度機(jī)械臂i的雅可比矩陣，為J_i的時間導(dǎo)數(shù)；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量，為的時間導(dǎo)數(shù)；j∈N(i)表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號；A_ij表示目標(biāo)冗余度機(jī)械臂i的權(quán)重矩陣，所述權(quán)重矩陣中的各個元素為目標(biāo)冗余度機(jī)械臂i與冗余度機(jī)械臂j之間的第一連通權(quán)值；ρ_i表示目標(biāo)冗余度機(jī)械臂i的第二連通權(quán)值；δ_i＝r_i-r_irp為所述位置信息，r_i為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器位置，r_irp為預(yù)設(shè)的目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器相對于所述目標(biāo)參考點(diǎn)的距離向量；為所述速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器速度，r_d為所述目標(biāo)參考點(diǎn)的期望位置，為所述目標(biāo)參考點(diǎn)的期望速度；c₀＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

上述雅可比矩陣等式(1)主要負(fù)責(zé)實現(xiàn)多機(jī)械臂之間的分布式協(xié)同運(yùn)動約束，通過獲取其他冗余度機(jī)械臂末端執(zhí)行器的位置和速度信息，即可將第i個冗余度機(jī)械臂的期望加速度約束設(shè)置為上述其它冗余度機(jī)械臂相關(guān)位置、速度的加權(quán)平均(權(quán)重即為雅可比等式約束右邊所述)，從而實現(xiàn)協(xié)同運(yùn)動。基于雅可比矩陣等式約束(1)構(gòu)建的多機(jī)械臂協(xié)同運(yùn)動系統(tǒng)具有高魯棒性，低通信成本的特點(diǎn)，具體而言，對于多機(jī)械臂系統(tǒng)，當(dāng)有一個機(jī)械臂加入或者退出都不影響該系統(tǒng)的穩(wěn)定。此外，每個機(jī)械臂只需要和數(shù)量較少的相鄰機(jī)械臂通信即可，不必滿足全局通信的要求。假如有100個機(jī)械臂，則在全局通信情況下，總共需要的通信鏈接數(shù)為100*99/2＝4950；而利用本發(fā)明實施例提出的分布式方案最少只需要99個通信鏈接數(shù)，大大減小了通信負(fù)載。

105、在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；

在構(gòu)建好所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式之后，在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號。

進(jìn)一步的，如圖4所示，步驟105具體可以包括：

1051、設(shè)置最小化性能指標(biāo)為重復(fù)運(yùn)動，在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下確定二次型優(yōu)化結(jié)果；

本發(fā)明實施例需要實現(xiàn)多機(jī)械臂系統(tǒng)的分布式協(xié)同重復(fù)運(yùn)動，因此在二次型優(yōu)化過程中設(shè)置最小化性能指標(biāo)為重復(fù)運(yùn)動，然后在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下確定二次型優(yōu)化結(jié)果。

更進(jìn)一步的，若所述雅可比矩陣等式如公式(1)所示，則所述二次型優(yōu)化具體可以為：

在第一約束條件下使得第一式子最小化；

所述第一約束條件包括：

雅可比矩陣等式約束條件

關(guān)節(jié)角度極限約束條件

關(guān)節(jié)速度極限約束條件

關(guān)節(jié)加速度極限約束條件

所述第一式子為：

其中，上標(biāo)T表示矩陣和向量的轉(zhuǎn)置；c₁＞0和c₂＞0是用來控制關(guān)節(jié)位移幅值和收斂率的設(shè)計參數(shù)，θ_i(0)表示目標(biāo)冗余度機(jī)械臂i的初始關(guān)節(jié)角；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)角上下限；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度上下限，表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)加速度上下限。

1052、將所述二次型優(yōu)化結(jié)果轉(zhuǎn)換為標(biāo)準(zhǔn)二次規(guī)劃；

將二次型優(yōu)化結(jié)果轉(zhuǎn)換為標(biāo)準(zhǔn)二次規(guī)劃，以便進(jìn)行求解。

進(jìn)一步的，若所述二次型優(yōu)化如公式(2)至(6)所示，考慮到上述優(yōu)化問題是在加速度層上求解，因此需將第i個冗余度機(jī)械臂的關(guān)節(jié)角度約束(3)、關(guān)節(jié)速度約束(4)和關(guān)節(jié)加速度約束(5)合并，從而可以得到以下基于加速度的雙端不等式約束：

其中，和分別表示第i個冗余度機(jī)械臂的合成雙端約束的上下極限，它們的第p個元素分別定義為和其中為裕度，ip表示第i個冗余度機(jī)械臂的關(guān)節(jié)序號，p＝1，2，…，m，m為機(jī)械臂的自由度數(shù)目，κ₁＞0和κ₂＞0用來調(diào)節(jié)和保證關(guān)節(jié)加速度足夠大的可行域。用x_i表示第i個冗余度機(jī)械臂的關(guān)節(jié)加速度上述二次型優(yōu)化方案(2)至(6)便可描述為如下的標(biāo)準(zhǔn)二次規(guī)劃方案：

約束條件：C_ixi＝d_i (7)

最小化：

其中，W為單位矩陣，C_i＝J_i，

1053、對所述標(biāo)準(zhǔn)二次規(guī)劃進(jìn)行求解，得到求解結(jié)果；

可以利用標(biāo)準(zhǔn)二次規(guī)劃求解器或者數(shù)值方法對所述標(biāo)準(zhǔn)二次規(guī)劃問題進(jìn)行求解，得到各個目標(biāo)冗余度機(jī)械臂的加速度重復(fù)運(yùn)動規(guī)劃方法的最優(yōu)解。

1054、根據(jù)所述求解結(jié)果確定所述目標(biāo)冗余度機(jī)械臂的控制信號。

根據(jù)所述求解結(jié)果確定所述目標(biāo)冗余度機(jī)械臂的控制信號，然后利用所述控制信號對目標(biāo)冗余度機(jī)械臂進(jìn)行控制。

106、根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。

在確定所述目標(biāo)冗余度機(jī)械臂的控制信號后，根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，最終使得在各個機(jī)械臂末端執(zhí)行器保持與參考點(diǎn)相對距離不變的情況下，整個冗余度機(jī)械臂系統(tǒng)實現(xiàn)分布式協(xié)同加速度層重復(fù)運(yùn)動。所述重復(fù)運(yùn)動指機(jī)械臂在執(zhí)行完路徑為封閉曲線的任務(wù)后，各個關(guān)節(jié)角都能夠回到其初始位置，然后執(zhí)行下一次相同的任務(wù)。如果冗余度機(jī)械臂不能實現(xiàn)重復(fù)運(yùn)動，則其工作時可能危害自身或周圍操作人員的安全。

在本發(fā)明實施例中，確定多冗余度機(jī)械臂系統(tǒng)中的目標(biāo)冗余度機(jī)械臂；獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息；獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息；根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。利用本發(fā)明實施例提出的協(xié)同控制方法，由于目標(biāo)冗余度機(jī)械臂只需與數(shù)量較少的相鄰冗余度機(jī)械臂進(jìn)行通信，大大減小了通信負(fù)載，從而可在通信受限的情況下實現(xiàn)多機(jī)械臂系統(tǒng)的分布式協(xié)同重復(fù)運(yùn)動。

上面主要描述了一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法，下面將對一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制裝置進(jìn)行詳細(xì)描述。

請參閱圖5，示出了本發(fā)明實施例中一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制裝置，所述多冗余度機(jī)械臂系統(tǒng)包括兩個以上的冗余度機(jī)械臂，所述兩個以上的冗余度機(jī)械臂在通信拓?fù)鋱D中連通；

所述控制裝置包括：

目標(biāo)機(jī)械臂確定模塊501，用于確定所述兩個以上的冗余度機(jī)械臂中至少一個冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂；

信息獲取模塊502，用于獲取所述目標(biāo)冗余度機(jī)械臂的相鄰冗余度機(jī)械臂的位置和速度信息，所述相鄰冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂在通信拓?fù)鋱D中相鄰連接；

參考點(diǎn)軌跡信息獲取模塊503，用于獲取所述目標(biāo)冗余度機(jī)械臂的參考點(diǎn)軌跡信息，所述參考點(diǎn)軌跡信息由預(yù)設(shè)的目標(biāo)參考點(diǎn)和所述目標(biāo)參考點(diǎn)的期望軌跡確定；

等式構(gòu)建模塊504，用于根據(jù)所述位置和速度信息，以及所述參考點(diǎn)軌跡信息按照預(yù)設(shè)的規(guī)則構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式；

控制信號確定模塊505，用于在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號；

機(jī)械臂控制模塊506，用于根據(jù)所述控制信號控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂實現(xiàn)重復(fù)運(yùn)動。

進(jìn)一步的，所述等式構(gòu)建模塊504具體可以包括：

權(quán)重矩陣構(gòu)建單元，用于構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的權(quán)重矩陣，所述權(quán)重矩陣中的各個元素為所述目標(biāo)冗余度機(jī)械臂與所述兩個以上的冗余度機(jī)械臂中的各個機(jī)械臂之間分別對應(yīng)的第一連通權(quán)值；

第一確定單元，用于根據(jù)所述參考點(diǎn)軌跡信息的獲取狀態(tài)確定所述目標(biāo)冗余度機(jī)械臂的第二連通權(quán)值；

等式構(gòu)建單元，用于根據(jù)所述位置和速度信息、所述參考點(diǎn)軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機(jī)械臂對應(yīng)的雅可比矩陣等式。

更進(jìn)一步的，所述權(quán)重矩陣構(gòu)建單元具體可以包括：

第一設(shè)置模塊，用于將所述目標(biāo)冗余度機(jī)械臂與所述目標(biāo)冗余度機(jī)械臂自身之間的第一連通權(quán)值設(shè)置為1；

第二設(shè)置模塊，用于將所述目標(biāo)冗余度機(jī)械臂與相鄰冗余度機(jī)械臂之間的第一連通權(quán)值設(shè)置為1；

第三設(shè)置模塊，用于將所述目標(biāo)冗余度機(jī)械臂與所述兩個以上的冗余度機(jī)械臂中的非自身且非相鄰冗余度機(jī)械臂之間的第一連通權(quán)值設(shè)置為0；

所述第一確定單元具體可以包括：

判斷模塊，用于判斷所述目標(biāo)冗余度機(jī)械臂是否獲取到所述參考點(diǎn)軌跡信息；

第四設(shè)置模塊，用于若所述目標(biāo)冗余度機(jī)械臂獲取到所述參考點(diǎn)軌跡信息，則將所述第二連通權(quán)值設(shè)置為1；

第五設(shè)置模塊，用于若所述目標(biāo)冗余度機(jī)械臂未獲取到所述參考點(diǎn)軌跡信息，則將所述第二連通權(quán)值設(shè)置為0；

所述等式構(gòu)建單元構(gòu)建的雅可比矩陣等式的通用表達(dá)式具體可以為：

其中，J_i表示目標(biāo)冗余度機(jī)械臂i的雅可比矩陣，為J_i的時間導(dǎo)數(shù)；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量，為的時間導(dǎo)數(shù)；j∈N(i)表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號；A_ij表示目標(biāo)冗余度機(jī)械臂i的權(quán)重矩陣，所述權(quán)重矩陣中的各個元素為目標(biāo)冗余度機(jī)械臂i與冗余度機(jī)械臂j之間的第一連通權(quán)值；ρ_i表示目標(biāo)冗余度機(jī)械臂i的第二連通權(quán)值；δ_i＝r_i-r_irp為所述位置信息，r_i為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器位置，r_irp為預(yù)設(shè)的目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器相對于所述目標(biāo)參考點(diǎn)的距離向量；為所述速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器速度，r_d為所述目標(biāo)參考點(diǎn)的期望位置，為所述目標(biāo)參考點(diǎn)的期望速度；c₀＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

進(jìn)一步的，所述控制信號確定模塊505具體可以包括：

第二確定單元，用于設(shè)置最小化性能指標(biāo)為重復(fù)運(yùn)動，在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限的約束下確定二次型優(yōu)化結(jié)果；

轉(zhuǎn)換單元，用于將所述二次型優(yōu)化結(jié)果轉(zhuǎn)換為標(biāo)準(zhǔn)二次規(guī)劃；

求解單元，用于對所述標(biāo)準(zhǔn)二次規(guī)劃進(jìn)行求解，得到求解結(jié)果；

控制信號確定單元，用于根據(jù)所述求解結(jié)果確定所述目標(biāo)冗余度機(jī)械臂的控制信號。

進(jìn)一步的，所述控制信號確定模塊采用的二次型優(yōu)化具體可以為：

在第一約束條件下使得第一式子最小化；

所述第一約束條件包括：

雅可比矩陣等式約束條件

關(guān)節(jié)角度極限約束條件

關(guān)節(jié)速度極限約束條件

關(guān)節(jié)加速度極限約束條件

所述第一式子為

其中，上標(biāo)^T表示矩陣和向量的轉(zhuǎn)置；c₁＞0和c₂＞0是用來控制關(guān)節(jié)位移幅值和收斂率的設(shè)計參數(shù)，θ_i(0)表示目標(biāo)冗余度機(jī)械臂i的初始關(guān)節(jié)角；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)角上下限；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度上下限，表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)加速度上下限。

所屬領(lǐng)域的技術(shù)人員可以清楚地了解到，為描述的方便和簡潔，上述描述的系統(tǒng)，裝置和單元的具體工作過程，可以參考前述方法實施例中的對應(yīng)過程，在此不再贅述。

在本申請所提供的幾個實施例中，應(yīng)該理解到，所揭露的系統(tǒng)，裝置和方法，可以通過其它的方式實現(xiàn)。例如，以上所描述的裝置實施例僅僅是示意性的，例如，所述單元的劃分，僅僅為一種邏輯功能劃分，實際實現(xiàn)時可以有另外的劃分方式，例如多個單元或組件可以結(jié)合或者可以集成到另一個系統(tǒng)，或一些特征可以忽略，或不執(zhí)行。另一點(diǎn)，所顯示或討論的相互之間的耦合或直接耦合或通信連接可以是通過一些接口，裝置或單元的間接耦合或通信連接，可以是電性，機(jī)械或其它的形式。

所述作為分離部件說明的單元可以是或者也可以不是物理上分開的，作為單元顯示的部件可以是或者也可以不是物理單元，即可以位于一個地方，或者也可以分布到多個網(wǎng)絡(luò)單元上。可以根據(jù)實際的需要選擇其中的部分或者全部單元來實現(xiàn)本實施例方案的目的。

另外，在本發(fā)明各個實施例中的各功能單元可以集成在一個處理單元中，也可以是各個單元單獨(dú)物理存在，也可以兩個或兩個以上單元集成在一個單元中。上述集成的單元既可以采用硬件的形式實現(xiàn)，也可以采用軟件功能單元的形式實現(xiàn)。

所述集成的單元如果以軟件功能單元的形式實現(xiàn)并作為獨(dú)立的產(chǎn)品銷售或使用時，可以存儲在一個計算機(jī)可讀取存儲介質(zhì)中?；谶@樣的理解，本發(fā)明的技術(shù)方案本質(zhì)上或者說對現(xiàn)有技術(shù)做出貢獻(xiàn)的部分或者該技術(shù)方案的全部或部分可以以軟件產(chǎn)品的形式體現(xiàn)出來，該計算機(jī)軟件產(chǎn)品存儲在一個存儲介質(zhì)中，包括若干指令用以使得一臺計算機(jī)設(shè)備(可以是個人計算機(jī)，服務(wù)器，或者網(wǎng)絡(luò)設(shè)備等)執(zhí)行本發(fā)明各個實施例所述方法的全部或部分步驟。而前述的存儲介質(zhì)包括：U盤、移動硬盤、只讀存儲器(ROM，Read-Only Memory)、隨機(jī)存取存儲器(RAM，Random Access Memory)、磁碟或者光盤等各種可以存儲程序代碼的介質(zhì)。

以上所述，以上實施例僅用以說明本發(fā)明的技術(shù)方案，而非對其限制；盡管參照前述實施例對本發(fā)明進(jìn)行了詳細(xì)的說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解：其依然可以對前述各實施例所記載的技術(shù)方案進(jìn)行修改，或者對其中部分技術(shù)特征進(jìn)行等同替換；而這些修改或者替換，并不使相應(yīng)技術(shù)方案的本質(zhì)脫離本發(fā)明各實施例技術(shù)方案的精神和范圍。