朱元平，湯心溢，袁修鵬（1. 上海科技大學(xué) 信息學(xué)院，上海，200030；2. 中國(guó)科學(xué)院上海技術(shù)物理研究所，上海，200080；3. 中國(guó)科學(xué)院紅外探測(cè)與成像技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海，200080；4. 上海市現(xiàn)場(chǎng)物證重點(diǎn)實(shí)驗(yàn)室，上海，200080）

楔形穩(wěn)定平臺(tái)的等效機(jī)械臂模型

朱元平1，2，3，4，湯心溢1，2，3，4，袁修鵬2，3，4
（1. 上?？萍即髮W(xué) 信息學(xué)院，上海，200030；2. 中國(guó)科學(xué)院上海技術(shù)物理研究所，上海，200080；3. 中國(guó)科學(xué)院紅外探測(cè)與成像技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海，200080；4. 上海市現(xiàn)場(chǎng)物證重點(diǎn)實(shí)驗(yàn)室，上海，200080）

為使運(yùn)動(dòng)載體上的觀測(cè)儀器相對(duì)于大地保持靜止，需要使用穩(wěn)定平臺(tái)對(duì)運(yùn)動(dòng)載體的擾動(dòng)進(jìn)行補(bǔ)償。楔形穩(wěn)定平臺(tái)利用多個(gè)堆疊楔塊之間的相對(duì)轉(zhuǎn)動(dòng)，改變頂部平面的傾角以抵消船搖擾動(dòng)。它具有成本低、精度高、負(fù)載轉(zhuǎn)矩小以及重量輕、能耗低等優(yōu)點(diǎn)。然而楔形穩(wěn)定平臺(tái)獨(dú)特的結(jié)構(gòu)使得其分析較為困難。因此本文提出一種將三維空間內(nèi)楔塊的轉(zhuǎn)動(dòng)近似等效成二維空間內(nèi)機(jī)械臂運(yùn)動(dòng)的簡(jiǎn)化變換，并以此分析比較了雙層、改進(jìn)型雙層、多層等楔形穩(wěn)定平臺(tái)的結(jié)構(gòu)，得出改進(jìn)型雙層結(jié)構(gòu)是最佳實(shí)踐方案。最后利用旋轉(zhuǎn)矩陣進(jìn)行坐標(biāo)系變換，計(jì)算得到改進(jìn)型雙層結(jié)構(gòu)楔形穩(wěn)定平臺(tái)的控制函數(shù)。

楔形穩(wěn)定平臺(tái)；船搖；等效機(jī)械臂；層級(jí)結(jié)構(gòu)；

0 引 言

穩(wěn)定平臺(tái)通常用于隔離艦船、車輛等運(yùn)動(dòng)載體的擾動(dòng)，使其裝載的觀測(cè)儀器相對(duì)于大地坐標(biāo)系保持靜止。楔形穩(wěn)定平臺(tái)是一種新型穩(wěn)定平臺(tái)。它最先由荷蘭信號(hào)公司設(shè)計(jì)［1］，現(xiàn)已應(yīng)用于艦載“天狼星”遠(yuǎn)程紅外搜索與跟蹤系統(tǒng)［2］。與陀螺儀式穩(wěn)定平臺(tái)不同，楔形穩(wěn)定平臺(tái)主要由若干自由轉(zhuǎn)動(dòng)的楔塊組成。楔塊的轉(zhuǎn)動(dòng)能改變頂部平面的傾角，若傾角時(shí)時(shí)與載體搖擺的方向相反，就能抵消擾動(dòng)。

1 等效機(jī)械臂模型推導(dǎo)

楔塊的轉(zhuǎn)動(dòng)與頂部平面傾角的變化關(guān)系并不直觀。為了使分析更為形象，在此先引入等效機(jī)械臂的概念。圖 2 中，ns，nv，n1和 n2分別為海平面、艦船和 2 個(gè)楔塊上平面的單位法向量。假定 nv靜止，則艦船搖動(dòng)等效于 ns相對(duì)于 nv擺動(dòng)。再假設(shè)楔塊夾角均為β，則 n1與 nv和 n2與 n1的夾角也是 β。那么當(dāng)楔塊 1旋轉(zhuǎn)時(shí)，n1以固定夾角 β 圍繞 nv旋轉(zhuǎn)。同理當(dāng)楔塊 2旋轉(zhuǎn)時(shí)，n2以固定夾角 β 圍繞 n1旋轉(zhuǎn)。將上述單位向量平移使它們的起點(diǎn)重合，從而終點(diǎn)位于同一單位球面（見圖 3）。于是三維空間內(nèi)楔塊的旋轉(zhuǎn)可以等效為二維球面空間內(nèi)圓心做相同角度的旋轉(zhuǎn)。

圖 1 “天狼星”系統(tǒng)的楔形穩(wěn)定平臺(tái)［3］Fig. 1 The wedge-platform of Sirius IRST

圖 2 海平面、艦船和楔形穩(wěn)定平臺(tái)的單位法向量示意圖Fig. 2 Schematic diagram of unit normal vectors of sea level，vessel and wedge-platform

圖 3 海平面、艦船和楔形穩(wěn)定平臺(tái)的單位法向量球面圖Fig. 3 Sphere map of unit normal vectors of sea level， vessel and wedge-platform

因?yàn)榇瑩u的角度和楔塊的楔角都比較小，所以點(diǎn)Ns，N1，N2均僅在以 Nv為中心的小范圍內(nèi)運(yùn)動(dòng)。在不影響定性分析準(zhǔn)確性的前提下，不妨用從北極方向看去的平面投影（簡(jiǎn)稱“平面圖”）代替球面圖，同時(shí)將球面上的弧近似為平面上的線段。平面圖中 y 軸沿艦船縱軸方向指向船首，x 軸沿橫軸方向指向右舷。Ns在 Nv周圍一定區(qū)域內(nèi)做隨機(jī)運(yùn)動(dòng)，線段 ON1圍繞O（Nv）旋轉(zhuǎn)，N1N2圍繞 N1旋轉(zhuǎn)，轉(zhuǎn)角大小等于楔塊轉(zhuǎn)動(dòng)的角度。根據(jù) 2 個(gè)線段的運(yùn)動(dòng)形態(tài)，可以將 ON1和 N1N2等效為一個(gè)二維自由度機(jī)械臂的上臂和前臂，O 和 N1等效為機(jī)械臂肩關(guān)節(jié)和肘關(guān)節(jié)，N2等效為機(jī) 械手。用等效機(jī)械臂模型來描述楔形穩(wěn)定平臺(tái)的工作原理，就是控制機(jī)械臂 ON1N2讓機(jī)械手N2實(shí)時(shí)跟蹤目標(biāo)點(diǎn) Ns的運(yùn)動(dòng)。

圖 4 平面圖和等效機(jī)械臂Fig. 4 Plane graph and equivalent manipulator

2 楔角和層級(jí)結(jié)構(gòu)分析

在設(shè)計(jì)楔形穩(wěn)定平臺(tái)的機(jī)械結(jié)構(gòu)時(shí)，必須考慮楔角和層級(jí)結(jié)構(gòu)。楔角主要影響平定平臺(tái)的補(bǔ)償范圍，而層級(jí)結(jié)構(gòu)則對(duì)平臺(tái)的補(bǔ)償效果有決定性影響。

2.1 楔塊的楔角

平面圖中等效機(jī)械臂的臂長(zhǎng)與楔角大小成正比，同時(shí)臂長(zhǎng)決定了活動(dòng)范圍。較大的楔角雖然能增加活動(dòng)范圍，但也會(huì)使整體重心不穩(wěn)。對(duì)于雙層結(jié)構(gòu)楔形穩(wěn)定平臺(tái)而言如果 2 個(gè)楔角不相等（即臂長(zhǎng)不相等），機(jī)械手將無法觸及肩關(guān)節(jié)及其附近的盲點(diǎn)區(qū)域。因此雙層結(jié)構(gòu)的 2 個(gè)楔角必須相等。多層結(jié)構(gòu)雖然沒有這個(gè)問題，但在設(shè)計(jì)時(shí)也應(yīng)當(dāng)考慮到楔角變化對(duì)控制效果的影響，合理設(shè)計(jì)每層楔塊的楔角大小。

2.2 雙層結(jié)構(gòu)

圖 5 楔角不相等時(shí)雙層結(jié)構(gòu)楔形穩(wěn)定平臺(tái)的盲點(diǎn)區(qū)域Fig. 5 Blind zone of 2-layer structure when wedge angles are not equal

圖 6 雙層結(jié)構(gòu)和“轉(zhuǎn)角階躍”示意圖Fig. 6 Schematic diagram of 2-layer structure and rotation angle step

雙層結(jié)構(gòu)是指由一個(gè)水平底座和 2 個(gè)可旋轉(zhuǎn)的楔塊組成的楔形穩(wěn)定平臺(tái)的設(shè)計(jì)結(jié)構(gòu)。平面圖中雙層結(jié)構(gòu)等效為上臂與前臂等長(zhǎng)的機(jī)械臂，且肩關(guān)節(jié)與原點(diǎn)重合。雖然這種結(jié)構(gòu)最為簡(jiǎn)單，但是它有一個(gè)明顯缺陷——當(dāng)跟蹤軌跡過肩關(guān)節(jié)（原點(diǎn)）時(shí)，上臂（楔塊1）會(huì)發(fā)生“轉(zhuǎn)角階躍”（轉(zhuǎn)角變化不連續(xù)）。為說明這個(gè)情況，不妨假設(shè)目標(biāo)點(diǎn)的運(yùn)行軌跡為 P1經(jīng)過 O 到 P2的一段折線（見圖 6）。機(jī)械臂可以在 P1O 上方運(yùn)行或者下方運(yùn)行，兩者沒有本質(zhì)區(qū)別。假定在上方運(yùn)行，當(dāng)目標(biāo)點(diǎn)沿著 P1O 經(jīng)過原點(diǎn)時(shí)，機(jī)械臂與 y 軸重合。然而當(dāng)目標(biāo)點(diǎn)轉(zhuǎn)向沿著 O P2運(yùn)行時(shí)，上臂必須突然轉(zhuǎn)動(dòng) 90° 到與 y 軸垂直的位置才能使機(jī)械手繼續(xù)沿著 O P2運(yùn)動(dòng)。但實(shí)際上伺服系統(tǒng)的階躍響應(yīng)需要較長(zhǎng)時(shí)間，因此“轉(zhuǎn)角階躍”無法精確實(shí)現(xiàn)的。這就會(huì)導(dǎo)致雙層結(jié)構(gòu)楔形穩(wěn)定平臺(tái)的跟蹤精度大大下降。

2.3 改進(jìn)型雙層結(jié)構(gòu)

之所以發(fā)生“轉(zhuǎn)角階躍”是因?yàn)闄C(jī)械手要經(jīng)過與原點(diǎn)重合的肩關(guān)節(jié)。若將肩關(guān)節(jié)與原點(diǎn)分開，就可以避免這個(gè)問題。為此可以將底座改成相同楔角的楔塊（稱為楔塊 0）。從平面圖上看楔塊 0 的作用是將肩關(guān)節(jié)從原點(diǎn)移到了跟蹤區(qū)域外的 N0?？梢宰C明在肩關(guān)節(jié)和肘關(guān)節(jié)連續(xù)轉(zhuǎn)動(dòng)的情況下，機(jī)械手能夠跟蹤區(qū)域內(nèi)的任意軌跡。并且針對(duì)每一個(gè)區(qū)域內(nèi)的點(diǎn)，上臂和前臂的轉(zhuǎn)角 q1，q2（0°≤q1≤180°，0°≤q2≤180°）有且只有唯一解。

圖 7 改進(jìn)雙層結(jié)構(gòu)的示意圖和平面圖Fig. 7 Schematic diagram and plane graph of advanced 2-layer structure

2.4 多層結(jié)構(gòu)

如果有 3 層或 3 層以上可旋轉(zhuǎn)的楔塊，則稱之為多層結(jié)構(gòu)。平面圖中多層結(jié)構(gòu)等效為 3 節(jié)或 3 節(jié)以上機(jī)械臂。雖然多層結(jié)構(gòu)提供了更高的控制靈活性，但是鑒于雙層結(jié)構(gòu)已經(jīng)能夠達(dá)到二維活動(dòng)空間的自由度要求，增設(shè)的楔塊不僅提高了成本，而且多層結(jié)構(gòu)中楔塊轉(zhuǎn)角的解不唯一，徒增控制復(fù)雜度。

圖 8 多層結(jié)構(gòu)的示意圖和平面圖Fig. 8 Schematic diagram and plane graph of multi-layer structure

3. 楔形塊轉(zhuǎn)角的控制函數(shù)

通過第 2 節(jié)對(duì)不同的楔形穩(wěn)定平臺(tái)的結(jié)構(gòu)分析比較不難發(fā)現(xiàn)，改進(jìn)型雙層結(jié)構(gòu)平衡了可用性和經(jīng)濟(jì)性，符合大多數(shù)情況下對(duì)穩(wěn)臺(tái)的需求。接下來介紹改進(jìn)型雙層結(jié)構(gòu)楔形穩(wěn)定平臺(tái)的控制函數(shù)及求解方法。

3.1 平臺(tái)系統(tǒng)中坐標(biāo)系的定義

因?yàn)樾ㄐ畏€(wěn)定平臺(tái)主要用于隔離艦船偏航、橫滾和縱搖 3 種旋轉(zhuǎn)擾動(dòng)的影響，所以忽略各坐標(biāo)系原點(diǎn)的相對(duì)位置而僅考慮各坐標(biāo)系間旋轉(zhuǎn)關(guān)系。首先對(duì)初始狀態(tài)下各坐標(biāo)系的相對(duì)位置做設(shè)定。

自下而上依次為大地坐標(biāo)系：zs指向天空與海平面垂直，ys指向正北，xs指向正東；艦船載體坐標(biāo)系：zv軸指向艦船載體正上方與裝載平面垂直，yv軸指向艦首，xv軸指向右舷，初始狀態(tài)下艦船坐標(biāo)系與大地坐標(biāo)系重合；楔塊 0 坐標(biāo)系：z0垂直于楔塊 0 上平面，y0軸指向楔角方向，x0軸與 z0和 y0成右手螺旋關(guān)系，初始狀態(tài)下 x0指向右舷；楔塊 1 坐標(biāo)系：z1垂直于楔塊 1 上平面，y1軸指向楔角方向，x1軸與 z1和y1成右手螺旋關(guān)系，初始狀態(tài)下 x1指向左舷；楔塊 2坐標(biāo)系：z2垂直于楔塊 2 上平面，y2軸指向楔角方向，x2軸與 z2和 y2成右手螺旋關(guān)系，初始狀態(tài)下 x2指向右舷；圓盤坐標(biāo)系：zp垂直于圓盤，yp，xp與 zp成右手螺旋關(guān)系，初始狀態(tài)下圓盤坐標(biāo)系與楔塊 2 坐標(biāo)系重合。

圖 9 各坐標(biāo)系相對(duì)關(guān)系示意圖Fig. 9 Schematic diagram of relative coordinate systems

3.2 旋轉(zhuǎn)矩陣和坐標(biāo)系轉(zhuǎn)換

旋轉(zhuǎn)的正方向和轉(zhuǎn)角根據(jù)初始狀態(tài)坐標(biāo)系以右手螺旋法則確定。按照我國(guó)慣用的慣性元件檢測(cè)順序，從大地坐標(biāo)系向艦船坐標(biāo)系的轉(zhuǎn)換應(yīng)按照偏航-橫滾-縱搖的順序進(jìn)行［4］。用 H、R、P 分別表示偏航角、橫滾角和縱搖角，那么

偏航旋轉(zhuǎn)矩陣：

橫滾旋轉(zhuǎn)矩陣：

縱搖旋轉(zhuǎn)矩陣：

船搖擾動(dòng)等效矩陣：

楔塊對(duì)坐標(biāo)系旋轉(zhuǎn)變化的影響包括轉(zhuǎn)角旋轉(zhuǎn)和楔角旋轉(zhuǎn) 2 個(gè)部分。分別用 β 表示楔角，q1、q2和 q3表示楔塊 1、楔塊 2 和圓盤的轉(zhuǎn)角，那么

楔角旋轉(zhuǎn)矩陣：

楔塊 1 轉(zhuǎn)角旋轉(zhuǎn)矩陣：

楔塊 2 轉(zhuǎn)角旋轉(zhuǎn)矩陣：

頂圓盤轉(zhuǎn)角旋轉(zhuǎn)矩陣：

補(bǔ)償?shù)刃D(zhuǎn)矩陣：

于是從大地坐標(biāo)系到圓盤坐標(biāo)系的完整變換過程可以用這些旋轉(zhuǎn)矩陣的乘積來表示：

3.3 楔塊的控制函數(shù)

楔形穩(wěn)定平臺(tái)可以準(zhǔn)確補(bǔ)償偏航、橫滾和縱搖 3個(gè)方向的任何擾動(dòng)，也就是說對(duì)于大地坐標(biāo)系的任意被觀測(cè)向量 a 在經(jīng)過擾動(dòng)和補(bǔ)償之后，在圓盤坐標(biāo)系上應(yīng)當(dāng)保持不變［5］。用向量和旋轉(zhuǎn)矩陣則表示為：

即

式中 q1，q2為楔塊 1 與楔塊 2 的轉(zhuǎn)角。由于楔塊 1 與楔塊 2 的作用僅僅是保持被穩(wěn)定對(duì)象在垂直方向上的穩(wěn)定，不妨取為方便計(jì)算可以將式（12）一部分旋轉(zhuǎn)矩陣移動(dòng)到等式左邊，并代入 a'：

由上式可以算得：

因?yàn)?q1與 q2的取值范圍都是 0°～180°（見圖 7），

所以應(yīng)當(dāng)通過 cos（q1） 和 cos（q2） 計(jì)算q1和 q2。

式中 q3為圓盤的轉(zhuǎn)角，用于補(bǔ)償水平方向上偏航擾動(dòng)和楔塊 1 和楔塊 2 轉(zhuǎn)動(dòng)時(shí)附加的影響，因此可以取由于 q3的取值范圍是 0°～360°，所以要綜合考慮 cos（q3） 和 sin（q3） 才能計(jì)算 q3。調(diào)整式（12）并代入 a"

計(jì)算可得：

4 結(jié) 語

楔塊的轉(zhuǎn)角變化比平臺(tái)的傾角高一個(gè)數(shù)量級(jí)，因此它的控制精度高。又因?yàn)樾▔K基本是在水平面轉(zhuǎn)動(dòng)，所以伺服電機(jī)的負(fù)載轉(zhuǎn)矩小，也因此質(zhì)量輕、能耗低。加之它的結(jié)構(gòu)簡(jiǎn)單，相對(duì)于陀螺儀式穩(wěn)定平臺(tái)它的制造成本也更低?？傊ㄐ畏€(wěn)定平臺(tái)為穩(wěn)定平臺(tái)的設(shè)計(jì)提供了一種有益的新思路。

［1］MAAS A J. Wedge platform stabilization［J］. Revue Technique-Thomson-CSF， 1994， 26（3）: 553–569.

［2］孫成祿. 甩不掉的"尾巴"——艦載"天狼星"遠(yuǎn)程紅外搜索與跟蹤系統(tǒng)［J］. 現(xiàn)代兵器， 2004（6）: 28–29.

［3］郭隆華. 加拿大海軍驗(yàn)收首套天狼星紅外搜索與跟蹤系統(tǒng)［EB/OL］. （2008–5-20）. http://news.xinhuanet.com/ mil/2008–05/20/content_8213388.html.

［4］張志遠(yuǎn)， 羅國(guó)富. 艦船姿態(tài)坐標(biāo)變換及穩(wěn)定補(bǔ)償分析［J］. 艦船科學(xué)技術(shù)， 2009， 31（4）: 34–40. ZHANG Zhi-yuan， LUO Guo-fu. Coordinate transformation of warship pose and analysis of stabilization compensation［J］. Ship Science and Technology， 2009， 31（4）: 34–40.

［5］張坤石. 潛艇光電裝備技術(shù)［M］. 哈爾濱: 哈爾濱工程大學(xué)出版社， 2007: 429–437.

The equivalent manipulator model of wedge-platform

ZHU Yuan-ping1，2，3，4， TANG Xin-yi1，2，3，4， YUAN Xiu-peng2，3，4
（1. School of Information Science and Technology， Shanghai Technology University， Shanghai， 200030， China；2. Shanghai Institute of Technical Physics of the Chinese Academy of Sciences， Shanghai， 200080， China；3. Key Laboratory of Infrared System Detection and Imaging Technology， Chinese Academy of Sciences， Shanghai， 200080， China；4. Shanghai Key Laboratory of Crime Scene Evidence， Shanghai， 200080， China）

A stabilized platform is needed to compensate the disturbance of a moving carrier to keep the observation instrument relatively static to the ground. The wedge-platform changes the lean of its top plane to offset the vessel sway by rotating several wedges systematically， which has advantages as low produce cost， high accuracy， small load torque， low mass and low energy consumption. However it is confusing to analyze the wedge-platform for its unique structure. Therefore a simplifying transformation is created in this article which transforms the three-dimensional rotations of wedges to the twodimensional movement of an approximately equivalent manipulator. Then 2-layer， advanced 2-layer and multi-layer structures of wedge-platform are analyzed and compared， which leads to the conclusion that the advanced 2-layer structure is the most appropriate practice solution. Finally， a set of control functions for the advanced 2-layer wedge-platform is calculated through coordinate systems transformations by rotation matrices.

wedge-platform；sway；equivalent manipulator；layer；

U666.12+1

A

1672 – 7619（2016）04 – 0051 – 05

10.3404/j.issn.1672 – 7619.2016.04.011

2015 – 09 – 06；

2015 – 10 – 08

國(guó)家十二五國(guó)防預(yù)研資助項(xiàng)目（41101050501）；上海市現(xiàn)場(chǎng)物證重點(diǎn)實(shí)驗(yàn)室資金資助項(xiàng)目（2011xcwzk04）

朱元平（1990 – ），男，碩士研究生，研究方向?yàn)樾ㄐ畏€(wěn)定平臺(tái)。