錢昌年，傅俊勇，張 亮，朱康武

（上海航天控制技術(shù)研究所，上海 200233）

0 引言

火箭發(fā)動(dòng)機(jī)搖擺機(jī)構(gòu)是火箭控制系統(tǒng)的執(zhí)行機(jī)構(gòu)，其作用是響應(yīng)控制系統(tǒng)的指令，通過調(diào)整發(fā)動(dòng)機(jī)姿態(tài)改變發(fā)動(dòng)機(jī)推力方向，從而實(shí)現(xiàn)調(diào)整箭體的姿態(tài)。雙向搖擺火箭發(fā)動(dòng)機(jī)采用2臺(tái)伺服機(jī)構(gòu)進(jìn)行擺動(dòng)控制，實(shí)質(zhì)上構(gòu)成了一種空間二自由度并聯(lián)機(jī)構(gòu)。根據(jù)伺服機(jī)構(gòu)的伸縮位移求取發(fā)動(dòng)機(jī)的空間姿態(tài)是火箭姿態(tài)控制設(shè)計(jì)的必要環(huán)節(jié)，傳統(tǒng)采用近似線性化方法處理［1］。某新型雙向搖擺發(fā)動(dòng)機(jī)由于伺服機(jī)構(gòu)支點(diǎn)與搖擺點(diǎn)不在一個(gè)平面內(nèi)，采用傳統(tǒng)方法設(shè)計(jì)忽略了火箭發(fā)動(dòng)機(jī)擺角的正負(fù)不對(duì)稱性和俯仰擺角與偏航擺角的互相干擾，存在較大的誤差，需從并聯(lián)機(jī)構(gòu)的角度考慮火箭發(fā)動(dòng)機(jī)擺角解析解求取問題。并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)正向求解指在已知機(jī)構(gòu)輸入關(guān)節(jié)的條件下求解機(jī)構(gòu)末端的位置和姿態(tài)，反向求解指在已知機(jī)構(gòu)末端位置和姿態(tài)的條件下求解機(jī)構(gòu)的輸入關(guān)節(jié)變量。因并聯(lián)機(jī)構(gòu)的正向運(yùn)動(dòng)學(xué)方程具高階非線性特點(diǎn)，求解過程非常復(fù)雜［2］。一般，并聯(lián)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)正向求解遠(yuǎn)難于反向求解，正解多采用數(shù)值解法［3］。本文對(duì)新一代運(yùn)載火箭發(fā)動(dòng)機(jī)雙向搖擺機(jī)構(gòu)運(yùn)動(dòng)學(xué)進(jìn)行了研究。

1 系統(tǒng)描述

新一代運(yùn)載火箭并聯(lián)雙向搖擺機(jī)構(gòu)主要由發(fā)動(dòng)機(jī)，電液伺服機(jī)構(gòu)A、B，動(dòng)支架，常平架（與箭體固連）等組成，如圖1所示。常平架與動(dòng)支架以及動(dòng)支架與發(fā)動(dòng)機(jī)間分別由兩個(gè)互相垂直的鉸鏈連接，兩個(gè)鉸鏈關(guān)節(jié)的軸線相交于點(diǎn)O，發(fā)動(dòng)機(jī)能繞旋轉(zhuǎn)中心O作自由度定點(diǎn)轉(zhuǎn)動(dòng)。發(fā)動(dòng)機(jī)搖擺運(yùn)動(dòng)由2臺(tái)電液伺服機(jī)構(gòu)驅(qū)動(dòng)，其上下支點(diǎn)分別通過球鉸鏈與發(fā)動(dòng)機(jī)噴管和常平架連接。伺服機(jī)構(gòu)與發(fā)動(dòng)機(jī)的鉸接點(diǎn)分別為A，B；伺服機(jī)構(gòu)與常平架的鉸接點(diǎn)分別表示為OA，OB；平面OAOA垂直于平面OBOB。

圖1 發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)（初始姿態(tài)）Fig.1 Parallel two－way swiveling mechanism of launcher engine（initial configuration）

為描述發(fā)動(dòng)機(jī)姿態(tài)，定義以下坐標(biāo)系。

a）坐標(biāo)系O－xyz：固連于常平架，xOy平面與水平面平行，Ox軸位于平面OAOA；Oy軸位于平面OBOB。分別定義發(fā)動(dòng)機(jī)繞Ox，Oy軸的轉(zhuǎn)角為俯仰角和偏航角，發(fā)動(dòng)機(jī)姿態(tài)控制算法設(shè)計(jì)中主要考慮這兩者。

b）坐標(biāo)系O－x1y1z1：固連于常平架，由O－xyz系繞Oz軸逆時(shí)針旋轉(zhuǎn)45°而得，Ox1軸為連接動(dòng)支架和常平架的鉸鏈軸線；Oy1軸為連接發(fā)動(dòng)機(jī)和動(dòng)支架的鉸鏈軸線。

c）坐標(biāo)系O－x2y2z2：固連于動(dòng)支架。

d）坐標(biāo)系O－x3y3z3：固連于發(fā)動(dòng)機(jī)噴管。

發(fā)動(dòng)機(jī)在任意時(shí)刻的姿態(tài)可描述為：O－x3y3z3系和O－x2y2z2系在零位姿態(tài)與O－x1y1z1系重合，發(fā)動(dòng)機(jī)連體坐標(biāo)系先繞Ox1軸轉(zhuǎn)動(dòng)α，再繞Oy2軸轉(zhuǎn)動(dòng)β，如圖2所示。如已知發(fā)動(dòng)機(jī)兩個(gè)鉸鏈轉(zhuǎn)角α，β，就可通過轉(zhuǎn)換得到發(fā)動(dòng)機(jī)的俯仰角和偏航角（即發(fā)動(dòng)機(jī)繞Ox，Oy軸的轉(zhuǎn)角）。

用旋轉(zhuǎn)矩陣表示坐標(biāo)系間的相對(duì)方位［4］。O－x3y3z3系相對(duì)O－xyz系的方位可表示為

式中：

圖2 發(fā)動(dòng)機(jī)姿態(tài)Fig.2 Description for configuration of engine

展開式（1），可得

式中：nA，nB分別為n′A，n′B在O－xyz系中的表示。在O－xyz系中定義矢量

2 運(yùn)動(dòng)學(xué)正解

將兩臺(tái)伺服機(jī)構(gòu)作動(dòng)筒的伸縮量ΔlA，ΔlB分別作為發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)的兩個(gè)主動(dòng)輸入，發(fā)動(dòng)機(jī)的兩個(gè)鉸鏈轉(zhuǎn)角α，β為發(fā)動(dòng)機(jī)搖擺機(jī)構(gòu)的輸出（或是發(fā)動(dòng)機(jī)俯仰角和偏航角）。發(fā)動(dòng)機(jī)雙搖擺機(jī)構(gòu)的運(yùn)動(dòng)學(xué)正解是已知ΔlA，ΔlB求發(fā)動(dòng)機(jī)轉(zhuǎn)角α，β。因現(xiàn)有火箭發(fā)動(dòng)機(jī)不具備轉(zhuǎn)角傳感器，發(fā)動(dòng)機(jī)的姿態(tài)只能由伺服機(jī)構(gòu)作動(dòng)筒的伸縮量求得，故有必要求解搖擺機(jī)構(gòu)正向運(yùn)動(dòng)學(xué)問題。

在O－xyz系中，存在關(guān)系

式中：｜IA｜2＝（lA0＋ΔlA）2；｜IB｜2＝（lB0＋ΔlB）2。此處：lA0，lB0分別為已知的兩臺(tái)伺服機(jī)構(gòu)的初始長度。當(dāng)給定輸入ΔlA，ΔlB時(shí)，式（9）、（10）左邊為已知量，此即火箭發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)的運(yùn)動(dòng)約束方程。將式（2）～（8）代入式（9）、（10）并展開，可得關(guān)于α，β的一組方程

式（11）、（12）中只有α，β是未知量，其他參數(shù)均為結(jié)構(gòu)已知參數(shù)。式（11）、（12）是關(guān)于α，β超越方程，為便于求解，將半角公式

代入式（11）、（12），整理后得

成立［5］?；喓罂傻藐P(guān)于a的16次方程

式中：k1，k2，…，k17為已知參數(shù)的表達(dá)式。

用上述方法消去未知量b，將一組二元高次方程組轉(zhuǎn)為一元高次方程組，求解方程（15）可得a的16個(gè)解。此即求解高次方程的Sylvester消元法，但求解過程中需剔除引入的無效解。

式（15）是式（13）、（14）存在共同解的必要條件，并非所有a的實(shí)數(shù)解都能使式（13）、（14）同時(shí)成立。分別將a的實(shí)數(shù)解代入式（13）、（14），每個(gè)a的實(shí)數(shù)解都能由式（13）、（14）解出兩組b的解（每組4個(gè)），若兩組b的解中有相等的，則說明當(dāng)前a為有效解，否則舍去。根據(jù)發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)結(jié)構(gòu)特點(diǎn)，如不考慮運(yùn)動(dòng)范圍，對(duì)每一組輸入 ｛ΔlA，ΔlB｝，發(fā)動(dòng)機(jī)均有兩個(gè)姿態(tài)滿足約束條件（發(fā)動(dòng)機(jī)噴管朝上或朝下），故對(duì)一組輸入 ｛ΔlA，ΔlB｝，均能由式（13）～（15）確定兩組有效的｛a，b｝，對(duì)每個(gè)a或b的實(shí)數(shù)解，都能在［0°，360°］唯一確定α＝2arctana，β＝2arctanb，即可由兩組｛a，b｝確定兩組｛α，β｝。實(shí)際發(fā)動(dòng)機(jī)只限定在噴管朝上（圖1為發(fā)動(dòng)機(jī)實(shí)驗(yàn)環(huán)境，實(shí)際發(fā)動(dòng)機(jī)噴管朝下）的范圍內(nèi)運(yùn)動(dòng)，且發(fā)動(dòng)機(jī)在運(yùn)行過程中α，β∈（－10°，10°），由這些條件可在一系列求解結(jié)果中篩選出唯一解。在求解唯一解時(shí)，先判定解的有效性，再判斷解是否處于運(yùn)動(dòng)范圍內(nèi)，也可調(diào)換兩個(gè)過程的順序，能有效減少求解高次方程的次數(shù)。

設(shè)發(fā)動(dòng)機(jī)雙搖擺機(jī)構(gòu)結(jié)構(gòu)參數(shù)為

令輸入｛ΔlA，ΔlB｝＝｛－60mm，105mm｝，將參數(shù)代入式（15），可得

求解并舍去復(fù) 數(shù) 解，有a1＝－1.980 2，a2＝－0.065 9，a3＝0.058 0，a4＝5.406 6，與之相應(yīng)的α解 為α1＝－126.413 5°，α2＝－7.539 5°，α3＝6.633 6°，α4＝159.042 1°。顯然只有α2，α3在運(yùn)動(dòng)范圍（－10°，10°）內(nèi)，舍去其余解，為剔除無效解，將a2＝－0.065 9分別代入式（13）、（14）求解并舍去復(fù)數(shù)解，分別得b1＝－5.055 0，b2＝0.099 3；b1＝－3.205 5，b2＝－0.157 2?？芍猘2＝－0.065 9不能使式（13）、（14）同時(shí)成立，為無效解。將a3＝0.058 0分別代入式（13）、（14），求解并舍去復(fù)數(shù)解，分 別 得b1＝ －5.635 9，b2＝－ 0.018 0；b1＝－3.172 0，b2＝－0.018 0。判斷可知a3＝0.058 0，b2＝－0.018 0是式（13）、（14）的一組共同解，則β＝－2.062 4°，處于有效運(yùn)動(dòng)范圍內(nèi)。由此，當(dāng)輸入為｛ΔlA，ΔlB｝＝｛－60mm，105mm｝時(shí)，發(fā)動(dòng)機(jī)的鉸鏈轉(zhuǎn)角 ｛α，β｝＝ ｛6.633 6°，－2.062 4°｝。

3 運(yùn)動(dòng)學(xué)反解

發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)的運(yùn)動(dòng)學(xué)反解，就是在已知發(fā)動(dòng)機(jī)姿態(tài)（即已知鉸鏈轉(zhuǎn)角α，β）的前提下求解兩臺(tái)伺服機(jī)構(gòu)作動(dòng)筒的伸縮量，即已知發(fā)動(dòng)機(jī)推力矢量的方向 ｛α，β｝求 ｛ΔlA，ΔlB｝，搖擺機(jī)構(gòu)運(yùn)動(dòng)學(xué)反解直接用于火箭姿態(tài)控制算法。與運(yùn)動(dòng)學(xué)正向求解過程相比，運(yùn)動(dòng)學(xué)反向求解簡單。簡化式（9）、（10）并將已知參數(shù)代入，可得

式中：LA，LB（＞0）為已知參數(shù)的函數(shù)表達(dá)式，其物理意義為電液伺服機(jī)構(gòu)的總長度，且

由｜lA｜＝lA0＋ΔlA，｜lB｜＝lB0＋ΔlB，可得運(yùn)動(dòng)學(xué)反解 ΔlA＝LA－lA0，ΔlB＝LB－lB0。此處：lA0，lB0分別為兩臺(tái)伺服機(jī)構(gòu)的初始長度。

將 上 述 運(yùn) 動(dòng) 學(xué) 正 解 ｛α，β｝ ＝ ｛6.633 6°，－2.062 4°｝代 入 式 （16），（17）可 得LA＝1 782.885 5mm，LB＝1 947.990 9mm，與之對(duì)應(yīng)的｛ΔlA，ΔlB｝＝｛－60.114 5mm，104.990 9mm｝，該結(jié)果與上述正向求解的輸入值｛ΔlA，ΔlB｝＝｛－60mm，105mm｝基本一致。

4 SimMechanics仿真驗(yàn)證

為驗(yàn)證運(yùn)動(dòng)學(xué)正向求解算法在整個(gè)發(fā)動(dòng)機(jī)運(yùn)動(dòng)范圍內(nèi)的有效性和精度，根據(jù)發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)的物理和幾何關(guān)系，在SimMechanics環(huán)境中建立驗(yàn)證模型，在表1兩種輸入條件下對(duì)機(jī)構(gòu)模型進(jìn)行計(jì)算和仿真，結(jié)果如圖3～6所示［6］。

圖3 ΔlA＝－10mm時(shí)αFig.3 Calculating and simulation values of αatΔlA＝－10mm

圖4 ΔlA＝－10mm時(shí)βFig.4 Calculating and simulation values of βatΔlA＝－10mm

由圖可知：計(jì)算和仿真結(jié)果幾乎完全重合，證明計(jì)算結(jié)果和模型正確。其中α，β的誤差量級(jí)分別為10－4，10－3（β的誤差大于α的原因是求解β過程中多次截?cái)嗾`差累加），該誤差級(jí)別遠(yuǎn)小于機(jī)構(gòu)彈性變形和加工工藝導(dǎo)致的誤差，由此可見本文計(jì)算方法有充分的精度。

圖5 ΔlB＝－8mm時(shí)αFig.5 Calculating and simulation values of αatΔlB＝－10mm

圖6 ΔlB＝－10mm時(shí)βFig.6 Calculating and simulation values of βatΔlB＝－10mm

表1 兩種輸入條件Tab.1 Two input conditions

5 線性化方法誤差分析

某新型火箭研制過程中，用傳統(tǒng)線性化方法處理搖擺機(jī)構(gòu)伺服作動(dòng)筒伸縮位移和發(fā)動(dòng)機(jī)俯仰角與偏航角關(guān)系，即簡單認(rèn)為伺服機(jī)構(gòu)最大伸縮量110mm對(duì)應(yīng)于發(fā)動(dòng)機(jī)偏航角或俯仰角最大值6.5°，伺服機(jī)構(gòu)最小伸縮量－110mm對(duì)應(yīng)于發(fā)動(dòng)機(jī)偏航角或俯仰角最小值－6.5°，將兩者視為比例關(guān)系，其處理必存在誤差。另外，由于發(fā)動(dòng)機(jī)搖擺中心與伺服機(jī)構(gòu)支點(diǎn)不在一個(gè)平面，發(fā)動(dòng)機(jī)俯仰角和偏航角存在干擾。

在圖1中，當(dāng)俯仰伺服機(jī)構(gòu)B推動(dòng)發(fā)動(dòng)機(jī)運(yùn)動(dòng)時(shí)，發(fā)動(dòng)機(jī)繞Ox，Oy軸均有轉(zhuǎn)動(dòng)，即俯仰伺服機(jī)構(gòu)的運(yùn)動(dòng)對(duì)發(fā)動(dòng)機(jī)俯仰角和偏航角都有作用，偏航伺服機(jī)構(gòu)亦是如此。傳統(tǒng)近似處理方法忽略了伺服機(jī)構(gòu)運(yùn)動(dòng)對(duì)另一個(gè)轉(zhuǎn)角的影響。在表1輸入條件下分析上述近似線性化方法存在的誤差，結(jié)果如圖7～10所示。由圖可知：俯仰角（θx）和偏航角（θy）用近似線性化方法計(jì)算的結(jié)果存在較大誤差，且偏差隨輸入值增大而變大，最大偏差為0.4°／6.5°＝6.15%。

圖7 ΔlA＝－10mm時(shí)θxFig.7 Calculating and simulation values of θxatΔlA＝－10mm

圖8 ΔlA＝－10mm時(shí)θyFig.8 Calculating and simulation values of θyatΔlA＝－10mm

圖9 ΔlB＝－8mm時(shí)θxFig.9 Calculating and simulation values of θxatΔlB＝－8mm

圖10 ΔlB＝－8mm時(shí)θyFig.10 Calculating and simulation values of θyatΔlB＝－8mm

6 結(jié)束語

本文對(duì)火箭發(fā)動(dòng)機(jī)并聯(lián)雙搖擺機(jī)構(gòu)進(jìn)行了運(yùn)動(dòng)學(xué)分析，根據(jù)機(jī)構(gòu)物理構(gòu)成特點(diǎn)，用矢量方法列出了運(yùn)動(dòng)約束方程，引入半角公式并用Sylvester消元方法得到了一元16次方程，給出了運(yùn)動(dòng)學(xué)正向求解方法，并導(dǎo)出了運(yùn)動(dòng)學(xué)反解的解析表達(dá)式。SimMechanics建模仿真驗(yàn)證結(jié)果證明運(yùn)動(dòng)學(xué)正向求解方法正確。用本文方法分析了現(xiàn)有線性化近似計(jì)算方法的誤差。研究可為火箭控制系統(tǒng)設(shè)計(jì)提供參考。

［1］ 朱忠惠.推力矢量控制伺服系統(tǒng)［M］.北京：宇航出版社，1995：360－376.

［2］ PETUYA V，GUTIERREZ J M，ALONSO A，et al.A numerical procedure to solve non－linearkinematic problems in spatial mechanism［J］.International Journal for Numerical Methods in Engineering，2008，73（6）：825－843.

［3］ 黃 真.高等空間機(jī)構(gòu)學(xué)［M］.北京：高等教育出版社，2006：141－146.

［4］ 蔡自興.機(jī)器人學(xué)［M］.北京：清華大學(xué)出版社，2000：46－51.

［5］ 羅佑新.機(jī)構(gòu)綜合的結(jié)式消元法及其 MAPLE實(shí)現(xiàn)［J］.紡織學(xué)報(bào)，2005，26（2）：79－81.

［6］ 王英波，黃其濤，鄭書濤，等.Simulink和SimMechanics環(huán)境下并聯(lián)機(jī)器人動(dòng)力學(xué)建模與分析［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2012，33（1）：100－105.