本發(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)效率。

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

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

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

本發(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é)速度極限、關(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ī)械臂，使所述兩個(gè)以上的冗余度機(jī)械臂實(shí)現(xiàn)分布式協(xié)同運(yùn)動(dòng)。

本發(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é)速度極限、關(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ī)械臂，使所述兩個(gè)以上的冗余度機(jī)械臂實(shí)現(xiàn)分布式協(xié)同運(yùn)動(dòng)。

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

附圖說明

圖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é)同運(yùn)動(dòng)的問題。

為使得本發(fā)明的發(fā)明目的、特征、優(yōu)點(diǎn)能夠更加的明顯和易懂，下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，下面所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而非全部的實(shí)施例。基于本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(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ī)械臂，使所述目標(biāo)冗余度機(jī)械臂實(shí)現(xiàn)重復(fù)運(yùn)動(dòng)。

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ī)械臂來說，其相鄰冗余度機(jī)械臂的個(gè)數(shù)為一個(gè)或以上。所述位置信息可以是冗余度機(jī)械臂的末端執(zhí)行器的位置信息，也可以是冗余度機(jī)械臂的其它部位的位置信息。同樣地，所述速度信息可以是冗余度機(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è)置一個(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來表示第i個(gè)冗余度機(jī)械臂的第二連通權(quán)值。

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

在構(gòu)建權(quán)重矩陣以及確定第二連通權(quán)值后，可以根據(jù)所述位置、速度和加速度信息、所述參考點(diǎn)軌跡信息、所述權(quán)重矩陣和所述第二連通權(quán)值構(gòu)建所述目標(biāo)冗余度機(jī)械臂對(duì)應(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ī)械臂與所述兩個(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ī)械臂之間無需直接交換信息，因此將所述第一連通權(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á)式具體可以為：

其中

ji表示目標(biāo)冗余度機(jī)械臂i的雅可比矩陣，與分別為ji的一階與二階時(shí)間導(dǎo)數(shù)；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量，與分別為的一階與二階時(shí)間導(dǎo)數(shù)；表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號(hào)；aij表示目標(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＝ri-rirp為所述位置信息，ri為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的位置，rirp為預(yù)設(shè)的目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器相對(duì)于所述目標(biāo)參考點(diǎn)的距離向量；為所述速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的速度；為所述加速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的加速度；rd為所述目標(biāo)參考點(diǎn)的期望位置，為所述目標(biāo)參考點(diǎn)的期望速度，為所述目標(biāo)參考點(diǎn)的期望加速度；c0＞0、c1＞0和c2＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

上述雅可比矩陣等式(1)主要負(fù)責(zé)實(shí)現(xiàn)多機(jī)械臂之間的分布式協(xié)同運(yùn)動(dòng)約束，通過獲取其他冗余度機(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é)速度極限、關(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é)速度極限、關(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)為突加度范數(shù)最小化，在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限、關(guān)節(jié)加速度極限和關(guān)節(jié)突加度極限的約束下確定二次型優(yōu)化結(jié)果；

本發(fā)明實(shí)施例需要實(shí)現(xiàn)基于突加度的多機(jī)械臂系統(tǒng)的分布式協(xié)同運(yùn)動(dòng)，因此在二次型優(yōu)化過程中設(shè)置最小化性能指標(biāo)為突加度范數(shù)最小化，然后在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(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é)加速度極限約束條件

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

所述第一式子為

其中，上標(biāo)t表示矩陣和向量的轉(zhuǎn)置；表示目標(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)至(7)所示，考慮到上述優(yōu)化問題是在突加度層上求解，因此需將第i個(gè)冗余度機(jī)械臂的關(guān)節(jié)角度約束(3)、關(guān)節(jié)速度約束(4)、關(guān)節(jié)加速度約束(5)和關(guān)節(jié)突加度約束(6)合并，從而可以得到以下基于突加度的雙端不等式約束：

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

約束條件：cixi＝di(8)

最小化：

其中，xi表示w為單位矩陣，ci＝j(luò)i，

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

可以利用標(biāo)準(zhǔn)二次規(guī)劃求解器或者數(shù)值方法對(duì)所述標(biāo)準(zhǔn)二次規(guī)劃問題進(jìn)行求解，得到各個(gè)目標(biāo)冗余度機(jī)械臂的加速度重復(fù)運(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ī)械臂，使所述兩個(gè)以上的冗余度機(jī)械臂實(shí)現(xiàn)分布式協(xié)同運(yùn)動(dòng)。

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

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

上面主要描述了一種基于突加度的多冗余度機(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é)速度極限、關(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ī)械臂，使所述兩個(gè)以上的冗余度機(jī)械臂實(shí)現(xiàn)分布式協(xié)同運(yùn)動(dòng)。

進(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ù)所述位置、速度和加速度信息、所述參考點(diǎn)軌跡信息、所述權(quán)重矩陣和所述第二連通權(quá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á)式具體可以為：

其中

ji表示目標(biāo)冗余度機(jī)械臂i的雅可比矩陣，與分別為ji的一階與二階時(shí)間導(dǎo)數(shù)；表示目標(biāo)冗余度機(jī)械臂i的關(guān)節(jié)速度向量，與分別為的一階與二階時(shí)間導(dǎo)數(shù)；表示與目標(biāo)冗余度機(jī)械臂i之間的第一連通權(quán)值為1的機(jī)械臂的編號(hào)；aij表示目標(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＝ri-rirp為所述位置信息，ri為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的位置，rirp為預(yù)設(shè)的目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器相對(duì)于所述目標(biāo)參考點(diǎn)的距離向量；為所述速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的速度；為所述加速度信息，為目標(biāo)冗余度機(jī)械臂i的末端執(zhí)行器的加速度；rd為所述目標(biāo)參考點(diǎn)的期望位置，為所述目標(biāo)參考點(diǎn)的期望速度，為所述目標(biāo)參考點(diǎn)的期望加速度；c0＞0、c1＞0和c2＞0為控制算法收斂速度的參數(shù)，其越大則代表收斂速度越快。

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

第二確定單元，用于設(shè)置性能指標(biāo)為突加度范數(shù)最小化，在所述雅可比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限、關(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é)速度極限約束條件

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

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

所述第一式子為

其中，上標(biāo)t表示矩陣和向量的轉(zhuǎn)置；表示目標(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ù)人員可以清楚地了解到，為描述的方便和簡(jiǎn)潔，上述描述的系統(tǒng)，裝置和單元的具體工作過程，可以參考前述方法實(shí)施例中的對(duì)應(yīng)過程，在此不再贅述。

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

所述作為分離部件說明的單元可以是或者也可以不是物理上分開的，作為單元顯示的部件可以是或者也可以不是物理單元，即可以位于一個(gè)地方，或者也可以分布到多個(gè)網(wǎng)絡(luò)單元上。可以根據(jù)實(shí)際的需要選擇其中的部分或者全部單元來實(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ì)上或者說對(duì)現(xiàn)有技術(shù)做出貢獻(xiàn)的部分或者該技術(shù)方案的全部或部分可以以軟件產(chǎn)品的形式體現(xiàn)出來，該計(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-onlymemory)、隨機(jī)存取存儲(chǔ)器(ram，randomaccessmemory)、磁碟或者光盤等各種可以存儲(chǔ)程序代碼的介質(zhì)。

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