本發(fā)明涉及一種空間雙機械臂系統(tǒng)運動協(xié)調控制方法，屬于空間機械臂技術領域。

背景技術：

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

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

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

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

一、空間機械臂系統(tǒng)運動建模

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

針對地面機械臂比較成熟的建模方法有牛頓-歐拉方程和拉格朗日方程等，均屬于多體系統(tǒng)的建模范疇。而空間機械臂系統(tǒng)強耦合、非線性、時變等特點使得建模問題較地面系統(tǒng)相對復雜，為推動空間機械臂技術的發(fā)展，各國學者紛紛針對空間機械臂的運動學與動力學特性，提出了一些極具學習價值的建模方法。

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

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

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

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

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

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

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

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

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

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

(1)pd控制

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

parlaktuna和ozkan利用dem方法將空間機械臂的控制問題由慣性空間轉化到關節(jié)空間，在得到參數線性化的動力學方程后，將適用于地面固定基座機械臂的控制方法應用于空間機械臂對關節(jié)期望軌跡的跟蹤^[9]。

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

(2)自適應控制

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

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

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

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

(3)神經網絡控制

神經網絡控制不需要過多的控制對象模型參數信息，高度并行的結構特點可以使得其具有很強的容錯能力和自學能力，可以對任意的非線性函數進行逼近，且不需要大量的計算，這些年在相關領域得以廣泛應用。

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

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

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

一種空間雙機械臂系統(tǒng)運動協(xié)調控制方法，包括：

步驟一、構建空間雙機械臂系統(tǒng)的運動學方程以及動力學方程；

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

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

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

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

附圖說明

圖1為本發(fā)明的雙臂空間機械臂結構模型；

圖2為本發(fā)明的空間雙機械臂系統(tǒng)協(xié)調控制結構框圖；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

具體實施方式

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

1.1旋轉矩陣表示法

描述兩坐標系之間的相對姿態(tài)可以采用旋轉矩陣方法。假設坐標系σj的三個軸向對應的單位矢量在σi中的表示分別為n、o、a∈r^3×3，則從σi到σj的旋轉矩陣為：

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

旋轉矩陣對時間的導數：

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

其中：

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

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

1.2拉格朗日動力學建模方法

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

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

其中v(q)∈r是系統(tǒng)的勢能，只與系統(tǒng)的廣義坐標有關，

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

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

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

1.3點到點軌跡規(guī)劃

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

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

1.可視圖法

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

2.多項式法

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

3.自由空間法

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

4.遺傳算法

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

5.其他方法

近年來，人工智能方法正在成為研究的熱點，神經網絡在機械臂路徑規(guī)劃中的應用得到了廣泛的重視。一些神經網絡模型通過學習可生成實時的路徑[12]。

1.4pd軌跡控制方法

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

式中，kp為比例系數；τ為微分時間常數。kp與τ都是可調的參數。

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

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

為統(tǒng)一標準和方便討論，對坐標系、基本變量及符號進行如下定義：

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

σi、σe1、σe2:分別表示慣性系、機械臂arm-1和arm-2末端坐標系；

σb：基座的幾何坐標系；

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

σi、bi：固連坐標系，zi正向為ji的旋轉方向；

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

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

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

ci^k:bi^k的質心；

oi:慣性系原點；

og：整個系統(tǒng)的質心；

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

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

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

ⁱdj∈r³：為σj原點在σi的位置矢量；

en：n維單位矩陣；

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

τ∈rⁿ：關節(jié)驅動力矩；

mi^k、m：為bi^k的質量，

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

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

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

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

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

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

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

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

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

^krij：表示從σi原點指向σj原點的矢量，在σk中的表示，如果σi或者σk為慣性系，則可以省去相應的符號i或者k；

^jvi,^jωi：分別表示σi相對于σj的線速度和角速度，如果σi或者σj為慣可以省去相應的符號i或者j；

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

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

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

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

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

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

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

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

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

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

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

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

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

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

本發(fā)明的空間雙機械臂系統(tǒng)運動協(xié)調控制方法，包括如下步驟：

步驟一、構建空間雙機械臂系統(tǒng)的運動學方程以及動力學方程。

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

p＝0(14)

由式(19)可知：

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

ω≈0(16)

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

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

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

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

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

(1)點到點路徑規(guī)劃

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

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

規(guī)劃關節(jié)運動：

使得：

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

使用五次多項式對關節(jié)函數進行參數化：

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

f(c)＝0(25)

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

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

1)初始化各變量：

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

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

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

ck+1＝△ck+ck(28)

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

4)輸出參數

完整流程圖如圖3所示。

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

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

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

其中

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

其中

其中

針對以上模型，設計空間本體和機械臂運動控制規(guī)律為：

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

下面針對本實施方式的三個步驟具體說明其原理與推導過程：

空間雙機械臂系統(tǒng)運動建模過程：

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

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

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

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

在空間機械臂系統(tǒng)中，整個系統(tǒng)的質心位置矢量與各連桿的質心位置矢量之間存在如下關系：

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

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

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

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

式中jb^k——機械臂末端速度與基座速度相關雅克比矩陣；

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

jb^k和jm^k可按照下列式子進行計算:

p0e^k——基座質心到機械臂末端的位置矢量，即：

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

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

令：

式(48)可表示為：

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

(1)線動量方程：

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

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

其中：

r0g＝r0-rg(58)

(2)角動量方程

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

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

其中：

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

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

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

令：

式(65)可表示為：

l＝l0+r0×p(6(6

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

r0——基座質心的位置矢量；

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

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

其中：

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

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

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

其中：

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

為實現(xiàn)機械臂從初始位姿運動到待抓捕物體位置，需要對機械臂的運動進行軌跡規(guī)劃。為此，實現(xiàn)需要對機械臂末端進行點到點的路徑規(guī)劃。

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

規(guī)劃關節(jié)運動：

使得：

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

因為姿態(tài)受控，姿態(tài)角變化很小，所以近似認為

末端位置可表示為：

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

tf時刻的末端位置與期望位置之差為：

使用五次多項式對關節(jié)函數進行參數化：

由(77)可知：

及

經整理可得：

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

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

f(c)＝0(92)

下面給出關節(jié)函數的參數歸一化處理過程。

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

其中

于是有：

由此可得關于參數c′^k的非線性方程組:

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

定義函數：

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

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

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

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

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

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

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

性質2：具有有界性，即

首先，我們構造如下形式的lyapunov函數

其中

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

其中

將控制器(109)帶入動力學方程可得

則對lyapunov函數(108)求導可得

將誤差動力學方程(108)帶入上式可得

利用性質2可得

因此，通過選取參數使得：即可得到

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

具體實施方式二：本實施方式與具體實施方式一不同的是：

步驟一具體包括：

步驟一一、根據如下公式建立空間雙機械臂系統(tǒng)的運動學方程：

其中，k表示第k個機械臂，且k為1或2；

為第k個機械臂第i個連桿的質量；ki^k表示ji^k的旋轉方向的單位矢量，ji^k為第k個機械臂中連接第i-1個連桿與第i個連桿的關節(jié)；pi^k表示關節(jié)ji^k的位置矢量；θ^k為第k個機械臂的關節(jié)角向量；

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

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

其它步驟及參數與具體實施方式一相同。

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

步驟二具體包括：

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

步驟二二、假設存在t，使得：

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

使用如下的五次多項式對關節(jié)函數進行參數化處理：

為待求解的參數；

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

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

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

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

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

步驟二三二、對運動學方程進行數值積分，得到末端位姿

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

若滿足，則輸出參數

若不滿足則進行如下計算：

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

ak+1＝△ak+ak

并將k值加1，跳轉至步驟二三二。

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

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

步驟三具體包括：

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

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

其中

其中

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

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

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

<實施例>：

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

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

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

基于上一部分所設計的pd控制器對平面雙臂空間機械臂進行軌跡跟蹤控制仿真。

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

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

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

因為我們要對平面雙臂空間機械臂進行仿真分析，所以將5.1節(jié)中的運動學方程代入能量守恒中，由第二類拉格朗日方程，可以得到載體姿態(tài)受控，位置不受控時的平面雙臂空間機械臂的系統(tǒng)動力學方程：

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

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

仿真結果如圖9至圖13所示。

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

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

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

仿真結果表明，機械臂的角度跟蹤誤差在10^-3量級以內，機械臂從平臺跟蹤運動到目標的時間為30s。

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

本體質心位置坐標為：

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)質心的位置坐標，且：

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

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

對式(117)，(118)，求時間t的導數，得到本體質心速度表達式：

各分體質心的速度表達式:

式中：

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兩自由度平面空間機械臂的標稱參數

仿真所使用的控制器設計參數：

期望關節(jié)角軌跡是用五次多項式插值得到的，期望關節(jié)角起始狀態(tài)為終止狀態(tài)實際的關節(jié)角初始位置為與初始期望位置有一定的偏差。基座航天器初始姿態(tài)角與期望值均設為0。

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

仿真分析：

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

基于式(116)的機械臂系統(tǒng)的動力學模型，我們對其采用協(xié)調控制的方法。

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

選擇空間機械臂運動控制規(guī)律為：

基于上述的控制律對平面雙臂空間機械臂進行軌跡跟蹤控制仿真。

仿真結果如圖14至圖19所示。

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

仿真結果表明，機械臂的角度跟蹤誤差在10^-5量級以內，機械臂從平臺跟蹤運動到目標的時間為30s。因此，該技術指標達到項目合同書中的要求。

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

參考文獻

[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]答雯.自由漂浮空間機器人的軌跡規(guī)劃與軌跡跟蹤控制研究[d].哈爾濱工業(yè)大學，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].自動化學報，1998，24(06)：761-767.

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

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

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

[12]徐栓鋒，楊保華，張篤周等.空間機械臂關節(jié)軌跡跟蹤的增廣自適應控制方案[j].空間科學學報，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.