并聯(lián)式加注機(jī)器人魯棒軌跡跟蹤控制

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

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

0 引 言

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

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

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

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

對于受風(fēng)載作用的火箭箭體，其運(yùn)動主要集中在3個平動自由度，而在3 個轉(zhuǎn)動自由度上的運(yùn)動量較小(不大于0.5°)。因此，自動加注機(jī)器人設(shè)計為三平動并聯(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)動簡圖與等效剛度模型見圖2，主要用于補(bǔ)償三平動并聯(lián)機(jī)器人在空間六自由度上的對接誤差。

圖2 柔順機(jī)構(gòu)運(yù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 個偏轉(zhuǎn)自由度幅值較小,因此柔順機(jī)構(gòu)在空間三平動自由度上近似解耦，其等效剛度模型如下：

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

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

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

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

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

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

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

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

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

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

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

式中q和q為液壓缸的輸出位移和速度；m為活塞桿和負(fù)載質(zhì)量；d為液壓缸的不匹配不確定誤差；τ為驅(qū)動力；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)完整動力學(xué)模型

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

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

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

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

2 控制律設(shè)計

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

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

定義虛擬控制量：

式中為正定對角矩陣。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 穩(wěn)定性分析

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

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

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

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

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

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

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

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

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

2.3 期望軌跡生成

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

3 仿真分析

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

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

針對液壓驅(qū)動器，有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。

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

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

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

3.2 仿真分析

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

3.2.1 工況1：對接保持階段工況仿真

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

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

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

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

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

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

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

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

圖6、圖7 為MRC 和VFPID 兩種算法對于低速運(yùn)動期望軌跡的跟蹤情況。仿真結(jié)果表明，MRC 和VFPID 這兩種算法對于低速運(yùn)動皆表現(xiàn)出較高的軌跡跟蹤精度，同時MRC 控制器相對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 控制器誤差的波動明顯小于VFPID。由此可見，VFPID 雖然具有一定的抗干擾能力，但是其本質(zhì)上是一種無模型控制算法，其對擾動的響應(yīng)總是起作用于誤差發(fā)生之后，因此當(dāng)瞬時擾動(軸向鎖緊力)產(chǎn)生時，其缺少快速響應(yīng)的能力，從而造成跟蹤誤差相對較大的惡化。

3.2.2 工況2：隨動脫落階段工況仿真

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

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

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

4 結(jié) 論

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

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

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

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