本發(fā)明涉及一種空間雙機(jī)械臂系統(tǒng)運(yùn)動(dòng)協(xié)調(diào)控制方法，屬于空間機(jī)械臂技術(shù)領(lǐng)域。

背景技術(shù)：

航天科技的發(fā)展，極大地改變了人類生活，空間技術(shù)的應(yīng)用領(lǐng)域在近一、二十年中得到了飛速的拓展，除了在空間科學(xué)應(yīng)用方面得到繼續(xù)深化外，在空間對(duì)抗、空間服務(wù)等領(lǐng)域也對(duì)空間技術(shù)提出了新的需求和任務(wù)，其中對(duì)空間目標(biāo)的在軌操作、交會(huì)對(duì)接技術(shù)由于在軍事和民用領(lǐng)域上的潛在價(jià)值，正被各國(guó)廣泛重視^[1]。通過(guò)使用空間機(jī)械臂系統(tǒng)對(duì)空間合作/非合作目標(biāo)進(jìn)行操作，包括對(duì)發(fā)生故障失效的衛(wèi)星、燃料耗盡壽命終結(jié)的衛(wèi)星等合作/非合作目標(biāo)實(shí)施在軌捕獲、元器件更換、燃料加注、助推離軌等操作可極大地延長(zhǎng)衛(wèi)星的使用壽命，大幅節(jié)省衛(wèi)星的研制成本，因而具有極大的應(yīng)用價(jià)值。

近年來(lái)，各航天大國(guó)及科研機(jī)構(gòu)，如歐洲esa、美國(guó)nasa、日本jaxa等通過(guò)使用空間機(jī)械臂技術(shù)對(duì)空間合作/非合作目標(biāo)開(kāi)展了大量的在軌驗(yàn)證項(xiàng)目。美國(guó)開(kāi)展了“鳳凰”計(jì)劃，其主要目的是通過(guò)尋求對(duì)現(xiàn)有衛(wèi)星發(fā)射方式、衛(wèi)星部署方式的突破實(shí)現(xiàn)對(duì)空間資源的有效的再利用，鳳凰計(jì)劃的服務(wù)航天器系統(tǒng)可以利用系統(tǒng)上安裝的空間機(jī)械臂裝置，實(shí)現(xiàn)對(duì)棄用衛(wèi)星的可以使用零部件(如天線)重新拆卸組裝，達(dá)到從新構(gòu)造衛(wèi)星的目標(biāo)，同時(shí)在上訴目標(biāo)完成后具有在軌轉(zhuǎn)移能力^[2]。航天大國(guó)德國(guó)在空間機(jī)械臂控制技術(shù)進(jìn)行了多年的核心技術(shù)攻關(guān)，到目前為止，德國(guó)主要的研究項(xiàng)目包括：機(jī)器人技術(shù)實(shí)驗(yàn)、試驗(yàn)服務(wù)衛(wèi)星、空間系統(tǒng)演示驗(yàn)證技術(shù)衛(wèi)星和德國(guó)在軌服務(wù)計(jì)劃。歐空局在空間機(jī)械臂應(yīng)用領(lǐng)域的研究成果代表性項(xiàng)目為jerico機(jī)械臂項(xiàng)目與歐洲機(jī)械臂項(xiàng)目。但是，應(yīng)該看到我國(guó)對(duì)空間機(jī)械臂系統(tǒng)在軌操作任務(wù)發(fā)展上仍然落后于世界其他航天大國(guó)。

按照在空間項(xiàng)目中的應(yīng)用，部署在空間的機(jī)械臂系統(tǒng)可分為空間單機(jī)械臂系統(tǒng)、空間雙機(jī)械臂系統(tǒng)及多機(jī)械臂系統(tǒng)。單臂系統(tǒng)目前研究的內(nèi)容較多，成果比較深入。與單臂系統(tǒng)相比，雙臂空間機(jī)器人的操作臂可以同時(shí)、獨(dú)立完成類似單臂空間人的各種操作，協(xié)調(diào)地執(zhí)行某一項(xiàng)共同的任務(wù),具有操作精度高、反映速度快、對(duì)復(fù)雜空間環(huán)境的適應(yīng)性強(qiáng)，能夠執(zhí)行的任務(wù)更復(fù)雜等優(yōu)點(diǎn)，因而更受各國(guó)的青睞^[3]。

由于機(jī)械臂和平臺(tái)載體之間存在著運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)耦合，會(huì)給系統(tǒng)本體姿態(tài)控制與機(jī)械臂系統(tǒng)的運(yùn)動(dòng)控制產(chǎn)生干擾，導(dǎo)致對(duì)于地面固定基座的機(jī)械臂控制技術(shù)不能直接應(yīng)用到空間機(jī)械臂系統(tǒng)上，因此必須建立空間機(jī)械臂運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型^[4]。對(duì)于雙臂空間機(jī)器人系統(tǒng)的動(dòng)力學(xué)建模問(wèn)題、軌跡規(guī)劃問(wèn)題和運(yùn)動(dòng)控制問(wèn)題，如何建立準(zhǔn)確、簡(jiǎn)潔的空間機(jī)械臂運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型并實(shí)現(xiàn)空間機(jī)械臂的高精度運(yùn)動(dòng)控制是目前理論和工程問(wèn)題研究的基礎(chǔ)和難點(diǎn)。

一、空間機(jī)械臂系統(tǒng)運(yùn)動(dòng)建模

由于難以在地面重現(xiàn)空間應(yīng)用環(huán)境，因此精確地建立空間機(jī)械臂系統(tǒng)運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型對(duì)系統(tǒng)設(shè)計(jì)、軌跡規(guī)劃和控制算法等方面的研究尤為重要，并且有利于對(duì)系統(tǒng)運(yùn)動(dòng)過(guò)程進(jìn)行更準(zhǔn)確的仿真?？臻g機(jī)械臂屬于多體系統(tǒng)，由運(yùn)動(dòng)基座和臂桿兩部分共同組成，因此，適用于其他多體系統(tǒng)的建模方法可同樣應(yīng)用于空間機(jī)械臂系統(tǒng)^[5]。

針對(duì)地面機(jī)械臂比較成熟的建模方法有牛頓-歐拉方程和拉格朗日方程等，均屬于多體系統(tǒng)的建模范疇。而空間機(jī)械臂系統(tǒng)強(qiáng)耦合、非線性、時(shí)變等特點(diǎn)使得建模問(wèn)題較地面系統(tǒng)相對(duì)復(fù)雜，為推動(dòng)空間機(jī)械臂技術(shù)的發(fā)展，各國(guó)學(xué)者紛紛針對(duì)空間機(jī)械臂的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)特性，提出了一些極具學(xué)習(xí)價(jià)值的建模方法。

hooker、margulies、roberson和wittenburg等人所提出的牛頓-歐拉法為多體系統(tǒng)的動(dòng)力學(xué)求解問(wèn)題打下了堅(jiān)實(shí)的基礎(chǔ)^[4]。他們的共同特點(diǎn)是利用樹(shù)形拓?fù)鋪?lái)描述多體系統(tǒng)的開(kāi)鏈結(jié)構(gòu)，用系統(tǒng)質(zhì)心(centerofmass,cm)表示平移自由度，引入增廣體(augmentedbodies)和增廣體質(zhì)心(barycenters)概念來(lái)簡(jiǎn)化系統(tǒng)。在ho、frisch和hooker提出的直接路徑法中，選取系統(tǒng)中的一個(gè)子系統(tǒng)作為基座，用該子系統(tǒng)上的一個(gè)點(diǎn)作為坐標(biāo)系的原點(diǎn)以表達(dá)系統(tǒng)的平移自由度，這種方法簡(jiǎn)單明了，但是存在一定的耦合^[4]。

美國(guó)學(xué)者z.vafa和s.dubowsky在假設(shè)空間機(jī)械臂系統(tǒng)不受外力或外力矩作用的情況下，提出了虛擬臂vm(virtualmanipulator)的建模方法^[6]，即當(dāng)系統(tǒng)質(zhì)心的位置不變時(shí)，描述空間機(jī)械臂系統(tǒng)的幾何結(jié)構(gòu)，vm方法的理論基礎(chǔ)為動(dòng)量守恒定理。vm是理想狀況下虛擬的一條運(yùn)動(dòng)鏈，將真實(shí)空間機(jī)械臂系統(tǒng)質(zhì)心選為該運(yùn)動(dòng)鏈的虛擬基座，由于在空間微重力環(huán)境下，質(zhì)心的位置保持不變，因此適用于地面機(jī)械臂的控制方法同樣也可以應(yīng)用到vm上。然而，該方法只能描述空間機(jī)械臂系統(tǒng)的運(yùn)動(dòng)學(xué)模型。

日本學(xué)者y.umetani和k.yoshida根據(jù)動(dòng)量守恒定理，推導(dǎo)出能夠表示空間機(jī)械臂系統(tǒng)微分運(yùn)動(dòng)學(xué)關(guān)系的廣義雅克比矩陣(generalizedjacobimatrix,gjm)^[7]。與地面機(jī)械臂系統(tǒng)不同的是，利用gjm建模是不僅需要系統(tǒng)的幾何參數(shù)，還需要各子系統(tǒng)的慣量參數(shù)，如基座和各連桿質(zhì)量、轉(zhuǎn)動(dòng)慣量等。而在實(shí)際空間應(yīng)用中，基座燃料、末端載荷等會(huì)發(fā)生不確定變化，若要實(shí)現(xiàn)精確的運(yùn)動(dòng)控制，必須實(shí)時(shí)在線地對(duì)相關(guān)參數(shù)進(jìn)行自適應(yīng)辨識(shí)，無(wú)論從實(shí)時(shí)性還是快速性方面來(lái)講，gjm方法都具有一定的局限性。

中國(guó)學(xué)者梁斌等人在z.vafa的理論基礎(chǔ)上將空間機(jī)械臂等價(jià)作一個(gè)地面機(jī)械臂，提出了動(dòng)力學(xué)等價(jià)臂dem的概念^[8]，并解釋了它與空間機(jī)械臂在運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)上的等價(jià)性。與vm不同的是，dem是真實(shí)的物理概念，可在實(shí)際中制造出來(lái)。dem具有vm的優(yōu)良性質(zhì)，可以較為準(zhǔn)確的描述系統(tǒng)的動(dòng)力學(xué)特性，適用于地面機(jī)械臂的控制方法都可通過(guò)dem方法應(yīng)用于空間機(jī)械臂，有助于進(jìn)一步對(duì)空間機(jī)械臂進(jìn)行設(shè)計(jì)、規(guī)劃和控制分析。但該方法在建模前期需要大量的計(jì)算，并且系統(tǒng)模型不直觀，因此同樣具有一定的局限性。

二、空間機(jī)械臂系統(tǒng)軌跡規(guī)劃

空間機(jī)械臂在執(zhí)行任務(wù)的過(guò)程中需要遵守一定的運(yùn)動(dòng)規(guī)律，如果沒(méi)有事先對(duì)機(jī)械臂的運(yùn)行軌跡進(jìn)行規(guī)劃，一來(lái)機(jī)械臂無(wú)法按照任務(wù)需求到達(dá)指定位置和指定的指向，另一方面還容易引起機(jī)械臂與航天器的碰撞，對(duì)航天器造成損壞。此外，運(yùn)行軌跡的合理性是保證機(jī)械臂能夠正常運(yùn)行的基礎(chǔ)，機(jī)械臂各關(guān)節(jié)存在構(gòu)型的約束，如轉(zhuǎn)角大小，轉(zhuǎn)速大小等，因此在設(shè)計(jì)控制系統(tǒng)之前必須綜合考慮以上問(wèn)題，這就需要對(duì)機(jī)械臂的運(yùn)行路徑進(jìn)行規(guī)劃。

機(jī)械臂路徑規(guī)劃的一個(gè)主要研究?jī)?nèi)容是規(guī)劃在給定的起始和目標(biāo)的位姿，選擇一條從起始點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑，使運(yùn)動(dòng)物體(機(jī)器人)能準(zhǔn)確到達(dá)目標(biāo)位姿。對(duì)于點(diǎn)到點(diǎn)的路徑規(guī)劃，s.pandey以及r.lampariello等通過(guò)對(duì)關(guān)節(jié)函數(shù)進(jìn)行參數(shù)化，再采用正運(yùn)動(dòng)學(xué)的方法規(guī)劃空間機(jī)器人笛卡爾空間點(diǎn)到點(diǎn)路徑。他們認(rèn)為多項(xiàng)式函數(shù)沒(méi)法約束關(guān)節(jié)變量的范圍，故使用正弦函數(shù)對(duì)關(guān)節(jié)變量進(jìn)行參數(shù)化，然后通過(guò)牛頓迭代法求解待定參數(shù)。由于迭代法需要對(duì)待定參數(shù)賦初值，對(duì)于空間機(jī)器人這樣的非完整系統(tǒng)，不同的初值導(dǎo)致不同的收斂結(jié)果。為此，r.lampariello等提出了一種賦初值的準(zhǔn)則，然而該準(zhǔn)則依賴于一定的條件，如規(guī)劃的總時(shí)間tf，當(dāng)條件改變了，該準(zhǔn)則需要重新建立。另外，在s.pandey和r.lampariello的方法中，沒(méi)有提出當(dāng)關(guān)節(jié)角加速度受限時(shí)的一般規(guī)劃方法。徐文福對(duì)r.lampariello等的方法進(jìn)行了改進(jìn)。首先，多項(xiàng)式函數(shù)可用于當(dāng)關(guān)節(jié)角、角速度、角加速度受限時(shí)的路徑規(guī)劃；其次，姿態(tài)誤差的表示采用四元數(shù)方法，使得在迭代過(guò)程中不會(huì)出現(xiàn)姿態(tài)奇異的問(wèn)題；再次，采用歸一化處理，可預(yù)先確定待定參數(shù)的范圍；最后，提出了一種更通用的賦初值的準(zhǔn)則，tf的改變不影響該準(zhǔn)則。

在執(zhí)行空間在軌服務(wù)任務(wù)中，根據(jù)空間機(jī)器人系統(tǒng)2種飛行狀態(tài)，將軌跡規(guī)劃分為自由飛行狀態(tài)軌跡規(guī)劃與自由飄浮狀態(tài)軌跡規(guī)劃。自由飛行狀態(tài)雙臂空間機(jī)器人系統(tǒng)基座的姿態(tài)保持不變。雙臂空間機(jī)器人系統(tǒng)不對(duì)載體的位置加以控制，只對(duì)載體的姿態(tài)進(jìn)行穩(wěn)定控制，屬于姿態(tài)受控的自由飛行機(jī)器人，因此在系統(tǒng)模型的變量中可以加以任意控制的只有機(jī)械臂的各個(gè)關(guān)節(jié)角。因此軌跡規(guī)劃的基本要求即是以期望的末端作用器或參考點(diǎn)和參考矢量的位置和姿態(tài)指向?yàn)槟繕?biāo)，來(lái)通過(guò)一定的算法確定出各個(gè)關(guān)節(jié)角的角位置、角速度和角加速度軌跡。經(jīng)過(guò)自由飛行狀態(tài)空間機(jī)械臂到達(dá)目標(biāo)抓取位姿初始點(diǎn)，為避免在抓取過(guò)程中的碰撞導(dǎo)致機(jī)械臂系統(tǒng)損壞故采用自由飄浮狀態(tài)進(jìn)行抓取。由于末端操作器已經(jīng)接近目標(biāo)，此時(shí)對(duì)于更進(jìn)一步的接近接觸目標(biāo)只需要各個(gè)關(guān)節(jié)角很小的調(diào)整，從而確定出各個(gè)關(guān)節(jié)角的角位置、角速度和角加速度軌跡。

三、空間機(jī)械臂系統(tǒng)跟蹤控制

由于空間機(jī)械臂是一種典型的不確定、非線性系統(tǒng)，同時(shí)系統(tǒng)又具有非完整特性，使得大部分在地面機(jī)械臂上取得良好應(yīng)用效果的控制方法并不適用于空間機(jī)械臂，控制問(wèn)題相對(duì)復(fù)雜。以下通過(guò)介紹幾種常用的控制算法及其在機(jī)械臂跟蹤控制中的應(yīng)用。

(1)pd控制

pd控制不依賴于空間機(jī)械臂的動(dòng)力學(xué)特性，根據(jù)系統(tǒng)位置跟蹤誤差乘以相應(yīng)的靜態(tài)增益即可確定控制量，從而使系統(tǒng)漸進(jìn)收斂，控制器結(jié)構(gòu)簡(jiǎn)單。

parlaktuna和ozkan利用dem方法將空間機(jī)械臂的控制問(wèn)題由慣性空間轉(zhuǎn)化到關(guān)節(jié)空間，在得到參數(shù)線性化的動(dòng)力學(xué)方程后，將適用于地面固定基座機(jī)械臂的控制方法應(yīng)用于空間機(jī)械臂對(duì)關(guān)節(jié)期望軌跡的跟蹤^[9]。

但是pd控制未考慮到實(shí)際系統(tǒng)中常存在的非線性因素和外界干擾，屬于一種線性控制器，且pd控制依賴于系統(tǒng)的雅克比矩陣，因此當(dāng)系統(tǒng)存在不確定因素時(shí)無(wú)法保證良好的動(dòng)態(tài)和靜態(tài)特性。而且，針對(duì)空間機(jī)械臂高精度的軌跡跟蹤控制問(wèn)題，pd控制會(huì)消耗較多的控制能量，對(duì)于空間應(yīng)用來(lái)講成本較高。

(2)自適應(yīng)控制

自適應(yīng)控制的顯著特點(diǎn)是可以實(shí)時(shí)調(diào)整參數(shù)，當(dāng)控制系統(tǒng)存在參數(shù)不確定因素時(shí)，可以用系統(tǒng)數(shù)學(xué)模型中的未知參數(shù)來(lái)描述不確定性，通過(guò)調(diào)整控制器參數(shù)來(lái)適應(yīng)參數(shù)變化，在線學(xué)習(xí)不確定參數(shù)，并根據(jù)學(xué)習(xí)值實(shí)時(shí)修正控制策略，從而達(dá)到期望的控制效果。由于空間機(jī)械臂存在參數(shù)不確定性問(wèn)題，給系統(tǒng)帶來(lái)了動(dòng)力學(xué)耦合不確定的問(wèn)題，導(dǎo)致難以精確地獲得系統(tǒng)的雅克比矩陣，并且無(wú)法將系統(tǒng)動(dòng)力學(xué)模型參數(shù)線性化，因此自適應(yīng)控制得以廣泛應(yīng)用于空間機(jī)械臂系統(tǒng)。

陳力針對(duì)2連桿空間機(jī)械臂系統(tǒng)的軌跡跟蹤控制問(wèn)題，提出了一種增廣自適應(yīng)的控制方法^[10]。gu等在了解基座基本信息的情況下提出自適應(yīng)控制方法可線性化系統(tǒng)動(dòng)力學(xué)模型中的慣性參數(shù)。taira、parlaktuna等提出了基于笛卡爾空間的自適應(yīng)控制方法，在線進(jìn)行參數(shù)自適應(yīng)估計(jì)^[11]。

陳力、劉延柱在此基礎(chǔ)上提出了一種基于參數(shù)不確定性的增廣自適應(yīng)控制方法。徐栓鋒等設(shè)計(jì)了一種自適應(yīng)擴(kuò)展雅克比零反作用控制方法，無(wú)需考慮末端執(zhí)行器的速度^[12]。

上述自適應(yīng)控制方法均能克服參數(shù)不確定因素對(duì)系統(tǒng)的影響，通過(guò)對(duì)不確定參數(shù)的在線系統(tǒng)辨識(shí)，實(shí)時(shí)地調(diào)整控制策略以適應(yīng)模型的參數(shù)不確定性。

(3)神經(jīng)網(wǎng)絡(luò)控制

神經(jīng)網(wǎng)絡(luò)控制不需要過(guò)多的控制對(duì)象模型參數(shù)信息，高度并行的結(jié)構(gòu)特點(diǎn)可以使得其具有很強(qiáng)的容錯(cuò)能力和自學(xué)能力，可以對(duì)任意的非線性函數(shù)進(jìn)行逼近，且不需要大量的計(jì)算，這些年在相關(guān)領(lǐng)域得以廣泛應(yīng)用。

sannerrm等引入神經(jīng)網(wǎng)絡(luò)控制，克制不確定因素對(duì)系統(tǒng)運(yùn)動(dòng)控制的影響^[13]。fengbm等利用徑向基神經(jīng)網(wǎng)絡(luò)逼近空間機(jī)械臂的動(dòng)力學(xué)模型，自適應(yīng)調(diào)整神經(jīng)網(wǎng)絡(luò)參數(shù)，抑制了外部干擾與參數(shù)攝動(dòng)對(duì)系統(tǒng)的影響^[14]。郭益深和陳力設(shè)計(jì)了一種徑向基神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制方法，能較好地實(shí)現(xiàn)空間機(jī)械臂系統(tǒng)對(duì)期望關(guān)節(jié)軌跡的跟蹤。謝箭在此基礎(chǔ)上設(shè)計(jì)的神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制方法可以有效解決空間機(jī)械臂存在不確定因素的問(wèn)題，不需要預(yù)先估計(jì)不確定性上界。

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

本發(fā)明的目的是為了解決目前還沒(méi)有針對(duì)于雙機(jī)械臂的空間機(jī)械臂系統(tǒng)，以及現(xiàn)有的空間機(jī)械臂系統(tǒng)未考慮到機(jī)械臂與衛(wèi)星本體間的協(xié)調(diào)關(guān)系，跟蹤誤差較高的缺點(diǎn)，而提出一種空間雙機(jī)械臂系統(tǒng)運(yùn)動(dòng)協(xié)調(diào)控制方法。

一種空間雙機(jī)械臂系統(tǒng)運(yùn)動(dòng)協(xié)調(diào)控制方法，包括：

步驟一、構(gòu)建空間雙機(jī)械臂系統(tǒng)的運(yùn)動(dòng)學(xué)方程以及動(dòng)力學(xué)方程；

步驟二、根據(jù)機(jī)械臂的初始位姿以及末端位姿，對(duì)空間雙機(jī)械臂系統(tǒng)進(jìn)行軌跡規(guī)劃；

步驟三、通過(guò)pd控制器對(duì)空間雙機(jī)械臂系統(tǒng)軌跡進(jìn)行跟蹤控制。

本發(fā)明的有益效果為：

①考慮了機(jī)械臂系統(tǒng)與衛(wèi)星本體之間的協(xié)調(diào)關(guān)系，通過(guò)實(shí)時(shí)的估計(jì)相互間的耦合干擾作用，實(shí)現(xiàn)了機(jī)械臂系統(tǒng)的高精度快速跟蹤；②所設(shè)計(jì)的控制器結(jié)構(gòu)簡(jiǎn)單，計(jì)算量小，且跟蹤速度快；③所設(shè)計(jì)的控制律與其他方法相比具有更高的控制精度和穩(wěn)定性(誤差可控制在10^-5數(shù)量級(jí))，并且控制力矩仍保持在原控制幅值內(nèi)。更加精確地實(shí)現(xiàn)機(jī)械臂末端軌跡跟蹤；④對(duì)比了自由漂浮和自由飛行兩種飛行狀態(tài)，考慮更加全面，具有實(shí)際的應(yīng)用價(jià)值；⑤與單機(jī)械臂相比，雙機(jī)械臂系統(tǒng)具有操作精度高，適應(yīng)性強(qiáng)，能夠執(zhí)行更復(fù)雜任務(wù)等優(yōu)點(diǎn)，更符合空間項(xiàng)目中的任務(wù)要求。⑥本發(fā)明的pd反饋控制器跟蹤誤差在0.01rad以內(nèi)，星臂協(xié)調(diào)控制算法的跟蹤誤差可以達(dá)到0.00001rad以內(nèi)。

附圖說(shuō)明

圖1為本發(fā)明的雙臂空間機(jī)械臂結(jié)構(gòu)模型；

圖2為本發(fā)明的空間雙機(jī)械臂系統(tǒng)協(xié)調(diào)控制結(jié)構(gòu)框圖；

圖3為本發(fā)明的自由飛行機(jī)械臂軌跡規(guī)劃的流程圖；

圖4(a)為公式(100)中f(t′)的曲線圖；

圖4(b)為公式(100)中f(t′)的一階導(dǎo)數(shù)曲線圖；

圖4(c)為公式(100)中f(t′)的二階導(dǎo)數(shù)曲線圖；

圖5為現(xiàn)有技術(shù)的機(jī)械臂軌跡規(guī)劃曲線與跟蹤曲線對(duì)比圖；其中呈上升趨勢(shì)的為基本重合的關(guān)節(jié)1軌跡曲線以及關(guān)節(jié)1規(guī)劃曲線；呈下降趨勢(shì)的為基本重合的關(guān)節(jié)2軌跡曲線以及關(guān)節(jié)2規(guī)劃曲線；

圖6為仿真實(shí)驗(yàn)中自由漂浮飛行模式下的機(jī)械臂跟蹤誤差曲線圖；其中呈先升后降趨勢(shì)的為關(guān)節(jié)1的曲線；

圖7為仿真實(shí)驗(yàn)中自由漂浮飛行模式下的機(jī)械臂控制力矩曲線圖；其中幅值變化較大的，且呈先升后降趨勢(shì)的是關(guān)節(jié)1的曲線；

圖8為仿真實(shí)驗(yàn)中自由漂浮飛行模式下的衛(wèi)星本體位姿變化曲線圖；由上至下依次為x變化曲線、q0變化曲線以及y變化曲線；

圖9為仿真實(shí)驗(yàn)中衛(wèi)星姿態(tài)受控而位置不受控時(shí)的機(jī)械臂1的跟蹤軌跡曲線圖；其中呈上升趨勢(shì)的為基本重合的關(guān)節(jié)1軌跡曲線以及關(guān)節(jié)1規(guī)劃曲線；呈下降趨勢(shì)的為關(guān)節(jié)2軌跡曲線以及關(guān)節(jié)2規(guī)劃曲線；持平的為衛(wèi)星本體的曲線；

圖10為仿真實(shí)驗(yàn)中衛(wèi)星姿態(tài)受控而位置不受控時(shí)對(duì)衛(wèi)星本體施加的控制力矩的曲線圖；

圖11為仿真實(shí)驗(yàn)中衛(wèi)星姿態(tài)受控而位置不受控時(shí)的控制力矩的曲線圖；其中幅值較大的為關(guān)節(jié)1的曲線，幅值小的為關(guān)節(jié)2的曲線；

圖12為仿真實(shí)驗(yàn)中衛(wèi)星姿態(tài)受控而位置不受控時(shí)的軌跡跟蹤誤差的曲線圖；其中呈先升后降趨勢(shì)的為關(guān)節(jié)1的曲線；

圖13為仿真實(shí)驗(yàn)中衛(wèi)星姿態(tài)受控而位置不受控時(shí)的軌跡跟蹤誤差曲線圖；

圖14為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例的機(jī)械臂1的跟蹤軌跡曲線圖；其中呈上升趨勢(shì)的為基本重合的關(guān)節(jié)1軌跡曲線以及關(guān)節(jié)1規(guī)劃曲線；呈下降趨勢(shì)的為基本重合的關(guān)節(jié)2軌跡曲線以及關(guān)節(jié)2規(guī)劃曲線；持平的為基本重合的衛(wèi)星本體規(guī)劃曲線以及衛(wèi)星本體軌跡曲線；

圖15為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例中對(duì)衛(wèi)星本體施加的控制力矩的曲線圖；

圖16為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例中對(duì)臂1是施加的控制力矩的曲線圖；其中幅值較大的為關(guān)節(jié)1的曲線；

圖17為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例的機(jī)械臂1的軌跡跟蹤誤差曲線圖；其中先上升后持平的曲線為關(guān)節(jié)1的曲線；

圖18為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例的本體基座軌跡跟蹤誤差曲線圖；

圖19為仿真實(shí)驗(yàn)中本發(fā)明實(shí)施例的臂1的軌跡跟蹤誤差的局部放大圖；其中幅值較大的為關(guān)節(jié)1的曲線。

具體實(shí)施方式

為了便于理解，在描述實(shí)施方式主要內(nèi)容之前，先介紹一下本發(fā)明中需要使用到的關(guān)鍵技術(shù)：

1.1旋轉(zhuǎn)矩陣表示法

描述兩坐標(biāo)系之間的相對(duì)姿態(tài)可以采用旋轉(zhuǎn)矩陣方法。假設(shè)坐標(biāo)系σj的三個(gè)軸向?qū)?yīng)的單位矢量在σi中的表示分別為n、o、a∈r^3×3，則從σi到σj的旋轉(zhuǎn)矩陣為：

a＝[n0a]∈r^3×3(1)

旋轉(zhuǎn)矩陣對(duì)時(shí)間的導(dǎo)數(shù)：

其中ω為σj的絕對(duì)角速度在σi中的表示。當(dāng)σi繞其x軸旋轉(zhuǎn)α角后各軸與σj平行，則其旋轉(zhuǎn)矩陣紀(jì)為rx(α)，同理繞y,z軸分別旋轉(zhuǎn)β、γ角的旋轉(zhuǎn)矩陣分別記為ry(β)、rz(γ)，則根據(jù)上述定義，可得:

其中：

cα＝cos(α),cβ＝cos(β),cγ＝cos(γ)

sα＝sin(α),sβ＝sin(β),sγ＝sin(γ)

1.2拉格朗日動(dòng)力學(xué)建模方法

拉格朗日方程是分析力學(xué)的一個(gè)重要方程，因物理學(xué)家約瑟夫·拉格朗日而命名，常用來(lái)描述各類物理機(jī)械系統(tǒng)的動(dòng)力學(xué)。拉格朗日方程可由如下所示的達(dá)朗貝爾原理(d’alembertprinciple)導(dǎo)出

其中q∈r^p為系統(tǒng)的廣義坐標(biāo)，為系統(tǒng)的廣義速度，δq為廣義坐標(biāo)q的虛位移，l∈r是系統(tǒng)的拉格朗日量，定義如下；

其中v(q)∈r是系統(tǒng)的勢(shì)能，只與系統(tǒng)的廣義坐標(biāo)有關(guān)，

是系統(tǒng)的勢(shì)能，有二次型的形式，m(q)∈r^p×p是系統(tǒng)對(duì)稱正定的慣量矩陣。由式(6)能推出如下的lagrange方程

將式(7)、(8)代入式(9)可得

其中g(shù)i(qi)代表系統(tǒng)的廣義有勢(shì)力，f∈r^p代表作用在系統(tǒng)上的外力，是系統(tǒng)的coriolis力和偏心力，具體形式如下

1.3點(diǎn)到點(diǎn)軌跡規(guī)劃

點(diǎn)到點(diǎn)的路徑規(guī)劃是針對(duì)室內(nèi)二維工作環(huán)境，其目標(biāo)是為機(jī)械臂尋找一條從起點(diǎn)到目標(biāo)點(diǎn)的能夠避開(kāi)障礙物的盡可能短的路徑。點(diǎn)到點(diǎn)的路徑規(guī)劃是一種從室內(nèi)移動(dòng)機(jī)械臂的路徑規(guī)劃技術(shù)。起始點(diǎn)到終點(diǎn)的最優(yōu)路徑策略，它要求尋找一條從起始點(diǎn)到終點(diǎn)的代價(jià)最小、路徑最短、時(shí)間最短并且合理的路徑，使自主機(jī)械臂能夠在工作空間內(nèi)順利地行走而不碰到任何障礙物。

目前，點(diǎn)到點(diǎn)的路徑規(guī)劃方法大致有以下幾種：

1.可視圖法

該方法視自主機(jī)械臂為一個(gè)質(zhì)點(diǎn)，將所有障礙物的頂點(diǎn)和機(jī)械臂起始點(diǎn)及目標(biāo)點(diǎn)用直線組合連接，要求機(jī)械臂和障礙物各頂點(diǎn)之間、目標(biāo)點(diǎn)和障礙物各頂點(diǎn)之間以及各障礙物頂點(diǎn)與頂點(diǎn)之間的連線均不能穿越障礙物，即直線是“可視的”，然后搜索從起始點(diǎn)到目標(biāo)點(diǎn)的最短距離的可視直線，尋求最優(yōu)的路徑。

2.多項(xiàng)式法

該方法的基本思路是把機(jī)械臂的運(yùn)動(dòng)假設(shè)為一條光滑的多項(xiàng)式曲線，通過(guò)機(jī)械臂在初始位置、末端位置的約束條件對(duì)多項(xiàng)式的系數(shù)進(jìn)行求解，該方法的優(yōu)點(diǎn)是計(jì)算量小，規(guī)劃曲線光滑。

3.自由空間法

該方法是把自主機(jī)械臂的工作空間分成兩部分，即自由空間和障礙物空間。用某種搜索策略在自由空間中找到一條路徑，自主機(jī)械臂沿著障礙物之間的通道中心運(yùn)動(dòng)。

4.遺傳算法

以自然遺傳機(jī)制和自然選擇等生物進(jìn)化理論為基礎(chǔ)，構(gòu)造了一類隨機(jī)化搜索算法。它利用選擇、交叉和變異等遺傳操作來(lái)優(yōu)化控制算法，不要求適應(yīng)度函數(shù)是可導(dǎo)或連續(xù)的，只要求適應(yīng)度函數(shù)為正，同時(shí)作為并行算法適用于全局搜索。

5.其他方法

近年來(lái)，人工智能方法正在成為研究的熱點(diǎn)，神經(jīng)網(wǎng)絡(luò)在機(jī)械臂路徑規(guī)劃中的應(yīng)用得到了廣泛的重視。一些神經(jīng)網(wǎng)絡(luò)模型通過(guò)學(xué)習(xí)可生成實(shí)時(shí)的路徑[12]。

1.4pd軌跡控制方法

具有比例/微分控制規(guī)律的控制器，稱為pd控制器，其輸出m(t)與輸入e(t)的關(guān)系如下式所示

式中，kp為比例系數(shù)；τ為微分時(shí)間常數(shù)。kp與τ都是可調(diào)的參數(shù)。

偏差的比例(p)、積分(i)和微分(d)進(jìn)行控制的pid控制器(亦稱pid調(diào)節(jié)器)是應(yīng)用最為廣泛的一種自動(dòng)控制器。它具有原理簡(jiǎn)單，易于實(shí)現(xiàn)，適用面廣，控制參數(shù)相互獨(dú)立，參數(shù)的選定比較簡(jiǎn)單等優(yōu)點(diǎn)。而在實(shí)際應(yīng)用中，pd調(diào)節(jié)器是一種理想調(diào)節(jié)器,是工業(yè)中最常用的一種調(diào)節(jié)方法。在90％以上的工業(yè)控制系統(tǒng)中采用了傳統(tǒng)的pd控制策略,這是因?yàn)閜d控制策略有以下優(yōu)點(diǎn)：(1)技術(shù)成熟；(2)結(jié)構(gòu)簡(jiǎn)單，在線控制實(shí)時(shí)性好；(3)不依賴精確的數(shù)學(xué)模型；(4)軟件系統(tǒng)靈活易修改完善，控制效果令人滿意。

2.1本發(fā)明所涉及的空間機(jī)器人主要由衛(wèi)星基座和若干個(gè)機(jī)械臂組成，本文以雙臂空間機(jī)械臂作為研究對(duì)象，其一般空間結(jié)構(gòu)如圖1所示，機(jī)械臂包括自由度為n1和n2的串聯(lián)機(jī)械臂arm-1和arm-2。自由度的個(gè)數(shù)意味著每個(gè)機(jī)械臂上關(guān)節(jié)的個(gè)數(shù)。

為統(tǒng)一標(biāo)準(zhǔn)和方便討論，對(duì)坐標(biāo)系、基本變量及符號(hào)進(jìn)行如下定義：

k＝1或2，當(dāng)k＝1時(shí)，i＝1,...,n1；當(dāng)k＝2時(shí)，i＝1,...,n2(若無(wú)特別指出，各矢量均在慣性系中表示)：

σi、σe1、σe2:分別表示慣性系、機(jī)械臂arm-1和arm-2末端坐標(biāo)系；

σb：基座的幾何坐標(biāo)系；

σh1、σh2：為位于目標(biāo)航天器上供arm-1和arm-2捕獲的目標(biāo)坐標(biāo)系；

σi、bi：固連坐標(biāo)系，zi正向?yàn)閖i的旋轉(zhuǎn)方向；

b0:剛體0，即航天器平臺(tái)，或稱衛(wèi)星本體；

bi^k(i＝1,......,n)：arm-k的第i個(gè)連桿；

ji^k:arm-k連接bi-1和bi的關(guān)節(jié)；

ci^k:bi^k的質(zhì)心；

oi:慣性系原點(diǎn)；

og：整個(gè)系統(tǒng)的質(zhì)心；

ⁱaj∈r^3×3:σj相對(duì)于σi的姿態(tài)變換矩陣，當(dāng)i缺省時(shí)，表示σj相對(duì)于慣性系的姿態(tài)變換矩陣，ⁱaj＝[ⁱnj,ⁱoj,ⁱaj]；

ⁱtj∈r^4×4：σj相對(duì)于σi的齊次變換矩陣，當(dāng)i缺省時(shí)，表示σj相對(duì)于慣性系的齊次變換矩陣，ⁱtj＝[ⁱnj,ⁱoj,ⁱaj,ⁱdj]；

ⁱnj,ⁱoj,ⁱaj∈r³：分別為σj各軸的單位矢量，在σi中的表示；

ⁱdj∈r³：為σj原點(diǎn)在σi的位置矢量；

en：n維單位矩陣；

on×m：n×m維零矩陣；

τ∈rⁿ：關(guān)節(jié)驅(qū)動(dòng)力矩；

mi^k、m：為bi^k的質(zhì)量，

i0,ii^k∈r^3×3(k＝a,b):分別表示b0和bi^k繞各自質(zhì)心的轉(zhuǎn)動(dòng)慣量矩陣；

ki^k∈r³：表示ji^k的旋轉(zhuǎn)方向的單位矢量；

ri^k∈r³：表示連桿質(zhì)心ci^k的位置矢量；

rg∈r³:系統(tǒng)質(zhì)心的位置矢量；

ro∈r³：表示系統(tǒng)基座質(zhì)心的位置矢量；

pi^k∈r³:ji^k的位置矢量；

pe^k∈r³：機(jī)械臂k的末端的位置矢量；

ai^k、bi^k∈r³：分別為從ji^k指向ci^k，ci^k指向ji+1^k的位置矢量；

li^k∈r³：表示由ji^k到j(luò)i^k+1的位置矢量，li＝ai+bi；

^krij：表示從σi原點(diǎn)指向σj原點(diǎn)的矢量，在σk中的表示，如果σi或者σk為慣性系，則可以省去相應(yīng)的符號(hào)i或者k；

^jvi,^jωi：分別表示σi相對(duì)于σj的線速度和角速度，如果σi或者σj為慣可以省去相應(yīng)的符號(hào)i或者j；

機(jī)械臂arm-k關(guān)節(jié)角向量，即θ^k＝[θ1^k,......,θn^k]^t；

期望的機(jī)械臂arm-k關(guān)節(jié)角向量，即θd^k＝[θd1^k,.....θdn^k]^t；

qb∈r³：基座的姿態(tài)向量，即qb＝[qb1qb2qb3]^t；

q∈rⁿ⁺³:空間機(jī)械臂系統(tǒng)關(guān)節(jié)角向量，由機(jī)械臂關(guān)節(jié)角向量和基座姿態(tài)向量組成，即q＝[qb^t,θ^t]^t＝[qb1,qb2,qb3,θ1,......,θn]^t；

v0、ω0∈r³：基座的線速度和角速度；

vi、ωi∈r³:第i個(gè)剛體bi的線速度和角速度；

ve^k、ωe^k∈r³：機(jī)械臂末端的線速度和角速度；

ved、ωed∈r³:機(jī)械臂末端的線速度和角速度的期望值；

ψb∈r³：基座姿態(tài)角，按z-y-x歐拉角表示；

ψe^k∈r³：機(jī)械臂arm-k末端姿態(tài)角，按z-y-x歐拉角表示；

q0∈r⁴:基座姿態(tài)四元數(shù)；

qe^k∈r⁴:為機(jī)械臂arm-k末端姿態(tài)的四元數(shù)表示。

對(duì)于矢量的表示，做如下約定：ⁱv為矢量在σi中的表示，而不加左上標(biāo)的v為矢量在慣性系中的表示。并定義如下操作數(shù)(叉乘操作數(shù)；)若ν＝[x,y,z]^t，則

具體實(shí)施方式一：在空間失重環(huán)境下，空間機(jī)械臂的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)與地面機(jī)械臂存在較大的差別，因此有必要從力學(xué)原理重新建立空間機(jī)械臂的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程。本專利分別利用空間雙機(jī)械臂系統(tǒng)的動(dòng)量守恒、拉格朗日方程推導(dǎo)空間雙機(jī)械臂系統(tǒng)在自由飛行狀態(tài)下的運(yùn)動(dòng)學(xué)方程和動(dòng)力學(xué)方程。其次，以空間雙機(jī)械臂系統(tǒng)抓取相對(duì)靜止物體為目標(biāo)，研究空間機(jī)械臂系統(tǒng)的軌跡規(guī)劃問(wèn)題。通過(guò)機(jī)械臂末端運(yùn)動(dòng)的點(diǎn)到點(diǎn)軌跡規(guī)劃，利用五次多項(xiàng)式對(duì)機(jī)械臂的運(yùn)動(dòng)軌跡進(jìn)行逼近，通過(guò)約束條件對(duì)五次多項(xiàng)式的系數(shù)進(jìn)行化簡(jiǎn)，利用牛頓迭代法對(duì)系數(shù)進(jìn)行迭代求解。最后研究空間雙機(jī)械臂系統(tǒng)的軌跡跟蹤控制問(wèn)題，通過(guò)設(shè)計(jì)星臂協(xié)調(diào)pd反饋控制器實(shí)現(xiàn)空間機(jī)械臂系統(tǒng)對(duì)期望運(yùn)動(dòng)軌跡的高精度快速跟蹤。圖2為本發(fā)明的空間雙機(jī)械臂系統(tǒng)協(xié)調(diào)控制結(jié)構(gòu)框圖。

本發(fā)明的空間雙機(jī)械臂系統(tǒng)運(yùn)動(dòng)協(xié)調(diào)控制方法，包括如下步驟：

步驟一、構(gòu)建空間雙機(jī)械臂系統(tǒng)的運(yùn)動(dòng)學(xué)方程以及動(dòng)力學(xué)方程。

在自由飛行模式時(shí)，基座的姿態(tài)受到控制，且保持恒定，但位置是自由的。假設(shè)施加于系統(tǒng)的外力為0，則系統(tǒng)的線動(dòng)量守恒，如果初始時(shí)刻線動(dòng)量為零，則可有如下的完整性約束方程：

p＝0(14)

由式(19)可知：

由于基座姿態(tài)受控，基座的角速度很小，近似為零，即：

ω≈0(16)

將式(21)代入式(20)，系統(tǒng)的線動(dòng)量守恒方程可化簡(jiǎn)為：

根據(jù)式(22)，可解出基座的線速度為：

利用拉格朗日方法可以建立空間雙機(jī)械臂系統(tǒng)在自由飛行狀態(tài)下的動(dòng)力學(xué)方程如下：

其中m(q)為機(jī)械臂的廣義慣性張量，為機(jī)械臂與基座相關(guān)的非線性力矩，τ為控制力矩。

步驟二、根據(jù)機(jī)械臂的初始位姿以及末端位姿，對(duì)空間雙機(jī)械臂系統(tǒng)進(jìn)行軌跡規(guī)劃。具體為：

(1)點(diǎn)到點(diǎn)路徑規(guī)劃

本部分需要的解決的問(wèn)題是：給出機(jī)械臂的初始位姿，末端的位姿，規(guī)劃一條運(yùn)動(dòng)路徑使之從初始位姿運(yùn)動(dòng)到末端位姿，即為點(diǎn)到點(diǎn)路徑規(guī)劃問(wèn)題，此處采用五次多項(xiàng)式對(duì)運(yùn)動(dòng)路徑進(jìn)行逼近。以下給出點(diǎn)到點(diǎn)路徑規(guī)劃的數(shù)學(xué)描述，五次多項(xiàng)式逼近的基本形式，最后將問(wèn)題轉(zhuǎn)化為求解非線性方程組的問(wèn)題，并給出利用牛頓迭代法的求解步驟。

空間機(jī)械臂末端的初始狀態(tài)、最終狀態(tài)以及期望狀態(tài)分別記為和則點(diǎn)到點(diǎn)的路徑規(guī)劃可以描述為：

規(guī)劃關(guān)節(jié)運(yùn)動(dòng)：

使得：

其中，分別為第k條機(jī)械臂的關(guān)節(jié)角、角速度和角加速度；和分別為的下限值和上限值，和分別為的下限值和上限值、和分別為的下限值和上限值；t0為軌跡規(guī)劃的初始時(shí)刻，tf為軌跡規(guī)劃的末端時(shí)刻。

使用五次多項(xiàng)式對(duì)關(guān)節(jié)函數(shù)進(jìn)行參數(shù)化：

令則路徑規(guī)劃問(wèn)題轉(zhuǎn)化成了求解下述非線性方程組的問(wèn)題：

f(c)＝0(25)

(2)空間機(jī)械臂系統(tǒng)規(guī)劃求解

采用迭代法對(duì)自由飛行狀態(tài)參數(shù)辨識(shí)問(wèn)題求解，算法步驟如下：

1)初始化各變量：

k＝1，ck＝c0，ε＝10^-6，xe0，xed，tf

2)求取xef，若滿足||f(ck)||≤ε跳轉(zhuǎn)第4步，否則

△ck＝-(f′(ck))^-1f(ck)(27)

ck+1＝△ck+ck(28)

3)循環(huán)加一，跳轉(zhuǎn)第二步

4)輸出參數(shù)

完整流程圖如圖3所示。

步驟三、通過(guò)pd控制器對(duì)空間雙機(jī)械臂系統(tǒng)軌跡進(jìn)行跟蹤控制。

考慮到自由飛行情況下，對(duì)本體和機(jī)械臂執(zhí)行完全一致的pd控制算法具有控制精度低、收斂速度慢等缺陷，這里設(shè)計(jì)一種星臂協(xié)調(diào)的控制算法。其基本思想為：對(duì)于衛(wèi)星本體，估計(jì)機(jī)械臂運(yùn)動(dòng)對(duì)本體的影響，并實(shí)時(shí)前饋補(bǔ)償至自身的pd控制器中，對(duì)于機(jī)械臂系統(tǒng)，估計(jì)衛(wèi)星本體運(yùn)動(dòng)對(duì)機(jī)械臂的影響，并將其前饋補(bǔ)償至pd控制器中。

基于以上思想，首先將自由飛行星臂系統(tǒng)動(dòng)力學(xué)方程拆分成如下形式

其中

將m矩陣分解為4個(gè)分塊子矩陣m11、m12、m21、m22，將矩陣也分解為4個(gè)分塊子矩陣b11、b12、b21、b22，則有

其中

其中

針對(duì)以上模型，設(shè)計(jì)空間本體和機(jī)械臂運(yùn)動(dòng)控制規(guī)律為：

其中qd,θd分別表示衛(wèi)星本體、機(jī)械臂的待跟蹤信號(hào)，即軌跡規(guī)劃中所得到的信號(hào)，em＝θd-θ為機(jī)械臂的跟蹤誤差，eb＝qpd-qb為衛(wèi)星平臺(tái)的跟蹤誤差。所得到的控制律可以使得pd控制器對(duì)空間雙機(jī)械臂系統(tǒng)軌跡進(jìn)行跟蹤控制。

下面針對(duì)本實(shí)施方式的三個(gè)步驟具體說(shuō)明其原理與推導(dǎo)過(guò)程：

空間雙機(jī)械臂系統(tǒng)運(yùn)動(dòng)建模過(guò)程：

為了使研究更加具有通用性，在研究時(shí)將對(duì)象a擴(kuò)展為n臂空間機(jī)械臂系統(tǒng)，設(shè)定n臂機(jī)械臂系統(tǒng)的第k條臂(k＝1,….,n)的關(guān)節(jié)自由度為nk，本項(xiàng)目的研究對(duì)象是雙臂空間機(jī)器系統(tǒng)，因此有k＝1，2。

由可知，多臂空間機(jī)械臂系統(tǒng)中第i個(gè)手臂的各剛體質(zhì)心及機(jī)械臂末端的位置矢量為：

各桿件及機(jī)械臂末端的固連坐標(biāo)系，相對(duì)于慣性系的姿態(tài)可表示為：

ai^k＝a0⁰a1^k1a2^k...^i-1ai^k(36)

在空間機(jī)械臂系統(tǒng)中，整個(gè)系統(tǒng)的質(zhì)心位置矢量與各連桿的質(zhì)心位置矢量之間存在如下關(guān)系：

其中，系統(tǒng)質(zhì)心與慣性系間的相對(duì)關(guān)系固定。根據(jù)式(32)和(36)，可得出基座質(zhì)心位置為：

由對(duì)位置矢量關(guān)系(32)和式(33)進(jìn)行微分可得各剛體質(zhì)心以及末端的線速度為：

另一方面，各剛體質(zhì)心及末端的角速度可表示為：

將式(39)和(41)寫(xiě)成矩陣的形式，有：

式中jb^k——機(jī)械臂末端速度與基座速度相關(guān)雅克比矩陣；

jm^k——機(jī)械臂末端速度與機(jī)械臂關(guān)節(jié)速度相關(guān)的的雅克比矩陣。

jb^k和jm^k可按照下列式子進(jìn)行計(jì)算:

p0e^k——基座質(zhì)心到機(jī)械臂末端的位置矢量，即：

式(42)即建立了多臂空間機(jī)械臂系統(tǒng)中arm-k，下同末端的一般運(yùn)動(dòng)學(xué)方程，即對(duì)于arm-1和arm-2分別有：

將(46)和式(47)組合在一起可得:

令：

式(48)可表示為：

式(46)和式(47)或式(53)即建立了雙臂空間機(jī)械臂系統(tǒng)末端的一般運(yùn)動(dòng)學(xué)方程。為了更好得到特定情況：自由飛行模式下的運(yùn)動(dòng)學(xué)方程，我們需要分析系統(tǒng)的線動(dòng)量和角動(dòng)量。

(1)線動(dòng)量方程：

空間機(jī)械臂系統(tǒng)總的線動(dòng)量為各連桿的線動(dòng)量之和，即：

將式(328)代入(54)，有：

其中：

r0g＝r0-rg(58)

(2)角動(dòng)量方程

多臂空間機(jī)械臂系統(tǒng)總的角動(dòng)量為各連桿的角動(dòng)量之和，即：

將式(38)和式(40)代入式(59)，有：

其中：

jri^k＝(k1^k,k2^k,...,ki^k,0,...,0)(65)

r0i^k＝ri^k-r0(66)

另一方面，將位置矢量關(guān)系(64)代入式(59)，進(jìn)一步整理，有：

令：

式(65)可表示為：

l＝l0+r0×p(6(6

式中l(wèi)0——空間機(jī)械臂系統(tǒng)相對(duì)于基座質(zhì)心的動(dòng)量矩；

r0——基座質(zhì)心的位置矢量；

p——系統(tǒng)的線動(dòng)量；

將和(38)式(40)代入式(66)，可計(jì)算l0得：

其中：

(3)空間機(jī)械臂系統(tǒng)線動(dòng)量和角動(dòng)量公式組合

由上述分析可知，式(55)和式(60)分別為空間機(jī)械臂系統(tǒng)的總線動(dòng)量和總角動(dòng)量的計(jì)算公式。將線動(dòng)量和角動(dòng)量計(jì)算公式(55)、(60)組合，可得空間機(jī)械臂系統(tǒng)的動(dòng)量方程為：

式(71)可寫(xiě)成如下矩陣形式：

其中：

空間雙機(jī)械臂系統(tǒng)軌跡規(guī)劃原理分析：

為實(shí)現(xiàn)機(jī)械臂從初始位姿運(yùn)動(dòng)到待抓捕物體位置，需要對(duì)機(jī)械臂的運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃。為此，實(shí)現(xiàn)需要對(duì)機(jī)械臂末端進(jìn)行點(diǎn)到點(diǎn)的路徑規(guī)劃。

空間機(jī)械臂末端的初始狀態(tài)、最終狀態(tài)以及期望狀態(tài)分別記為和則點(diǎn)到點(diǎn)的路徑規(guī)劃可以描述為：

規(guī)劃關(guān)節(jié)運(yùn)動(dòng)：

使得：

根據(jù)式(42)和(43)可知：

因?yàn)樽藨B(tài)受控，姿態(tài)角變化很小，所以近似認(rèn)為

末端位置可表示為：

tf時(shí)刻的末端姿態(tài)與期望姿態(tài)之差為：

tf時(shí)刻的末端位置與期望位置之差為：

使用五次多項(xiàng)式對(duì)關(guān)節(jié)函數(shù)進(jìn)行參數(shù)化：

由(77)可知：

及

經(jīng)整理可得：

由此可知，在給定tf的情況下，參數(shù)化后每個(gè)關(guān)節(jié)函數(shù)中僅含有一個(gè)參數(shù)定義待定參數(shù)

令則路徑規(guī)劃問(wèn)題轉(zhuǎn)化成了求解下述非線性方程組的問(wèn)題：

f(c)＝0(92)

下面給出關(guān)節(jié)函數(shù)的參數(shù)歸一化處理過(guò)程。

令t′＝t/tf，t∈(0，tf)，t′∈(0,1)，則有：

其中

于是有：

由此可得關(guān)于參數(shù)c′^k的非線性方程組:

因而，點(diǎn)到點(diǎn)路徑規(guī)劃的問(wèn)題轉(zhuǎn)化為求解上述非線性方程組的問(wèn)題。經(jīng)過(guò)歸一化處理后，根據(jù)關(guān)節(jié)角的限制范圍，可以預(yù)先確定參數(shù)c′^k的范圍。

定義函數(shù)：

則f(t′)的曲線以及該函數(shù)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)如圖4所示。

從上圖中可知，f(τ)在(0,1)上單調(diào)遞增，且其函數(shù)值、一階和二階微分的范圍為：

根據(jù)式(100)、以及可得滿足關(guān)節(jié)角限制的參數(shù)c^k的范圍為:

不等式(102)可寫(xiě)成矢量形式：

待定參數(shù)c^k的初值和收斂值必須位于此范圍內(nèi)。若迭代算法收斂是的c^k超出此范圍，則需要重新賦初值并重解非線性方程組。上述過(guò)程一直重復(fù)，直到c^k收斂到上述范圍內(nèi)或者方程組求解次數(shù)超過(guò)一定值(此時(shí)認(rèn)為不存在符合要求的參數(shù))。當(dāng)c^k收斂到符合要求的范圍時(shí)，根據(jù)式(100)、(101)，可按下列不等式確定滿足關(guān)節(jié)角速度、角加速度范圍的規(guī)劃時(shí)間tf：

空間雙機(jī)械臂系統(tǒng)軌跡跟蹤控制穩(wěn)定性證明：

我們通過(guò)lyapunov理論證明以上所提出的星臂協(xié)調(diào)控制算法的穩(wěn)定性，為此，我們首先需要用到機(jī)械臂方程(29)中m矩陣的如下性質(zhì)：

性質(zhì)2：具有有界性，即

首先，我們構(gòu)造如下形式的lyapunov函數(shù)

其中

將控制器(32)、(33)綜合，可以得到

其中

將控制器(109)帶入動(dòng)力學(xué)方程可得

則對(duì)lyapunov函數(shù)(108)求導(dǎo)可得

將誤差動(dòng)力學(xué)方程(108)帶入上式可得

利用性質(zhì)2可得

因此，通過(guò)選取參數(shù)使得：即可得到

同理，利用laselle定理可以得到系統(tǒng)的全局漸近穩(wěn)定性。

具體實(shí)施方式二：本實(shí)施方式與具體實(shí)施方式一不同的是：

步驟一具體包括：

步驟一一、根據(jù)如下公式建立空間雙機(jī)械臂系統(tǒng)的運(yùn)動(dòng)學(xué)方程：

其中，k表示第k個(gè)機(jī)械臂，且k為1或2；

為第k個(gè)機(jī)械臂第i個(gè)連桿的質(zhì)量；ki^k表示ji^k的旋轉(zhuǎn)方向的單位矢量，ji^k為第k個(gè)機(jī)械臂中連接第i-1個(gè)連桿與第i個(gè)連桿的關(guān)節(jié)；pi^k表示關(guān)節(jié)ji^k的位置矢量；θ^k為第k個(gè)機(jī)械臂的關(guān)節(jié)角向量；

步驟一二、根據(jù)如下公式建立空間雙機(jī)械臂系統(tǒng)的動(dòng)力學(xué)方程：

其中m(q)為機(jī)械臂的廣義慣性張量，為機(jī)械臂與基座相關(guān)的非線性力矩，τ為控制力矩。

其它步驟及參數(shù)與具體實(shí)施方式一相同。

具體實(shí)施方式三：本實(shí)施方式與具體實(shí)施方式一或二不同的是：

步驟二具體包括：

步驟二一、獲取空間機(jī)械臂末端的初始狀態(tài)最終狀態(tài)和期望狀態(tài)

步驟二二、假設(shè)存在t，使得：

其中，分別為第k條機(jī)械臂的關(guān)節(jié)角、角速度和角加速度；和分別為的下限值和上限值，和分別為的下限值和上限值、和分別為的下限值和上限值；t0為軌跡規(guī)劃的初始時(shí)刻，tf為軌跡規(guī)劃的末端時(shí)刻；

使用如下的五次多項(xiàng)式對(duì)關(guān)節(jié)函數(shù)進(jìn)行參數(shù)化處理：

為待求解的參數(shù)；

步驟二三、令求方程組f(c)＝0的解；其中：

其它步驟及參數(shù)與具體實(shí)施方式一或二相同。

具體實(shí)施方式四：本實(shí)施方式與具體實(shí)施方式一至三之一不同的是：

步驟二三中，使用牛頓迭代法求解方程組，具體為：

步驟二三一、對(duì)如下變量進(jìn)行初始化：迭代次數(shù)k、ck、極限中的極小正數(shù)ε、以及tf；

步驟二三二、對(duì)運(yùn)動(dòng)學(xué)方程進(jìn)行數(shù)值積分，得到末端位姿

步驟二三三、判斷是否滿足||f(ak)||≤ε；

若滿足，則輸出參數(shù)

若不滿足則進(jìn)行如下計(jì)算：

△ak＝-(f′(ak))^-1f(ak)

ak+1＝△ak+ak

并將k值加1，跳轉(zhuǎn)至步驟二三二。

其它步驟及參數(shù)與具體實(shí)施方式一至三之一相同。

具體實(shí)施方式五：本實(shí)施方式與具體實(shí)施方式一至四之一不同的是：

步驟三具體包括：

步驟三一、將步驟一二中的公式拆分成如下形式：

步驟三二、將步驟三一中的矩陣m(q)拆分成4個(gè)分塊子矩陣m11、m12、m21、m22，將矩陣也分解為4個(gè)分塊子矩陣b11、b12、b21、b22，則有

其中

其中

步驟三三、得出空間衛(wèi)星本體和機(jī)械臂的控制律為：

其中qd,θd分別表示衛(wèi)星本體、機(jī)械臂的待跟蹤信號(hào)，即軌跡規(guī)劃中所得到的信號(hào)，em＝θd-θ為機(jī)械臂的跟蹤誤差，eb＝qpd-qb為衛(wèi)星平臺(tái)的跟蹤誤差。

其它步驟及參數(shù)與具體實(shí)施方式一至四之一相同。

<實(shí)施例>：

現(xiàn)有技術(shù)的方案內(nèi)容及仿真效果：

機(jī)械臂的飛行模式有三種：基座固定，自由漂浮，自由飛行。此處首先通過(guò)自由漂浮下的機(jī)械臂運(yùn)動(dòng)過(guò)程進(jìn)行建模及控制，并得到相關(guān)的仿真曲線，以便于自由飛行下的協(xié)調(diào)控制方案進(jìn)行比較。由于第二類拉格朗日方程常用來(lái)描述物理機(jī)械系統(tǒng)的動(dòng)力學(xué)，因此由第二類拉格朗日方程，可以得到載體姿態(tài)、位置均不受控時(shí)的平面雙臂空間機(jī)械臂的系統(tǒng)動(dòng)力學(xué)方程：

其中:m(q)∈r^5×5為系統(tǒng)質(zhì)量矩陣；為包含科氏力、離心力的五階列陣；τ＝[0τ1τ2τ3τ4]^t∈r⁵為由系統(tǒng)施加于關(guān)節(jié)的控制力矩組成的五階列陣；q＝[qbqr^t]^t為系統(tǒng)的廣義坐標(biāo)，且qb＝θ0為本體的姿態(tài)角，qr＝[θ1θ2θ3θ4]^t為關(guān)節(jié)的轉(zhuǎn)動(dòng)角。

基于上一部分所設(shè)計(jì)的pd控制器對(duì)平面雙臂空間機(jī)械臂進(jìn)行軌跡跟蹤控制仿真。

由圖5至圖8的曲線可以得出pd控制器可以使機(jī)械臂跟蹤軌跡規(guī)劃得出的目標(biāo)軌線，且跟蹤誤差在10^-3量級(jí)。值得注意的是，由于自由漂浮飛行模式下，衛(wèi)星本體不受控，在機(jī)械臂的跟蹤過(guò)程中，衛(wèi)星本體的位姿均受到了影響。平臺(tái)的運(yùn)動(dòng)漂移在0.1m以內(nèi)，機(jī)械臂的控制力矩在0.1nm以內(nèi)。仿真結(jié)果表明，機(jī)械臂的角度跟蹤誤差在10^-2量級(jí)以內(nèi)，機(jī)械臂從平臺(tái)跟蹤運(yùn)動(dòng)到目標(biāo)的時(shí)間為30s。

本發(fā)明的仿真效果：

為了保持同數(shù)據(jù)中繼衛(wèi)星的通訊，并且保持太陽(yáng)能電池帆板對(duì)日定向，裝載機(jī)械臂的衛(wèi)星自身的姿態(tài)必須受到控制，因此，自由漂浮狀態(tài)只能在短時(shí)間或小范圍內(nèi)實(shí)現(xiàn)，大部分時(shí)間內(nèi)，空間機(jī)器人都處于自由飛行狀態(tài)下，那么，針對(duì)不同的任務(wù)要求，保持一定精度的衛(wèi)星(即基座)的姿態(tài)控制是必需的。

因?yàn)槲覀円獙?duì)平面雙臂空間機(jī)械臂進(jìn)行仿真分析，所以將5.1節(jié)中的運(yùn)動(dòng)學(xué)方程代入能量守恒中，由第二類拉格朗日方程，可以得到載體姿態(tài)受控，位置不受控時(shí)的平面雙臂空間機(jī)械臂的系統(tǒng)動(dòng)力學(xué)方程：

其中：m(q)∈r^5×5為系統(tǒng)質(zhì)量矩陣；為包含科氏力、離心力的五階列陣；τ＝[τ0τ1τ2τ3τ4]^t∈r⁵為由系統(tǒng)施加于機(jī)械臂基座和關(guān)節(jié)的控制力矩組成的五階列陣；q＝[qbqr^t]^t為系統(tǒng)的廣義坐標(biāo)，且qb＝θ0為本體姿態(tài)角，qr＝[θ1θ2θ3θ4]^t為關(guān)節(jié)轉(zhuǎn)動(dòng)角。

我們基于具體實(shí)施方式一或四中的pd控制器對(duì)平面雙臂空間機(jī)械臂進(jìn)行軌跡跟蹤控制仿真。

仿真結(jié)果如圖9至圖13所示。

對(duì)比空間機(jī)械臂自由漂浮和自由飛行的仿真結(jié)果，我們可以看出由于自由飛行狀體下需要對(duì)本體基座施加控制力矩，且本體基座質(zhì)量很大。所以仿真結(jié)果中的基座力矩τ0的幅值明顯大于關(guān)節(jié)力矩，而自由漂浮狀態(tài)下基座不控，所以所需要的控制能量明顯小于自由飛行狀態(tài)。

由于自由飛行姿態(tài)可控，所以基座的運(yùn)動(dòng)軌跡可以逐漸收斂于期望軌跡，即誤差趨近于0。而自由漂浮狀態(tài)不可控，所以基座的軌跡曲線逐漸發(fā)散，誤差不收斂。

從誤差仿真曲線中可以看出，雙臂空間機(jī)械臂的系統(tǒng)通過(guò)pd控制可以很好的跟蹤上期望的運(yùn)動(dòng)軌跡，使誤差可以穩(wěn)定在10-³的范圍內(nèi)，實(shí)現(xiàn)穩(wěn)定控制，滿足控制要求。

仿真結(jié)果表明，機(jī)械臂的角度跟蹤誤差在10^-3量級(jí)以內(nèi)，機(jī)械臂從平臺(tái)跟蹤運(yùn)動(dòng)到目標(biāo)的時(shí)間為30s。

本發(fā)明使用的仿真參數(shù)：由圖1可知：

本體質(zhì)心位置坐標(biāo)為：

x0＝xc+b01cosθ0+b02cos(θ1-θ0)+b03cos(θ1+θ2-θ0)-b04cos(θ0+θ3)-

b05cos(θ0+θ3+θ4)(117)

y0＝y(tǒng)c+b01sinθ0-b02sin(θ1-θ0)-b03sin(θ1+θ2-θ0)-b04sin(θ0+θ3)-

b05sin(θ0+θ3+θ4)(118)

其中xc和yc為系統(tǒng)質(zhì)心的位置坐標(biāo)，且：

b01＝l0(m1+m2-m3-m4)/m,b02＝(m2l1+m1a1)/m

b03＝m2a2/m,b04＝(m4l3+m3a3)/m,b05＝(m4a4)/m

對(duì)式(117)，(118)，求時(shí)間t的導(dǎo)數(shù)，得到本體質(zhì)心速度表達(dá)式：

各分體質(zhì)心的速度表達(dá)式:

式中：

b11＝l0(m1+2m3+2m4)/m

b12＝[(m0+m2+m3+m4)a1-m2l1]/m

b13＝m2a2/m，b14＝(m4l3+m3a3)/m,b15＝(m4a4)/m

b21＝l0(m1+2m3+2m4)/m

b22＝[(m0+m1+m3+m4)l1-m1a1]/m

b23＝(m0+m1+m3+m4)a2/m

b24＝(m4l3+m3a3)/m,b25＝(m4a4)/m

b31＝l0(m0+2m1+2m2)/m

b32＝(m2l1+m1a1)/m,b33＝m2a2/m

b14＝[-m4l3+(m0+m1+m2+m4)a3]/m,b35＝(m4a4)/m

b41＝l0(m0+2m1+2m2)/m

b42＝(m2l1+m1a1)/m,b43＝m2a2/m

b44＝[-m4a3+(m0+m1+m2+m3)l3]/m

b45＝(m0+m1+m2+m3)a4/m

表1兩自由度平面空間機(jī)械臂的標(biāo)稱參數(shù)

仿真所使用的控制器設(shè)計(jì)參數(shù)：

期望關(guān)節(jié)角軌跡是用五次多項(xiàng)式插值得到的，期望關(guān)節(jié)角起始狀態(tài)為終止?fàn)顟B(tài)實(shí)際的關(guān)節(jié)角初始位置為與初始期望位置有一定的偏差?；教炱鞒跏甲藨B(tài)角與期望值均設(shè)為0。

跟蹤控制器中的kp＝10i5,kd＝10i5。

仿真分析：

由以上仿真結(jié)果可以看出，機(jī)械臂的跟蹤誤差無(wú)法收斂，呈震蕩狀態(tài)，且衛(wèi)星本體的跟蹤誤差數(shù)值也較大，基于此，通過(guò)機(jī)械臂與衛(wèi)星本體分開(kāi)進(jìn)行協(xié)調(diào)控制的方式設(shè)計(jì)一種前饋補(bǔ)償?shù)膒d控制器來(lái)使系統(tǒng)的控制精度進(jìn)一步提高，且控制力矩仍保持在原最大控制幅值內(nèi)。

基于式(116)的機(jī)械臂系統(tǒng)的動(dòng)力學(xué)模型，我們對(duì)其采用協(xié)調(diào)控制的方法。

將m矩陣分解為4個(gè)分塊子矩陣m11、m12、m21、m22，將c矩陣也分解為4個(gè)分塊子矩陣c11、c12、c21、c22。

選擇空間機(jī)械臂運(yùn)動(dòng)控制規(guī)律為：

基于上述的控制律對(duì)平面雙臂空間機(jī)械臂進(jìn)行軌跡跟蹤控制仿真。

仿真結(jié)果如圖14至圖19所示。

根據(jù)仿真結(jié)果可以看出，應(yīng)用協(xié)調(diào)控制策略能夠使軌跡跟蹤的誤差控制在10^-5以內(nèi)。相比于其他控制算法，能夠達(dá)到更加精確的控制，更好地克服參數(shù)不確定性和非參數(shù)不確定性對(duì)控制系統(tǒng)帶來(lái)的影響，且控制力矩仍保持在原控制力矩范圍內(nèi)，更好地達(dá)到控制系統(tǒng)的要求，實(shí)現(xiàn)穩(wěn)定跟蹤控制。

仿真結(jié)果表明，機(jī)械臂的角度跟蹤誤差在10^-5量級(jí)以內(nèi)，機(jī)械臂從平臺(tái)跟蹤運(yùn)動(dòng)到目標(biāo)的時(shí)間為30s。因此，該技術(shù)指標(biāo)達(dá)到項(xiàng)目合同書(shū)中的要求。

本發(fā)明還可有其它多種實(shí)施例，在不背離本發(fā)明精神及其實(shí)質(zhì)的情況下，本領(lǐng)域技術(shù)人員當(dāng)可根據(jù)本發(fā)明作出各種相應(yīng)的改變和變形，但這些相應(yīng)的改變和變形都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護(hù)范圍。

參考文獻(xiàn)

[1]zhangq,jil,weidzx.nonholonomicmotionplanningforminimizingbasedisturbancesofspacemanipulatorsbasedonmulti-swarmpso[j].robotica,2015:1-15.

[2]ratajczakj,tchońk.dynamicallyconsistentjacobianinverseformobilemanipulators[j].internationaljournalofcontrol,2015,89(6):1-16.

[3]sartorinatalg,chemoria,pierrotf.dual-spacecontrolofextremelyfastparallelmanipulators:payloadchangesandthe100gexperiment[j].ieeetransactionsoncontrolsystemstechnology,2015,23(4):1520-1535.

[4]答雯.自由漂浮空間機(jī)器人的軌跡規(guī)劃與軌跡跟蹤控制研究[d].哈爾濱工業(yè)大學(xué)，2012.

[5]huq,zhangj.maneuverandvibrationcontrolofflexiblemanipulatorsusingvariable-speedcontrolmomentgyros[j].actaastronautica,2015,113:105-119.

[6]zvafa,sdubowsky.onthedynamicsofspacemanipulatorusingthev'trtualmanipulatorwitllapplicationtopathplanning.thejournaloftheastronauticalscience,1990,38(4):441-472.

[7]yumentaniandkyoshida.resolvedmotionratecontrolofspacemanipulatorswithgeneralizedjacobianmatrix.ieeetransactionsonroboticsandautomation,1989,5(3):303-314.

[8]梁斌，劉良棟，李庚田.空間機(jī)器人的動(dòng)力學(xué)等價(jià)機(jī)械臂[j].自動(dòng)化學(xué)報(bào)，1998，24(06)：761-767.

[9]parlaktunao,ozkanm.adaptivecontroloffree-floatingspacemanipulatorsusingdynamicallyequivalentmanipulatormodel[j].roboticsandautonomoussystem,2004,46(3):185-193.

[10]劉隨軍，孫勇，雷小寧等.工業(yè)環(huán)境高精度空調(diào)的模糊pid控制方法及裝置：cn,cn102032640a[p].2011.

[11]balintts,stevensj.wickedproblemsinspacetechnologydevelopmentatnasa[j].actaastronautica,2015,118.

[12]徐栓鋒，楊保華，張篤周等.空間機(jī)械臂關(guān)節(jié)軌跡跟蹤的增廣自適應(yīng)控制方案[j].空間科學(xué)學(xué)報(bào)，2000，20(4):348-356.

[13]ottc,dietricha,a.prioritizedmulti-taskcompliancecontrolofredundantmanipulators[j].automatica,2015,53:416-423.

[14]umetaniy，yoshidak.resolvedmotionratecontrolofspacemanipulatorswithgeneralizedjacobianmatrix[j].roboticsandautomation,ieeetransactionson,1989,5(3)；303-314.