趙 純，于存貴，徐 華，徐 強(qiáng)

（1. 南京理工大學(xué)，南京，210094；2. 上海宇航系統(tǒng)工程研究所，上海，201108）

0 引 言

運(yùn)載火箭燃料加注的自動(dòng)化是實(shí)現(xiàn)火箭安全發(fā)射的重要前提。考慮到加注機(jī)器人需安裝在發(fā)射架擺桿上，同時(shí)需承載管路和燃料的質(zhì)量，因此需具備高功質(zhì)比、承載力強(qiáng)、動(dòng)態(tài)性能好等特性。液壓驅(qū)動(dòng)型并聯(lián)機(jī)器人滿足以上所有特性要求。

并聯(lián)機(jī)器人雖具有以上諸多優(yōu)點(diǎn)，但是由于其本質(zhì)上是一個(gè)多變量耦合的機(jī)械系統(tǒng)，其高性能控制仍是一個(gè)難點(diǎn)。目前針對(duì)并聯(lián)機(jī)器人的控制策略主要分為兩類。一類是基于分散控制方法的策略，其主要針對(duì)各個(gè)關(guān)節(jié)進(jìn)行算法設(shè)計(jì)，把并聯(lián)機(jī)構(gòu)的動(dòng)力學(xué)耦合干擾當(dāng)作各關(guān)節(jié)的一項(xiàng)擾動(dòng)進(jìn)行處理，通過設(shè)計(jì)關(guān)節(jié)擾動(dòng)觀測(cè)器進(jìn)行補(bǔ)償控制。該類方法具有無需建模、算法結(jié)構(gòu)簡(jiǎn)單的優(yōu)點(diǎn)，但是其控制精度主要在于其觀測(cè)器精度，對(duì)于瞬時(shí)強(qiáng)干擾，有可能造成系統(tǒng)的失穩(wěn)。另一類是基于動(dòng)力學(xué)模型的控制策略。目前，自適應(yīng)控制、滑?？刂啤⒅悄芸刂扑惴ǘ嫉玫搅藦V泛應(yīng)用。魯棒控制策略從設(shè)計(jì)上來說，雖然具有一定的保守性，但是具有算法簡(jiǎn)單、穩(wěn)定性好的優(yōu)點(diǎn)，目前也得到了廣泛的研究與應(yīng)用。

本文以某液壓驅(qū)動(dòng)型并聯(lián)式加注機(jī)器人為研究對(duì)象。首先建立包含并聯(lián)機(jī)器人和液壓驅(qū)動(dòng)器動(dòng)態(tài)特性的系統(tǒng)高階多輸入多輸出模型。其次，基于反步方法設(shè)計(jì)一種加注機(jī)器人高精度魯棒控制器，并通過Lyapunov 方法證明其穩(wěn)定性。仿真結(jié)果表明，該控制器有較高的軌跡跟蹤精度，能夠滿足實(shí)際加注場(chǎng)合的對(duì)接應(yīng)用需求。

1 系統(tǒng)建模與問題描述

對(duì)于受風(fēng)載作用的火箭箭體，其運(yùn)動(dòng)主要集中在3個(gè)平動(dòng)自由度，而在3 個(gè)轉(zhuǎn)動(dòng)自由度上的運(yùn)動(dòng)量較小(不大于0.5°)。因此，自動(dòng)加注機(jī)器人設(shè)計(jì)為三平動(dòng)并聯(lián)機(jī)構(gòu)串聯(lián)六自由度柔順機(jī)構(gòu)的混聯(lián)結(jié)構(gòu)方案，其總體結(jié)構(gòu)如圖1 所示。

圖1 并聯(lián)式加注機(jī)器人結(jié)構(gòu)示意Fig.1 Structural Diagram of Parallel Filling Robot

1.1 柔順機(jī)構(gòu)等效剛度模型

柔順機(jī)構(gòu)的運(yùn)動(dòng)簡(jiǎn)圖與等效剛度模型見圖2，主要用于補(bǔ)償三平動(dòng)并聯(lián)機(jī)器人在空間六自由度上的對(duì)接誤差。

圖2 柔順機(jī)構(gòu)運(yùn)動(dòng)簡(jiǎn)圖與結(jié)構(gòu)等效剛度示意Fig.2 Kinematic Diagram of Compliant Mechanism and Schematic Diagram of Structural Equivalent Stiffness

柔順機(jī)構(gòu)具有六自由度的空間位姿補(bǔ)償能力，但在實(shí)際加注任務(wù)中，箭體3 個(gè)偏轉(zhuǎn)自由度幅值較小,因此柔順機(jī)構(gòu)在空間三平動(dòng)自由度上近似解耦，其等效剛度模型如下：

式中k、k和k分別是左右、上下和前后支鏈中單個(gè)彈簧的剛度系數(shù)。

1.2 3-PSS 并聯(lián)機(jī)構(gòu)動(dòng)力學(xué)建模

3-PSS 機(jī)構(gòu)結(jié)構(gòu)簡(jiǎn)化單桿等效模型見圖3。

圖3 3-PSS 機(jī)構(gòu)結(jié)構(gòu)Fig.3 Structure Diagram of 3-PSS Mechanism

在基座和動(dòng)平臺(tái)中心點(diǎn)分別建立固定參考坐標(biāo)系和連體參考坐標(biāo)系。3-PSS 機(jī)構(gòu)的動(dòng)力學(xué)模型有如下形式：

式中=[,,]為在固定坐標(biāo)系中的位置矢量；()為慣性矩陣；(, ˙)為哥氏力和離心力矩陣；()為重力矢量；()為系統(tǒng)建模誤差和外部干擾；為系統(tǒng)的驅(qū)動(dòng)力矢量。

式（2）描述的動(dòng)力學(xué)系統(tǒng)具有如下性質(zhì)：

1.3 液壓驅(qū)動(dòng)器建模

液壓驅(qū)動(dòng)器的結(jié)構(gòu)原理如圖4 所示。

圖4 液壓驅(qū)動(dòng)器原理Fig.4 Schematic Diagram of Hydraulic Actuator

驅(qū)動(dòng)器的動(dòng)態(tài)模型可以有如下表示：

式中q和q為液壓缸的輸出位移和速度；m為活塞桿和負(fù)載質(zhì)量；d為液壓缸的不匹配不確定誤差；τ為驅(qū)動(dòng)力；u為控制輸入；d為液壓缸的匹配不確定誤差。

式中P和P為液壓缸兩腔壓力；和為液壓缸兩腔的有效面積。V，V分別為兩腔有效容積，V=V+q，V=V-q，其中V和V為其初始容積；β為油液彈性模量；C為內(nèi)泄漏系數(shù)；P=P-P為兩腔壓差；為總的流量增益；為供油壓力；為回油壓力。(x)={1, ifx≥0; 0, ifx＜0}。

1.4 系統(tǒng)完整動(dòng)力學(xué)模型

綜合式（2）和（3），可得系統(tǒng)完整的動(dòng)態(tài)模型為

為了便于控制器設(shè)計(jì)，有如下假設(shè)：

假設(shè)1：液壓驅(qū)動(dòng)器在常規(guī)工況下工作，即有0＜P＜P＜P，0＜P＜P＜P。

式中，均為大小已知的正數(shù)。

2 控制律設(shè)計(jì)

2.1 反步魯棒控制器設(shè)計(jì)

步驟1：定義為笛卡爾空間期望軌跡，=-。對(duì)求導(dǎo)得：

定義虛擬控制量：

式中為正定對(duì)角矩陣。

步驟2：定義動(dòng)態(tài)誤差=-，則有：

為設(shè)計(jì)一個(gè)如下形式的虛擬控制函數(shù)：

式中為系統(tǒng)的名義模型補(bǔ)償項(xiàng)；為魯棒控制項(xiàng)；為正定對(duì)角矩陣。

定義=-，同時(shí)將式（9）代入（8），可得：

設(shè)計(jì)滿足如下的鎮(zhèn)定條件：

式中 ε為可任意小的正數(shù)。

令≥，則滿足條件（11）的可以設(shè)計(jì)為

式中為正定非線性增益矩陣，為三階單位陣。

步驟3：對(duì)求導(dǎo)可得：

因此，期望控制律可以設(shè)計(jì)為

式中u為模型補(bǔ)償控制項(xiàng)，u為魯棒控制律，為正定對(duì)角矩陣。

將式（15）代入（13）可得：

設(shè)計(jì)u滿足如下的鎮(zhèn)定條件：

式中為可任意小的正數(shù)。

2.2 穩(wěn)定性分析

定義系統(tǒng)Lyapunov 函數(shù)為

對(duì)式（19）兩端求導(dǎo)可得：

針對(duì)上式，同時(shí)注意到性質(zhì)2，有：

結(jié)合式（5）、（7）、（16）、（20）和（21），可得：

注意到式（11）和（17），因此有：

定義=()，=()，=()分別為正定對(duì)角矩陣1、21和31的最小特征值。因此有：

式中=()為矩陣的最大特征值。

考慮到性質(zhì)1，由（25）可得：

定義=+，根據(jù)對(duì)比原理有：

2.3 期望軌跡生成

式中、和分別為濾波器系數(shù)。

3 仿真分析

3.1 系統(tǒng)模型參數(shù)和控制器參數(shù)

針對(duì)并聯(lián)機(jī)器人，有OA=0.32 m，O’B=0.14 m，連桿長(zhǎng)度=0.45 m，動(dòng)平臺(tái)及負(fù)載質(zhì)量m=35 kg，連桿質(zhì)量m=9.8 kg，連桿轉(zhuǎn)動(dòng)慣量I=I=I=0.2 kg·m，導(dǎo)向桿質(zhì)量m=22 kg，綜合彈簧剛度k=k=k=15 N/mm。

針對(duì)液壓驅(qū)動(dòng)器，有m=3 kg，A=1.25×10m，A=6.4×10m，β=2×10Pa，P=10 MPa，P=0，C=9×10m/s/Pa，k=4×10m/s/V/ Pa，V=1.2×10m，V=2.96×10m。

仿真步長(zhǎng)設(shè)置為0.5 ms。為了更好的驗(yàn)證控制器性能，針對(duì)以下兩種控制器進(jìn)行仿真對(duì)比分析。

a）基于動(dòng)力學(xué)模型的魯棒控制器(Model-based Robust Control ， MRC) 。 控 制 器 參 數(shù) 設(shè) 計(jì) 為=diag{1000,1000,1000}，=+=diag(800,800,800)，=+=diag(500,500,500)，三階濾波器的系數(shù)為=12500，=7500，=150。

b）速度前饋PID 控制器(VFPID)。控制器參數(shù)設(shè)計(jì)為：k=1200，k=600，k=2，k=10V·s/m。

3.2 仿真分析

加注機(jī)器人的加注任務(wù)可分為兩個(gè)階段的運(yùn)動(dòng)：對(duì)接保持階段和隨動(dòng)脫落階段。

3.2.1 工況1：對(duì)接保持階段工況仿真

該階段加注機(jī)器人與火箭箭體均處于封閉的發(fā)射塔架中，此時(shí)不受風(fēng)載的作用，對(duì)接任務(wù)可以簡(jiǎn)化為動(dòng)平臺(tái)中心點(diǎn)一個(gè)點(diǎn)到點(diǎn)的運(yùn)動(dòng)。

箭/地連接器面板的對(duì)接過程示意如圖5 所示。

圖5 對(duì)接導(dǎo)向桿與導(dǎo)向孔相對(duì)狀態(tài)示意Fig.5 Schematic Diagram of Relative State of Guide Rod and Guide Hole

假設(shè)面板間的初始距離為100 mm，對(duì)接速度為10 mm/s，因此對(duì)接軌跡r可以設(shè)計(jì)為

加注機(jī)器人受到的外部干擾主要由導(dǎo)向桿/孔對(duì)接誤差引起的柔順機(jī)構(gòu)彈簧形變力。在≥98mm 時(shí)，鎖緊機(jī)構(gòu)完成箭/地面板的鎖緊。同時(shí)，假設(shè)導(dǎo)向桿/孔的接觸運(yùn)動(dòng)是順滑的，則根據(jù)桿/孔的相對(duì)位置可以定義出箭體對(duì)加注機(jī)器人的干擾力為

式中 △，△，△分別為加注機(jī)器人在笛卡爾空間的對(duì)接跟蹤誤差，△=-x，△=-y，△=-z。

由于整個(gè)對(duì)接過程加注機(jī)器人的運(yùn)動(dòng)速度都較為緩慢，因此其建模誤差項(xiàng)可以認(rèn)為是一恒值擾動(dòng)，假設(shè)該擾動(dòng)為

結(jié)合式（30）和（31），式（4）中的系統(tǒng)中干擾項(xiàng)()可以定義為

圖6、圖7 為MRC 和VFPID 兩種算法對(duì)于低速運(yùn)動(dòng)期望軌跡的跟蹤情況。仿真結(jié)果表明，MRC 和VFPID 這兩種算法對(duì)于低速運(yùn)動(dòng)皆表現(xiàn)出較高的軌跡跟蹤精度，同時(shí)MRC 控制器相對(duì)VFPID 控制器軌跡跟蹤性能更優(yōu)。

圖6 MRC 空間跟蹤誤差Fig.6 Tracking Errors in Workspace of MRC

圖7 VFPID 空間跟蹤誤差Fig.7 Tracking Errors in Workspace of VFPID

此外，在接近=10s 處，由于軌跡過渡和鎖緊機(jī)構(gòu)鎖緊力的引入，使得兩種控制器均有跟蹤誤差一定的放大，但是MRC 控制器誤差的波動(dòng)明顯小于VFPID。由此可見，VFPID 雖然具有一定的抗干擾能力，但是其本質(zhì)上是一種無模型控制算法，其對(duì)擾動(dòng)的響應(yīng)總是起作用于誤差發(fā)生之后，因此當(dāng)瞬時(shí)擾動(dòng)(軸向鎖緊力)產(chǎn)生時(shí)，其缺少快速響應(yīng)的能力，從而造成跟蹤誤差相對(duì)較大的惡化。

3.2.2 工況2：隨動(dòng)脫落階段工況仿真

該階段發(fā)射塔架打開，當(dāng)接收到脫落指令后，加注機(jī)器人需完成在隨動(dòng)過程中的3 s 脫落任務(wù)，此時(shí)箭體因風(fēng)載作用會(huì)有小幅擺動(dòng)行為?？紤]到箭體受左右風(fēng)載時(shí)，其運(yùn)動(dòng)狀態(tài)最為惡劣，因此針對(duì)該工況進(jìn)行仿真。假設(shè)箭體擺動(dòng)為x=0.05sin(π)，在=10 s 時(shí)接收到脫落指令。因此，可將動(dòng)平臺(tái)中心點(diǎn)的期望軌跡設(shè)計(jì)為

箭體對(duì)加注機(jī)器人施加的干擾力可以定義為

考慮到加注機(jī)器人在動(dòng)態(tài)運(yùn)動(dòng)時(shí)，其建模誤差將是一個(gè)時(shí)變的動(dòng)態(tài)量，因此定義該動(dòng)態(tài)誤差為

結(jié)合式（33）和式（34），式（4）中的()定義為

仿真結(jié)果如圖8 和圖9 所示。

圖8 MRC 空間跟蹤誤差Fig.8 Tracking Errors in Workspace of MRC

圖9 VFPID 空間跟蹤誤差Fig.9 Tracking Errors in Workspace of VFPID

結(jié)果表明，對(duì)于隨動(dòng)脫落工況，兩種控制器的跟蹤誤差均有一定的放大，但得益于系統(tǒng)較好的抗干擾能力，MRC 的控制性能仍遠(yuǎn)優(yōu)于VFPID。同時(shí)，注意到在脫落階段=(10~13) s，兩種控制器并未表現(xiàn)出明顯的軌跡跟蹤性能惡化的情況，一方面這是由于脫落軌跡在設(shè)計(jì)時(shí)已經(jīng)考慮到整個(gè)脫落過程的平滑性問題，即在=10 s 處對(duì)脫落原軌跡做了過渡處理；另一方面軌跡初始化技術(shù)的引入進(jìn)一步保證了系統(tǒng)的暫態(tài)性能。

4 結(jié) 論

通過理論和仿真分析，得到了如下成果：

a）建立了包含并聯(lián)機(jī)器人和液壓驅(qū)動(dòng)器動(dòng)態(tài)特性的加注機(jī)器人完整的高階多輸入多輸出模型。針對(duì)系統(tǒng)中存在的建模誤差和外界干擾，設(shè)計(jì)了一種具有擾動(dòng)抑制能力的高性能魯棒控制策略；

b）結(jié)合加注任務(wù)剖面，完成了對(duì)接軌跡設(shè)計(jì)和加注過程擾動(dòng)建模，對(duì)加注過程進(jìn)行了完整的仿真分析。仿真結(jié)果表明，魯棒控制器相較傳統(tǒng)PID 式控制器具有更強(qiáng)的軌跡跟蹤性能和抗干擾能力。

本文的研究成果對(duì)后續(xù)原理樣機(jī)的研究開展具有較強(qiáng)的參考價(jià)值，同時(shí)對(duì)其它并聯(lián)機(jī)器人的控制器設(shè)計(jì)同樣具有一定的借鑒意義。