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

背景技術(shù)：

冗余度機(jī)械臂具有自由度大于任務(wù)空間所需最少自由度的特性，廣泛應(yīng)用于裝備制造、產(chǎn)品加工、機(jī)器作業(yè)等國(guó)民經(jīng)濟(jì)生產(chǎn)活動(dòng)中，相較于傳統(tǒng)的人工作業(yè)能大幅提高生產(chǎn)效率。當(dāng)冗余度機(jī)械臂執(zhí)行任務(wù)時(shí)，如何使得機(jī)械臂的可操作性最優(yōu)化，以最大限度地躲避奇異，這種問(wèn)題叫做機(jī)械臂可操作度最大化問(wèn)題，在冗余度機(jī)械臂的運(yùn)動(dòng)規(guī)劃中應(yīng)當(dāng)盡量保證可操作度的最大化。

在醫(yī)療手術(shù)、汽車裝配等應(yīng)用領(lǐng)域中，通常需要使用由多個(gè)冗余度機(jī)械臂組成的多冗余度機(jī)械臂系統(tǒng)，因此如何對(duì)多冗余度機(jī)械臂系統(tǒng)進(jìn)行協(xié)同控制，保證多冗余度機(jī)械臂系統(tǒng)中各個(gè)冗余度機(jī)械臂可操作度的最大化，成為人們必須考慮的問(wèn)題。

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

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

本發(fā)明提出一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法與裝置，旨在解決如何在通信受限的情況下實(shí)現(xiàn)多機(jī)械臂系統(tǒng)的可操作度最大化分布式協(xié)同控制的問(wèn)題。

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

所述控制方法包括：

確定所述兩個(gè)以上的冗余度機(jī)械臂中至少一個(gè)冗余度機(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ī)械臂對(duì)應(yīng)的雅可比矩陣等式；

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

根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。

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

所述控制裝置包括：

目標(biāo)機(jī)械臂確定模塊，用于確定所述兩個(gè)以上的冗余度機(jī)械臂中至少一個(gè)冗余度機(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ī)械臂對(duì)應(yīng)的雅可比矩陣等式；

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

機(jī)械臂控制模塊，用于根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。

在本發(fā)明實(shí)施例中，確定兩個(gè)以上的冗余度機(jī)械臂中至少一個(gè)冗余度機(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ī)械臂對(duì)應(yīng)的雅可比矩陣等式；在所述雅可比矩陣等式、障礙物躲避不等式、關(guān)節(jié)角度極限和關(guān)節(jié)速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號(hào)；根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。利用本發(fā)明實(shí)施例提出的協(xié)同控制方法，由于目標(biāo)冗余度機(jī)械臂只需與數(shù)量較少的相鄰冗余度機(jī)械臂進(jìn)行通信，大大減小了通信負(fù)載，從而可在通信受限的情況下實(shí)現(xiàn)多機(jī)械臂系統(tǒng)的可操作度最大化分布式協(xié)同控制。

附圖說(shuō)明

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

圖2為圖1中步驟104的一個(gè)實(shí)施例的具體流程圖；

圖3為圖2中步驟1042的一個(gè)實(shí)施例的具體流程圖；

圖4為圖1中步驟105的一個(gè)實(shí)施例的具體流程圖；

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

具體實(shí)施方式

本發(fā)明提出一種多冗余度機(jī)械臂系統(tǒng)的協(xié)同控制方法與裝置，旨在解決如何在通信受限的情況下實(shí)現(xiàn)多機(jī)械臂系統(tǒng)的可操作度最大化分布式協(xié)同控制的問(wèn)題。

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

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

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

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

所述兩個(gè)以上的冗余度機(jī)械臂在工作空間中按需分布，確定其中的一個(gè)或多個(gè)冗余度機(jī)械臂為目標(biāo)冗余度機(jī)械臂，對(duì)所述目標(biāo)冗余度機(jī)械臂實(shí)現(xià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中是連通的，因此對(duì)于某個(gè)目標(biāo)冗余度機(jī)械臂來(lái)說(shuō)，其相鄰冗余度機(jī)械臂的個(gè)數(shù)為一個(gè)或以上。所述位置信息可以是冗余度機(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è)置一個(gè)控制中心，由所述控制中心將所述參考點(diǎn)軌跡信息發(fā)送給各個(gè)目標(biāo)冗余度機(jī)械臂，也可以利用相鄰冗余度機(jī)械臂或其它方式將所述參考點(diǎn)軌跡信息發(fā)送給各個(gè)目標(biāo)冗余度機(jī)械臂。

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

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

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

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

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

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

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

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

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

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

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

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

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

對(duì)于上述步驟(1)，所述目標(biāo)冗余度機(jī)械臂與其自身之間具備通信連接關(guān)系，因此將所述第一連通權(quán)值設(shè)置為1。對(duì)于上述步驟(2)，所述目標(biāo)冗余度機(jī)械臂與相鄰冗余度機(jī)械臂之間可以直接交換信息，因此將所述第一連通權(quán)值設(shè)置為1。對(duì)于上述步驟(3)，所述目標(biāo)冗余度機(jī)械臂與非自身且非相鄰冗余度機(jī)械臂之間無(wú)需直接交換信息，因此將所述第一連通權(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的雅可比矩陣；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量；表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號(hào)；A_ij表示目標(biāo)冗余度機(jī)械臂i的權(quán)重矩陣，所述權(quán)重矩陣中的各個(gè)元素為目標(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í)行器相對(duì)于所述目標(biāo)參考點(diǎn)的距離向量；r_d為所述目標(biāo)參考點(diǎn)的期望軌跡；c₀＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

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

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

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

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

1051、設(shè)置性能指標(biāo)為可操作度最大化，在所述雅可比矩陣等式、障礙物躲避不等式、關(guān)節(jié)角度極限和關(guān)節(jié)速度極限的約束下確定二次型優(yōu)化結(jié)果；

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

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

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

所述第一約束條件包括：

雅可比矩陣等式約束條件

障礙物躲避不等式約束條件

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

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

所述第一式子為：

其中，上標(biāo)^T表示矩陣和向量的轉(zhuǎn)置；表示矩陣的行列式值；c₁＞0是用來(lái)控制可操作度最大化速率的設(shè)計(jì)參數(shù)；B_i和e_i是障礙物躲避參數(shù)；表示目標(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)化問(wèn)題是在速度層上求解，因此需將第i個(gè)冗余度機(jī)械臂的關(guān)節(jié)角度約束(4)、關(guān)節(jié)速度約束(5)合并，從而可以得到以下基于速度的雙端不等式約束：

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

約束條件：C_ix_i＝d_i (7)

B_ix_i≤e_i (8)

最小化：

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

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

可以利用標(biāo)準(zhǔn)二次規(guī)劃求解器或者數(shù)值方法對(duì)所述標(biāo)準(zhǔn)二次規(guī)劃問(wèn)題進(jìn)行求解，得到各個(gè)目標(biāo)冗余度機(jī)械臂的可操作度最大化運(yùn)動(dòng)規(guī)劃方法的最優(yōu)解。

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

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

106、根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。

在確定所述目標(biāo)冗余度機(jī)械臂的控制信號(hào)后，根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂。最終使得在各個(gè)機(jī)械臂末端執(zhí)行器保持與參考點(diǎn)相對(duì)距離不變的情況下，整個(gè)冗余度機(jī)械臂系統(tǒng)實(shí)現(xiàn)分布式協(xié)同可操作度最大化運(yùn)動(dòng)。

在本發(fā)明實(shí)施例中，確定兩個(gè)以上的冗余度機(jī)械臂中至少一個(gè)冗余度機(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ī)械臂對(duì)應(yīng)的雅可比矩陣等式；在所述雅可比矩陣等式、障礙物躲避不等式、關(guān)節(jié)角度極限和關(guān)節(jié)速度極限的約束下，根據(jù)二次型優(yōu)化和標(biāo)準(zhǔn)二次規(guī)劃方法確定所述目標(biāo)冗余度機(jī)械臂的控制信號(hào)；根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。利用本發(fā)明實(shí)施例提出的協(xié)同控制方法，由于目標(biāo)冗余度機(jī)械臂只需與數(shù)量較少的相鄰冗余度機(jī)械臂進(jìn)行通信，大大減小了通信負(fù)載，從而可在通信受限的情況下實(shí)現(xiàn)多機(jī)械臂系統(tǒng)的可操作度最大化分布式協(xié)同控制。

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

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

所述控制裝置包括：

目標(biāo)機(jī)械臂確定模塊501，用于確定所述兩個(gè)以上的冗余度機(jī)械臂中至少一個(gè)冗余度機(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ī)械臂對(duì)應(yīng)的雅可比矩陣等式；

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

機(jī)械臂控制模塊506，用于根據(jù)所述控制信號(hào)控制所述目標(biāo)冗余度機(jī)械臂，使得所述目標(biāo)冗余度機(jī)械臂的可操作度最大化。

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

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

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

等式構(gòu)建單元，用于根據(jù)所述權(quán)重矩陣、所述第二連通權(quán)值、所述位置信息和所述參考點(diǎn)軌跡信息構(gòu)建所述目標(biāo)冗余度機(jī)械臂對(duì)應(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ī)械臂與所述兩個(gè)以上的冗余度機(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的雅可比矩陣；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量；表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號(hào)；A_ij表示目標(biāo)冗余度機(jī)械臂i的權(quán)重矩陣，所述權(quán)重矩陣中的各個(gè)元素為目標(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í)行器相對(duì)于所述目標(biāo)參考點(diǎn)的距離向量；r_d為所述目標(biāo)參考點(diǎn)的期望軌跡；c₀＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

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

第二確定單元，用于設(shè)置性能指標(biāo)為可操作度最大化，在所述雅可比矩陣等式、障礙物躲避不等式、關(guān)節(jié)角度極限和關(guān)節(jié)速度極限的約束下確定二次型優(yōu)化結(jié)果；

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

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

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

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

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

所述第一約束條件包括：

雅可比矩陣等式約束條件

障礙物躲避不等式約束條件

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

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

所述第一式子為

其中，上標(biāo)^T表示矩陣和向量的轉(zhuǎn)置；表示矩陣的行列式值；c₁＞0是用來(lái)控制可操作度最大化速率的設(shè)計(jì)參數(shù)；B_i和e_i是障礙物躲避參數(shù)；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)角上下限；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度上下限。

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

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

所述作為分離部件說(shuō)明的單元可以是或者也可以不是物理上分開(kāi)的，作為單元顯示的部件可以是或者也可以不是物理單元，即可以位于一個(gè)地方，或者也可以分布到多個(gè)網(wǎng)絡(luò)單元上?？梢愿鶕?jù)實(shí)際的需要選擇其中的部分或者全部單元來(lái)實(shí)現(xiàn)本實(shí)施例方案的目的。

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

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

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