3RPS-SPS并聯(lián)機構(gòu)的力位混合冗余驅(qū)動控制

張氫，趙伯倫，陳文韜，秦仙蓉，孫遠韜

(同濟大學(xué) 機械與能源工程學(xué)院，上海，201804)

并聯(lián)機構(gòu)具有結(jié)構(gòu)緊湊、運動精度高、機構(gòu)剛度大和承載能力強[1]等優(yōu)點。目前，以并聯(lián)機構(gòu)為主的執(zhí)行機構(gòu)在醫(yī)療器械、先進制造業(yè)等行業(yè)中得到了廣泛應(yīng)用[2-4]，例如，六軸外固定支架(6-UPS 并聯(lián)機構(gòu)[5])、踝關(guān)節(jié)康復(fù)機構(gòu)(3-RPS/R 并聯(lián)機構(gòu)[6])、Tricept 混聯(lián)機床(3UPS-UP 并聯(lián)機構(gòu)[7])和Exechon 混聯(lián)機器人(2UPR-SPR 并聯(lián)機構(gòu)[8])等。但并聯(lián)機構(gòu)末端器的工作空間較小，動平臺的靈活性也較差，容易產(chǎn)生奇異位形，這也極大限制了它的應(yīng)用領(lǐng)域。為了克服上述不足，學(xué)者們提出了冗余驅(qū)動并聯(lián)機構(gòu)。冗余驅(qū)動并聯(lián)機構(gòu)是指的驅(qū)動關(guān)節(jié)數(shù)目大于結(jié)構(gòu)所具有的自由度數(shù)目的并聯(lián)機構(gòu)。與一般的并聯(lián)機構(gòu)相比，冗余并聯(lián)機構(gòu)具有較多的驅(qū)動部件，因而具有更高的結(jié)構(gòu)剛度和更強的承載能力，更適合于高速重載的應(yīng)用場合[9-12]。

并聯(lián)機構(gòu)具有強耦合、非線性的動力學(xué)特性，運動學(xué)控制方法只能適用于對速度和精度要求不高的應(yīng)用場合[13]。冗余并聯(lián)機構(gòu)具有閉環(huán)結(jié)構(gòu)，使用運動學(xué)控制方法在控制過程中將產(chǎn)生較大的內(nèi)力，嚴重影響機構(gòu)運動性能[14]。如果所有執(zhí)行器都使用力控制模式，需要精確的動力學(xué)模型[15]。在現(xiàn)實中，很難獲得準確的機器人動力學(xué)參數(shù)(如摩擦因數(shù))，尤其是對于復(fù)雜空間多自由度機器人。因此，對于冗余并聯(lián)機構(gòu)需要采用力位混合控制策略[16]，在運動學(xué)控制的基礎(chǔ)上考慮并聯(lián)機構(gòu)的動力學(xué)模型，控制器根據(jù)逆動力學(xué)方程補償并聯(lián)機構(gòu)因強耦合、非線性的動力學(xué)特性導(dǎo)致的運動誤差，使機器人具有更高的控制性能[17]。

吳宇列等[18]提出了一種基于靜態(tài)力平衡的改進PD 控制方法，并以2 自由度的冗余平面并聯(lián)機構(gòu)為對象，驗證該控制方法的正確性。張耀欣等[19]設(shè)計了一種采用離散最優(yōu)線性二次調(diào)節(jié)器方法，利用該方法控制并聯(lián)機構(gòu)的位移及速度。米建偉等[20]針對平面二自由度冗余并聯(lián)機構(gòu)提出一種同步控制方法，將慣性矩陣和離心力矩陣取定值，并引入了同步誤差和耦合誤差項。龍億等[21]基于軌跡輪廓誤差設(shè)計了自適應(yīng)滑模同步控制器，將動力學(xué)方程進行線性化，并分析了平面冗余驅(qū)動并聯(lián)機構(gòu)的穩(wěn)定性。崔學(xué)良等[22]建立了3RPS/UPS冗余驅(qū)動并聯(lián)機構(gòu)的魯棒性軌跡跟蹤控制模型，采用模糊PID控制策略對冗余驅(qū)動支鏈目標阻抗模型內(nèi)力進行實時跟蹤，實現(xiàn)了對冗余驅(qū)動并聯(lián)機構(gòu)的柔順性控制。劉曉飛等[23-24]在傳統(tǒng)力位混合驅(qū)動的基礎(chǔ)上，提出了一種驅(qū)動力同步協(xié)調(diào)控制策略，并以6PUS+UPU 冗余驅(qū)動并聯(lián)機構(gòu)為對象進行了驗證。李永泉等[25]設(shè)計了基于機器人系統(tǒng)動力學(xué)模型的前饋控制策略，并通過實驗驗證了該控制策略的可行性。

本文以3RPS-SPS 冗余驅(qū)動并聯(lián)機構(gòu)為對象，從運動學(xué)和動力學(xué)2個方面研究該新型機構(gòu)，并根據(jù)3RPS-SPS并聯(lián)機構(gòu)的特點，設(shè)計了力位混合的PID冗余驅(qū)動控制策略。

1 機構(gòu)構(gòu)型與自由度分析

3RPS-SPS 并聯(lián)機構(gòu)如圖1所示。3RPS-SPS 并聯(lián)機構(gòu)包括驅(qū)動支鏈、定平臺和動平臺3 個部分，動平臺通過4個驅(qū)動支鏈與定平臺相連。

圖1 3RPS-SPS并聯(lián)機構(gòu)示意圖Fig.1 Schematic diagrams of 3RPS-SPS parallel mechanism

為了便于分析，記4個驅(qū)動支鏈在動平臺上的安裝鉸點分別為A1～A4，其中A1A2垂直于A3A4且相交于A3點；記驅(qū)動支鏈在定平臺上的安裝鉸點分別為B1～B4，其中B1B2垂直于B3B4且相交于O點；AiBi對應(yīng)于驅(qū)動支鏈i。動平臺上安裝有末端器，末端器端點為Q且A3Q垂直于動平臺平面。驅(qū)動支鏈1～3 從定平臺到動平臺依次通過R 副、P副和S 副連接，其中，驅(qū)動支鏈1 和驅(qū)動支鏈2 的R 副軸線與B3B4平行，驅(qū)動支鏈3 的R 副軸線與B1B2平行。驅(qū)動支鏈4從定平臺到動平臺依次通過S副、P副和S副連接。4個驅(qū)動支鏈的P副軸線方向都是由定平臺上的安裝鉸點指向動平臺上的安裝鉸點。

為了分析該并聯(lián)機器人的運動特性，建立如下坐標系：取B1B2和B3B4的交點O為坐標系原點，OB4方向為X軸正方向，OB2方向為Y軸正方向，建立定平臺坐標系，坐標系固結(jié)在定平臺上；取A3為坐標系原點，A3A4方向為X′軸正方向，A3A2方向為Y′軸正方向，建立動平臺坐標系，坐標系固結(jié)在動平臺上。

表1所示為并聯(lián)機構(gòu)驅(qū)動支鏈的運動副分析。由螺旋理論可以得出4 個驅(qū)動支鏈的運動螺旋系，如圖2所示。對4個驅(qū)動支鏈運動螺旋系求反螺旋可得4個驅(qū)動支鏈的約束螺旋，聯(lián)立各驅(qū)動支鏈的約束螺旋即可獲得4個驅(qū)動支鏈對動平臺的約束螺旋系。

表1 并聯(lián)機構(gòu)驅(qū)動支鏈的運動副分析Table 1 Kinematic pair analysis of parallel mechanism

圖2 驅(qū)動支鏈運動螺旋Fig.2 Motion spiral of driving branch chains

求解上述約束螺旋系的反螺旋，可得動平臺的運動螺旋系：

式中：$1為繞定平臺坐標系X軸方向的旋轉(zhuǎn)自由度；$2為繞動平臺坐標系Y′軸的旋轉(zhuǎn)自由度；$3為沿著定平臺坐標系Z軸的平移自由度。

由式(2)可知，并聯(lián)機構(gòu)具有3個獨立自由度和4個驅(qū)動支鏈，驅(qū)動數(shù)量大于自由度數(shù)量，為冗余驅(qū)動并聯(lián)機構(gòu)。

2 運動學(xué)分析

2.1 位置反解的解析表達

式(3)所示的旋轉(zhuǎn)矩陣為通用旋轉(zhuǎn)矩陣，包含4個自由度，而根據(jù)自由度分析可知該并聯(lián)機構(gòu)僅有3個獨立自由度。因此，可根據(jù)各驅(qū)動支鏈對動平臺的約束關(guān)系建立約束方程，對式(3)進行簡化。取γ=0°，代入到式(3)可得

根據(jù)機構(gòu)的幾何參數(shù)可知，在定平臺坐標系下，各點坐標分別為：B1(0，-d，0)，B2(0，d，0)，B3(ad，0，0)，B4(d，0，0)。在動平臺坐標系下，各點坐標分別為：A1(0，-a，0)，A2(0，a，0)，A3(0，0，0)，A4(a，0，0)。

為了便于分析，以定平臺坐標系作為參考坐標系，通過式(4)將Ai轉(zhuǎn)化到定平臺坐標系下表示，由定平臺坐標系下的Ai和Bi坐標，可得到各驅(qū)動支鏈桿長li和并聯(lián)機構(gòu)位姿參數(shù)關(guān)系為

式(5)即為機構(gòu)的運動反解的解析表達式，其中(α，β，pz)為描述動平臺位姿的狀態(tài)變量。

2.2 位置正解的解析表達

對設(shè)計的并聯(lián)機器人求位置正解，即已知4個驅(qū)動支鏈的桿長，求解動平臺的輸出位姿。假設(shè)4個驅(qū)動支鏈的桿長分別為l1、l2、l3和l4。

由式(5)變形可得

經(jīng)化簡可得：

式(9)中，

由式(7)～(9)即可求得動平臺的輸出位姿參數(shù)α、β、pz。

3 動力學(xué)分析

3.1 速度與加速度分析

對式(5)關(guān)于時間t求導(dǎo)可得

式(11)中矩陣J即為并聯(lián)機器人的速度雅可比矩陣。

繼續(xù)將式(11)對時間t求導(dǎo)可得

式(11)即為速度微分方程，式(13)即為加速度微分方程。

3.2 驅(qū)動支鏈受力分析

3RPS-SPS 并聯(lián)機構(gòu)定平臺固定，運動部件包括4 條驅(qū)動支鏈和動平臺。其中，4 條驅(qū)動支鏈的結(jié)構(gòu)形式類似，皆以移動副為驅(qū)動，兩端通過旋轉(zhuǎn)副或者球副與定平臺、動平臺連接。采用拉格朗日法建立并聯(lián)機構(gòu)的動力學(xué)模型，拉格朗日方程如下：

式中：L=Ek-Ep；Ek為系統(tǒng)動能；Ep為系統(tǒng)勢能；q為系統(tǒng)廣義坐標；τ為對應(yīng)于廣義坐標的廣義力。

選取動平臺的位姿參數(shù)q=(pz，α，β)作為廣義坐標，分別對動平臺和驅(qū)動支鏈進行動力學(xué)建模。

對動平臺進行簡化建模，取慣性張量矩陣I為

式中：Ix、Iy、Iz分別為動平臺繞自身坐標系X'軸、Y'軸、Z'軸的轉(zhuǎn)動慣量，可根據(jù)動平臺的結(jié)構(gòu)尺寸求得；md為動平臺質(zhì)量。選取定平臺坐標系原點所在水平面作為零勢能面，動平臺勢能Epd為

由式(15)和式(17)代入拉格朗日方程，求解拉格朗日方程可得動平臺的廣義力τd為

然后，對驅(qū)動支鏈進行動力學(xué)建模。并聯(lián)機構(gòu)的4條驅(qū)動支鏈都是以移動副為驅(qū)動，兩端通過旋轉(zhuǎn)副或者球副與動平臺、定平臺連接，鉸接點分別為Ai和Bi(i=1，2，3，4)。根據(jù)驅(qū)動支鏈的運動特點，每個支鏈都可以分解為驅(qū)動桿和驅(qū)動缸體2個部分，其中，驅(qū)動缸體可繞著驅(qū)動支鏈與定平臺的鉸接點Bi旋轉(zhuǎn)，而驅(qū)動桿不僅繞著鉸接點Bi旋轉(zhuǎn)，還沿著驅(qū)動支鏈方向的軸向移動。

以驅(qū)動支鏈1為例，建立動力學(xué)模型，驅(qū)動支鏈1的運動簡圖如圖3所示。

圖3 驅(qū)動支鏈1運動簡圖Fig.3 Motion diagram of drive branch chain No.1

首先，列出驅(qū)動支鏈1的動能表達式和勢能表達式。根據(jù)圖3，驅(qū)動支鏈1的動能Ek1可表示為驅(qū)動缸體的動能和驅(qū)動桿的動能之和。

式中：mb為驅(qū)動缸體的質(zhì)量；vb為驅(qū)動缸體質(zhì)心的速度；Ib為驅(qū)動缸體的轉(zhuǎn)動慣量；ω為驅(qū)動支鏈的角速度；mt為驅(qū)動桿的質(zhì)量；vt為驅(qū)動桿質(zhì)心的速度；It為驅(qū)動桿的轉(zhuǎn)動慣量。

選取定平臺坐標系原點所在水平面作為零勢能面，驅(qū)動支鏈1的勢能Ep1為

式中：lb為驅(qū)動缸體的質(zhì)心到驅(qū)動支鏈與定平臺鉸接點B1的距離；lt為驅(qū)動桿的質(zhì)心到驅(qū)動支鏈與動平臺鉸接點A1的距離；θ1為驅(qū)動支鏈與水平線間的夾角。

將式(19)和(20)代入拉格朗日方程，即可求得驅(qū)動支鏈1產(chǎn)生的廣義力τ1。同理，按照上述推導(dǎo)對支鏈2、支鏈3 和支鏈4 列拉格朗日方程可以得各驅(qū)動支鏈產(chǎn)生的廣義力τ2、τ3、τ4。

在對動平臺和驅(qū)動支鏈建立動力學(xué)模型時，選取的廣義坐標和零勢能面相同。因此，只需將對動平臺和驅(qū)動支鏈求解得到的廣義力相加即可得到并聯(lián)機構(gòu)廣義力：

3.3 驅(qū)動力分析

用虛功原理求解各支鏈驅(qū)動力。假設(shè)驅(qū)動支鏈的虛位移為δl=[δl1δl2δl3δl4]T，驅(qū)動支鏈的驅(qū)動力為F=[δF1δF2δF3δF4]T；廣義坐標的虛位移為δq=[δpzδαδβ]T，廣義驅(qū)動力為τ=[τ1τ2τ3τ4]T。根據(jù)虛功原理，當(dāng)機構(gòu)發(fā)生虛位移時，慣性力和外力所作的虛功總和等于零，因此，在確定的運動狀態(tài)下即慣性力確定時，廣義驅(qū)動力在廣義坐標的虛位移上做的虛功與實際每個驅(qū)動支鏈的驅(qū)動力在驅(qū)動支鏈的虛位移上做的虛功相等，即

對于確定的構(gòu)型，廣義坐標的虛位移和驅(qū)動支鏈的虛位移需要滿足機構(gòu)構(gòu)型的幾何約束要求，可以通過雅克比矩陣進行轉(zhuǎn)換。

將式(23)代入式(22)可得廣義驅(qū)動力與驅(qū)動支鏈驅(qū)動力之間的關(guān)系為

此時，式(24)中的(JT)-1∈R4×3具有無窮多組解，不同形式的(JT)-1就對應(yīng)了不同的驅(qū)動支鏈驅(qū)動力分配方案。因此，可以根據(jù)某一特定的優(yōu)化目標確定(JT)-1矩陣對驅(qū)動力進行優(yōu)化配置，使得機構(gòu)具有更好的運動性能。

對于并聯(lián)機構(gòu)最常見的優(yōu)化目標為驅(qū)動力優(yōu)化，優(yōu)化問題可表述為：在約束條件JTF=τ下，使得目標函數(shù)Z=FTWF的支鏈驅(qū)動力F最小。式中，W為對角加權(quán)矩陣，表示各支鏈驅(qū)動力的權(quán)重。

對上述優(yōu)化問題，可采用拉格朗日乘子法，引入拉格朗日乘子λ，構(gòu)造新的函數(shù)Z′：

為使Z′取得極值，Z′對F和λ的偏導(dǎo)數(shù)等于0，求解得

取加權(quán)矩陣W為單位矩陣，則可得

4 力位混合控制策略

4.1 控制策略設(shè)計

根據(jù)機構(gòu)自由度分析可知，3RPS-SPS 并聯(lián)機構(gòu)為冗余并聯(lián)機構(gòu)，存在冗余支鏈。因此，可在常規(guī)的非冗余驅(qū)動控制的基礎(chǔ)上對冗余支鏈添加力控制。根據(jù)動力學(xué)模型設(shè)計了一種改進的計算力矩算法，即動力學(xué)前饋算法。

式(28)可分解為2個部分τ1和τ2疊加，其中，

式(29)僅與理論廣義位置、速度、加速度有關(guān)，與實時數(shù)據(jù)無關(guān)，可在軌跡規(guī)劃時提前計算；式(30)形式簡單，計算方便，減輕了實時計算負擔(dān)?？刂屏厮惴驁D如圖4所示。

圖4 控制力矩算法框圖Fig.4 Control torque algorithm block diagram

力位混合的冗余驅(qū)動控制的系統(tǒng)框圖如圖5所示，位置驅(qū)動控制的系統(tǒng)框圖如圖6所示。力位混合驅(qū)動控制系統(tǒng)與位置驅(qū)動控制系統(tǒng)相比，增加了力控制支鏈。

圖5 力位混合冗余驅(qū)動控制系統(tǒng)框圖Fig.5 Block diagram of force position hybrid redundant drive control system

圖6 位置驅(qū)動控制系統(tǒng)框圖Fig.6 Block diagram of position drive control system

在力位混合的冗余驅(qū)動控制系統(tǒng)中，驅(qū)動支鏈按照控制方式分為力控制支鏈與位置控制支鏈2類。

1)對于位置控制支鏈，首先，將要求路徑進行軌跡規(guī)劃，并反解得到各支鏈的桿長信息；然后，根據(jù)各支鏈位置傳感器的實時測量數(shù)據(jù)與規(guī)劃桿長信息進行比較，得到位置差值信號；最后，將差值信號傳入PID控制器，對伺服系統(tǒng)進行PID控制。

2)對于力控制支鏈，軌跡規(guī)劃獲得支鏈的位置、速度、加速度，并通過動力學(xué)反解得到系統(tǒng)驅(qū)動力。動力學(xué)反解模塊可根據(jù)不同的控制要求進行設(shè)計。

3RPS-SPS 機構(gòu)為了平衡各支鏈的驅(qū)動力，提升系統(tǒng)的驅(qū)動能力，采用3.3節(jié)中的驅(qū)動力優(yōu)化算法。將動平臺位姿的位差信號傳入PD控制器，再乘以慣性矩陣M(qd)，對根據(jù)理論位置計算的驅(qū)動力進行反饋調(diào)節(jié)。在整個控制系統(tǒng)中，位置控制支鏈保證了機構(gòu)的位置精度，力控制支鏈則通過調(diào)整驅(qū)動力以平衡所有支鏈的驅(qū)動力，使機構(gòu)具有較高的整體剛度和較好的運動性能。

4.2 聯(lián)合仿真實驗

圖7所示為建立的ADAMS仿真模型。為了驗證冗余驅(qū)動控制系統(tǒng)的性能，在ADAMS動力學(xué)模型的基礎(chǔ)上結(jié)合Simulink搭建聯(lián)合仿真平臺，如圖8所示。并聯(lián)機構(gòu)的物理參數(shù)如表2所示。驅(qū)動杠為伺服電動缸，其內(nèi)部為滾動絲杠結(jié)構(gòu)可將伺服電機的旋轉(zhuǎn)運動轉(zhuǎn)化為電動缸的直線伸縮運動。

表2 并聯(lián)機構(gòu)物理參數(shù)Table 2 Physical parameters of parallel mechanism

圖7 ADAMS仿真模型Fig.7 ADAMS simulation model

圖8 冗余驅(qū)動控制聯(lián)合仿真系統(tǒng)Fig.8 Joint simulation system of redundant drive control

設(shè)置動平臺位姿參數(shù)隨時間變化規(guī)律為

設(shè)定運動時間為5 s，將上述軌跡作為圖8所示控制系統(tǒng)的輸入，進行仿真，結(jié)果如圖9至圖11所示。

圖9 力位混合冗余驅(qū)動運動軌跡Fig.9 Force/position hybrid redundant drive motion trajectory

對圖9所示實際軌跡和理論軌跡的誤差進行分析，得到圖10。由圖10 可得，動平臺沿Z軸位移平均誤差為4.01×10-2mm，最大誤差為4.71×10-1mm；繞X軸旋轉(zhuǎn)角平均誤差為5.66×10-4rad，最大誤差為4.52×10-3rad；繞Y′軸旋轉(zhuǎn)角平均誤差為3.45×10-4rad，最大誤差為9.42×10-3rad。在冗余驅(qū)動方式下，動平臺實際軌跡和理論軌跡的誤差極小，軌跡跟蹤效果明顯，說明使用冗余驅(qū)動方式的并聯(lián)機構(gòu)具有較高的運動精度。

圖10 力位混合冗余驅(qū)動運動軌跡誤差Fig.10 Force/position hybrid redundant drive motion trajectory error

由圖11 可知：采用力位混合冗余驅(qū)動方式，支鏈2出現(xiàn)最大驅(qū)動力約為66 N，小于非冗余位置驅(qū)動方式時支鏈3 出現(xiàn)的最大驅(qū)動力190 N。對比非冗余位置驅(qū)動的3個驅(qū)動支鏈的驅(qū)動力，可以發(fā)現(xiàn)驅(qū)動力分配極不均衡，支鏈1的驅(qū)動力小于支鏈3 的驅(qū)動力。而力位混合冗余驅(qū)動的4 個驅(qū)動支鏈的驅(qū)動力分配較均衡，支鏈1、支鏈2 和支鏈3 的驅(qū)動力在64 N 附近變化，而支鏈4 的驅(qū)動力在45 N 附近變化。因此，在力位冗余驅(qū)動方式下，各驅(qū)動支鏈的驅(qū)動力變化平穩(wěn)，并且由于采用了驅(qū)動力優(yōu)化算法，各支鏈驅(qū)動力分配較均勻，最大驅(qū)動力較小，可有效提升被控機構(gòu)的承載能力。

圖11 不同驅(qū)動方式下驅(qū)動支鏈驅(qū)動力曲線Fig.11 Driving force curve of different driving modes

5 結(jié)論

1)3RPS-SPS 驅(qū)動支鏈數(shù)量大于自由度數(shù)量，為冗余并聯(lián)機構(gòu)。

2)建立并聯(lián)機構(gòu)的正、逆運動學(xué)模型，得到機構(gòu)的速度和加速度解析表達式。建立3RPS-SPS冗余并聯(lián)機構(gòu)動力學(xué)模型，提出了并聯(lián)機構(gòu)驅(qū)動支鏈的驅(qū)動力計算方法，優(yōu)化了驅(qū)動力分配。

3)設(shè)計了力位混合的冗余控制策略，建立了ADAMS和Simulink聯(lián)合仿真平臺并進行了系統(tǒng)仿真。所設(shè)計控制策略使3RPS-SPS冗余并聯(lián)機構(gòu)具有較高的運動精度和較強的承載能力。