推力矢量控制機(jī)械臂運(yùn)動(dòng)規(guī)劃及仿真

許路路，郭衛(wèi)東，杜志貴，鄒慶曉

（1.北京航空航天大學(xué)機(jī)器人研究所，北京 100191；2.中國(guó)科學(xué)院光電研究院，北京 100094）

1 引言

推力矢量控制技術(shù)作為一種非傳統(tǒng)的飛行控制技術(shù)，主要應(yīng)用在軍用飛機(jī)的設(shè)計(jì)和改型方面，是指飛機(jī)的推進(jìn)系統(tǒng)除提供向前的推力外，還能通過(guò)改變飛機(jī)發(fā)動(dòng)機(jī)的噴射方向來(lái)提供飛機(jī)俯仰、偏航和翻滾的推力，因此在短距起降和超機(jī)動(dòng)性能方面相對(duì)于常規(guī)類(lèi)型的飛機(jī)有著不可比擬的優(yōu)勢(shì)。目前常采用三軸承推力矢量噴管[1]的結(jié)構(gòu)形式，并成功應(yīng)用于蘇聯(lián)雅克141戰(zhàn)斗機(jī)和美國(guó)F-35B戰(zhàn)斗機(jī)[2-3]?？臻g飛行器常采用離子發(fā)動(dòng)機(jī)作為推進(jìn)系統(tǒng)，結(jié)構(gòu)復(fù)雜，成本高，并且只能提供向前的推力，機(jī)動(dòng)性很差。而將推力矢量技術(shù)應(yīng)用在空間飛行器的應(yīng)用上很少有人研究，結(jié)合近年來(lái)機(jī)械臂技術(shù)發(fā)展迅猛，提出一種將推力矢量噴管與機(jī)械臂相結(jié)合的機(jī)構(gòu)，作為空間飛行器的推進(jìn)系統(tǒng)，并且機(jī)械臂的控制方法采用推力矢量控制。這種結(jié)構(gòu)簡(jiǎn)單、成本低，并大大提高了空間飛行器的機(jī)動(dòng)性能。

推力矢量控制技術(shù)應(yīng)用在機(jī)械臂上主要是指在機(jī)械臂末端安裝矢量噴管，噴管?chē)姎馐乖摍C(jī)械臂末端獲得反推力，通過(guò)調(diào)整矢量噴管的方向，可對(duì)飛行器的位置和姿態(tài)進(jìn)行控制。通過(guò)力的傳遞性原理可知，力的作用點(diǎn)只需在該力的作用線上即可，具體位置并不影響力臂的大小。因此可知，推力矢量控制不同于常規(guī)機(jī)械臂的確定位置和確定姿態(tài)的控制，該機(jī)械臂的末端位置不是固定的，末端原點(diǎn)坐標(biāo)只需在該力的作用線上即可。該區(qū)別主要體現(xiàn)在對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)反解的過(guò)程中。氣浮平臺(tái)是指通過(guò)壓縮空氣在氣浮軸承和軸承座之間形成氣膜，從而使氣浮臺(tái)懸浮，抵消重力影響的同時(shí)實(shí)現(xiàn)了近似無(wú)摩擦的相對(duì)運(yùn)動(dòng)條件，能夠模擬飛行器在軌失重條件下的姿態(tài)運(yùn)動(dòng)，是在地面條件下研究空間飛行器姿態(tài)動(dòng)力學(xué)和控制系統(tǒng)的理想平臺(tái)[4]。搭載矢量控制機(jī)械臂的氣浮平臺(tái)系統(tǒng)，如圖1所示。

圖1 氣浮平臺(tái)Fig.1 Air-floating Platform

推力矢量控制機(jī)械臂具有四個(gè)自由度，如圖1所示。采用相鄰轉(zhuǎn)軸兩兩垂直的構(gòu)型。采用單臂的控制方式，只模擬飛行器的位置控制，主要控制因素為末端機(jī)械臂輸出力的方向，而力臂的大小在此不作為控制因素。因此已知條件為：機(jī)械臂末端噴嘴軸線的方向（i，j，k）和該方向通過(guò)一定點(diǎn)（x0，y0，z0），改點(diǎn)設(shè)定為系統(tǒng)的質(zhì)心，力的作用線經(jīng)過(guò)系統(tǒng)的質(zhì)心，因此可使系統(tǒng)沿該力的方向移動(dòng)。

首先采用參數(shù)法對(duì)該機(jī)械臂進(jìn)行正向和逆向運(yùn)動(dòng)學(xué)的求解[5-9]，通過(guò)求解結(jié)果論證了該結(jié)構(gòu)應(yīng)用于機(jī)械臂末端矢量控制的正確性，其中逆解的數(shù)目取決于關(guān)節(jié)數(shù)目、連桿參數(shù)和關(guān)節(jié)變量的活動(dòng)范圍；然后利用MATLAB中的robotics工具箱仿真分析了該機(jī)械臂的工作范圍；最后進(jìn)行算例驗(yàn)證所建立的運(yùn)動(dòng)學(xué)模型的準(zhǔn)確性。

2 運(yùn)動(dòng)學(xué)分析

2.1 正運(yùn)動(dòng)學(xué)分析

利用D-H參數(shù)法建立坐標(biāo)系，如圖2所示。

圖2 D-H坐標(biāo)系Fig.2 D-H Coordinates

機(jī)械臂各連桿參數(shù)以及關(guān)節(jié)變量，如表1所示。

表1 D-H參數(shù)表Tab.1 D-H Parameters

為建立運(yùn)動(dòng)學(xué)方程，用齊次變換矩陣來(lái)表示連桿坐標(biāo)系i在連桿坐標(biāo)系i-1中的位置和姿態(tài)，可得：

即可求得機(jī)械臂末端的位姿，即噴管反推力的位置和方向。

2.2 逆運(yùn)動(dòng)學(xué)分析

一般對(duì)常規(guī)機(jī)械臂進(jìn)行控制時(shí)，已知條件為機(jī)械臂的確定位置和確定姿態(tài)。前言中提到，采用推力矢量控制時(shí)，該機(jī)械臂的末端位置不是固定的，因此在進(jìn)行運(yùn)動(dòng)學(xué)反解的過(guò)程中，需要根據(jù)條件首先求解機(jī)械臂末端的位置，然后再求解機(jī)械臂各關(guān)節(jié)的轉(zhuǎn)角。

已知條件為：

（1）機(jī)械臂末端坐標(biāo)系O4x4y4z4中x4在基坐標(biāo)系下的方向余弦，即：

（2）機(jī)械臂末端所在直線過(guò)一定點(diǎn)，設(shè)該點(diǎn)為M（xm，ym，zm），即空間飛行器的質(zhì)心坐標(biāo)。

通過(guò)上述兩個(gè)條件，反推力方向確定，并經(jīng)過(guò)空間飛行器質(zhì)心。將運(yùn)動(dòng)學(xué)反解過(guò)程分為兩部，首先求解機(jī)械臂末端位置坐標(biāo)，其次求解各關(guān)節(jié)轉(zhuǎn)角。

2.2.1 求解機(jī)械臂末端位置坐標(biāo)

設(shè)機(jī)械臂末端所在直線l的方程為：

式中：S（nx，ny，nz）—直線 l的方向向量，即 x4—軸在基坐標(biāo)下的方向余弦，P（px，py，pz）—坐標(biāo)系 O4x4y4z4的原點(diǎn)位置。

已知直線 l過(guò)點(diǎn) M（xm，ym，zm），則聯(lián)立式（4）、式（5）和式（9），消元 t和 sθ2可得：

根據(jù)費(fèi)拉里法可求得式（10）中pz的四個(gè)解，具體過(guò)程在此不做介紹。

接下來(lái)聯(lián)立式（4）、式（5）和式（9），并判斷 nz是否為 0，即可求得px，py，pz的數(shù)值。至此，機(jī)械臂末端位置坐標(biāo)求解完畢。

2.2.2 求解各關(guān)節(jié)轉(zhuǎn)角

根據(jù)式（6），可求解出：

已知 l1＞l2+l3，因此 l1+（l2+l3）sθ2＞0，因此根據(jù)式（4）和式（5）可知：

根據(jù)式（7），可求解出：

已知-90°＜θ4＜90°，因此可知 cθ4≠0，根據(jù)式（8），可求解出：

至此，關(guān)節(jié)轉(zhuǎn)角 θ1、θ2、θ3、θ4可全部求解。

3 MATLAB仿真分析

3.1 利用link和robot函數(shù)建立機(jī)械臂模型

link 函數(shù)的調(diào)用形式為：L=link（［alpha A theta D sigma］，CONVENTION）；robot函數(shù)的調(diào)用形式為：robot（link，…）。即用link創(chuàng)建機(jī)械臂對(duì)象。矢量控制機(jī)械臂模型，如圖3所示。

圖3 矢量控制機(jī)械臂模型Fig.3 The Model of Vector Control Manipulator

已知連桿參數(shù)：l1=200mm，l2=150mm，l3=30mm，-90°＜θ1＜90°，0°＜θ2＜180°，-180°＜θ3＜180°，-90°＜θ4＜90°。

建立機(jī)械臂模型需要的命令如下：

L1=link（［00000］，‘mod’）；

L2=link（［-pi/2200000］，‘mod’）；

L3=link（［-pi/20pi/21500］，‘mod’）；

L4=link（［pi/200300］，‘mod’）；

RRRR.name=‘推力矢量控制機(jī)械臂’；

drivebot（RRRR）；

在MATLAB上運(yùn)行可生成矢量控制機(jī)械臂模型，如圖4所示。

3.2 模型的驗(yàn)證

基于主要對(duì)機(jī)械臂末端推力矢量的方向進(jìn)行控制的要求，完成了該機(jī)械臂的運(yùn)動(dòng)模型的建立，下面主要是驗(yàn)證該模型是否滿足規(guī)定要求。

假設(shè)質(zhì)心M（xm，ym，zm）在基坐標(biāo)系的平面y0o0z0，即xm=-200，需要驗(yàn)證的是：

（1）驗(yàn)證點(diǎn)M在y0o0z0平面中的范圍，即質(zhì)心得取值范圍。

（2）給定確定數(shù)值的質(zhì)心坐標(biāo)，機(jī)械臂末端繞y4軸和z4軸的擺動(dòng)范圍，即反推力的方向。

fkine函數(shù)能夠?qū)崿F(xiàn)機(jī)械臂正向運(yùn)動(dòng)學(xué)的求解。調(diào)用形式為：T=fkine（ROBOT，q）。

由式（2）可以得到機(jī)械臂末端的位置及姿態(tài)相關(guān)參數(shù)nx，ny，nz，px，py，pz。

[2] James E. Dougherty, Robert L. Pfaltzgraff, Contending Theories of International Relations: a Comprehensive Survey(5th Edition), 北京：北京大學(xué)出版社, 2004年, pp. 165-166.

而點(diǎn) M（xm，ym，zm）滿足直線 l方程，并已知 xm=-200，因此可確定點(diǎn)M（xm，ym，zm）的值。

3.2.1 驗(yàn)證點(diǎn)M在y0o0z0平面中的范圍

設(shè)定各關(guān)節(jié)轉(zhuǎn)動(dòng)步長(zhǎng)為9°，將計(jì)算得出的點(diǎn)M（xm，ym，zm）在y0o0z0平面表示，如圖5所示。

圖4 平面內(nèi)的質(zhì)心坐標(biāo)Fig.4 Centroid Coordinates in XY Plane

由圖4可知，質(zhì)心坐標(biāo)集中在以坐標(biāo)原點(diǎn)為圓心，半徑為500mm的圓形區(qū)域中，通過(guò)調(diào)節(jié)連桿l1，l2，l3的長(zhǎng)度及各關(guān)節(jié)轉(zhuǎn)角范圍，即可改變扇形區(qū)域的大小，從而使得機(jī)構(gòu)的質(zhì)心坐標(biāo)在圓形區(qū)域內(nèi)，滿足使用要求。

3.2.2 驗(yàn)證機(jī)械臂末端的擺動(dòng)范圍

選定質(zhì)心的坐標(biāo)為 M（-200，100，0），由式（13）可知 px，py，pz，nx，ny，nz滿足方程：

設(shè)定各關(guān)節(jié)轉(zhuǎn)動(dòng)步長(zhǎng)為 3°，利用 fkine 函數(shù)求解出 px，py，pz，nx，ny，nz，根據(jù)轉(zhuǎn)動(dòng)步長(zhǎng)為 2°，修改式（13）為：

通過(guò)式（16）將數(shù)據(jù)進(jìn)行篩選，提取符合要求的數(shù)據(jù)中的ny和nz，求解ny和nz的反余弦，如圖6所示。

圖5 機(jī)械臂末端的擺動(dòng)范圍Fig.5 The Swing Range of ManipulatorEnd-effector

擺角是在基坐標(biāo)系O0x0y0z0下衡量的角度，如圖5所示。由此可知，給定質(zhì)心坐標(biāo)后，擺角范圍即可確定，工作空間即確定。通過(guò)調(diào)節(jié)連桿的長(zhǎng)度以及各關(guān)節(jié)轉(zhuǎn)角范圍，可改變?cè)摲秶拇笮?，從而使該機(jī)械臂滿足使用要求。

4 算例驗(yàn)證

選定機(jī)構(gòu)質(zhì)心M的坐標(biāo)為（-200，100，0），機(jī)械臂末端x4軸的擺角為 y=60°，z=80°，由式（3）可得：

將 xm，ym，zm，nx，ny，nz代入已解出各反解方程，可得關(guān)節(jié)轉(zhuǎn)角，如表2所示。

表2 反解結(jié)果Tab.2 The Results of Inverse Kinematics

由表2可知存在8組解，需要找到最優(yōu)解，可根據(jù)以下準(zhǔn)則尋優(yōu)：

首先，排除虛數(shù)解的情況；其次，因?yàn)棣?有兩組解，但并非所有的解都適合方程，可通過(guò)驗(yàn)證正向運(yùn)動(dòng)學(xué)模型是否成立來(lái)判斷哪組解滿足要求；最后，如果有多組解都滿足要求，根據(jù)運(yùn)動(dòng)的連續(xù)性，各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)角度最小的原則，選取與機(jī)械臂上一姿態(tài)最為接近的解作為最優(yōu)解。

通過(guò)分析可知第1組解即為需要的最優(yōu)解，由正解可解此時(shí)質(zhì)心坐標(biāo)為（-200，100，0），并且 nx=0.8484，ny=0.5，nz=0.1736，滿足要求。

5 結(jié)論

提出了一種采用推力矢量控制方式的四自由度機(jī)械臂。首先分析了該機(jī)械臂異于常規(guī)機(jī)械臂的控制原理；其次，對(duì)該機(jī)械臂進(jìn)行了運(yùn)動(dòng)學(xué)建模，并驗(yàn)證了結(jié)構(gòu)的正確性；最后利用MATLAB軟件對(duì)正向運(yùn)動(dòng)學(xué)進(jìn)行了仿真分析，得到了一定桿長(zhǎng)下質(zhì)心坐標(biāo)的范圍，并對(duì)一定質(zhì)心坐標(biāo)下機(jī)械臂末端的擺動(dòng)范圍進(jìn)行了分析。

結(jié)合推力矢量控制技術(shù)在軍用飛機(jī)上大范圍的應(yīng)用以及目前機(jī)械臂的迅猛發(fā)展，提出的推力矢量控制機(jī)械臂為機(jī)械臂在飛行器上作為推進(jìn)器的應(yīng)用提供了一定的參考價(jià)值。